Incorrect data from MPU6050 with ESP32S2 Wrover

Using Arduino Project Guidance
1

Hello everyone, I am using a ESP32S2 Wrover powered drone: Hardware Reference — ESP-Drone documentation .

I am programming the drone from scratch on my own. I recorded that the roll picth and yaw reading were fluctuating a little: (the Yaw is drifting a bit, the pitch is not so stable, you cannot see it because I did not put the full log of the file due to the huge amount of data)

R: -0.07	P: 0.06	Y: 0.00	F: 0.13 Hz
R: -0.14	P: 0.06	Y: -0.00	F: 997.01 Hz
R: -0.20	P: 0.06	Y: -0.00	F: 996.02 Hz
R: -0.25	P: 0.06	Y: -0.00	F: 996.02 Hz
R: -0.31	P: 0.06	Y: -0.00	F: 1000.00 Hz
R: -0.35	P: 0.05	Y: -0.00	F: 998.00 Hz
R: -0.40	P: 0.05	Y: -0.00	F: 998.00 Hz
R: -0.44	P: 0.05	Y: -0.00	F: 1000.00 Hz
R: -0.48	P: 0.05	Y: 0.00	F: 995.02 Hz
R: -0.51	P: 0.05	Y: 0.00	F: 1000.00 Hz
R: -0.47	P: 0.05	Y: 0.00	F: 995.02 Hz
R: -0.44	P: 0.05	Y: 0.00	F: 996.02 Hz
R: -0.41	P: 0.04	Y: 0.00	F: 997.01 Hz
R: -0.45	P: 0.04	Y: 0.00	F: 996.02 Hz
R: -0.49	P: 0.04	Y: -0.00	F: 995.02 Hz
R: -0.52	P: 0.04	Y: -0.00	F: 998.00 Hz
R: -0.55	P: 0.04	Y: -0.00	F: 1000.00 Hz
R: -0.58	P: 0.04	Y: -0.00	F: 995.02 Hz
R: -0.61	P: 0.03	Y: -0.00	F: 999.00 Hz
R: -0.63	P: 0.03	Y: -0.00	F: 996.02 Hz
R: -0.66	P: 0.04	Y: -0.00	F: 995.02 Hz
R: -0.68	P: 0.04	Y: -0.00	F: 995.02 Hz
R: -0.70	P: 0.04	Y: -0.00	F: 999.00 Hz
R: -0.71	P: 0.04	Y: -0.00	F: 999.00 Hz
R: -0.73	P: 0.04	Y: -0.00	F: 998.00 Hz
R: -0.74	P: 0.05	Y: -0.00	F: 995.02 Hz
R: -0.75	P: 0.05	Y: -0.00	F: 998.00 Hz
R: -0.77	P: 0.05	Y: -0.00	F: 999.00 Hz
R: -0.78	P: 0.05	Y: -0.00	F: 999.00 Hz
R: -0.79	P: 0.05	Y: -0.00	F: 999.00 Hz
R: -0.80	P: 0.05	Y: -0.00	F: 997.01 Hz
R: -0.81	P: 0.06	Y: -0.00	F: 999.00 Hz
R: -0.82	P: 0.06	Y: -0.00	F: 998.00 Hz
R: -0.82	P: 0.06	Y: -0.00	F: 998.00 Hz
R: -0.83	P: 0.06	Y: -0.00	F: 995.02 Hz
R: -0.83	P: 0.06	Y: -0.00	F: 996.02 Hz
R: -0.84	P: 0.06	Y: -0.00	F: 995.02 Hz
R: -0.84	P: 0.06	Y: -0.00	F: 996.02 Hz
R: -0.84	P: 0.06	Y: -0.00	F: 1000.00 Hz
R: -0.85	P: 0.06	Y: -0.00	F: 1000.00 Hz
R: -0.85	P: 0.06	Y: -0.00	F: 996.02 Hz
R: -0.86	P: 0.06	Y: -0.00	F: 995.02 Hz
R: -0.87	P: 0.07	Y: -0.00	F: 997.01 Hz
R: -0.87	P: 0.07	Y: -0.00	F: 1000.00 Hz
R: -0.88	P: 0.07	Y: -0.00	F: 999.00 Hz
R: -0.88	P: 0.07	Y: -0.00	F: 998.00 Hz
R: -0.89	P: 0.07	Y: -0.00	F: 999.00 Hz
R: -0.89	P: 0.07	Y: -0.00	F: 998.00 Hz
R: -0.90	P: 0.07	Y: -0.00	F: 999.00 Hz
R: -0.83	P: 0.07	Y: -0.00	F: 1000.00 Hz
R: -0.77	P: 0.07	Y: -0.00	F: 997.01 Hz
R: -0.78	P: 0.07	Y: -0.00	F: 998.00 Hz
R: -0.72	P: 0.07	Y: -0.00	F: 1000.00 Hz
R: -0.67	P: 0.07	Y: -0.00	F: 999.00 Hz
R: -0.62	P: 0.07	Y: -0.00	F: 997.01 Hz
R: -0.57	P: 0.07	Y: -0.00	F: 1000.00 Hz
R: -0.60	P: 0.07	Y: -0.00	F: 999.00 Hz
R: -0.62	P: 0.06	Y: -0.00	F: 1000.00 Hz
R: -0.65	P: 0.06	Y: -0.00	F: 999.00 Hz
R: -0.67	P: 0.06	Y: -0.00	F: 1000.00 Hz
R: -0.69	P: 0.06	Y: -0.00	F: 999.00 Hz
R: -0.71	P: 0.06	Y: -0.00	F: 1000.00 Hz
R: -0.72	P: 0.06	Y: -0.00	F: 998.00 Hz
R: -0.74	P: 0.06	Y: -0.00	F: 1000.00 Hz
R: -0.75	P: 0.06	Y: -0.00	F: 996.02 Hz
R: -0.77	P: 0.06	Y: -0.00	F: 995.02 Hz
R: -0.78	P: 0.06	Y: -0.00	F: 1000.00 Hz
R: -0.79	P: 0.05	Y: -0.00	F: 1000.00 Hz
R: -0.80	P: 0.05	Y: -0.00	F: 998.00 Hz
R: -0.81	P: 0.05	Y: -0.00	F: 997.01 Hz
R: -0.82	P: 0.04	Y: -0.00	F: 1000.00 Hz
R: -0.82	P: 0.04	Y: -0.00	F: 998.00 Hz
R: -0.83	P: 0.04	Y: -0.00	F: 999.00 Hz
R: -0.84	P: 0.04	Y: -0.00	F: 1000.00 Hz
R: -0.84	P: 0.03	Y: -0.00	F: 999.00 Hz
R: -0.85	P: 0.03	Y: -0.00	F: 1000.00 Hz
R: -0.86	P: 0.03	Y: -0.00	F: 1000.00 Hz
R: -0.86	P: 0.03	Y: -0.00	F: 1000.00 Hz
R: -0.87	P: 0.03	Y: -0.00	F: 997.01 Hz
R: -0.87	P: 0.02	Y: -0.00	F: 999.00 Hz
R: -0.87	P: 0.02	Y: -0.00	F: 999.00 Hz
R: -0.88	P: 0.02	Y: -0.00	F: 998.00 Hz
R: -0.88	P: 0.02	Y: -0.00	F: 998.00 Hz
R: -0.88	P: 0.02	Y: -0.00	F: 995.02 Hz
R: -0.88	P: 0.02	Y: -0.00	F: 997.01 Hz
R: -0.89	P: 0.02	Y: -0.00	F: 998.00 Hz
R: -0.89	P: 0.02	Y: -0.00	F: 999.00 Hz
R: -0.89	P: 0.02	Y: -0.00	F: 999.00 Hz
R: -0.90	P: 0.02	Y: -0.00	F: 995.02 Hz
R: -0.90	P: 0.02	Y: -0.00	F: 999.00 Hz
R: -0.83	P: 0.02	Y: -0.00	F: 997.01 Hz
R: -0.77	P: 0.02	Y: -0.00	F: 998.00 Hz
R: -0.71	P: 0.02	Y: -0.00	F: 997.01 Hz
R: -0.66	P: 0.02	Y: -0.00	F: 996.02 Hz
R: -0.61	P: 0.02	Y: -0.00	F: 999.00 Hz
R: -0.57	P: 0.02	Y: -0.00	F: 1000.00 Hz
R: -0.53	P: 0.02	Y: -0.00	F: 998.00 Hz
R: -0.49	P: 0.02	Y: -0.00	F: 1000.00 Hz
R: -0.45	P: 0.02	Y: -0.00	F: 995.02 Hz
R: -0.42	P: 0.02	Y: -0.00	F: 995.02 Hz
R: -0.39	P: 0.02	Y: -0.00	F: 995.02 Hz
R: -0.43	P: 0.02	Y: -0.00	F: 1000.00 Hz
R: -0.47	P: 0.02	Y: -0.00	F: 997.01 Hz
R: -0.51	P: 0.02	Y: -0.00	F: 1000.00 Hz
R: -0.54	P: 0.02	Y: -0.00	F: 999.00 Hz
R: -0.57	P: 0.02	Y: -0.00	F: 998.00 Hz
R: -0.53	P: 0.02	Y: -0.00	F: 1000.00 Hz
R: -0.49	P: 0.03	Y: -0.00	F: 999.00 Hz
R: -0.45	P: 0.03	Y: -0.00	F: 1000.00 Hz
R: -0.49	P: 0.03	Y: -0.00	F: 995.02 Hz
R: -0.52	P: 0.04	Y: -0.00	F: 999.00 Hz
R: -0.55	P: 0.04	Y: -0.00	F: 999.00 Hz
R: -0.58	P: 0.04	Y: -0.00	F: 1000.00 Hz
R: -0.61	P: 0.04	Y: -0.00	F: 999.00 Hz
R: -0.63	P: 0.05	Y: -0.00	F: 999.00 Hz
R: -0.66	P: 0.05	Y: -0.00	F: 1000.00 Hz
R: -0.68	P: 0.05	Y: -0.00	F: 998.00 Hz
R: -0.63	P: 0.06	Y: -0.00	F: 999.00 Hz
R: -0.58	P: 0.06	Y: -0.00	F: 999.00 Hz
R: -0.54	P: 0.06	Y: -0.00	F: 999.00 Hz
R: -0.50	P: 0.06	Y: -0.00	F: 999.00 Hz
R: -0.46	P: 0.06	Y: -0.00	F: 1000.00 Hz
R: -0.50	P: 0.06	Y: -0.00	F: 999.00 Hz
R: -0.53	P: 0.06	Y: -0.00	F: 999.00 Hz
R: -0.56	P: 0.07	Y: -0.00	F: 994.04 Hz
R: -0.59	P: 0.07	Y: -0.00	F: 999.00 Hz
R: -0.62	P: 0.07	Y: -0.00	F: 998.00 Hz
R: -0.65	P: 0.07	Y: -0.00	F: 999.00 Hz
R: -0.67	P: 0.07	Y: -0.00	F: 1000.00 Hz
R: -0.69	P: 0.07	Y: -0.00	F: 998.00 Hz
R: -0.71	P: 0.07	Y: -0.00	F: 996.02 Hz
R: -0.72	P: 0.07	Y: -0.00	F: 995.02 Hz
R: -0.74	P: 0.07	Y: -0.00	F: 998.00 Hz
R: -0.75	P: 0.07	Y: -0.00	F: 1000.00 Hz
R: -0.77	P: 0.07	Y: -0.00	F: 998.00 Hz
R: -0.78	P: 0.07	Y: -0.00	F: 1000.00 Hz
R: -0.79	P: 0.07	Y: -0.00	F: 999.00 Hz
R: -0.80	P: 0.07	Y: -0.00	F: 998.00 Hz
R: -0.81	P: 0.07	Y: -0.00	F: 998.00 Hz
R: -0.82	P: 0.06	Y: -0.00	F: 1000.00 Hz
R: -0.83	P: 0.06	Y: -0.00	F: 1000.00 Hz
R: -0.84	P: 0.06	Y: -0.00	F: 998.00 Hz
R: -0.85	P: 0.06	Y: -0.00	F: 1000.00 Hz
R: -0.85	P: 0.05	Y: -0.00	F: 998.00 Hz
R: -0.86	P: 0.05	Y: -0.00	F: 999.00 Hz
R: -0.86	P: 0.05	Y: -0.00	F: 999.00 Hz
R: -0.87	P: 0.05	Y: -0.00	F: 998.00 Hz
R: -0.87	P: 0.05	Y: -0.00	F: 997.01 Hz
R: -0.88	P: 0.05	Y: -0.00	F: 1000.00 Hz
R: -0.88	P: 0.04	Y: -0.00	F: 998.00 Hz
R: -0.89	P: 0.04	Y: -0.00	F: 997.01 Hz
R: -0.89	P: 0.04	Y: -0.00	F: 999.00 Hz
R: -0.89	P: 0.03	Y: -0.00	F: 998.00 Hz
R: -0.90	P: 0.03	Y: -0.00	F: 997.01 Hz
R: -0.90	P: 0.03	Y: -0.00	F: 997.01 Hz
R: -0.90	P: 0.03	Y: -0.00	F: 998.00 Hz
R: -0.91	P: 0.03	Y: -0.00	F: 998.00 Hz
R: -0.91	P: 0.02	Y: -0.00	F: 999.00 Hz
R: -0.91	P: 0.02	Y: -0.00	F: 999.00 Hz
R: -0.91	P: 0.02	Y: -0.00	F: 999.00 Hz
R: -0.91	P: 0.02	Y: -0.00	F: 995.02 Hz
R: -0.92	P: 0.02	Y: -0.00	F: 998.00 Hz
R: -0.92	P: 0.02	Y: -0.00	F: 998.00 Hz
R: -0.85	P: 0.02	Y: -0.00	F: 1000.00 Hz
R: -0.86	P: 0.02	Y: -0.00	F: 999.00 Hz
R: -0.86	P: 0.02	Y: -0.00	F: 999.00 Hz
R: -0.80	P: 0.02	Y: -0.00	F: 997.01 Hz
R: -0.74	P: 0.02	Y: -0.00	F: 997.01 Hz
R: -0.68	P: 0.02	Y: -0.00	F: 996.02 Hz
R: -0.63	P: 0.02	Y: -0.00	F: 997.01 Hz
R: -0.59	P: 0.02	Y: -0.00	F: 999.00 Hz
R: -0.54	P: 0.02	Y: -0.00	F: 999.00 Hz
R: -0.50	P: 0.02	Y: -0.00	F: 997.01 Hz
R: -0.47	P: 0.02	Y: -0.00	F: 995.02 Hz
R: -0.43	P: 0.03	Y: -0.00	F: 1000.00 Hz
R: -0.40	P: 0.03	Y: -0.00	F: 996.02 Hz
R: -0.37	P: 0.03	Y: -0.00	F: 995.02 Hz
R: -0.35	P: 0.03	Y: -0.00	F: 1000.00 Hz
R: -0.32	P: 0.03	Y: -0.00	F: 995.02 Hz
R: -0.30	P: 0.03	Y: -0.00	F: 996.02 Hz
R: -0.28	P: 0.03	Y: -0.00	F: 998.00 Hz
R: -0.26	P: 0.03	Y: -0.00	F: 997.01 Hz
R: -0.25	P: 0.03	Y: -0.00	F: 998.00 Hz
R: -0.23	P: 0.03	Y: -0.00	F: 998.00 Hz
R: -0.22	P: 0.03	Y: -0.00	F: 1000.00 Hz
R: -0.21	P: 0.03	Y: -0.00	F: 999.00 Hz
R: -0.19	P: 0.03	Y: -0.00	F: 998.00 Hz
R: -0.18	P: 0.03	Y: -0.00	F: 999.00 Hz
R: -0.18	P: 0.03	Y: -0.00	F: 1000.00 Hz
R: -0.17	P: 0.02	Y: -0.00	F: 999.00 Hz
R: -0.16	P: 0.02	Y: -0.00	F: 1000.00 Hz
R: -0.15	P: 0.02	Y: -0.00	F: 997.01 Hz
R: -0.14	P: 0.02	Y: -0.00	F: 999.00 Hz
R: -0.14	P: 0.02	Y: -0.00	F: 1000.00 Hz
R: -0.13	P: 0.02	Y: -0.00	F: 999.00 Hz
R: -0.12	P: 0.02	Y: -0.00	F: 998.00 Hz
R: -0.12	P: 0.02	Y: -0.00	F: 995.02 Hz
R: -0.11	P: 0.02	Y: -0.00	F: 997.01 Hz
R: -0.10	P: 0.02	Y: -0.00	F: 995.02 Hz
R: -0.10	P: 0.02	Y: -0.00	F: 1000.00 Hz
R: -0.09	P: 0.02	Y: -0.00	F: 997.01 Hz
R: -0.09	P: 0.02	Y: -0.00	F: 998.00 Hz
R: -0.09	P: 0.02	Y: -0.00	F: 999.00 Hz
R: -0.08	P: 0.02	Y: -0.00	F: 1000.00 Hz
R: -0.08	P: 0.02	Y: -0.00	F: 995.02 Hz
R: -0.08	P: 0.02	Y: -0.00	F: 996.02 Hz
R: -0.07	P: 0.02	Y: -0.00	F: 998.00 Hz
R: -0.07	P: 0.02	Y: -0.00	F: 998.00 Hz
R: -0.07	P: 0.02	Y: -0.00	F: 997.01 Hz
R: -0.06	P: 0.02	Y: -0.00	F: 996.02 Hz
R: -0.06	P: 0.03	Y: -0.00	F: 997.01 Hz
R: -0.06	P: 0.03	Y: -0.00	F: 995.02 Hz
R: -0.06	P: 0.03	Y: -0.00	F: 1000.00 Hz
R: -0.06	P: 0.03	Y: -0.00	F: 999.00 Hz
R: -0.06	P: 0.03	Y: -0.00	F: 999.00 Hz
R: -0.06	P: 0.03	Y: -0.00	F: 1000.00 Hz
R: -0.06	P: 0.02	Y: -0.00	F: 996.02 Hz
R: -0.06	P: 0.02	Y: -0.00	F: 998.00 Hz
R: -0.06	P: 0.02	Y: -0.00	F: 996.02 Hz
R: -0.06	P: 0.02	Y: -0.00	F: 996.02 Hz
R: -0.05	P: 0.02	Y: -0.00	F: 999.00 Hz
R: -0.05	P: 0.02	Y: -0.00	F: 997.01 Hz
R: -0.05	P: 0.02	Y: -0.00	F: 995.02 Hz
R: -0.05	P: 0.02	Y: -0.00	F: 996.02 Hz
R: -0.05	P: 0.02	Y: -0.00	F: 996.02 Hz
R: -0.04	P: 0.02	Y: -0.00	F: 998.00 Hz
R: -0.04	P: 0.02	Y: -0.00	F: 999.00 Hz
R: -0.04	P: 0.02	Y: -0.00	F: 1000.00 Hz
R: -0.04	P: 0.02	Y: -0.00	F: 996.02 Hz
R: -0.04	P: 0.02	Y: -0.00	F: 997.01 Hz
R: -0.04	P: 0.02	Y: -0.00	F: 995.02 Hz
R: -0.04	P: 0.01	Y: -0.00	F: 996.02 Hz
R: -0.04	P: 0.02	Y: -0.00	F: 996.02 Hz
R: -0.03	P: 0.02	Y: -0.00	F: 996.02 Hz
R: -0.03	P: 0.02	Y: -0.00	F: 999.00 Hz
R: -0.03	P: 0.02	Y: -0.00	F: 995.02 Hz
R: -0.03	P: 0.02	Y: -0.00	F: 997.01 Hz
R: -0.03	P: 0.02	Y: -0.00	F: 996.02 Hz
R: -0.03	P: 0.02	Y: -0.00	F: 998.00 Hz
R: -0.03	P: 0.02	Y: -0.00	F: 996.02 Hz
R: -0.04	P: 0.02	Y: -0.00	F: 995.02 Hz
R: -0.04	P: 0.02	Y: -0.00	F: 997.01 Hz
R: -0.04	P: 0.02	Y: -0.00	F: 1000.00 Hz
R: -0.04	P: 0.02	Y: -0.00	F: 999.00 Hz
R: -0.04	P: 0.02	Y: -0.00	F: 1000.00 Hz
R: -0.04	P: 0.02	Y: -0.00	F: 1000.00 Hz
R: -0.04	P: 0.03	Y: -0.00	F: 998.00 Hz
R: -0.04	P: 0.03	Y: -0.00	F: 995.02 Hz
R: -0.04	P: 0.03	Y: -0.00	F: 1000.00 Hz
R: -0.04	P: 0.03	Y: -0.00	F: 998.00 Hz
R: -0.04	P: 0.03	Y: -0.00	F: 995.02 Hz
R: -0.04	P: 0.03	Y: -0.00	F: 1000.00 Hz
R: -0.04	P: 0.03	Y: -0.00	F: 999.00 Hz
R: -0.03	P: 0.03	Y: -0.00	F: 1000.00 Hz
R: -0.03	P: 0.03	Y: -0.00	F: 999.00 Hz
R: -0.03	P: 0.03	Y: -0.00	F: 999.00 Hz
R: -0.03	P: 0.03	Y: -0.00	F: 995.02 Hz
R: -0.03	P: 0.03	Y: -0.00	F: 997.01 Hz
R: -0.03	P: 0.03	Y: -0.00	F: 998.00 Hz
R: -0.03	P: 0.03	Y: -0.00	F: 998.00 Hz
R: -0.03	P: 0.03	Y: -0.00	F: 998.00 Hz
R: -0.03	P: 0.03	Y: -0.00	F: 997.01 Hz
R: -0.02	P: 0.03	Y: -0.00	F: 999.00 Hz
R: -0.02	P: 0.03	Y: -0.00	F: 1000.00 Hz
R: -0.02	P: 0.04	Y: -0.00	F: 999.00 Hz
R: -0.02	P: 0.04	Y: -0.00	F: 1000.00 Hz
R: -0.02	P: 0.04	Y: -0.00	F: 999.00 Hz
R: -0.02	P: 0.04	Y: -0.00	F: 999.00 Hz
R: -0.02	P: 0.04	Y: -0.00	F: 1000.00 Hz
R: -0.02	P: 0.04	Y: -0.00	F: 999.00 Hz
R: -0.02	P: 0.04	Y: -0.00	F: 998.00 Hz
R: -0.02	P: 0.03	Y: -0.00	F: 999.00 Hz
R: -0.02	P: 0.03	Y: -0.00	F: 995.02 Hz
R: -0.02	P: 0.03	Y: -0.00	F: 998.00 Hz
R: -0.02	P: 0.03	Y: -0.00	F: 1000.00 Hz
R: -0.02	P: 0.03	Y: -0.00	F: 999.00 Hz
R: -0.02	P: 0.03	Y: -0.00	F: 1000.00 Hz
R: -0.02	P: 0.03	Y: -0.00	F: 999.00 Hz
R: -0.02	P: 0.03	Y: -0.00	F: 999.00 Hz
R: -0.01	P: 0.02	Y: -0.00	F: 995.02 Hz
R: -0.01	P: 0.02	Y: -0.00	F: 1000.00 Hz
R: -0.01	P: 0.02	Y: -0.00	F: 999.00 Hz
R: -0.08	P: 0.02	Y: -0.00	F: 999.00 Hz
R: -0.15	P: 0.02	Y: -0.00	F: 995.02 Hz
R: -0.21	P: 0.01	Y: -0.00	F: 999.00 Hz
R: -0.27	P: 0.01	Y: -0.00	F: 999.00 Hz
R: -0.25	P: 0.01	Y: -0.00	F: 999.00 Hz
R: -0.23	P: 0.01	Y: -0.00	F: 1000.00 Hz
R: -0.21	P: 0.01	Y: -0.00	F: 999.00 Hz
R: -0.20	P: 0.01	Y: -0.00	F: 995.02 Hz
R: -0.19	P: 0.01	Y: -0.00	F: 999.00 Hz
R: -0.18	P: 0.01	Y: -0.00	F: 1000.00 Hz
R: -0.17	P: 0.01	Y: -0.00	F: 1000.00 Hz
R: -0.16	P: 0.01	Y: -0.00	F: 1000.00 Hz
R: -0.15	P: 0.01	Y: 0.00	F: 997.01 Hz
R: -0.14	P: 0.00	Y: 0.00	F: 997.01 Hz
R: -0.13	P: 0.00	Y: 0.00	F: 997.01 Hz
R: -0.12	P: 0.00	Y: 0.00	F: 995.02 Hz
R: -0.12	P: 0.00	Y: 0.00	F: 996.02 Hz
R: -0.11	P: -0.00	Y: 0.00	F: 996.02 Hz
R: -0.10	P: -0.00	Y: 0.00	F: 1000.00 Hz
R: -0.10	P: -0.00	Y: 0.00	F: 999.00 Hz
R: -0.09	P: -0.00	Y: 0.00	F: 999.00 Hz
R: -0.09	P: -0.00	Y: 0.00	F: 999.00 Hz
R: -0.09	P: -0.00	Y: 0.00	F: 995.02 Hz
R: -0.08	P: -0.00	Y: 0.00	F: 995.02 Hz
R: -0.08	P: -0.00	Y: 0.00	F: 996.02 Hz
R: -0.08	P: -0.00	Y: 0.00	F: 1000.00 Hz

