mirror of https://github.com/ArduPilot/ardupilot
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
1ea1b500a6
commit
e445a455d0
|
@ -282,6 +282,7 @@ void AP_Quaternion::update(void)
|
|||
// compensate for centripetal acceleration
|
||||
float veloc;
|
||||
veloc = _gps->ground_speed / 100;
|
||||
accel.x -= _gps->acceleration();
|
||||
accel.y -= _gyro_smoothed.z * veloc;
|
||||
accel.z += _gyro_smoothed.y * veloc;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue