MPU6050 - Gyroscope tilt angle measurement for self balancing robot?

Question

I have calculated the tilt angle of accelerometer by following formula:

Angle_Accel = atan(Ax/sqrt(Ay*Ay+Az*Az))*(180/PI)

I want to calculate the tilt angle from gyroscope now and I am using Gx co-ordinate to integrate as follows but my result is not correct.

Psuedo Code

• Measure Gx every 0.1 seconds.
• After sensitivity factor and bias correction, I multiply with 180/PI to convert into degress.
• Then I divide by frequency ie 10 and add it to final angle.

C Code

Gx = (((float)GYRO_PLAY.Gyroscope_X  )* GYRO_PLAY.Gyro_Mult)-Gx_Correction;

Gy = (((float)GYRO_PLAY.Gyroscope_Y  )*  GYRO_PLAY.Gyro_Mult)-Gy_Correction;

Gz = (((float)GYRO_PLAY.Gyroscope_Z  )* GYRO_PLAY.Gyro_Mult)-Gz_Correction;

Gx_temp = (Gx*degrees)/10.0; //degrees = 180/PI

Gx_Theta = Gx_Theta + Gx_temp;

My angle is not correct. How should I integrate? Any help is much appreciated.

PS: I know that there is a question like that here but it does not answer my problem so kindly help me.

Answer 1

10Hz sampling seems far too low, and you are in any case doing unnecessary work on each sample. Apply the raw bias offset and integrate the raw value - the conversion to degrees/sec if needed can be done at presentation, and teh intermediat conversion to radians/sec serves no purpose.

The robot does not care about or even understand units you don't need any conversion; just sign and magnitude - the sensitivity can be dealt with by your closed-loop controller coefficients.

How is Gx_Correction determined - it will vary over time with thermal drift; if it is incorrect or not tracked in some way, your integrator will magnify the error.

Note that higher sample rates over the SPI may not be possible - that is what the on-chip DPM is for.

Another possible source of error is the use of float . The STM32F4 has a single precision FPU, so the operation will be done in hardware - however if you are using floating point in interrupt or thread contexts be-aware that the floating point registers are unlikely to be preserved between contexts unless you have explicitly implemented it, so for example a floating point operation will be corrupted if interrupted by an interrupt that performs floating point operations.

If the integrator only has to work the raw data values, the floating point is unnecessary:

Gx_Theta += GYRO_PLAY.Gyroscope_X - Gx_Zero_Bias;

Answer 2

Have to integrate Gy instead of Gx. Also, the angle measured by gyroscope is in deg/s so no need to multiply with degrees . Frequency of integration also needs to be 50Hz.