So, I printed the values of the accelerometer and gyroscope for each axis:

accelX: 0.00 accelY: -0.00 accelZ: 0.94 gyroX: 0.03 gyroY: -0.02 gyroZ: 0.03
accelX: 0.00 accelY: -0.00 accelZ: 0.94 gyroX: 0.04 gyroY: -0.02 gyroZ: 0.03
accelX: 0.00 accelY: -0.00 accelZ: 0.94 gyroX: 0.04 gyroY: -0.02 gyroZ: 0.04
accelX: 0.00 accelY: -0.00 accelZ: 0.94 gyroX: 0.04 gyroY: -0.02 gyroZ: 0.05
accelX: 0.00 accelY: -0.00 accelZ: 0.94 gyroX: 0.04 gyroY: -0.02 gyroZ: 0.05
accelX: 0.00 accelY: -0.00 accelZ: 0.94 gyroX: 0.04 gyroY: -0.02 gyroZ: 0.04
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.04 gyroY: -0.01 gyroZ: 0.03
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.04 gyroY: -0.01 gyroZ: 0.03
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.04 gyroY: 0.00 gyroZ: 0.03
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.03 gyroY: 0.00 gyroZ: 0.03
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.03 gyroY: -0.01 gyroZ: 0.03
accelX: 0.00 accelY: 0.00 accelZ: 1.00 gyroX: 0.04 gyroY: -0.01 gyroZ: 0.03
accelX: 0.00 accelY: 0.00 accelZ: 1.00 gyroX: 0.03 gyroY: 0.00 gyroZ: 0.03
accelX: 0.00 accelY: -0.00 accelZ: 1.00 gyroX: 0.03 gyroY: 0.00 gyroZ: 0.03
accelX: 0.00 accelY: -0.00 accelZ: 1.00 gyroX: 0.03 gyroY: 0.00 gyroZ: 0.03
accelX: 0.00 accelY: -0.00 accelZ: 0.94 gyroX: 0.03 gyroY: 0.00 gyroZ: 0.03
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.03 gyroY: 0.00 gyroZ: 0.04
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.04 gyroY: 0.01 gyroZ: 0.03
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.04 gyroY: 0.00 gyroZ: 0.03
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.03 gyroY: 0.00 gyroZ: 0.03
accelX: 0.00 accelY: 0.00 accelZ: 1.00 gyroX: 0.03 gyroY: 0.01 gyroZ: 0.03
accelX: 0.00 accelY: 0.00 accelZ: 1.00 gyroX: 0.03 gyroY: 0.00 gyroZ: 0.04
accelX: 0.00 accelY: 0.00 accelZ: 1.00 gyroX: 0.03 gyroY: 0.01 gyroZ: 0.05
accelX: 0.00 accelY: 0.00 accelZ: 1.00 gyroX: 0.03 gyroY: 0.01 gyroZ: 0.04
accelX: 0.00 accelY: 0.00 accelZ: 1.00 gyroX: 0.02 gyroY: 0.01 gyroZ: 0.03
accelX: 0.00 accelY: 0.00 accelZ: 1.00 gyroX: 0.02 gyroY: 0.01 gyroZ: 0.03
accelX: 0.00 accelY: 0.00 accelZ: 1.00 gyroX: 0.03 gyroY: 0.00 gyroZ: 0.02
accelX: 0.00 accelY: 0.00 accelZ: 1.00 gyroX: 0.03 gyroY: -0.01 gyroZ: 0.02
accelX: 0.00 accelY: 0.00 accelZ: 1.00 gyroX: 0.03 gyroY: 0.00 gyroZ: 0.02
accelX: 0.00 accelY: 0.00 accelZ: 1.00 gyroX: 0.03 gyroY: 0.00 gyroZ: 0.02
accelX: 0.00 accelY: 0.00 accelZ: 1.00 gyroX: 0.04 gyroY: 0.01 gyroZ: 0.01
accelX: 0.00 accelY: 0.00 accelZ: 1.00 gyroX: 0.04 gyroY: 0.01 gyroZ: 0.01
accelX: 0.00 accelY: 0.00 accelZ: 1.00 gyroX: 0.04 gyroY: 0.01 gyroZ: 0.01
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.03 gyroY: 0.02 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.04 gyroY: 0.02 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.04 gyroY: 0.02 gyroZ: 0.02
accelX: 0.00 accelY: 0.00 accelZ: 1.00 gyroX: 0.04 gyroY: 0.02 gyroZ: 0.02
accelX: 0.00 accelY: 0.00 accelZ: 1.00 gyroX: 0.03 gyroY: 0.02 gyroZ: 0.02
accelX: 0.00 accelY: 0.00 accelZ: 1.00 gyroX: 0.03 gyroY: 0.02 gyroZ: 0.03
accelX: 0.00 accelY: 0.00 accelZ: 1.00 gyroX: 0.04 gyroY: 0.02 gyroZ: 0.03
accelX: 0.00 accelY: 0.00 accelZ: 1.00 gyroX: 0.04 gyroY: 0.02 gyroZ: 0.03
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.04 gyroY: 0.01 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.04 gyroY: 0.01 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.04 gyroY: 0.01 gyroZ: 0.02
accelX: 0.02 accelY: -0.00 accelZ: 1.00 gyroX: 0.04 gyroY: 0.01 gyroZ: 0.02
accelX: 0.02 accelY: -0.00 accelZ: 1.00 gyroX: 0.03 gyroY: 0.01 gyroZ: 0.02
accelX: 0.02 accelY: -0.00 accelZ: 1.00 gyroX: 0.03 gyroY: 0.01 gyroZ: 0.02
accelX: 0.02 accelY: -0.00 accelZ: 1.00 gyroX: 0.03 gyroY: 0.01 gyroZ: 0.02
accelX: 0.02 accelY: -0.00 accelZ: 1.00 gyroX: 0.02 gyroY: 0.01 gyroZ: 0.02
accelX: 0.02 accelY: -0.00 accelZ: 1.00 gyroX: 0.02 gyroY: 0.01 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.02 gyroY: 0.01 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.02 gyroY: 0.01 gyroZ: 0.01
accelX: 0.02 accelY: -0.00 accelZ: 1.00 gyroX: 0.02 gyroY: 0.00 gyroZ: 0.01
accelX: 0.02 accelY: -0.00 accelZ: 1.00 gyroX: 0.02 gyroY: 0.00 gyroZ: 0.01
accelX: 0.02 accelY: -0.00 accelZ: 1.00 gyroX: 0.02 gyroY: 0.00 gyroZ: 0.01
accelX: 0.02 accelY: -0.00 accelZ: 1.00 gyroX: 0.02 gyroY: 0.00 gyroZ: 0.01
accelX: 0.02 accelY: -0.00 accelZ: 1.00 gyroX: 0.02 gyroY: 0.00 gyroZ: 0.01
accelX: 0.02 accelY: -0.00 accelZ: 1.00 gyroX: 0.02 gyroY: 0.01 gyroZ: 0.01
accelX: 0.02 accelY: -0.00 accelZ: 1.00 gyroX: 0.02 gyroY: 0.02 gyroZ: 0.01
accelX: 0.02 accelY: -0.00 accelZ: 1.00 gyroX: 0.01 gyroY: 0.02 gyroZ: 0.01
accelX: 0.02 accelY: -0.00 accelZ: 1.00 gyroX: 0.01 gyroY: 0.02 gyroZ: 0.01
accelX: 0.02 accelY: -0.00 accelZ: 1.00 gyroX: 1.95 gyroY: 0.01 gyroZ: 0.00
accelX: 0.02 accelY: -0.00 accelZ: 1.00 gyroX: 1.95 gyroY: 0.00 gyroZ: 0.00
accelX: 0.02 accelY: -0.00 accelZ: 1.00 gyroX: 1.95 gyroY: 0.00 gyroZ: 0.00
accelX: 0.02 accelY: -0.00 accelZ: 1.00 gyroX: 1.95 gyroY: 0.01 gyroZ: 0.01
accelX: 0.02 accelY: -0.00 accelZ: 1.00 gyroX: 1.95 gyroY: 0.01 gyroZ: 0.02
accelX: 0.02 accelY: -0.00 accelZ: 1.00 gyroX: 1.95 gyroY: 0.00 gyroZ: 0.02
accelX: 0.02 accelY: -0.00 accelZ: 1.00 gyroX: 1.95 gyroY: 0.00 gyroZ: 0.02
accelX: 0.02 accelY: -0.00 accelZ: 1.00 gyroX: 1.95 gyroY: 0.00 gyroZ: 0.02
accelX: 0.02 accelY: -0.00 accelZ: 1.00 gyroX: 1.95 gyroY: 0.00 gyroZ: 0.02
accelX: 0.01 accelY: -0.00 accelZ: 1.00 gyroX: 1.95 gyroY: 0.00 gyroZ: 0.02
accelX: 0.01 accelY: -0.00 accelZ: 1.00 gyroX: 1.95 gyroY: 0.00 gyroZ: 0.02
accelX: 0.01 accelY: -0.00 accelZ: 1.00 gyroX: 1.94 gyroY: 0.00 gyroZ: 0.01
accelX: 0.01 accelY: -0.00 accelZ: 1.00 gyroX: 1.94 gyroY: 0.00 gyroZ: 0.00
accelX: 0.01 accelY: -0.00 accelZ: 1.00 gyroX: 1.94 gyroY: 0.00 gyroZ: 0.00
accelX: 0.01 accelY: 0.00 accelZ: 1.00 gyroX: 1.95 gyroY: 0.00 gyroZ: -0.01
accelX: 0.01 accelY: 0.00 accelZ: 1.00 gyroX: 1.95 gyroY: 0.00 gyroZ: -0.01
accelX: 0.01 accelY: 0.00 accelZ: 1.00 gyroX: 1.95 gyroY: 0.00 gyroZ: -0.01
accelX: 0.01 accelY: 0.00 accelZ: 1.00 gyroX: 1.95 gyroY: -0.01 gyroZ: 0.00
accelX: 0.01 accelY: 0.00 accelZ: 1.00 gyroX: 1.95 gyroY: 0.00 gyroZ: 0.00
accelX: 0.01 accelY: 0.00 accelZ: 1.00 gyroX: 1.95 gyroY: 0.00 gyroZ: -0.01
accelX: 0.01 accelY: -0.00 accelZ: 1.00 gyroX: 1.95 gyroY: 0.00 gyroZ: -0.01
accelX: 0.01 accelY: -0.00 accelZ: 1.00 gyroX: 1.95 gyroY: 0.00 gyroZ: 0.00
accelX: 0.01 accelY: -0.00 accelZ: 1.00 gyroX: 0.01 gyroY: 0.00 gyroZ: 0.00
accelX: 0.01 accelY: -0.00 accelZ: 1.00 gyroX: 0.01 gyroY: 0.00 gyroZ: 0.00
accelX: 0.01 accelY: -0.00 accelZ: 1.00 gyroX: 0.01 gyroY: 0.00 gyroZ: 0.00
accelX: 0.01 accelY: -0.00 accelZ: 1.00 gyroX: 0.01 gyroY: 0.01 gyroZ: 0.00
accelX: 0.01 accelY: -0.00 accelZ: 1.00 gyroX: 0.01 gyroY: 0.01 gyroZ: 0.01
accelX: 0.01 accelY: -0.00 accelZ: 0.94 gyroX: 0.01 gyroY: 0.01 gyroZ: 0.00
accelX: 0.01 accelY: -0.00 accelZ: 0.94 gyroX: 0.01 gyroY: 0.01 gyroZ: 0.00
accelX: 0.02 accelY: -0.00 accelZ: 0.94 gyroX: 1.95 gyroY: 0.00 gyroZ: -0.01
accelX: 0.01 accelY: 0.00 accelZ: 1.00 gyroX: 1.95 gyroY: 0.01 gyroZ: -0.01
accelX: 0.01 accelY: 0.00 accelZ: 1.00 gyroX: 0.01 gyroY: 0.01 gyroZ: -0.02
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.01 gyroY: 0.00 gyroZ: -0.02
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.01 gyroY: 0.00 gyroZ: -0.02
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.01 gyroY: -0.01 gyroZ: -0.02
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.01 gyroY: -0.02 gyroZ: -0.01
accelX: 0.01 accelY: 0.00 accelZ: 1.00 gyroX: 0.01 gyroY: -0.01 gyroZ: -0.01
accelX: 0.01 accelY: 0.00 accelZ: 1.00 gyroX: 1.95 gyroY: -0.01 gyroZ: -0.01
accelX: 0.01 accelY: 0.00 accelZ: 1.00 gyroX: 1.95 gyroY: -0.01 gyroZ: -0.01
accelX: 0.01 accelY: 0.00 accelZ: 0.94 gyroX: 1.95 gyroY: -0.01 gyroZ: 0.00
accelX: 0.01 accelY: 0.00 accelZ: 0.94 gyroX: 1.95 gyroY: -0.01 gyroZ: 0.00
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.01 gyroY: 0.00 gyroZ: 0.00
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.02 gyroY: 0.00 gyroZ: 0.01
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.02 gyroY: 0.00 gyroZ: 0.01
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.03 gyroY: 0.00 gyroZ: 0.01
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.03 gyroY: 0.00 gyroZ: 0.01
accelX: 0.02 accelY: 0.00 accelZ: 0.94 gyroX: 0.03 gyroY: -0.01 gyroZ: 0.01
accelX: 0.02 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: -0.01 gyroZ: 0.01
accelX: 0.02 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: -0.01 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.02 gyroY: -0.02 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.02 gyroY: -0.02 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.02 gyroY: -0.02 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.02 gyroY: -0.02 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.02 gyroY: -0.02 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.01 gyroY: -0.02 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.01 gyroY: -0.02 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.02 gyroY: -0.02 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.02 gyroY: -0.02 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.02 gyroY: -0.02 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 0.94 gyroX: 0.01 gyroY: -0.03 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: -0.03 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: -0.02 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 0.94 gyroX: 0.01 gyroY: -0.02 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 0.94 gyroX: 0.01 gyroY: -0.02 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: -0.02 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: -0.02 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: -0.02 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 0.94 gyroX: 0.01 gyroY: -0.02 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 0.94 gyroX: 1.95 gyroY: -0.01 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 0.94 gyroX: 1.95 gyroY: -0.01 gyroZ: 0.01
accelX: 0.02 accelY: 0.00 accelZ: 0.94 gyroX: 1.95 gyroY: -0.01 gyroZ: 0.01
accelX: 0.02 accelY: 0.00 accelZ: 0.94 gyroX: 1.95 gyroY: -0.01 gyroZ: 0.01
accelX: 0.02 accelY: 0.00 accelZ: 0.94 gyroX: 1.95 gyroY: -0.01 gyroZ: 0.01
accelX: 0.02 accelY: 0.00 accelZ: 0.94 gyroX: 0.01 gyroY: -0.01 gyroZ: 0.01
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.01 gyroY: -0.01 gyroZ: 0.01
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.01 gyroY: -0.01 gyroZ: 0.00
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.01 gyroY: -0.01 gyroZ: 0.00
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: -0.01 gyroZ: 0.00
accelX: 0.02 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: -0.02 gyroZ: 0.01
accelX: 0.02 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: -0.02 gyroZ: 0.01
accelX: 0.02 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: -0.02 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: -0.02 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: -0.02 gyroZ: 0.02
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: -0.02 gyroZ: 0.02
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: -0.02 gyroZ: 0.02
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: -0.02 gyroZ: 0.02
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: -0.02 gyroZ: 0.02
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: -0.02 gyroZ: 0.02
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: -0.02 gyroZ: 0.02
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.03 gyroY: -0.01 gyroZ: 0.02
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.03 gyroY: -0.01 gyroZ: 0.02
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.03 gyroY: -0.01 gyroZ: 0.02
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.03 gyroY: 0.00 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 0.94 gyroX: 0.03 gyroY: 0.00 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: 0.00 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: 0.01 gyroZ: 0.02
accelX: 0.02 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: 0.02 gyroZ: 0.01
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: 0.02 gyroZ: 0.00
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: 0.02 gyroZ: -0.01
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: 0.02 gyroZ: -0.01
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: 0.01 gyroZ: -0.01
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: 0.02 gyroZ: -0.01
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: 0.01 gyroZ: -0.01
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: 0.01 gyroZ: -0.01
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: 0.01 gyroZ: 0.00
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: 0.01 gyroZ: 0.00
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: 0.02 gyroZ: 0.00
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: 0.02 gyroZ: 0.00
accelX: 0.02 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: 0.02 gyroZ: 0.00
accelX: 0.02 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: 0.02 gyroZ: 0.00
accelX: 0.00 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: 0.02 gyroZ: 0.00
accelX: 0.02 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: 0.02 gyroZ: 0.00
accelX: 0.02 accelY: 0.00 accelZ: 0.94 gyroX: 0.02 gyroY: 0.01 gyroZ: 0.00
accelX: 0.02 accelY: 0.00 accelZ: 0.94 gyroX: 0.03 gyroY: 0.01 gyroZ: 0.00
accelX: 0.00 accelY: 0.00 accelZ: 1.00 gyroX: 0.03 gyroY: 0.01 gyroZ: 0.00
accelX: 0.00 accelY: 0.00 accelZ: 1.00 gyroX: 0.04 gyroY: 0.01 gyroZ: 0.00
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.04 gyroY: 0.01 gyroZ: 0.00
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.04 gyroY: 0.01 gyroZ: 0.00
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.03 gyroY: 0.01 gyroZ: 0.00
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.03 gyroY: 0.01 gyroZ: 0.00
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.02 gyroY: 0.02 gyroZ: 0.00
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.02 gyroY: 0.02 gyroZ: 0.00
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.02 gyroY: 0.02 gyroZ: -0.01
accelX: 0.02 accelY: 0.00 accelZ: 1.00 gyroX: 0.02 gyroY: 0.02 gyroZ: -0.01

