From e445a455d0d2ed0a8e81faa03ad30c9896c77b5e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 4 Mar 2012 20:35:40 +1100 Subject: [PATCH] Quaternion: use GPS to correct for linear acceleration this gives much better pitch estimates. We should do this with the airspeed sensor if available. --- libraries/AP_Quaternion/AP_Quaternion.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_Quaternion/AP_Quaternion.cpp b/libraries/AP_Quaternion/AP_Quaternion.cpp index a50f9ef86d..1b4346657f 100644 --- a/libraries/AP_Quaternion/AP_Quaternion.cpp +++ b/libraries/AP_Quaternion/AP_Quaternion.cpp @@ -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; }