Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: rename DCM members to clarify EAS vs TAS

This commit is contained in:
Peter Barker 2024-11-25 10:50:17 +11:00 committed by Peter Barker
parent 73710d888d
commit 84440108e3
7 changed files with 53 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -346,9 +346,9 @@ void AP_AHRS::update_state(void)
state.primary_core = _get_primary_core_index(); state.primary_core = _get_primary_core_index();
state.wind_estimate_ok = _wind_estimate(state.wind_estimate); state.wind_estimate_ok = _wind_estimate(state.wind_estimate);
state.EAS2TAS = AP_AHRS_Backend::get_EAS2TAS(); state.EAS2TAS = AP_AHRS_Backend::get_EAS2TAS();
state.airspeed_ok = _airspeed_estimate(state.airspeed, state.airspeed_estimate_type); state.airspeed_ok = _airspeed_EAS(state.airspeed, state.airspeed_estimate_type);
state.airspeed_true_ok = _airspeed_estimate_true(state.airspeed_true); state.airspeed_true_ok = _airspeed_TAS(state.airspeed_true);
state.airspeed_vec_ok = _airspeed_vector_true(state.airspeed_vec); state.airspeed_vec_ok = _airspeed_TAS(state.airspeed_vec);
state.quat_ok = _get_quaternion(state.quat); state.quat_ok = _get_quaternion(state.quat);
state.secondary_attitude_ok = _get_secondary_attitude(state.secondary_attitude); state.secondary_attitude_ok = _get_secondary_attitude(state.secondary_attitude);
state.secondary_quat_ok = _get_secondary_quaternion(state.secondary_quat); 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 // return an airspeed estimate if available. return true
// if we have an estimate // 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) #if AP_AHRS_DCM_ENABLED || (AP_AIRSPEED_ENABLED && AP_GPS_ENABLED)
const uint8_t idx = get_active_airspeed_index(); 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 #if AP_AHRS_DCM_ENABLED
case EKFType::DCM: case EKFType::DCM:
airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC; airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC;
return dcm.airspeed_estimate(idx, airspeed_ret); return dcm.airspeed_EAS(idx, airspeed_ret);
#endif #endif
#if AP_AHRS_SIM_ENABLED #if AP_AHRS_SIM_ENABLED
case EKFType::SIM: case EKFType::SIM:
airspeed_estimate_type = AirspeedEstimateType::SIM; airspeed_estimate_type = AirspeedEstimateType::SIM;
return sim.airspeed_estimate(airspeed_ret); return sim.airspeed_EAS(airspeed_ret);
#endif #endif
#if HAL_NAVEKF2_AVAILABLE #if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO: case EKFType::TWO:
#if AP_AHRS_DCM_ENABLED #if AP_AHRS_DCM_ENABLED
airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC; airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC;
return dcm.airspeed_estimate(idx, airspeed_ret); return dcm.airspeed_EAS(idx, airspeed_ret);
#else #else
return false; return false;
#endif #endif
@ -999,7 +999,7 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &airs
case EKFType::EXTERNAL: case EKFType::EXTERNAL:
#if AP_AHRS_DCM_ENABLED #if AP_AHRS_DCM_ENABLED
airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC; airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC;
return dcm.airspeed_estimate(idx, airspeed_ret); return dcm.airspeed_EAS(idx, airspeed_ret);
#else #else
return false; return false;
#endif #endif
@ -1027,18 +1027,18 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &airs
#if AP_AHRS_DCM_ENABLED #if AP_AHRS_DCM_ENABLED
// fallback to DCM // fallback to DCM
airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC; airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC;
return dcm.airspeed_estimate(idx, airspeed_ret); return dcm.airspeed_EAS(idx, airspeed_ret);
#endif #endif
return false; 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()) { switch (active_EKF_type()) {
#if AP_AHRS_DCM_ENABLED #if AP_AHRS_DCM_ENABLED
case EKFType::DCM: case EKFType::DCM:
return dcm.airspeed_estimate_true(airspeed_ret); return dcm.airspeed_TAS(airspeed_ret);
#endif #endif
#if HAL_NAVEKF2_AVAILABLE #if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO: 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 // return estimate of true airspeed vector in body frame in m/s
// returns false if estimate is unavailable // 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()) { switch (active_EKF_type()) {
#if AP_AHRS_DCM_ENABLED #if AP_AHRS_DCM_ENABLED
@ -1126,14 +1126,14 @@ bool AP_AHRS::airspeed_health_data(float &innovation, float &innovationVariance,
return false; 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 // other than an actual airspeed sensor), if available. return
// true if we have a synthetic airspeed. ret will not be modified // true if we have a synthetic airspeed. ret will not be modified
// on failure. // on failure.
bool AP_AHRS::synthetic_airspeed(float &ret) const bool AP_AHRS::synthetic_airspeed(float &ret) const
{ {
#if AP_AHRS_DCM_ENABLED #if AP_AHRS_DCM_ENABLED
return dcm.synthetic_airspeed(ret); return dcm.synthetic_airspeed_EAS(ret);
#endif #endif
return false; return false;
} }

10
libraries/AP_AHRS/AP_AHRS.h
View File

@ -152,7 +152,7 @@ public:
// get air density / sea level density - decreases as altitude climbs // get air density / sea level density - decreases as altitude climbs
float get_air_density_ratio(void) const; 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 // if we have an estimate
bool airspeed_estimate(float &airspeed_ret) const; bool airspeed_estimate(float &airspeed_ret) const;
@ -195,7 +195,7 @@ public:
return AP_AHRS_Backend::airspeed_sensor_enabled(airspeed_index); 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 // other than an actual airspeed sensor), if available. return
// true if we have a synthetic airspeed. ret will not be modified // true if we have a synthetic airspeed. ret will not be modified
// on failure. // on failure.
@ -873,7 +873,7 @@ private:
// return an airspeed estimate if available. return true // return an airspeed estimate if available. return true
// if we have an estimate // 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 // return secondary attitude solution if available, as eulers in radians
bool _get_secondary_attitude(Vector3f &eulers) const; bool _get_secondary_attitude(Vector3f &eulers) const;
@ -892,11 +892,11 @@ private:
// return a true airspeed estimate (navigation airspeed) if // return a true airspeed estimate (navigation airspeed) if
// available. return true if we have an estimate // 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 // return estimate of true airspeed vector in body frame in m/s
// returns false if estimate is unavailable // 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 // return the quaternion defining the rotation from NED to XYZ (body) axes
bool _get_quaternion(Quaternion &quat) const WARN_IF_UNUSED; bool _get_quaternion(Quaternion &quat) const WARN_IF_UNUSED;

9
libraries/AP_AHRS/AP_AHRS_Backend.h
View File

@ -135,13 +135,13 @@ public:
// return an airspeed estimate if available. return true // return an airspeed estimate if available. return true
// if we have an estimate // if we have an estimate
virtual bool airspeed_estimate(float &airspeed_ret) const WARN_IF_UNUSED { return false; } virtual bool airspeed_EAS(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(uint8_t airspeed_index, float &airspeed_ret) const { return false; }
// return a true airspeed estimate (navigation airspeed) if // return a true airspeed estimate (navigation airspeed) if
// available. return true if we have an estimate // available. return true if we have an estimate
bool airspeed_estimate_true(float &airspeed_ret) const WARN_IF_UNUSED { bool airspeed_TAS(float &airspeed_ret) const WARN_IF_UNUSED {
if (!airspeed_estimate(airspeed_ret)) { if (!airspeed_EAS(airspeed_ret)) {
return false; return false;
} }
airspeed_ret *= get_EAS2TAS(); airspeed_ret *= get_EAS2TAS();
@ -156,6 +156,7 @@ public:
// get apparent to true airspeed ratio // get apparent to true airspeed ratio
static float get_EAS2TAS(void); 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 // return true if airspeed comes from an airspeed sensor, as
// opposed to an IMU estimate // opposed to an IMU estimate

32
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -724,16 +724,16 @@ AP_AHRS_DCM::drift_correction(float deltat)
return; return;
} }
float airspeed = _last_airspeed; float airspeed_TAS = _last_airspeed_TAS;
#if AP_AIRSPEED_ENABLED #if AP_AIRSPEED_ENABLED
if (airspeed_sensor_enabled()) { if (airspeed_sensor_enabled()) {
airspeed = AP::airspeed()->get_airspeed(); airspeed_TAS = AP::airspeed()->get_airspeed();
} }
#endif #endif
// use airspeed to estimate our ground velocity in // use airspeed to estimate our ground velocity in
// earth frame by subtracting the wind // earth frame by subtracting the wind
velocity = _dcm_matrix.colx() * airspeed; velocity = _dcm_matrix.colx() * airspeed_TAS;
// add in wind estimate // add in wind estimate
velocity += _wind; velocity += _wind;
@ -762,7 +762,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
// take positive component in X direction. This mimics a pitot // take positive component in X direction. This mimics a pitot
// tube // tube
_last_airspeed = MAX(airspeed.x, 0); _last_airspeed_TAS = MAX(airspeed.x, 0);
} }
if (have_gps()) { if (have_gps()) {
@ -1084,31 +1084,31 @@ bool AP_AHRS_DCM::get_location(Location &loc) const
return _have_position; 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 #if AP_AIRSPEED_ENABLED
const auto *airspeed = AP::airspeed(); const auto *airspeed = AP::airspeed();
if (airspeed != nullptr) { if (airspeed != nullptr) {
return airspeed_estimate(airspeed->get_primary(), airspeed_ret); return airspeed_EAS(airspeed->get_primary(), airspeed_ret);
} }
#endif #endif
// airspeed_estimate will also make this nullptr check and act // airspeed_estimate will also make this nullptr check and act
// appropriately when we call it with a dummy sensor ID. // 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 // - from a real sensor if available
// - otherwise from a GPS-derived wind-triangle estimate (if GPS available) // - otherwise from a GPS-derived wind-triangle estimate (if GPS available)
// - otherwise from a cached wind-triangle estimate value (but returning false) // - 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 // airspeed as filled-in by an enabled airspeed sensor
// if no airspeed sensor: airspeed estimated using the GPS speed & wind_speed_estimation // 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 // Or if none of the above, fills-in using the previous airspeed estimate
// Return false: if we are 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; return false;
} }
@ -1127,12 +1127,12 @@ bool AP_AHRS_DCM::airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret)
return true; 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 // airspeed as filled-in by an enabled airspeed sensor
// if no airspeed sensor: airspeed estimated using the GPS speed & wind_speed_estimation // 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 // Or if none of the above, fills-in using the previous airspeed estimate
// Return false: if we are 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 AP_AIRSPEED_ENABLED
if (airspeed_sensor_enabled(airspeed_index)) { 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()) { if (AP::ahrs().get_wind_estimation_enabled() && have_gps()) {
// estimated via GPS speed and wind // estimated via GPS speed and wind
airspeed_ret = _last_airspeed; airspeed_ret = _last_airspeed_TAS;
return true; return true;
} }
// Else give the last estimate, but return false. // Else give the last estimate, but return false.
// This is used by the dead-reckoning code // This is used by the dead-reckoning code
airspeed_ret = _last_airspeed; airspeed_ret = _last_airspeed_TAS;
return false; return false;
} }
@ -1182,7 +1182,7 @@ Vector2f AP_AHRS_DCM::groundspeed_vector(void)
Vector2f gndVelADS; Vector2f gndVelADS;
Vector2f gndVelGPS; Vector2f gndVelGPS;
float airspeed = 0; 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); const bool gotGPS = (AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D);
if (gotAirspeed) { if (gotAirspeed) {
const Vector2f airspeed_vector{_cos_yaw * airspeed, _sin_yaw * airspeed}; const Vector2f airspeed_vector{_cos_yaw * airspeed, _sin_yaw * airspeed};

16
libraries/AP_AHRS/AP_AHRS_DCM.h
View File

@ -82,18 +82,18 @@ public:
// return an airspeed estimate if available. return true // return an airspeed estimate if available. return true
// if we have an estimate // 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 // return an airspeed estimate if available. return true
// if we have an estimate from a specific sensor index // 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 // other than an actual airspeed sensor), if available. return
// true if we have a synthetic airspeed. ret will not be modified // true if we have a synthetic airspeed. ret will not be modified
// on failure. // on failure.
bool synthetic_airspeed(float &ret) const WARN_IF_UNUSED { bool synthetic_airspeed_EAS(float &ret) const WARN_IF_UNUSED {
ret = _last_airspeed; ret = _last_airspeed_TAS;
return true; return true;
} }
@ -173,12 +173,12 @@ private:
// DCM matrix from the eulers. Called internally we may. // DCM matrix from the eulers. Called internally we may.
void reset(bool recover_eulers); 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 // airspeed as filled-in by an enabled airspeed sensor
// if no airspeed sensor: airspeed estimated using the GPS speed & wind_speed_estimation // 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 // Or if none of the above, fills-in using the previous airspeed estimate
// Return false: if we are 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 // primary representation of attitude of board used for all inertial calculations
Matrix3f _dcm_matrix; Matrix3f _dcm_matrix;
@ -262,7 +262,7 @@ private:
Vector3f _last_fuse; Vector3f _last_fuse;
Vector3f _last_vel; Vector3f _last_vel;
uint32_t _last_wind_time; uint32_t _last_wind_time;
float _last_airspeed; float _last_airspeed_TAS;
uint32_t _last_consistent_heading; uint32_t _last_consistent_heading;
// estimated wind in m/s // estimated wind in m/s

6
libraries/AP_AHRS/AP_AHRS_SIM.cpp
View File

@ -41,7 +41,7 @@ bool AP_AHRS_SIM::wind_estimate(Vector3f &wind) const
return true; 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) { if (_sitl == nullptr) {
return false; return false;
@ -52,9 +52,9 @@ bool AP_AHRS_SIM::airspeed_estimate(float &airspeed_ret) const
return true; 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 bool AP_AHRS_SIM::get_quaternion(Quaternion &quat) const

4
libraries/AP_AHRS/AP_AHRS_SIM.h
View File

@ -68,11 +68,11 @@ public:
// return an airspeed estimate if available. return true // return an airspeed estimate if available. return true
// if we have an estimate // 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 // return an airspeed estimate if available. return true
// if we have an estimate from a specific sensor index // 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 // return a ground vector estimate in meters/second, in North/East order
Vector2f groundspeed_vector() override; Vector2f groundspeed_vector() override;