As you can see: All data seems to be stationary within 0.01 error except the AZ which was changing between the two specific values of 1.0 a 0.94. The GX value was corrupted, the data would be close to 0 with 0.02 error and then suddenly jump to 1.95 and then go back to 0 and repeat. It is still not clear the reason for this behaviour.

The setup was done with the drone stationary.

This is what I tried to do to solve the issue but I have been not successful as the same error remained:

  • Change the drone: assuming that the MPU6050 might be faulty.
  • Decrease the loop frequency from 1KHz to 500 Hz: thinking it would give more time to the sensor to stabilise the data.
  • Reduce the I2C communication speed from 1MHz to 400KHz: increasing the reliability of the communication.

None of these solutions worked. Anyone has an insight of what could be the error?

Here is the full code:

/*
 THIS CODE READS THE MPU6050 AT THE MAX SPEED SUPPORTED 
 PRINTS THE ROLL YAW AND PITCH AS FAST AS POSSIBLE
 THE LOOP FREQUENCY IS LIMITED AT 1 KHZ 
*/

#include <Wire.h>
#include <math.h>

#define LED_BLE_PIN 7
#define LED_RED_PIN 8
#define LED_GRN_PIN 9

const int MPU_ADDR = 0x68; // I2C address of the MPU-6050
const int CONFIG = 0x1A;  // Address of the low pass filter register
float alpha = 0.98f;  // Initial complementary filter constant

