Quaternion: use GPS to correct for linear acceleration
this gives much better pitch estimates. We should do this with the airspeed sensor if available.
This commit is contained in:
parent
c7ef72cf28
commit
7256652de5
@ -282,6 +282,7 @@ void AP_Quaternion::update(void)
|
|||||||
// compensate for centripetal acceleration
|
// compensate for centripetal acceleration
|
||||||
float veloc;
|
float veloc;
|
||||||
veloc = _gps->ground_speed / 100;
|
veloc = _gps->ground_speed / 100;
|
||||||
|
accel.x -= _gps->acceleration();
|
||||||
accel.y -= _gyro_smoothed.z * veloc;
|
accel.y -= _gyro_smoothed.z * veloc;
|
||||||
accel.z += _gyro_smoothed.y * veloc;
|
accel.z += _gyro_smoothed.y * veloc;
|
||||||
}
|
}
|
||||||
|
Loading…
Reference in New Issue
Block a user