Hey, I'm trying to code a program that will output yaw pitch and roll angle values for a glider from data extracted from a MPU6050 sensor. I'm using a MPU6050 and an Arduino Uno for troubleshooting and testing purposes.
So far my code is this:
#include<Wire.h>
long accX, accY, accZ;
float gForceX, gForceY, gForceZ, degAX, degAY, totalAX, totalAY, totalAZ, errorAX, errorAY, errorAZ;
long gyroX, gyroY, gyroZ;
float rotX, rotY, rotZ, degGX, degGY, degGZ, errorGX, errorGY, errorGZ, totalGX, totalGY, totalGZ;
float currentTime, previousTime, elapsedTime, degX, degY, degZ, atanX, atanY;
void setup() {
Serial.begin(9600);
Wire.begin();
setupMPU();
int errorLoop;
for (errorLoop = 0; errorLoop < 200; errorLoop++) {
recordAcc();
recordGyro();
accError();
gyroError();
delay(20);
}
errorGX = totalGX / 200;
errorGY = totalGY / 200;
errorGZ = totalGZ / 200;
errorAX = totalAX / 200;
errorAY = totalAY / 200;
errorAZ = totalAZ / 200;
}
void loop() {
recordAcc();
recordGyro();
rotX -= errorGX;
rotY -= errorGY;
rotZ -= errorGZ;
gForceX -= errorAX;
gForceY -= errorAY;
gForceZ += errorAZ;
convertAcc();
convertGyro();
degX = degAX * 0.96 + degGX * 0.04;
degY = degAY * 0.96 + degGY * 0.04;
degZ = degGZ;
yawPitchRoll();
delay(500);
}
void setupMPU() {
Wire.beginTransmission(0b1101000);
Wire.write(0x6B);
Wire.write(0b00000000);
Wire.endTransmission();
Wire.beginTransmission(0b1101000);
Wire.write(0x1B);
Wire.write(0x00000000);
Wire.endTransmission();
Wire.beginTransmission(0b1101000);
Wire.write(0x1c);
Wire.write(0b00000000);
Wire.endTransmission();
}
void recordAcc() {
Wire.beginTransmission(0b1101000);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0b1101000, 6);
while (Wire.available() < 6);
accX = Wire.read() << 8 | Wire.read();
accY = Wire.read() << 8 | Wire.read();
accZ = Wire.read() << 8 | Wire.read();
processAccData();
}
void processAccData() {
gForceX = accX / 16384.0;
gForceY = accY / 16384.0;
gForceZ = accZ / 16384.0;
}
void accError() {
totalAX += gForceX;
totalAY += gForceY;
totalAZ += gForceZ;
}
void convertAcc() {
// degAY = atan2(gForceX,gForceZ)*360/PI;
degAX = 360 * atan(gForceY / sqrt((gForceX * gForceX) + (gForceZ * gForceZ))) / PI;
degAY = 360 * atan(gForceX / sqrt((gForceY * gForceY) + (gForceZ * gForceZ))) / PI;
// degAX = atan2(gForceY,gForceZ)*360/PI;
}
void recordGyro() {
Wire.beginTransmission(0b1101000);
Wire.write(0x43);
Wire.endTransmission();
Wire.requestFrom(0b1101000, 6);
while (Wire.available() < 6);
gyroX = Wire.read() << 8 | Wire.read();
gyroY = Wire.read() << 8 | Wire.read();
gyroZ = Wire.read() << 8 | Wire.read();
processGyroData();
}
void processGyroData() {
rotX = gyroX / 131.0;
rotY = gyroY / 131.0;
rotZ = gyroZ / 131.0;
}
void gyroError() {
totalGX += rotX;
totalGY += rotY;
totalGZ += rotZ;
}
void convertGyro() {
previousTime = currentTime;
currentTime = millis();
elapsedTime = (currentTime - previousTime) / 1000;
if (rotX > 0.2 || rotX < -0.2) {
degGX = degGX + rotX * elapsedTime;
}
else {
degGX = degGX;
}
if (rotY > 0.4 || rotY < -0.25) {
degGY = degGY + rotY * elapsedTime;
}
else {
degGY = degGY;
}
if (rotZ > 0.4 || rotZ < -0.4) {
degGZ = degGZ + rotZ * elapsedTime;
}
else {
degGZ = degGZ;
}
}
void yawPitchRoll() {
Serial.print("Yaw: ");
Serial.print(degZ);
Serial.print(" Pitch: ");
Serial.print(degY);
Serial.print(" Roll: ");
Serial.println(degX);
Serial.println(gForceX);
Serial.println(gForceY);
Serial.println(gForceZ);
Serial.println(rotX);
Serial.println(rotY);
Serial.println(rotZ);
}
Basically, I'm finding the error of the values that I extract from the sensor by compiling together the first 200 sensor data points and calculating the error. Then I use this error to correct any further values that the sensor gives. With these error corrected acceleration values I calculate my pitch and roll angles and put them through a complementary filter to output a fairly decent approximation of the sensor orientation in terms of yaw pitch and roll (when it does work ofcourse).
The problem that I'm running into with my current code is that my expected values for YPR should all be around 0.00 however that isn't the case and my pitch and roll values are jumping all over the place while the sensor is stationary (I've attached an image to show you my output). I've identified the cause of this problem, it seems to be line 45 where I do gForceZ -= errorAZ. I've done as much Serial.print troubleshooting as I can think of, and I've tested to make sure the division inside the atan equations isn't resulting in a inf value (which it isn't as printing the float values with more decimals shows they're not truly 0). I've used if statements to make sure the value was being passed correctly and going through everything as planned. I've change -= to += for the x or y values and it doesn't fix it. I've done the calculations for degAX and degAY with a separate calculator using values that were outputted. I still can't explain why the number are all over the place
The only thing Ive tried so far that's made the code work as intended and output accurate pitch and roll values is if I change line 45 to gForceZ += errorAZ which doesn't exactly calibrate the stationary sensor Z accelerometer value to ~0 but to ~1.8 and it outputs my YPR values as expected and the degrees change accordingly to movement of the sensor. I've also attached an image to what the output looks like when I make this change to line 45
I have no way to explain why this makes any sense and I've tried to research what might be going wrong here but can't figure it out. If the code works with my calibrated accelerometer Z value at ~1.8 I don't mind sticking with it but I have no way of justifying why I'm not calibrating it properly as with all the other values.
As for my wiring, my SDA and SCL for the MPU6050 are connected to A4 and A5 respectively. AD0 and GND are connected to ground on the Uno and VCC is connected to 5V.
I feel as though the solution might be simple and I'm going to feel dumb after having spent a couple hours troubleshooting this to find an explanation.
Thank you 
(I've also attached a copy of my code)
