From 09dde518df89f7d263177130dc4db849bec7aebd Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 5 Sep 2018 08:01:59 +1000
Subject: [PATCH] AP_AHRS: fixed dead-reckoning groundspeed vector

this is essential for RTL in planes on GPS loss
---
 libraries/AP_AHRS/AP_AHRS.cpp     | 18 ++++++++++++++++--
 libraries/AP_AHRS/AP_AHRS_DCM.cpp |  5 +++++
 2 files changed, 21 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp
index 3d9be88da7..b42e7a6f88 100644
--- a/libraries/AP_AHRS/AP_AHRS.cpp
+++ b/libraries/AP_AHRS/AP_AHRS.cpp
@@ -237,13 +237,13 @@ Vector2f AP_AHRS::groundspeed_vector(void)
     // Generate estimate of ground speed vector using air data system
     Vector2f gndVelADS;
     Vector2f gndVelGPS;
-    float airspeed;
+    float airspeed = 0;
     const bool gotAirspeed = airspeed_estimate_true(&airspeed);
     const bool gotGPS = (AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D);
     if (gotAirspeed) {
         const Vector3f wind = wind_estimate();
         const Vector2f wind2d(wind.x, wind.y);
-        const Vector2f airspeed_vector(cosf(yaw) * airspeed, sinf(yaw) * airspeed);
+        const Vector2f airspeed_vector(_cos_yaw * airspeed, _sin_yaw * airspeed);
         gndVelADS = airspeed_vector - wind2d;
     }
 
@@ -281,6 +281,20 @@ Vector2f AP_AHRS::groundspeed_vector(void)
     if (!gotAirspeed && gotGPS) {
         return gndVelGPS;
     }
+
+    if (airspeed > 0) {
+        // we have a rough airspeed, and we have a yaw. For
+        // dead-reckoning purposes we can create a estimated
+        // groundspeed vector
+        Vector2f ret(cosf(yaw), sinf(yaw));
+        ret *= airspeed;
+        // adjust for estimated wind
+        Vector3f wind = wind_estimate();
+        ret.x += wind.x;
+        ret.y += wind.y;
+        return ret;
+    }
+
     return Vector2f(0.0f, 0.0f);
 }
 
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
index c75c87bf25..6ed1f7a731 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -1023,6 +1023,11 @@ bool AP_AHRS_DCM::airspeed_estimate(float *airspeed_ret) const
                                         gnd_speed + _wind_max);
         *airspeed_ret = true_airspeed / get_EAS2TAS();
     }
+    if (!ret) {
+        // give the last estimate, but return false. This is used by
+        // dead-reckoning code
+        *airspeed_ret = _last_airspeed;
+    }
     return ret;
 }