|
|
@ -122,7 +122,6 @@ void AP_AHRS_MPU6000::drift_correction( float deltat ) |
|
|
|
// 0.65*0.04 / 0.005 = 5.2
|
|
|
|
// 0.65*0.04 / 0.005 = 5.2
|
|
|
|
float drift_limit = _mpu6000->get_gyro_drift_rate() * deltat
|
|
|
|
float drift_limit = _mpu6000->get_gyro_drift_rate() * deltat
|
|
|
|
/ _gyro_bias_from_gravity_gain; |
|
|
|
/ _gyro_bias_from_gravity_gain; |
|
|
|
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) |
|
|
|
|
|
|
|
errorRollPitch[0] = constrain(errorRollPitch[0], |
|
|
|
errorRollPitch[0] = constrain(errorRollPitch[0], |
|
|
|
-drift_limit, drift_limit); |
|
|
|
-drift_limit, drift_limit); |
|
|
|
errorRollPitch[1] = constrain(errorRollPitch[1], |
|
|
|
errorRollPitch[1] = constrain(errorRollPitch[1], |
|
|
|