diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 3f96a83504..e1b5719385 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/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(); - break; + return dcm.wind_estimate(wind); #if AP_AHRS_SIM_ENABLED case EKFType::SIM: - wind = sim.wind_estimate(); - break; + return sim.wind_estimate(wind); #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); - break; + return EKF3.getWind(wind); #endif #if HAL_EXTERNAL_AHRS_ENABLED case EKFType::EXTERNAL: - wind.zero(); - break; + return false; #endif - } - return wind; + return false; +} +Vector3f AP_AHRS::wind_estimate(void) const +{ + Vector3f ret; + UNUSED_RESULT(wind_estimate(ret)); + return ret; } /* diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 96032e4530..3ac55c7b5f 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/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. diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.cpp b/libraries/AP_AHRS/AP_AHRS_Backend.cpp index e98c4ade85..fe74611a1e 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.cpp +++ b/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; diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.h b/libraries/AP_AHRS/AP_AHRS_Backend.h index cfc4967fc9..3314787f5e 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.h +++ b/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 diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index d6906a4730..15dbfec327 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/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 { - return _wind; + bool wind_estimate(Vector3f &wind) const override { + wind = _wind; + return true; } // return an airspeed estimate if available. return true diff --git a/libraries/AP_AHRS/AP_AHRS_SIM.cpp b/libraries/AP_AHRS/AP_AHRS_SIM.cpp index 1e49c5f9be..a14b251911 100644 --- a/libraries/AP_AHRS/AP_AHRS_SIM.cpp +++ b/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 diff --git a/libraries/AP_AHRS/AP_AHRS_SIM.h b/libraries/AP_AHRS/AP_AHRS_SIM.h index eced82ce6c..037ac789a9 100644 --- a/libraries/AP_AHRS/AP_AHRS_SIM.h +++ b/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 diff --git a/libraries/AP_AHRS/AP_AHRS_View.h b/libraries/AP_AHRS/AP_AHRS_View.h index 80abb2c628..109cbd1781 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.h +++ b/libraries/AP_AHRS/AP_AHRS_View.h @@ -89,8 +89,8 @@ public: return ahrs.get_location(loc); } - Vector3f wind_estimate(void) { - return ahrs.wind_estimate(); + bool wind_estimate(Vector3f &wind) { + return ahrs.wind_estimate(wind); } bool airspeed_estimate(float &airspeed_ret) const WARN_IF_UNUSED {