// // Add these variables for offsets
// int16_t ax_offset = -263, ay_offset = -95, az_offset = -1157;
// int16_t gx_offset = -226, gy_offset = 58, gz_offset = 179;

// Offset variables
int16_t ax_offset = 0, ay_offset = 0, az_offset = 0;
int16_t gx_offset = 0, gy_offset = 0, gz_offset = 0;

float roll = 0.0f;
float pitch = 0.0f;
float yaw = 0.0f;

unsigned long lastTime = 0;
unsigned long currentTime = 0;
double dt = 0.001f; // Initial dt value for the first loop
int loopFreq = 1000;  //in microseconds

float linear_acc_threshold = 0.2f; //variable to decide when to use or discard accelerometer data

int i=0, cyclesPrint = 10;  //debug printing variable and cycles

void setup() {
  pinMode(LED_GRN_PIN, OUTPUT);
  pinMode(LED_BLE_PIN, OUTPUT);
  pinMode(LED_RED_PIN, OUTPUT);

  Serial.begin(921600); // highest speed possible on my setup, higher than this I get errors
  Wire.begin(11, 10); // Initialize I2C with SDA on GPIO11 and SCL on GPIO10
  Wire.setClock(1000000); // Clock speed at 1MHz (highest available)

  // Initialize MPU6050
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B); // PWR_MGMT_1 register
  Wire.write(0);    // Set to 0 to wake up the MPU-6050
  Wire.endTransmission(true);

  // Set accelerometer sensitivity to ±2g
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x1C); // ACCEL_CONFIG register
  Wire.write(0);    // Set sensitivity to ±2g
  Wire.endTransmission(true);

  // Set gyroscope sensitivity to ±250 degrees/second
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x1B); // GYRO_CONFIG register
  Wire.write(0);    // Set sensitivity to ±250 degrees/second
  Wire.endTransmission(true);

  // Configure the DLPF to 5 Hz
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(CONFIG);
  Wire.write(0x06); // DLPF_CFG = 6 (5 Hz)
  Wire.endTransmission(true);

  // Initial delay to allow the sensor to stabilize
  delay(2000);

  //calibrate sensor at each startup
  digitalWrite(LED_BLE_PIN, HIGH);
  calibrateSensors();
  digitalWrite(LED_BLE_PIN, LOW);

}

