From 84440108e3ad4333b6f581d87ab8f5e4bf0fcc12 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 25 Nov 2024 10:50:17 +1100 Subject: [PATCH] AP_AHRS: rename DCM members to clarify EAS vs TAS --- libraries/AP_AHRS/AP_AHRS.cpp | 28 ++++++++++++------------- libraries/AP_AHRS/AP_AHRS.h | 10 ++++----- libraries/AP_AHRS/AP_AHRS_Backend.h | 9 ++++---- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 32 ++++++++++++++--------------- libraries/AP_AHRS/AP_AHRS_DCM.h | 16 +++++++-------- libraries/AP_AHRS/AP_AHRS_SIM.cpp | 6 +++--- libraries/AP_AHRS/AP_AHRS_SIM.h | 4 ++-- 7 files changed, 53 insertions(+), 52 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 945ad67d41..e8fa5cd623 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -346,9 +346,9 @@ void AP_AHRS::update_state(void) state.primary_core = _get_primary_core_index(); state.wind_estimate_ok = _wind_estimate(state.wind_estimate); state.EAS2TAS = AP_AHRS_Backend::get_EAS2TAS(); - state.airspeed_ok = _airspeed_estimate(state.airspeed, state.airspeed_estimate_type); - state.airspeed_true_ok = _airspeed_estimate_true(state.airspeed_true); - state.airspeed_vec_ok = _airspeed_vector_true(state.airspeed_vec); + state.airspeed_ok = _airspeed_EAS(state.airspeed, state.airspeed_estimate_type); + state.airspeed_true_ok = _airspeed_TAS(state.airspeed_true); + state.airspeed_vec_ok = _airspeed_TAS(state.airspeed_vec); state.quat_ok = _get_quaternion(state.quat); state.secondary_attitude_ok = _get_secondary_attitude(state.secondary_attitude); state.secondary_quat_ok = _get_secondary_quaternion(state.secondary_quat); @@ -931,7 +931,7 @@ bool AP_AHRS::_should_use_airspeed_sensor(uint8_t airspeed_index) const // return an airspeed estimate if available. return true // if we have an estimate -bool AP_AHRS::_airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &airspeed_estimate_type) const +bool AP_AHRS::_airspeed_EAS(float &airspeed_ret, AirspeedEstimateType &airspeed_estimate_type) const { #if AP_AHRS_DCM_ENABLED || (AP_AIRSPEED_ENABLED && AP_GPS_ENABLED) const uint8_t idx = get_active_airspeed_index(); @@ -970,20 +970,20 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &airs #if AP_AHRS_DCM_ENABLED case EKFType::DCM: airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC; - return dcm.airspeed_estimate(idx, airspeed_ret); + return dcm.airspeed_EAS(idx, airspeed_ret); #endif #if AP_AHRS_SIM_ENABLED case EKFType::SIM: airspeed_estimate_type = AirspeedEstimateType::SIM; - return sim.airspeed_estimate(airspeed_ret); + return sim.airspeed_EAS(airspeed_ret); #endif #if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: #if AP_AHRS_DCM_ENABLED airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC; - return dcm.airspeed_estimate(idx, airspeed_ret); + return dcm.airspeed_EAS(idx, airspeed_ret); #else return false; #endif @@ -999,7 +999,7 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &airs case EKFType::EXTERNAL: #if AP_AHRS_DCM_ENABLED airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC; - return dcm.airspeed_estimate(idx, airspeed_ret); + return dcm.airspeed_EAS(idx, airspeed_ret); #else return false; #endif @@ -1027,18 +1027,18 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &airs #if AP_AHRS_DCM_ENABLED // fallback to DCM airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC; - return dcm.airspeed_estimate(idx, airspeed_ret); + return dcm.airspeed_EAS(idx, airspeed_ret); #endif return false; } -bool AP_AHRS::_airspeed_estimate_true(float &airspeed_ret) const +bool AP_AHRS::_airspeed_TAS(float &airspeed_ret) const { switch (active_EKF_type()) { #if AP_AHRS_DCM_ENABLED case EKFType::DCM: - return dcm.airspeed_estimate_true(airspeed_ret); + return dcm.airspeed_TAS(airspeed_ret); #endif #if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: @@ -1064,7 +1064,7 @@ bool AP_AHRS::_airspeed_estimate_true(float &airspeed_ret) const // return estimate of true airspeed vector in body frame in m/s // returns false if estimate is unavailable -bool AP_AHRS::_airspeed_vector_true(Vector3f &vec) const +bool AP_AHRS::_airspeed_TAS(Vector3f &vec) const { switch (active_EKF_type()) { #if AP_AHRS_DCM_ENABLED @@ -1126,14 +1126,14 @@ bool AP_AHRS::airspeed_health_data(float &innovation, float &innovationVariance, return false; } -// return a synthetic airspeed estimate (one derived from sensors +// return a synthetic EAS estimate (one derived from sensors // other than an actual airspeed sensor), if available. return // true if we have a synthetic airspeed. ret will not be modified // on failure. bool AP_AHRS::synthetic_airspeed(float &ret) const { #if AP_AHRS_DCM_ENABLED - return dcm.synthetic_airspeed(ret); + return dcm.synthetic_airspeed_EAS(ret); #endif return false; } diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 10f9deed01..04ce489dad 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -152,7 +152,7 @@ public: // get air density / sea level density - decreases as altitude climbs float get_air_density_ratio(void) const; - // return an airspeed estimate if available. return true + // return an (equivalent) airspeed estimate if available. return true // if we have an estimate bool airspeed_estimate(float &airspeed_ret) const; @@ -195,7 +195,7 @@ public: return AP_AHRS_Backend::airspeed_sensor_enabled(airspeed_index); } - // return a synthetic airspeed estimate (one derived from sensors + // return a synthetic (equivalent) airspeed estimate (one derived from sensors // other than an actual airspeed sensor), if available. return // true if we have a synthetic airspeed. ret will not be modified // on failure. @@ -873,7 +873,7 @@ private: // return an airspeed estimate if available. return true // if we have an estimate - bool _airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &status) const; + bool _airspeed_EAS(float &airspeed_ret, AirspeedEstimateType &status) const; // return secondary attitude solution if available, as eulers in radians bool _get_secondary_attitude(Vector3f &eulers) const; @@ -892,11 +892,11 @@ private: // return a true airspeed estimate (navigation airspeed) if // available. return true if we have an estimate - bool _airspeed_estimate_true(float &airspeed_ret) const; + bool _airspeed_TAS(float &airspeed_ret) const; // return estimate of true airspeed vector in body frame in m/s // returns false if estimate is unavailable - bool _airspeed_vector_true(Vector3f &vec) const; + bool _airspeed_TAS(Vector3f &vec) const; // return the quaternion defining the rotation from NED to XYZ (body) axes bool _get_quaternion(Quaternion &quat) const WARN_IF_UNUSED; diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.h b/libraries/AP_AHRS/AP_AHRS_Backend.h index 0e70ce9662..99ad8f2dc8 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.h +++ b/libraries/AP_AHRS/AP_AHRS_Backend.h @@ -135,13 +135,13 @@ public: // return an airspeed estimate if available. return true // if we have an estimate - virtual bool airspeed_estimate(float &airspeed_ret) const WARN_IF_UNUSED { return false; } - virtual bool airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const { return false; } + virtual bool airspeed_EAS(float &airspeed_ret) const WARN_IF_UNUSED { return false; } + virtual bool airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const { return false; } // return a true airspeed estimate (navigation airspeed) if // available. return true if we have an estimate - bool airspeed_estimate_true(float &airspeed_ret) const WARN_IF_UNUSED { - if (!airspeed_estimate(airspeed_ret)) { + bool airspeed_TAS(float &airspeed_ret) const WARN_IF_UNUSED { + if (!airspeed_EAS(airspeed_ret)) { return false; } airspeed_ret *= get_EAS2TAS(); @@ -156,6 +156,7 @@ public: // get apparent to true airspeed ratio static float get_EAS2TAS(void); + static float get_TAS2EAS(void) { return 1.0/get_EAS2TAS(); } // return true if airspeed comes from an airspeed sensor, as // opposed to an IMU estimate diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index d7aa5c7cd3..a64821c145 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -724,16 +724,16 @@ AP_AHRS_DCM::drift_correction(float deltat) return; } - float airspeed = _last_airspeed; + float airspeed_TAS = _last_airspeed_TAS; #if AP_AIRSPEED_ENABLED if (airspeed_sensor_enabled()) { - airspeed = AP::airspeed()->get_airspeed(); + airspeed_TAS = AP::airspeed()->get_airspeed(); } #endif // use airspeed to estimate our ground velocity in // earth frame by subtracting the wind - velocity = _dcm_matrix.colx() * airspeed; + velocity = _dcm_matrix.colx() * airspeed_TAS; // add in wind estimate velocity += _wind; @@ -762,7 +762,7 @@ AP_AHRS_DCM::drift_correction(float deltat) // take positive component in X direction. This mimics a pitot // tube - _last_airspeed = MAX(airspeed.x, 0); + _last_airspeed_TAS = MAX(airspeed.x, 0); } if (have_gps()) { @@ -1084,31 +1084,31 @@ bool AP_AHRS_DCM::get_location(Location &loc) const return _have_position; } -bool AP_AHRS_DCM::airspeed_estimate(float &airspeed_ret) const +bool AP_AHRS_DCM::airspeed_EAS(float &airspeed_ret) const { #if AP_AIRSPEED_ENABLED const auto *airspeed = AP::airspeed(); if (airspeed != nullptr) { - return airspeed_estimate(airspeed->get_primary(), airspeed_ret); + return airspeed_EAS(airspeed->get_primary(), airspeed_ret); } #endif // airspeed_estimate will also make this nullptr check and act // appropriately when we call it with a dummy sensor ID. - return airspeed_estimate(0, airspeed_ret); + return airspeed_EAS(0, airspeed_ret); } -// return an airspeed estimate: +// return an (equivalent) airspeed estimate: // - from a real sensor if available // - otherwise from a GPS-derived wind-triangle estimate (if GPS available) // - otherwise from a cached wind-triangle estimate value (but returning false) -bool AP_AHRS_DCM::airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const +bool AP_AHRS_DCM::airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const { - // airspeed_ret: will always be filled-in by get_unconstrained_airspeed_estimate which fills in airspeed_ret in this order: + // airspeed_ret: will always be filled-in by get_unconstrained_airspeed_EAS which fills in airspeed_ret in this order: // airspeed as filled-in by an enabled airspeed sensor // if no airspeed sensor: airspeed estimated using the GPS speed & wind_speed_estimation // Or if none of the above, fills-in using the previous airspeed estimate // Return false: if we are using the previous airspeed estimate - if (!get_unconstrained_airspeed_estimate(airspeed_index, airspeed_ret)) { + if (!get_unconstrained_airspeed_EAS(airspeed_index, airspeed_ret)) { return false; } @@ -1127,12 +1127,12 @@ bool AP_AHRS_DCM::airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) return true; } -// airspeed_ret: will always be filled-in by get_unconstrained_airspeed_estimate which fills in airspeed_ret in this order: +// airspeed_ret: will always be filled-in by get_unconstrained_airspeed_EAS which fills in airspeed_ret in this order: // airspeed as filled-in by an enabled airspeed sensor // if no airspeed sensor: airspeed estimated using the GPS speed & wind_speed_estimation // Or if none of the above, fills-in using the previous airspeed estimate // Return false: if we are using the previous airspeed estimate -bool AP_AHRS_DCM::get_unconstrained_airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const +bool AP_AHRS_DCM::get_unconstrained_airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const { #if AP_AIRSPEED_ENABLED if (airspeed_sensor_enabled(airspeed_index)) { @@ -1143,13 +1143,13 @@ bool AP_AHRS_DCM::get_unconstrained_airspeed_estimate(uint8_t airspeed_index, fl if (AP::ahrs().get_wind_estimation_enabled() && have_gps()) { // estimated via GPS speed and wind - airspeed_ret = _last_airspeed; + airspeed_ret = _last_airspeed_TAS; return true; } // Else give the last estimate, but return false. // This is used by the dead-reckoning code - airspeed_ret = _last_airspeed; + airspeed_ret = _last_airspeed_TAS; return false; } @@ -1182,7 +1182,7 @@ Vector2f AP_AHRS_DCM::groundspeed_vector(void) Vector2f gndVelADS; Vector2f gndVelGPS; float airspeed = 0; - const bool gotAirspeed = airspeed_estimate_true(airspeed); + const bool gotAirspeed = airspeed_TAS(airspeed); const bool gotGPS = (AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D); if (gotAirspeed) { const Vector2f airspeed_vector{_cos_yaw * airspeed, _sin_yaw * airspeed}; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index e4e13ded40..8f9288d014 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -82,18 +82,18 @@ public: // return an airspeed estimate if available. return true // if we have an estimate - bool airspeed_estimate(float &airspeed_ret) const override; + bool airspeed_EAS(float &airspeed_ret) const override; // return an airspeed estimate if available. return true // if we have an estimate from a specific sensor index - bool airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const override; + bool airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const override; - // return a synthetic airspeed estimate (one derived from sensors + // return a synthetic EAS estimate (one derived from sensors // other than an actual airspeed sensor), if available. return // true if we have a synthetic airspeed. ret will not be modified // on failure. - bool synthetic_airspeed(float &ret) const WARN_IF_UNUSED { - ret = _last_airspeed; + bool synthetic_airspeed_EAS(float &ret) const WARN_IF_UNUSED { + ret = _last_airspeed_TAS; return true; } @@ -173,12 +173,12 @@ private: // DCM matrix from the eulers. Called internally we may. void reset(bool recover_eulers); - // airspeed_ret: will always be filled-in by get_unconstrained_airspeed_estimate which fills in airspeed_ret in this order: + // airspeed_ret: will always be filled-in by get_unconstrained_airspeed_EAS which fills in airspeed_ret in this order: // airspeed as filled-in by an enabled airspeed sensor // if no airspeed sensor: airspeed estimated using the GPS speed & wind_speed_estimation // Or if none of the above, fills-in using the previous airspeed estimate // Return false: if we are using the previous airspeed estimate - bool get_unconstrained_airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const; + bool get_unconstrained_airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const; // primary representation of attitude of board used for all inertial calculations Matrix3f _dcm_matrix; @@ -262,7 +262,7 @@ private: Vector3f _last_fuse; Vector3f _last_vel; uint32_t _last_wind_time; - float _last_airspeed; + float _last_airspeed_TAS; uint32_t _last_consistent_heading; // estimated wind in m/s diff --git a/libraries/AP_AHRS/AP_AHRS_SIM.cpp b/libraries/AP_AHRS/AP_AHRS_SIM.cpp index c1e26f1890..4f9b223cdb 100644 --- a/libraries/AP_AHRS/AP_AHRS_SIM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_SIM.cpp @@ -41,7 +41,7 @@ bool AP_AHRS_SIM::wind_estimate(Vector3f &wind) const return true; } -bool AP_AHRS_SIM::airspeed_estimate(float &airspeed_ret) const +bool AP_AHRS_SIM::airspeed_EAS(float &airspeed_ret) const { if (_sitl == nullptr) { return false; @@ -52,9 +52,9 @@ bool AP_AHRS_SIM::airspeed_estimate(float &airspeed_ret) const return true; } -bool AP_AHRS_SIM::airspeed_estimate(uint8_t index, float &airspeed_ret) const +bool AP_AHRS_SIM::airspeed_EAS(uint8_t index, float &airspeed_ret) const { - return airspeed_estimate(airspeed_ret); + return airspeed_EAS(airspeed_ret); } bool AP_AHRS_SIM::get_quaternion(Quaternion &quat) const diff --git a/libraries/AP_AHRS/AP_AHRS_SIM.h b/libraries/AP_AHRS/AP_AHRS_SIM.h index 037b64c388..924a0d6bea 100644 --- a/libraries/AP_AHRS/AP_AHRS_SIM.h +++ b/libraries/AP_AHRS/AP_AHRS_SIM.h @@ -68,11 +68,11 @@ public: // return an airspeed estimate if available. return true // if we have an estimate - bool airspeed_estimate(float &airspeed_ret) const override; + bool airspeed_EAS(float &airspeed_ret) const override; // return an airspeed estimate if available. return true // if we have an estimate from a specific sensor index - bool airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const override; + bool airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const override; // return a ground vector estimate in meters/second, in North/East order Vector2f groundspeed_vector() override;