I'm programming a flight controller for my quad. So far it works great in rate mode, so I decided to add angle mode. This requires the fusion of my accelerometer and gyroscope data to get the true position of the quad in space. So I was able to integrate the degrees/sec from the gyro into an angle and to get the angle from the accel as shown below:
Microcontroller: STM32F103
Ps: All variables are struct{ double x,y,z; }, dt is a uint64_t
//getting angle from gyro rate
gyroAngle.x += gyroRate.x * dt / 1000;
gyroAngle.y += gyroRate.y * dt / 1000;
gyroAngle.z += gyroRate.z * dt / 1000;
//getting angle from accel
accelAngle.x = atan2(accel.y, sqrt(pow(accel.x,2) + pow(accel.z,2))) * (180/M_PI);
accelAngle.y = -atan2(accel.x, sqrt(pow(accel.y,2) + pow(accel.z,2))) * (180/M_PI);
//combines the two into true rotational position
angle.x = .85 * (angle.x + gyroAngle.x * (dt/1000)) + (.15 * accelAngle.x);
angle.y = .85 * (angle.y + gyroAngle.y * (dt/1000)) + (.15 * accelAngle.y);
Problem: I have this on a 250hz loop. Unfortunately, after a minute or two gyroAngle start returning gibberish (I think it overflows), as a result angle also returns gabage since gyroAngle is used in it. accelAngle has not given me any trouble.
This is some output from the serial monitor:
angle: -1 3 /*
angle: -1 3 */normal until here
-----------------------------------------------------------
angle: -98 406 /*
angle: -338 582 *
angle: -710 646 *
angle: -1113 716 * starts giving me garbage
angle: -1616 849 *
angle: -2327 1016 *
angle: -3047 1377 */
I am completely puzzled to why this happens. Any help is greatly appreciated.
Most likely you are going into an integration overflow. This is quite normal and accounts for 'drift' in many similar situations.
What happens is that your measurements are finite and not infinitely precise (limit of resolution of your sensor). Over time, the fractional bits below the 10 or 12 bits you are reading add up.
Very soon this causes you to have a register overflow, or a value that is out by the error* number of sample you have taken. The vibration on your quad will exacerbate this problem.
The solution is simply to reset the integration to zero every once in a while or when it exceeds some limit (value or iterations).
Trying to maintain an absolute result from a long, limited precision numerical integration is bound to fail.
the /1000 you have in several places also causes lost precision by truncation - you are always rounding down. I wouldn't try to fix this by rounding, but it probably accounts for a lot of error.
Floating point may be an option? perhaps even a F4 component to replace the F103 - you can get a pin compatible one. Take the chip to your local cellphone repair shop if you don't have experience working with SMDs
You may need to add a magnetometer so you can reset to a better estimate.