void loop() {
  currentTime = micros();
  dt = (currentTime - lastTime); // Convert dt to seconds

  if (dt >= loopFreq) { // Ensure the loop runs at approximately 1 kHz (1ms per loop)
    lastTime = currentTime;
    dt /= 1000000.0f;
    // Read raw values from MPU6050
    int16_t ax, ay, az, gx, gy, gz;
    readMPU6050Data(ax, ay, az, gx, gy, gz);
   
    // Convert raw values to accelerometer (g) and gyroscope (degrees/second)
    // Negative signs to match the orientation of the drone
    float accelX = -ax / 16384.0;
    float accelY = -ay / 16384.0;
    float accelZ = az / 16384.0;
    float gyroX = -gx / 131.0;
    float gyroY = -gy / 131.0;
    float gyroZ = gz / 131.0;
   
    // Calculate roll and pitch from the accelerometer data
    float accelPitch = atan2(accelY, accelZ) * 180 / PI;
    float accelRoll = atan2(-accelX, sqrt(accelY * accelY + accelZ * accelZ)) * 180 / PI;

    // Calculate the magnitude of the acceleration vector
    float accelMagnitude = sqrt(accelX * accelX + accelY * accelY + accelZ * accelZ);
    
    /*// Integrate gyroscope data to get angles, accounting for cross-coupling effects using small angle approximation
    float rollRad = roll * PI / 180;
    float pitchRad = pitch * PI / 180;
    
    roll += gyroY * dt + gyroX * rollRad * dt + gyroZ * pitchRad * dt;
    pitch += gyroX * dt - gyroZ * rollRad * dt;
    yaw += (gyroX * rollRad / cos(pitchRad) + gyroZ / cos(pitchRad)) * dt;
    */
    // Integrate gyroscope data to get angles, accounting for cross-coupling effects witout small angle approximation
    roll += gyroY * dt + gyroX * sin(roll * PI / 180) * tan(pitch * PI / 180) * dt + gyroZ * cos(roll * PI / 180) * tan(pitch * PI / 180) * dt;
    pitch += gyroX * cos(roll * PI / 180) * dt - gyroZ * sin(roll * PI / 180) * dt;
    yaw += (gyroX * sin(roll * PI / 180) / cos(pitch * PI / 180) + gyroZ * cos(roll * PI / 180) / cos(pitch * PI / 180)) * dt;
    
    // Adjust the complementary filter coefficient based on the acceleration magnitude
    if (fabs(1.0 - accelMagnitude) < linear_acc_threshold) {
      // If the acceleration magnitude does NOT deviate significantly from 1g
      // calculate dynamic filter coefficient
      //numbers are multipled to fit match map func requirements & increase resolution
      alpha = map(fabs(1.0 - accelMagnitude)*1000, 0,linear_acc_threshold*1000, 900,980);
      alpha = alpha/1000; //bring back original number format
      // Apply complementary filter
      roll = alpha * roll + (1.0 - alpha) * accelRoll;
      pitch = alpha * pitch + (1.0 - alpha) * accelPitch;
      // Yaw is only from the gyroscope since accelerometer doesn't give yaw angle
      //Visualise on the drone the accelerometer data is valid
      digitalWrite(LED_RED_PIN, LOW);
      digitalWrite(LED_GRN_PIN, HIGH);

    } 
    else{
      //Visualise on the drone the accelerometer data is discarded
      digitalWrite(LED_RED_PIN, HIGH);
      digitalWrite(LED_GRN_PIN, LOW);
    }

    // Print the results
    if (++i == cyclesPrint){
      //attitudePrint();
      sensorPrint(accelX,accelY,accelZ,gyroX,gyroY,gyroZ);
      i = 0;
    }
  }
}

void readMPU6050Data(int16_t &ax, int16_t &ay, int16_t &az, int16_t &gx, int16_t &gy, int16_t &gz) {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B); // Starting register for accelerometer readings
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_ADDR, 14, true); // Request 14 bytes

  ax = Wire.read() << 8 | Wire.read() - ax_offset; 
  ay = Wire.read() << 8 | Wire.read() - ay_offset;
  az = Wire.read() << 8 | Wire.read() - az_offset;
  Wire.read(); Wire.read(); // Ignore temperature readings
  gx = Wire.read() << 8 | Wire.read() - gx_offset;
  gy = Wire.read() << 8 | Wire.read() - gy_offset;
  gz = Wire.read() << 8 | Wire.read() - gz_offset;
}

void calibrateSensors() {
  int32_t ax_sum = 0, ay_sum = 0, az_sum = 0;
  int32_t gx_sum = 0, gy_sum = 0, gz_sum = 0;
  const int num_samples = 1000;

  for (int i = 0; i < num_samples; i++) {
    int16_t ax, ay, az, gx, gy, gz;
    readMPU6050Data(ax, ay, az, gx, gy, gz);
    ax_sum += ax;
    ay_sum += ay;
    az_sum += az;
    gx_sum += gx;
    gy_sum += gy;
    gz_sum += gz;
    delay(3); // Small delay between readings
    digitalWrite(LED_GRN_PIN, digitalRead(LED_GRN_PIN) == HIGH ? LOW : HIGH);

  }

  // Calculate and store the offsets
  ax_offset = ax_sum / num_samples;
  ay_offset = ay_sum / num_samples;
  az_offset = (az_sum / num_samples) - 16384; // Adjust for gravity
  gx_offset = gx_sum / num_samples;
  gy_offset = gy_sum / num_samples;
  gz_offset = gz_sum / num_samples;

  Serial.print("ax: ");
  Serial.print(ax_offset);
  Serial.print(" ay: ");
  Serial.print(ay_offset);
  Serial.print(" az: ");
  Serial.print(az_offset);
  Serial.print(" gx: ");
  Serial.print(gx_offset);
  Serial.print(" gy: ");
  Serial.print(gy_offset);
  Serial.print(" gz: ");
  Serial.println(gz_offset);
  delay(2000);
}

void attitudePrint(){
  // Print the results
  Serial.print("R: ");
  Serial.print(roll);
  Serial.print("\tP: ");
  Serial.print(pitch);
  Serial.print("\tY: ");
  Serial.print(yaw);
  Serial.print("\tF: ");
  Serial.print(1.0 / dt);
  Serial.println(" Hz");
}

void sensorPrint(float accelX,float accelY,float accelZ,float gyroX,float gyroY,float gyroZ){
  Serial.print("accelX: ");
  Serial.print(accelX, 2);  // Print accelX with 4 decimal places

  Serial.print(" accelY: ");
  Serial.print(accelY, 2);  // Print accelY with 4 decimal places

  Serial.print(" accelZ: ");
  Serial.print(accelZ, 2);  // Print accelZ with 4 decimal places

  Serial.print(" gyroX: ");
  Serial.print(gyroX, 2);   // Print gyroX with 4 decimal places

  Serial.print(" gyroY: ");
  Serial.print(gyroY,2);   // Print gyroY with 4 decimal places

  Serial.print(" gyroZ: ");
  Serial.println(gyroZ, 2);   // Print gyroZ with 4 decimal places
}

Thanks in advance to take your time reading this post and posting a solution.

2

That would likely be due to the angular range of 0 to 2 Pi radians, similar to the transition from 359 to 0 for angles measured in degrees.

3

Your values graphed...
graph

4

Can you show a photo so we can see how far the sensor is from the ESP32-S2 board and how the wiring is and which MPU-6050 module you have and if there are pullup resistors.

You can use "double", the optimization for "float" is not needed.

When using millis() and micros(), always use "unsigned long".
I prefer to use local variables where that is possible.Can you do this:

const unsigned long interval = 1000;

void loop();
  unsigned long currentTime = micros();
  unsigned long elapsedMicros = currentTime - lastTime;

  if (elapsedMicros >= interval) 
  {
    lastTime = currentTime;
    double dt = (double) elapsedMicros / 1000000.0;  // Convert dt to seconds
    ...

If you use the code, please check that variables are declared just once. If you declare the same variable as global and as local, then the compiler will give no warning.

You could check if the I2C transfer was successful. If it was not, then do not update the values and let the variable 'dt' solve skipping the values.

bool readMPU6050Data(int16_t &ax, int16_t &ay, int16_t &az, int16_t &gx, int16_t &gy, int16_t &gz) 
{
  bool success = false;

  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B); // Starting register for accelerometer readings
  int error = Wire.endTransmission(false);
  if(error == 0)
  {
    int n = Wire.requestFrom(MPU_ADDR, 14, true); // Request 14 bytes
    if(n == 14)     // received the amount of data that was requested ?
    {
      ax = Wire.read() << 8 | Wire.read(); 
      ay = Wire.read() << 8 | Wire.read();
      az = Wire.read() << 8 | Wire.read();
      Wire.read(); Wire.read(); // Ignore temperature readings
      gx = Wire.read() << 8 | Wire.read();
      gy = Wire.read() << 8 | Wire.read();
      gz = Wire.read() << 8 | Wire.read();

      ax -= ax_offset;
      ay -= ay_offset;
      az -= az_offset;
      gx -= gx_offset;
      gy -= gy_offset;
      gz -= gz_offset;
      success = true;
    }
  }
  return(success);
}

And then only use the data if they are valid:

    if(readMPU6050Data(ax, ay, az, gx, gy, gz))
    {
      ...
    }

The real problem is that you might have a counterfeit MPU-6050 sensor. The MPU-6050 sensor is outdated. The I2C bus is slow.
Can you get a fast sensor with a SPI bus and use a library with sensor fusion that is known to work ?

1 Like
5

Thanks for your reply, I came with two interpretations based on your response:

  1. The GX data over and flows where the limits are 0 and 1.95. I tested this and it is not the case as the limits are approximately -250 and +250 (verified to be similar practically).

  2. The data gets corrupted when the angles of roll and pitch go from positive to negative. This is not be the case as I am just checking the values from the sensors with little processing. I am not applying any formula to calculate the attitude for them, and they are not affected by the attitude values either.

