AP_AHRS: add a bool wind_estimate(...)

NavEKF3 can fail to return a wind estimate, and we lose that value

libraries/AP_AHRS/AP_AHRS.cpp

@@ -734,40 +734,40 @@ float AP_AHRS::get_error_yaw(void) const
 }
 
 // return a wind estimation vector, in m/s
-Vector3f AP_AHRS::wind_estimate(void) const
+bool AP_AHRS::wind_estimate(Vector3f &wind) const
 {
-    Vector3f wind;
     switch (active_EKF_type()) {
     case EKFType::NONE:
-        wind = dcm.wind_estimate();
+        return dcm.wind_estimate(wind);
-        break;
 
 #if AP_AHRS_SIM_ENABLED
     case EKFType::SIM:
-        wind = sim.wind_estimate();
+        return sim.wind_estimate(wind);
-        break;
 #endif
 
 #if HAL_NAVEKF2_AVAILABLE
     case EKFType::TWO:
         EKF2.getWind(wind);
-        break;
+        return true;
 #endif
 
 #if HAL_NAVEKF3_AVAILABLE
     case EKFType::THREE:
-        EKF3.getWind(wind);
+        return EKF3.getWind(wind);
-        break;
 #endif
 
 #if HAL_EXTERNAL_AHRS_ENABLED
     case EKFType::EXTERNAL:
-        wind.zero();
+        return false;
-        break;
 #endif
-        
     }
-    return wind;
+    return false;
+}
+Vector3f AP_AHRS::wind_estimate(void) const
+{
+    Vector3f ret;
+    UNUSED_RESULT(wind_estimate(ret));
+    return ret;
 }
 
 /*

libraries/AP_AHRS/AP_AHRS.h

@@ -108,7 +108,7 @@ public:
     // wind_estimation_enabled returns true if wind estimation is enabled
     bool get_wind_estimation_enabled() const { return wind_estimation_enabled; }
 
-    // return a wind estimation vector, in m/s
+    // return a wind estimation vector, in m/s; returns 0,0,0 on failure
     Vector3f wind_estimate() const;
 
     // instruct DCM to update its wind estimate:
@@ -784,6 +784,9 @@ private:
      */
     bool wind_estimation_enabled;
 
+    // return a wind estimation vector, in m/s
+    bool wind_estimate(Vector3f &wind) const WARN_IF_UNUSED;
+
     /*
      * fly_forward is set by the vehicles to indicate the vehicle
      * should generally be moving in the direction of its heading.

libraries/AP_AHRS/AP_AHRS_Backend.cpp

@@ -100,10 +100,10 @@ Vector2f AP_AHRS_DCM::groundspeed_vector(void)
     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{_cos_yaw * airspeed, _sin_yaw * airspeed};
-        gndVelADS = airspeed_vector + wind2d;
+        Vector3f wind;
+        UNUSED_RESULT(wind_estimate(wind));
+        gndVelADS = airspeed_vector + wind.xy();
     }
 
     // Generate estimate of ground speed vector using GPS
@@ -148,7 +148,8 @@ Vector2f AP_AHRS_DCM::groundspeed_vector(void)
         Vector2f ret{_cos_yaw, _sin_yaw};
         ret *= airspeed;
         // adjust for estimated wind
-        const Vector3f wind = wind_estimate();
+        Vector3f wind;
+        UNUSED_RESULT(wind_estimate(wind));
         ret.x += wind.x;
         ret.y += wind.y;
         return ret;

libraries/AP_AHRS/AP_AHRS_Backend.h

@@ -117,7 +117,7 @@ public:
     virtual bool get_hagl(float &height) const WARN_IF_UNUSED { return false; }
 
     // return a wind estimation vector, in m/s
-    virtual Vector3f wind_estimate(void) const = 0;
+    virtual bool wind_estimate(Vector3f &wind) const = 0;
 
     // return an airspeed estimate if available. return true
     // if we have an estimate

libraries/AP_AHRS/AP_AHRS_DCM.h

@@ -72,8 +72,9 @@ public:
     }
 
     // return a wind estimation vector, in m/s
-    Vector3f wind_estimate() const override {
+    bool wind_estimate(Vector3f &wind) const override {
-        return _wind;
+        wind = _wind;
+        return true;
     }
 
     // return an airspeed estimate if available. return true

libraries/AP_AHRS/AP_AHRS_SIM.cpp

@@ -31,13 +31,14 @@ bool AP_AHRS_SIM::get_velocity_NED(Vector3f &vec) const
     return true;
 }
 
-Vector3f AP_AHRS_SIM::wind_estimate() const
+bool AP_AHRS_SIM::wind_estimate(Vector3f &wind) const
 {
     if (_sitl == nullptr) {
-        return Vector3f{};
+        return false;
     }
 
-    return _sitl->state.wind_ef;
+    wind = _sitl->state.wind_ef;
+    return true;
 }
 
 bool AP_AHRS_SIM::airspeed_estimate(float &airspeed_ret) const

libraries/AP_AHRS/AP_AHRS_SIM.h

@@ -67,7 +67,7 @@ public:
     bool get_hagl(float &hagl) const override WARN_IF_UNUSED;
 
     // return a wind estimation vector, in m/s
-    Vector3f wind_estimate() const override;
+    bool wind_estimate(Vector3f &wind) const override;
 
     // return an airspeed estimate if available. return true
     // if we have an estimate

libraries/AP_AHRS/AP_AHRS_View.h

@@ -89,8 +89,8 @@ public:
         return ahrs.get_location(loc);
     }
 
-    Vector3f wind_estimate(void) {
+    bool wind_estimate(Vector3f &wind) {
-        return ahrs.wind_estimate();
+        return ahrs.wind_estimate(wind);
     }
 
     bool airspeed_estimate(float &airspeed_ret) const WARN_IF_UNUSED {