Hello,
using MPU-6050 for angle measurements the angle value starts with significant offset (about 10 degree) and will stabilize to expected value (around 0 degree) after a quite long time of about 1.5 seconds.
Is it possible to avoid this long stabilization phase?
- I'm using complementary filter to calculate the final angle value
- there is no delay() in the loop slowing down the measurement
// determine angle arround y-axis through gyro
void determineYAngleAndTime() {
float accX,accY,accZ,gyroY;
float accYAngle,gyroYAngle;
Wire.beginTransmission(MPU);
Wire.write(0x3B); // Start with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true); // Read 6 registers total, each axis value is stored in 2 registers
//For a range of +-2g, we need to divide the raw values by 16384, according to the datasheet
accX = (Wire.read() << 8 | Wire.read()) / accSens; // X-axis value
accY = (Wire.read() << 8 | Wire.read()) / accSens; // Y-axis value
accZ = (Wire.read() << 8 | Wire.read()) / accSens; // Z-axis value
// read gyro value for Y-axis only
Wire.beginTransmission(MPU); // begin transmission to I2C slave device
Wire.write(0x45); // Gyro data first register address 0x45 --> gyro Y axis data
Wire.endTransmission(false); // restarts transmission to I2C slave device
Wire.requestFrom(MPU,2,true); // request 2 registers in total, 2 registers for each axis value
previousTime = atTimeStamp; // Previous time is stored before the actual time read
gyroY = (Wire.read() << 8 | Wire.read()) / gyroSens; // For a 250deg/s range we have to divide first the raw value by 131.0, according to the datasheet
atTimeStamp = millis(); // timeStamp for angle measurement event
elapsedTime = atTimeStamp - previousTime; // Divide by 1000 to get seconds?
// calculate y-Angle from acc data
accYAngle = (atan(-1 * accX / sqrt(pow(accY, 2) + pow(accZ, 2))) * 180 / PI); //+ accXError?
// calculate y-Angle from gyro data
// gyroY = gyroY + gyroYError;
gyroYAngle = yAngle + gyroY * float(elapsedTime) / 1000.0; // deg/s * s = deg
// Complementary filter - combine accelerometer and gyro angle values
yAngle = 0.96 * gyroYAngle + 0.04 * accYAngle;
}