6

@xfpd Thanks a lot for plotting them, now it is super clear that all values are stable except gX and aZ is a bit wobbly

7

hello @Koepel, Here is the picture of the drone and looking at the datasheet I can confirm that there are 10K Ohms pullup resistors on the I2C bus line.

IMG_20240817_171220
IMG_20240817_1712201920×3415 319 KB

I have implemented all the changes in the code as you suggested. Here is the updated code:

/*
 THIS CODE READS THE MPU6050 AT THE MAX SPEED SUPPORTED 
 PRINTS THE ROLL YAW AND PITCH AS FAST AS POSSIBLE
 THE LOOP FREQUENCY IS LIMITED AT 1 KHZ 
*/

#include <Wire.h>
#include <math.h>

#define LED_BLE_PIN 7
#define LED_RED_PIN 8
#define LED_GRN_PIN 9

const int MPU_ADDRESS = 0x68; // I2C address of the MPU-6050
const int CONFIG_REGISTER = 0x1A;  // Address of the low pass filter register
float complementary_filter_alpha = 0.98f;  // Initial complementary filter constant

// Offset variables
int16_t accel_x_offset = 0, accel_y_offset = 0, accel_z_offset = 0;
int16_t gyro_x_offset = 0, gyro_y_offset = 0, gyro_z_offset = 0;

float roll_angle = 0.0f;
float pitch_angle = 0.0f;
float yaw_angle = 0.0f;

unsigned long previous_time = 0;
const unsigned long loop_frequency = 1000;  //in microseconds
float current_frequency;

float accel_threshold = 0.2f; //variable to decide when to use or discard accelerometer data

int debug_counter = 0, debug_cycles = 10;  //debug printing variable and cycles

void setup() {
  pinMode(LED_GRN_PIN, OUTPUT);
  pinMode(LED_BLE_PIN, OUTPUT);
  pinMode(LED_RED_PIN, OUTPUT);

  Serial.begin(921600); // highest speed possible on my setup, higher than this I get errors
  Wire.begin(11, 10); // Initialize I2C with SDA on GPIO11 and SCL on GPIO10
  Wire.setClock(1000000); // Clock speed at 1MHz (highest available)

  // Initialize MPU6050
  Wire.beginTransmission(MPU_ADDRESS);
  Wire.write(0x6B); // PWR_MGMT_1 register
  Wire.write(0);    // Set to 0 to wake up the MPU-6050
  Wire.endTransmission(true);

  // Set accelerometer sensitivity to ±2g
  Wire.beginTransmission(MPU_ADDRESS);
  Wire.write(0x1C); // ACCEL_CONFIG register
  Wire.write(0);    // Set sensitivity to ±2g
  Wire.endTransmission(true);

  // Set gyroscope sensitivity to ±250 degrees/second
  Wire.beginTransmission(MPU_ADDRESS);
  Wire.write(0x1B); // GYRO_CONFIG register
  Wire.write(0);    // Set sensitivity to ±250 degrees/second
  Wire.endTransmission(true);

  // Configure the DLPF to 5 Hz
  Wire.beginTransmission(MPU_ADDRESS);
  Wire.write(CONFIG_REGISTER);
  Wire.write(0x06); // DLPF_CFG = 6 (5 Hz)
  Wire.endTransmission(true);

  // Initial delay to allow the sensor to stabilize
  delay(2000);

  // Calibrate sensor at each startup
  digitalWrite(LED_BLE_PIN, HIGH);
  calibrateSensors();
  digitalWrite(LED_BLE_PIN, LOW);
}

void loop() {

  unsigned long current_time = micros();
  unsigned long elapsed_time = current_time - previous_time;

  if (elapsed_time >= loop_frequency) { // Ensure the loop runs at approximately 1 kHz (1ms per loop)
    previous_time = current_time;
    double delta_time = double(elapsed_time) / 1000000.0f;
    current_frequency = 1 / delta_time; // just use for debugging
    
    // Read raw values from MPU6050
    int16_t accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z;

    if (readMPU6050Data(accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z)) {
    
      // Convert raw values to accelerometer (g) and gyroscope (degrees/second)
      // Negative signs to match the orientation of the drone
      float accel_x_g = -accel_x / 16384.0;
      float accel_y_g = -accel_y / 16384.0;
      float accel_z_g = accel_z / 16384.0;
      float gyro_x_dps = -gyro_x / 131.0;
      float gyro_y_dps = -gyro_y / 131.0;
      float gyro_z_dps = gyro_z / 131.0;
    
      // Calculate roll and pitch from the accelerometer data
      float accel_pitch_angle = atan2(accel_y_g, accel_z_g) * 180 / PI;
      float accel_roll_angle = atan2(-accel_x_g, sqrt(accel_y_g * accel_y_g + accel_z_g * accel_z_g)) * 180 / PI;

      // Calculate the magnitude of the acceleration vector
      float accel_magnitude = sqrt(accel_x_g * accel_x_g + accel_y_g * accel_y_g + accel_z_g * accel_z_g);
      
      // Integrate gyroscope data to get angles, accounting for cross-coupling effects without small angle approximation
      roll_angle += gyro_y_dps * delta_time + gyro_x_dps * sin(roll_angle * PI / 180) * tan(pitch_angle * PI / 180) * delta_time + gyro_z_dps * cos(roll_angle * PI / 180) * tan(pitch_angle * PI / 180) * delta_time;
      pitch_angle += gyro_x_dps * cos(roll_angle * PI / 180) * delta_time - gyro_z_dps * sin(roll_angle * PI / 180) * delta_time;
      yaw_angle += (gyro_x_dps * sin(roll_angle * PI / 180) / cos(pitch_angle * PI / 180) + gyro_z_dps * cos(roll_angle * PI / 180) / cos(pitch_angle * PI / 180)) * delta_time;
      
      // Adjust the complementary filter coefficient based on the acceleration magnitude
      if (fabs(1.0 - accel_magnitude) < accel_threshold) {
        // If the acceleration magnitude does NOT deviate significantly from 1g
        // calculate dynamic filter coefficient
        // numbers are multiplied to fit match map func requirements & increase resolution
        complementary_filter_alpha = map(fabs(1.0 - accel_magnitude) * 1000, 0, accel_threshold * 1000, 900, 980);
        complementary_filter_alpha = complementary_filter_alpha / 1000; // bring back original number format
        
        // Apply complementary filter
        roll_angle = complementary_filter_alpha * roll_angle + (1.0 - complementary_filter_alpha) * accel_roll_angle;
        pitch_angle = complementary_filter_alpha * pitch_angle + (1.0 - complementary_filter_alpha) * accel_pitch_angle;
        
        // Yaw is only from the gyroscope since accelerometer doesn't give yaw angle
        // Visualize on the drone the accelerometer data is valid
        digitalWrite(LED_RED_PIN, LOW);
        digitalWrite(LED_GRN_PIN, HIGH);

      } else {
        // Visualize on the drone the accelerometer data is discarded
        digitalWrite(LED_RED_PIN, HIGH);
        digitalWrite(LED_GRN_PIN, LOW);
      }
    
      // Print the results
      if (++debug_counter == debug_cycles) {
        sensorPrint(accel_x_g, accel_y_g, accel_z_g, gyro_x_dps, gyro_y_dps, gyro_z_dps);
        debug_counter = 0;
      }
    }
  }
}

bool readMPU6050Data(int16_t &accel_x_out, int16_t &accel_y_out, int16_t &accel_z_out, int16_t &gyro_x_out, int16_t &gyro_y_out, int16_t &gyro_z_out) {
  bool success_flag = false;

  Wire.beginTransmission(MPU_ADDRESS);
  Wire.write(0x3B); // Starting register for accelerometer readings
  int transmission_error = Wire.endTransmission(false);
  if (transmission_error == 0) {
    int bytes_received = Wire.requestFrom(MPU_ADDRESS, 14, true); // Request 14 bytes
    if (bytes_received == 14) { // received the amount of data that was requested?
      accel_x_out = Wire.read() << 8 | Wire.read() - accel_x_offset; 
      accel_y_out = Wire.read() << 8 | Wire.read() - accel_y_offset;
      accel_z_out = Wire.read() << 8 | Wire.read() - accel_z_offset;
      Wire.read(); Wire.read(); // Ignore temperature readings
      gyro_x_out = Wire.read() << 8 | Wire.read() - gyro_x_offset;
      gyro_y_out = Wire.read() << 8 | Wire.read() - gyro_y_offset;
      gyro_z_out = Wire.read() << 8 | Wire.read() - gyro_z_offset;
      success_flag = true;
    }
  }
  return success_flag;
}

void calibrateSensors() {
  int32_t accel_x_sum = 0, accel_y_sum = 0, accel_z_sum = 0;
  int32_t gyro_x_sum = 0, gyro_y_sum = 0, gyro_z_sum = 0;
  const int num_samples = 1000;

  for (int sample_index = 0; sample_index < num_samples; sample_index++) {
    int16_t accel_x_sample, accel_y_sample, accel_z_sample, gyro_x_sample, gyro_y_sample, gyro_z_sample;
    readMPU6050Data(accel_x_sample, accel_y_sample, accel_z_sample, gyro_x_sample, gyro_y_sample, gyro_z_sample);
    accel_x_sum += accel_x_sample;
    accel_y_sum += accel_y_sample;
    accel_z_sum += accel_z_sample;
    gyro_x_sum += gyro_x_sample;
    gyro_y_sum += gyro_y_sample;
    gyro_z_sum += gyro_z_sample;
    delay(3); // Small delay between readings
    digitalWrite(LED_GRN_PIN, digitalRead(LED_GRN_PIN) == HIGH ? LOW : HIGH);
  }

  // Calculate and store the offsets
  accel_x_offset = accel_x_sum / num_samples;
  accel_y_offset = accel_y_sum / num_samples;
  accel_z_offset = (accel_z_sum / num_samples) - 16384; // Adjust for gravity
  gyro_x_offset = gyro_x_sum / num_samples;
  gyro_y_offset = gyro_y_sum / num_samples;
  gyro_z_offset = gyro_z_sum / num_samples;

  Serial.print("ax: ");
  Serial.print(accel_x_offset);
  Serial.print(" ay: ");
  Serial.print(accel_y_offset);
  Serial.print(" az: ");
  Serial.print(accel_z_offset);
  Serial.print(" gx: ");
  Serial.print(gyro_x_offset);
  Serial.print(" gy: ");
  Serial.print(gyro_y_offset);
  Serial.print(" gz: ");
  Serial.println(gyro_z_offset);
  delay(2000);
}

void attitudePrint() {
  // Print the results
  Serial.print("R: ");
  Serial.print(roll_angle);
  Serial.print("\tP: ");
  Serial.print(pitch_angle);
  Serial.print("\tY: ");
  Serial.print(yaw_angle);
  Serial.print("\tF: ");
  Serial.print(current_frequency);
  Serial.println(" Hz");
}

