Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-08 17:03:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: add airspeed estimate status logging

Browse Source
This commit is contained in:
Henry Wurzburg 2023-10-02 20:43:49 -05:00 committed by Randy Mackay
parent 85234b5b18
commit 5a64cc9e8e
2 changed files with 34 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -330,7 +330,7 @@ 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_ok = _airspeed_estimate(state.airspeed, state.airspeed_estimate_type);
state.airspeed_true_ok = _airspeed_estimate_true(state.airspeed_true); state.airspeed_true_ok = _airspeed_estimate_true(state.airspeed_true);
state.airspeed_vec_ok = _airspeed_vector_true(state.airspeed_vec); state.airspeed_vec_ok = _airspeed_vector_true(state.airspeed_vec);
state.quat_ok = _get_quaternion(state.quat); state.quat_ok = _get_quaternion(state.quat);
@ -829,7 +829,7 @@ bool AP_AHRS::airspeed_sensor_enabled(void) 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) const bool AP_AHRS::_airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &airspeed_estimate_type) const
{ {
#if AP_AIRSPEED_ENABLED && AP_GPS_ENABLED #if AP_AIRSPEED_ENABLED && AP_GPS_ENABLED
if (airspeed_sensor_enabled()) { if (airspeed_sensor_enabled()) {
@ -846,12 +846,13 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret) const
gnd_speed + _wind_max); gnd_speed + _wind_max);
airspeed_ret = true_airspeed / get_EAS2TAS(); airspeed_ret = true_airspeed / get_EAS2TAS();
} }
airspeed_estimate_type = AirspeedEstimateType::AIRSPEED_SENSOR;
return true; return true;
} }
#endif #endif
if (!get_wind_estimation_enabled()) { if (!get_wind_estimation_enabled()) {
airspeed_estimate_type = AirspeedEstimateType::NO_NEW_ESTIMATE;
return false; return false;
} }
@ -864,17 +865,20 @@ bool AP_AHRS::_airspeed_estimate(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:
airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC;
return dcm.airspeed_estimate(get_active_airspeed_index(), airspeed_ret); return dcm.airspeed_estimate(get_active_airspeed_index(), 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;
return sim.airspeed_estimate(airspeed_ret); return sim.airspeed_estimate(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;
return dcm.airspeed_estimate(get_active_airspeed_index(), airspeed_ret); return dcm.airspeed_estimate(get_active_airspeed_index(), airspeed_ret);
#else #else
return false; return false;
@ -889,7 +893,8 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret) const
#if HAL_EXTERNAL_AHRS_ENABLED #if HAL_EXTERNAL_AHRS_ENABLED
case EKFType::EXTERNAL: case EKFType::EXTERNAL:
return false; airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC;
return dcm.airspeed_estimate(get_active_airspeed_index(), airspeed_ret);
#endif #endif
} }
@ -907,11 +912,13 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret) const
true_airspeed = MAX(0.0f, true_airspeed); true_airspeed = MAX(0.0f, true_airspeed);
} }
airspeed_ret = true_airspeed / get_EAS2TAS(); airspeed_ret = true_airspeed / get_EAS2TAS();
airspeed_estimate_type = AirspeedEstimateType::EKF3_SYNTHETIC;
return true; return true;
} }
#if AP_AHRS_DCM_ENABLED #if AP_AHRS_DCM_ENABLED
// fallback to DCM // fallback to DCM
airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC;
return dcm.airspeed_estimate(get_active_airspeed_index(), airspeed_ret); return dcm.airspeed_estimate(get_active_airspeed_index(), airspeed_ret);
#endif #endif
@ -3389,6 +3396,15 @@ bool AP_AHRS::airspeed_estimate(float &airspeed_ret) const
return state.airspeed_ok; return state.airspeed_ok;
} }
// return an airspeed estimate if available. return true
// if we have an estimate
bool AP_AHRS::airspeed_estimate(float &airspeed_ret, AP_AHRS::AirspeedEstimateType &type) const
{
airspeed_ret = state.airspeed;
type = state.airspeed_estimate_type;
return state.airspeed_ok;
}
// 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 AP_AHRS::airspeed_estimate_true(float &airspeed_ret) const bool AP_AHRS::airspeed_estimate_true(float &airspeed_ret) const

15
libraries/AP_AHRS/AP_AHRS.h
View File

@ -139,6 +139,18 @@ public:
// if we have an estimate // if we have an estimate
bool airspeed_estimate(float &airspeed_ret) const; bool airspeed_estimate(float &airspeed_ret) const;
enum AirspeedEstimateType : uint8_t {
NO_NEW_ESTIMATE = 0,
AIRSPEED_SENSOR = 1,
DCM_SYNTHETIC = 2,
EKF3_SYNTHETIC = 3,
SIM = 4,
};
// return an airspeed estimate if available. return true
// if we have an estimate
bool airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &type) const;
// 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_estimate_true(float &airspeed_ret) const;
@ -833,7 +845,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) const; bool _airspeed_estimate(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;
@ -926,6 +938,7 @@ private:
float EAS2TAS; float EAS2TAS;
bool airspeed_ok; bool airspeed_ok;
float airspeed; float airspeed;
AirspeedEstimateType airspeed_estimate_type;
bool airspeed_true_ok; bool airspeed_true_ok;
float airspeed_true; float airspeed_true;
Vector3f airspeed_vec; Vector3f airspeed_vec;