From 042265b4c42006a79a3589db9d07eead89aaf892 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 14 Sep 2018 10:49:00 +1000 Subject: [PATCH] AP_AHRS: fixed synthetic airspeed to be along +ve X axis this prevents us from thinking we have +ve airspeed when flying backwards with no pitot tube --- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index aedd4fd625..c69d1f2343 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -668,8 +668,14 @@ AP_AHRS_DCM::drift_correction(float deltat) // keep last airspeed estimate for dead-reckoning purposes Vector3f airspeed = velocity - _wind; - airspeed.z = 0; - _last_airspeed = airspeed.length(); + + // rotate vector to body frame + const Matrix3f &rot = get_rotation_body_to_ned(); + airspeed = rot.mul_transpose(airspeed); + + // take positive component in X direction. This mimics a pitot + // tube + _last_airspeed = MAX(airspeed.x, 0); } if (have_gps()) {