void sensorPrint(float accel_x_print, float accel_y_print, float accel_z_print, float gyro_x_print, float gyro_y_print, float gyro_z_print) {
  Serial.print("accelX: ");
  Serial.print(accel_x_print, 2);  // Print accel_x_print with 4 decimal places

  Serial.print(" accelY: ");
  Serial.print(accel_y_print, 2);  // Print accel_y_print with 4 decimal places

  Serial.print(" accelZ: ");
  Serial.print(accel_z_print, 2);  // Print accel_z_print with 4 decimal places

  Serial.print(" gyroX: ");
  Serial.print(gyro_x_print, 2);   // Print gyro_x_print with 4 decimal places

  Serial.print(" gyroY: ");
  Serial.print(gyro_y_print, 2);   // Print gyro_y_print with 4 decimal places

  Serial.print(" gyroZ: ");
  Serial.println(gyro_z_print, 2);   // Print gyro_z_print with 4 decimal places
}

The drone components are not the best, but the drone original code works really well, the drone over is very stabilised etc... I even opened the original code and printed the same values, in those values I do not get the errors I get in my own code with the same setup. It defenitily has to do with the code I made, (also other drones give me the same results).

8

Removing gX (was green) and aZ (was yellow) shows patterns on the other traces.
graph2

9

@xfpd do you think those fluctuation are within the margin of error or not? It is a error of 0.02 which is very low. Since Accelerometer is configured at +-2 g, the error is 1%, the gyroscope is configured at +-250 dps, the error is 0.008%.

10

I do not know the answer to your question. Sorry.

1 Like
11

I just made another test, I tilted the drone left, right and front, back. I noticed that there are intervals where the data is close to 0. i.e. I tilt the drone 45 degree on the left. therefore, the AZ goes down and the AX goes up. I noticed that there are interval where instead of seeing AX = 0.36g, I see AZ = 0g, there is this flickering affecting all the readings.

I am happy to pay to get this AHRS system solved.

12

Not all the changes. I would split this over two lines:

accel_x_out = Wire.read() << 8 | Wire.read() - accel_x_offset;

I can not tell if that code is good or bad, it just gives me a bad feeling.

The 10k pullup is a high value. You could add more pullup.
The maximum sink current (to pull a signal low) is 3mA. I prefer a value between 1mA and 3mA.
3.3V / 10k = 0.33mA
You can add 2k2 pullup resistors to 3.3V for an extra 1.5mA.

Did you know that you can measure the sink current in the circuit ? I wrote it in a Wiki on Github here.

Is the MPU-6050 one of the chips on the board ?
The quality of the pcb seems good, but that is not a guaranteed that the MPU-6050 is genuine and not a counterfeit.

I think that a I2C bus of 1MHz is too fast. Can you do tests with 100kHz or 200kHz ? I would not go higher than 400kHz.

Can we have a look at the original drone code ? I can not believe that they use 1MHz for the I2C bus. I assume that they use a software filter too make the value less noisy. A little filtering over a few samples is still fast and reduces already a lot of noise.
It is possible that the original drone code has something special to make it work with a counterfeit MPU-6050.

Do you know this: https://github.com/jremington/MPU-6050-Fusion
Just as in your code, the elapsed number of microseconds is used in the calculation.

1 Like
13

@Koepel I have split the line were the data is read and the calibration value is subtracted.

  • I have lowered the I2C speed to 200 KHz.

  • I have lowered the Serial speed to 250 KHz.

  • I have lowered the loop frequency to 500Hz.

I have repeated the stationary readings and they look promising!

I do not know if the original code is using the communication speed at max settings, I did not check.

Instead of uploading the full folder of the original code, I am going to post the link where it is found: GitHub - espressif/esp-drone: Mini Drone/Quadcopter Firmware for ESP32 and ESP32-S Series SoCs.

I know about other libraries to implement sensor fusion algorithm, I have tried them and obviously the readings were wrong because the initial readings of the MPU itself were corrupted.

Here are the readings when the sensor is stationary:

aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.03 gY: 0.01 gZ: 0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.01 gY: 0.00 gZ: 0.02
aX: -0.00 aY: 0.00 aZ: 1.00 gX: -0.01 gY: 0.01 gZ: -0.01
aX: -0.00 aY: 0.00 aZ: 1.00 gX: -0.01 gY: -0.01 gZ: 0.01
aX: 0.00 aY: -0.00 aZ: 1.00 gX: 0.00 gY: -0.04 gZ: 0.02
aX: 0.00 aY: -0.00 aZ: 1.00 gX: -0.02 gY: 0.02 gZ: -0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.03 gY: 0.02 gZ: -0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.02 gY: 0.02 gZ: 0.01
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.01 gY: 0.01 gZ: 0.01
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.01 gY: 0.01 gZ: -0.01
aX: -0.00 aY: 0.00 aZ: 1.00 gX: -0.02 gY: -0.01 gZ: -0.01
aX: 0.00 aY: -0.00 aZ: 1.00 gX: -0.02 gY: 0.00 gZ: -0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.01 gY: 0.00 gZ: -0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.00 gY: 0.02 gZ: -0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.00 gY: 0.01 gZ: -0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.02 gY: 0.01 gZ: -0.02
aX: 0.00 aY: -0.00 aZ: 1.00 gX: 0.02 gY: 0.00 gZ: -0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.02 gY: -0.01 gZ: 0.02
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.02 gY: -0.01 gZ: 0.02
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.02 gY: 0.01 gZ: 0.01
aX: -0.00 aY: -0.00 aZ: 1.00 gX: 0.00 gY: 0.00 gZ: -0.01
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.00 gY: -0.02 gZ: -0.02
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.01 gY: -0.02 gZ: 0.00
aX: -0.00 aY: -0.00 aZ: 1.00 gX: 0.02 gY: -0.02 gZ: 0.01
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.02 gY: -0.02 gZ: 0.00
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.02 gY: -0.03 gZ: 0.01
aX: 0.00 aY: -0.00 aZ: 1.00 gX: 0.03 gY: -0.01 gZ: 0.00
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.02 gY: 0.00 gZ: 0.00
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.02 gY: 0.02 gZ: 0.00
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.02 gY: 0.02 gZ: 0.00
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.01 gY: -0.01 gZ: 0.00
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.00 gY: -0.02 gZ: 0.03
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.01 gY: -0.02 gZ: 0.01
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.00 gY: -0.01 gZ: 0.01
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.02 gY: -0.01 gZ: 0.01
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.02 gY: -0.03 gZ: 0.01
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.02 gY: -0.02 gZ: 0.02
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.02 gY: -0.02 gZ: 0.02
aX: 0.00 aY: -0.00 aZ: 1.00 gX: 0.02 gY: -0.02 gZ: 0.00
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.01 gY: 0.00 gZ: 0.01
aX: 0.00 aY: -0.00 aZ: 1.00 gX: 0.02 gY: 0.01 gZ: 0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.00 gY: -0.01 gZ: 0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.02 gY: -0.02 gZ: 0.02
aX: 0.00 aY: -0.00 aZ: 1.00 gX: -0.01 gY: -0.02 gZ: 0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.01 gY: 0.00 gZ: 0.01
aX: 0.00 aY: -0.00 aZ: 1.00 gX: 0.02 gY: -0.02 gZ: -0.01
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.02 gY: 0.00 gZ: -0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.02 gY: 0.01 gZ: 0.00
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.01 gY: 0.02 gZ: 0.01
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.00 gY: 0.02 gZ: -0.01
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.02 gY: 0.01 gZ: 0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.01 gY: 0.00 gZ: 0.04
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.01 gY: -0.02 gZ: 0.00
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.03 gY: 0.01 gZ: -0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.02 gY: 0.02 gZ: 0.00
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.01 gY: 0.00 gZ: -0.01
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.01 gY: 0.02 gZ: 0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.01 gY: 0.01 gZ: 0.03
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.01 gY: 0.00 gZ: -0.01
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.00 gY: -0.02 gZ: 0.01
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.00 gY: -0.01 gZ: 0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.02 gY: 0.01 gZ: 0.00
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.00 gY: 0.02 gZ: 0.01
aX: 0.00 aY: -0.00 aZ: 1.00 gX: 0.02 gY: 0.02 gZ: 0.00
aX: 0.00 aY: -0.00 aZ: 1.00 gX: 0.02 gY: 0.02 gZ: 0.00
aX: 0.00 aY: -0.00 aZ: 1.00 gX: -0.01 gY: 0.00 gZ: -0.02
aX: 0.00 aY: -0.00 aZ: 1.00 gX: -0.01 gY: 0.00 gZ: -0.01
aX: 0.00 aY: -0.00 aZ: 1.00 gX: 0.01 gY: 0.01 gZ: 0.02
aX: 0.00 aY: -0.00 aZ: 1.00 gX: 0.02 gY: 0.02 gZ: 0.01
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.00 gY: 0.02 gZ: 0.01
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.02 gY: 0.02 gZ: 0.01
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.02 gY: 0.02 gZ: 0.01
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.01 gY: 0.00 gZ: 0.02
aX: -0.00 aY: -0.00 aZ: 1.00 gX: 0.02 gY: 0.02 gZ: 0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.03 gY: 0.02 gZ: 0.01
aX: 0.00 aY: -0.00 aZ: 1.00 gX: 0.02 gY: 0.01 gZ: -0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.00 gY: 0.01 gZ: -0.01
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.01 gY: 0.03 gZ: 0.01
aX: -0.00 aY: -0.00 aZ: 1.00 gX: 0.00 gY: 0.03 gZ: 0.02
aX: 0.00 aY: -0.00 aZ: 1.00 gX: 0.04 gY: 0.00 gZ: 0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.02 gY: 0.00 gZ: -0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.00 gY: 0.01 gZ: 0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.02 gY: -0.01 gZ: 0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.03 gY: -0.01 gZ: -0.01
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.01 gY: 0.00 gZ: 0.01
aX: 0.00 aY: -0.00 aZ: 1.00 gX: 0.01 gY: 0.00 gZ: 0.02
aX: 0.00 aY: -0.00 aZ: 1.00 gX: 0.01 gY: 0.02 gZ: -0.02
aX: 0.00 aY: -0.00 aZ: 1.00 gX: -0.02 gY: 0.00 gZ: 0.00
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.01 gY: 0.02 gZ: 0.01
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.01 gY: 0.01 gZ: -0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.01 gY: -0.02 gZ: 0.00
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.03 gY: -0.03 gZ: 0.02
aX: 0.00 aY: -0.00 aZ: 1.00 gX: 0.02 gY: -0.01 gZ: 0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.01 gY: 0.01 gZ: 0.02
aX: 0.00 aY: -0.00 aZ: 1.00 gX: 0.02 gY: 0.01 gZ: 0.00
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.02 gY: 0.00 gZ: 0.02
aX: 0.00 aY: -0.00 aZ: 1.00 gX: 0.01 gY: -0.01 gZ: 0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.01 gY: -0.02 gZ: 0.00
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.01 gY: -0.01 gZ: 0.01
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.00 gY: 0.02 gZ: 0.01
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.01 gY: 0.02 gZ: 0.01
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.01 gY: 0.00 gZ: 0.02
aX: -0.00 aY: -0.00 aZ: 1.00 gX: 0.01 gY: -0.02 gZ: 0.02
aX: -0.00 aY: 0.00 aZ: 1.00 gX: -0.01 gY: 0.02 gZ: 0.00
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.00 gY: -0.02 gZ: -0.02
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.02 gY: 0.01 gZ: 0.00
aX: -0.00 aY: 0.00 aZ: 1.00 gX: -0.01 gY: 0.02 gZ: 0.02
aX: -0.00 aY: 0.00 aZ: 1.00 gX: -0.01 gY: 0.01 gZ: -0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.00 gY: 0.01 gZ: -0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.01 gY: -0.01 gZ: -0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.01 gY: -0.01 gZ: -0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.00 gY: -0.02 gZ: -0.03
aX: -0.00 aY: -0.00 aZ: 1.00 gX: -0.01 gY: -0.03 gZ: -0.03
aX: -0.00 aY: -0.00 aZ: 1.00 gX: 0.00 gY: -0.02 gZ: 0.00
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.00 gY: -0.02 gZ: -0.02
aX: 0.00 aY: -0.00 aZ: 1.00 gX: -0.01 gY: 0.00 gZ: 0.00
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.02 gY: 0.00 gZ: 0.00
aX: 0.00 aY: -0.00 aZ: 1.00 gX: -0.02 gY: 0.00 gZ: -0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.02 gY: -0.02 gZ: 0.01
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.01 gY: 0.00 gZ: -0.01
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.00 gY: 0.01 gZ: 0.00
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.01 gY: -0.02 gZ: 0.01
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.01 gY: -0.02 gZ: 0.00
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.00 gY: -0.03 gZ: -0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.00 gY: 0.00 gZ: 0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.01 gY: -0.01 gZ: -0.01
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.02 gY: -0.01 gZ: -0.01
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.03 gY: 0.00 gZ: -0.02
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.02 gY: 0.00 gZ: -0.02
aX: -0.00 aY: -0.00 aZ: 1.00 gX: 0.02 gY: -0.02 gZ: 0.02
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.01 gY: -0.02 gZ: -0.01
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.01 gY: -0.02 gZ: -0.02
aX: 0.00 aY: -0.00 aZ: 1.00 gX: -0.01 gY: -0.01 gZ: -0.02
aX: 0.00 aY: -0.00 aZ: 1.00 gX: 0.01 gY: 0.01 gZ: -0.02
aX: -0.00 aY: -0.00 aZ: 1.00 gX: 0.01 gY: 0.01 gZ: -0.01
aX: 0.00 aY: -0.00 aZ: 1.00 gX: 0.02 gY: -0.02 gZ: 0.00
aX: 0.00 aY: -0.00 aZ: 1.00 gX: 0.01 gY: 0.00 gZ: 0.00
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.01 gY: 0.01 gZ: -0.01
aX: 0.00 aY: -0.00 aZ: 1.00 gX: 0.00 gY: 0.00 gZ: -0.04
aX: -0.00 aY: -0.00 aZ: 1.00 gX: 0.01 gY: -0.01 gZ: -0.02
aX: 0.00 aY: -0.00 aZ: 1.00 gX: 0.00 gY: -0.01 gZ: 0.00
aX: -0.00 aY: -0.00 aZ: 1.00 gX: -0.01 gY: -0.02 gZ: 0.01
aX: 0.00 aY: -0.00 aZ: 1.00 gX: -0.01 gY: -0.01 gZ: 0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.02 gY: 0.02 gZ: 0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.00 gY: 0.02 gZ: 0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.01 gY: 0.02 gZ: -0.01
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.00 gY: 0.01 gZ: -0.01
aX: -0.00 aY: 0.00 aZ: 1.00 gX: -0.01 gY: -0.01 gZ: -0.01
aX: -0.00 aY: 0.00 aZ: 1.00 gX: -0.01 gY: 0.00 gZ: 0.00
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.00 gY: -0.01 gZ: -0.03
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.02 gY: -0.02 gZ: -0.03
aX: -0.00 aY: 0.00 aZ: 1.00 gX: -0.01 gY: -0.01 gZ: -0.01
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.02 gY: 0.01 gZ: -0.01
aX: -0.00 aY: -0.00 aZ: 1.00 gX: 0.02 gY: 0.00 gZ: 0.00
aX: -0.00 aY: -0.00 aZ: 1.00 gX: 0.00 gY: -0.01 gZ: -0.02
aX: -0.00 aY: -0.00 aZ: 1.00 gX: 0.02 gY: -0.01 gZ: -0.01
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.00 gY: 0.00 gZ: 0.01
aX: 0.00 aY: -0.00 aZ: 1.00 gX: -0.01 gY: -0.02 gZ: 0.01
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.01 gY: -0.02 gZ: 0.00
aX: -0.00 aY: 0.00 aZ: 1.00 gX: -0.02 gY: -0.01 gZ: 0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.02 gY: -0.01 gZ: 0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.01 gY: 0.01 gZ: 0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.02 gY: 0.02 gZ: 0.01
aX: -0.00 aY: -0.00 aZ: 1.00 gX: -0.02 gY: -0.01 gZ: 0.02
aX: -0.00 aY: -0.00 aZ: 1.00 gX: -0.01 gY: 0.00 gZ: 0.01
aX: -0.00 aY: -0.00 aZ: 1.00 gX: -0.01 gY: -0.01 gZ: -0.01
aX: -0.00 aY: 0.00 aZ: 1.00 gX: -0.01 gY: -0.02 gZ: 0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.02 gY: -0.02 gZ: 0.02
aX: -0.00 aY: 0.00 aZ: 1.00 gX: -0.02 gY: 0.02 gZ: 0.01
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.00 gY: -0.01 gZ: 0.00
aX: -0.00 aY: 0.00 aZ: 1.00 gX: -0.01 gY: -0.01 gZ: -0.01
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.01 gY: 0.01 gZ: -0.02
aX: -0.00 aY: -0.00 aZ: 1.00 gX: 0.01 gY: 0.01 gZ: -0.03
aX: -0.00 aY: -0.00 aZ: 1.00 gX: 0.00 gY: 0.00 gZ: -0.02
aX: -0.00 aY: 0.00 aZ: 1.00 gX: -0.01 gY: 0.00 gZ: -0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.01 gY: 0.01 gZ: -0.02
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.00 gY: 0.01 gZ: -0.01
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.02 gY: -0.01 gZ: 0.00
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.01 gY: 0.01 gZ: 0.00
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.01 gY: -0.01 gZ: -0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.01 gY: -0.02 gZ: 0.01
aX: 0.00 aY: -0.00 aZ: 1.00 gX: 0.01 gY: -0.02 gZ: 0.02
aX: 0.00 aY: -0.00 aZ: 1.00 gX: 0.01 gY: -0.03 gZ: 0.02
aX: 0.00 aY: -0.00 aZ: 1.00 gX: 0.00 gY: -0.02 gZ: 0.02
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.02 gY: -0.01 gZ: 0.02
aX: -0.00 aY: -0.00 aZ: 1.00 gX: 0.03 gY: -0.01 gZ: 0.03
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.01 gY: -0.02 gZ: 0.01
aX: 0.00 aY: -0.00 aZ: 1.00 gX: 0.04 gY: -0.02 gZ: 0.00
aX: -0.00 aY: -0.00 aZ: 1.00 gX: 0.00 gY: -0.04 gZ: -0.01
aX: -0.00 aY: -0.00 aZ: 1.00 gX: 0.00 gY: -0.05 gZ: 0.02
aX: -0.00 aY: -0.00 aZ: 1.00 gX: 0.00 gY: -0.02 gZ: 0.02
aX: -0.00 aY: -0.00 aZ: 1.00 gX: 0.02 gY: -0.02 gZ: 0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.00 gY: -0.02 gZ: 0.02
aX: -0.00 aY: -0.00 aZ: 1.00 gX: 0.00 gY: 0.01 gZ: -0.01
aX: -0.00 aY: -0.00 aZ: 1.00 gX: 0.00 gY: -0.02 gZ: -0.02
aX: -0.00 aY: -0.00 aZ: 1.00 gX: 0.02 gY: -0.05 gZ: -0.03
aX: 0.00 aY: -0.00 aZ: 1.00 gX: 0.02 gY: -0.03 gZ: -0.03
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.02 gY: -0.02 gZ: -0.01
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.00 gY: -0.02 gZ: -0.03
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.00 gY: 0.00 gZ: -0.01
aX: -0.00 aY: -0.00 aZ: 1.00 gX: 0.01 gY: 0.01 gZ: -0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.01 gY: 0.00 gZ: -0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.02 gY: -0.02 gZ: -0.01
aX: 0.00 aY: -0.00 aZ: 1.00 gX: 0.01 gY: -0.02 gZ: 0.00
aX: 0.00 aY: -0.00 aZ: 1.00 gX: 0.02 gY: -0.01 gZ: -0.02
aX: 0.00 aY: -0.00 aZ: 1.00 gX: 0.02 gY: -0.02 gZ: 0.01
aX: 0.00 aY: -0.00 aZ: 1.00 gX: 0.02 gY: -0.01 gZ: 0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.00 gY: -0.02 gZ: 0.01
aX: -0.00 aY: 0.00 aZ: 1.00 gX: 0.01 gY: -0.02 gZ: 0.01
aX: -0.00 aY: 0.00 aZ: 1.00 gX: -0.01 gY: -0.02 gZ: 0.01
aX: 0.00 aY: 0.00 aZ: 1.00 gX: -0.01 gY: -0.02 gZ: 0.01
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.02 gY: -0.01 gZ: 0.00
aX: 0.00 aY: -0.00 aZ: 1.00 gX: 0.02 gY: -0.02 gZ: -0.01
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.02 gY: 0.00 gZ: -0.02
aX: 0.00 aY: 0.00 aZ: 1.00 gX: 0.02 gY: -0.01 gZ: -0.02
aX: -0.00 aY: -0.00 aZ: 1.00 gX: 0.02 gY: 0.00 gZ: -0.02
aX: -0.00 aY: -0.00 aZ: 1.00 gX: 0.01 gY: 0.00 gZ: -0.01
aX: -0.00 aY: -0.00 aZ: 1.00 gX: 0.00 gY: -0.02 gZ: -0.01

I have visually tested the code if there are any missing data when I tilt the drone in any direction. It does not appear to be the case, the data goes smoothly up and down when I move the drone.

I am trying to make a graph on GPT to show the values but I am unable to do so, same with excel, the values are not separated, I do not know how @xfpd managed to make those graphs.

EDIT:

Here is the graph without the aZ:

image
image1259×647 252 KB

The readings seems to be fluctuating very little which should be within the margin of error.

14

There are also online graph plotters.

The original drone code is a lot, but they use the FreeRTOS multitasking system. That makes the whole project running smooth. The code looks and feels good. They did a good job.

Arduino is for easy prototyping and for learning about electronics and writing code. But Arduino is not for a fast drone. They avoid that by not using Arduino code. They use the so called "ESP-IDF". That is the programming level provided by the Espressif, the manufacturer of the ESP32 chips.

The sensors use a 400kHz bus and there is also 100kHz for the rest: https://github.com/espressif/esp-drone/blob/master/components/drivers/i2c_bus/i2c_drv.c#L47
They seem to use a Kalman filter: https://github.com/espressif/esp-drone/blob/master/components/core/crazyflie/modules/interface/math3d.h

When Sensor Fusion is used, such as a Kalman filter, then the disadvantages of a sensor is reduced by the other sensors. Especially with a 9DOF sensor (accelerometer + gyro + magnetometer)
An accelerometer is noisy and a gyro drifts, those things will be reduced by Sensor Fusion.

1 Like
15

I will mark your previous reply as the solution, the root cause was the instability caused by the high frequency signals.

I might open another thread regarding how to make the control system to balance the drone.

16

I think that you should still measure the pullup.
If it is below 1mA, then you can improve the reliability by increasing that current.