I am trying to write a simple sketch for the MPU6050.
The problem is that the sensor is pretty much solid when stationary or changing the angle steadily at slow to mid angular velocities but if I try to change the angle too fast there's a huge drift in the readings by pretty much tens of degrees in either direction as shown in the below images from the serial plotter.
On slow to mid changes in the pitch angle the gyro is tracking the accelerometer pitch almost identically (it drifts a little over time but it's normal and acceptable for the complementary filter).
On fast changes however the pitch drifts instantly by tens of degrees from the accelerometer. The huge drift makes the complementary filter pretty much useless in correcting the output
It's my first try with the sensor so I don't have any experience with its parameters. But the problem seems to be in the lines of integrating the new angular velocities in roll and pitch.
I got lazy and used the default sensitivity scale factors; does this have anything to do with this behaviour?
The dt measured for the Arduino is 20 ms between readings; is this too slow for the fast changes or something?
The datasheet also mentioned a programmable output range. Does this need to be programmed for more bandwidth and accurate results?
EDIT:
I tried to read a bit about the sensitivity scale factor. It seemed that increasing the angular velocity beyond 250 degrees per second is not measured when selecting the default configuration.
After some testing I realized that 1000 deg/s is a good configuration for me; the drift certainly is much better but still not what I hoped for. It still drifts by +- 10 degrees in two or three seconds of moving it around.
Slow to Mid changes output:
Fast changes output:
#include <Wire.h>
#define CAL_AVERAGE 2000
const int MPU = 0x68; //I2C address of MPU6050
const float alpha = 0.95;
double ax, ay, az;
double gx, gy, gz;
//float accAngleX, accAngleY, accAngleZ;
//float gyroAgnleX, gyroAngleY, gyroAngleZ;
double accErrorX, accErrorY, accErrorZ;
double gyroErrorx, gyroErrorY, gyroErrorZ;
double accRoll, accPitch;
double gyroRoll, gyroPitch, gyroYaw;
double roll, pitch, yaw;
double dt, currentTime, previousTime;
void setup(){
previousTime = 0;
Serial.begin(115200);
Wire.begin(); //init the communication
//reset the mpu
Wire.beginTransmission(MPU);
Wire.write(0x6B);
Wire.write(0x00);
Wire.endTransmission(true);
//calibrate here
calculateError();
}
void loop(){
readAcc();
readGyro();
//get roll and pitch of accelerometer
accRoll = atan2(ay, az) * 180 / PI;
accPitch = atan2(ax, az) * 180 / PI;
currentTime = millis();
//set gyro roll and pitch for accelerometer roll and pitch on first reading only
if (previousTime != 0) {
//get roll, pitch and yaw of gyro
dt = currentTime - previousTime;
dt /= 1000;
gyroRoll += gx * dt;
gyroPitch += -gy * dt;
gyroYaw += gz * dt;
} else {
gyroRoll = accRoll;
gyroPitch = accPitch;
gyroYaw = 0;
}
previousTime = currentTime;
roll = alphagyroRoll + (1 - alpha)accRoll;
pitch = alphagyroPitch + (1 - alpha)accPitch;
yaw = gyroYaw;
//finally print the values collected
printData();
}
void readAcc(){
//first read accelerometer data
Wire.beginTransmission(MPU);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true);
//accelerometer raw data
ax = ( (Wire.read() << 8) | Wire.read() ) / 16384.0;
ay = ( (Wire.read() << 8) | Wire.read() ) / 16384.0;
az = ( (Wire.read() << 8) | Wire.read() ) / 16384.0;
ax -= accErrorX;
ay -= accErrorY;
az -= accErrorZ;
}
void readGyro(){
//second read gyro
Wire.beginTransmission(MPU);
Wire.write(0x43);
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true);
//gyro raw data
gx = ( (Wire.read() << 8) | Wire.read() ) / 131.0;
gy = ( (Wire.read() << 8) | Wire.read() ) / 131.0;
gz = ( (Wire.read() << 8) | Wire.read() ) / 131.0;
gx -= gyroErrorx;
gy -= gyroErrorY;
gz -= gyroErrorZ;
}
void calculateError(){
float axErr = 0, ayErr = 0, azErr = 0;
float gxErr = 0, gyErr = 0, gzErr = 0;
for (int i = 0; i < CAL_AVERAGE; i++) {
readAcc();
az = az - 1;
axErr += ax;
ayErr += ay;
azErr += az;
readGyro();
gxErr += gx;
gyErr += gy;
gzErr += gz;
}
accErrorX = axErr/CAL_AVERAGE;
accErrorY = ayErr/CAL_AVERAGE;
accErrorZ = azErr/CAL_AVERAGE;
gyroErrorx = gxErr/CAL_AVERAGE;
gyroErrorY = gyErr/CAL_AVERAGE;
gyroErrorZ = gzErr/CAL_AVERAGE;
}
void printData() {
Serial.print(ax);
Serial.print(", ");
Serial.print(ay);
Serial.print(", ");
Serial.print(az);
Serial.print(", ");
Serial.print(gx);
Serial.print(", ");
Serial.print(gy);
Serial.print(", ");
Serial.print(gz);
Serial.print(", ");
Serial.print(accPitch);
Serial.print(", ");
Serial.print(accRoll);
Serial.print(", ");
Serial.print(gyroPitch);
Serial.print(", ");
Serial.print(gyroRoll);
Serial.print(", ");
Serial.print(gyroYaw);
Serial.print(", ");
Serial.print(pitch);
Serial.print(", ");
Serial.print(roll);
Serial.print(", ");
Serial.print(yaw);
Serial.print(", ");
Serial.println(dt);
}
