aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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.wind_estimate_ok = _wind_estimate(state.wind_estimate);
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_vec_ok = _airspeed_vector_true(state.airspeed_vec);
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
// 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 (airspeed_sensor_enabled()) {
@ -846,12 +846,13 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret) const
gnd_speed + _wind_max);
airspeed_ret = true_airspeed / get_EAS2TAS();
}
airspeed_estimate_type = AirspeedEstimateType::AIRSPEED_SENSOR;
return true;
}
#endif
if (!get_wind_estimation_enabled()) {
airspeed_estimate_type = AirspeedEstimateType::NO_NEW_ESTIMATE;
return false;
}
@ -864,17 +865,20 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret) const
switch (active_EKF_type()) {
#if AP_AHRS_DCM_ENABLED
case EKFType::DCM:
airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC;
return dcm.airspeed_estimate(get_active_airspeed_index(), airspeed_ret);
#endif
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
airspeed_estimate_type = AirspeedEstimateType::SIM;
return sim.airspeed_estimate(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(get_active_airspeed_index(), airspeed_ret);
#else
return false;
@ -889,7 +893,8 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret) const
#if HAL_EXTERNAL_AHRS_ENABLED
case EKFType::EXTERNAL:
return false;
airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC;
return dcm.airspeed_estimate(get_active_airspeed_index(), airspeed_ret);
#endif
}
@ -907,11 +912,13 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret) const
true_airspeed = MAX(0.0f, true_airspeed);
}
airspeed_ret = true_airspeed / get_EAS2TAS();
airspeed_estimate_type = AirspeedEstimateType::EKF3_SYNTHETIC;
return true;
}
#if AP_AHRS_DCM_ENABLED
// fallback to DCM
airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC;
return dcm.airspeed_estimate(get_active_airspeed_index(), airspeed_ret);
#endif
@ -3389,6 +3396,15 @@ bool AP_AHRS::airspeed_estimate(float &airspeed_ret) const
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
// available. return true if we have an estimate
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
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
// available. return true if we have an estimate
bool airspeed_estimate_true(float &airspeed_ret) const;
@ -833,7 +845,7 @@ private:
// return an airspeed estimate if available. return true
// 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
bool _get_secondary_attitude(Vector3f &eulers) const;
@ -926,6 +938,7 @@ private:
float EAS2TAS;
bool airspeed_ok;
float airspeed;
AirspeedEstimateType airspeed_estimate_type;
bool airspeed_true_ok;
float airspeed_true;
Vector3f airspeed_vec;