MPU6050 gyroscope not working correctly

[M7890.1]

I am using the bluepill board and was trying to compute the angle from gyroscope raw data. However, when I tilt the sensor, the angle measured doesn't change as expected. It's changing between 1.xx and 2.xx regardless of the tilt angle. The sensor is being read every 4ms (I have configured the timer to interrupt at every 4ms). I have attached my code here, can anyone please help to to figure out what is the problem? Thanks!

Update: I try to remove the offset but the reading is still inaccurate. When the row angle is 45 degrees, it shows around 20 degrees; when the row angle is 30, it shows around 13 degrees. Both the angle calculated seems to be around 2.3 times less than the actual angle.

[TDK]

> However, when I tilt the sensor, the angle measured doesn't change as expected. It's changing between 1.xx and 2.xx regardless of the tilt angle

A gyro doesn't measure angle, it measures the rate of change in angle.

[M7890.1]

Yes. So I am actually integrating the angular velocity to get the angle. Sorry for not describing it clearly. See the code snippet below:

gyro_roll_angle += gyro_y_raw/65.5*0.004;
gyro_pitch_angle += gyro_x_raw/65.5*0.004;
gyro_yaw_angle += gyro_z_raw/65.5*0.004;

[TDK]

Integrating it like this is going to amplify the noise in the signal. It’s just not a viable strategy for getting the absolute angle.
Flight control algorithms use both the accelerometer and the gyro to find the angle with respect to gravity, but it’s not straightforward.

[M7890.1]

I do know that some people use complimentary filter to fuse the reading from both sensor to get a more accurate result. But I think if the flow of the program is correct, the reading should be reasonable, even if there is some noise in there, right?

[TDK]

You're right, you should be seeing some variation or at least correlation with the angle as you spin it.

Since you're reading from the chip okay (WHO_AM_I is read correctly), I would break the problem down to report the raw value reported by the gyro rather than the integrated value. Ensure that comes back okay, if not, double check configuration. Are the accel readings accurate?

Edit: seems you've made progress. If just the value is off by a %, recheck your math converting the gyro reading to angle offset.

[M7890.1]

I have thought of what you suggested as well. The raw data read from the sensor looks OK when I try to read it and print it out. The accel readings quite accurate.

I found out the reason why the reading is inaccurate. I am integrating the values at a dt of 4ms, but when I check it, the data is being read at at 13ms interval and I think the culprit for this is the sprintf. This explains why the reading is inaccurate.

So, do you have any suggestion on how to print the value in a faster way? I have tried the multiply-divide-modulus method, but it doesn't really solve the problem. Sprintf takes around 13.6ms to print all those data while the multiply-divide-modulus method takes around 12ms.

Here's the code snippet I used for print the values (using multiply-divide-modulus method):

void mpu6050_print_float(char *param_name, float float_val, char row, char col)
{
	int val = float_val*10;
 
	lcd_put_cur(row, col);
 
	val < 0 ? lcd_send_data('-'): lcd_send_data('+');
 
	lcd_send_data(abs(val)/1000 + 48);
	lcd_send_data((abs(val)/100)%10 + 48);
	lcd_send_data((abs(val)/10)%10 + 48);
	lcd_send_data('.');
	lcd_send_data(abs(val)%10 + 48);
}

[TDK]

Why print every iteration? You could consider converting it to a large integer for the printing portion. Obviously the range is limited to 0-360. Uint32_t probably prints a ton faster.
Or print every X iterations in the main loop to avoid slowing down the PID loop.

[M7890.1]

Uint32_t probably prints a ton faster.

I just knew about this. What is the theory behind it?

My intention of printing it every iteration is that I can see the difference immediately after I tilt the sensor. So what you suggest right now is that try to put something like a if-else in the main loop and print it after a number of iteration, like print the value for every 10 iteration?

[TDK]

> I just knew about this. What is the theory behind it?

Your chip doesn't have an FPU so all floating point calculations (float, double) are done in software, which is extremely slow.

> My intention of printing it every iteration is that I can see the difference immediately after I tilt the sensor.

Probably every 4ms is just as good as every 100ms if you're just reading it manually. Not like you can read 250 readings a second.

> So what you suggest right now is that try to put something like a if-else in the main loop and print it after a number of iteration, like print the value for every 10 iteration?

Yes.