From 0b4007d22cf4556cd05862eb524f911430c607a0 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 18 Sep 2023 16:09:01 +1000 Subject: [PATCH] AP_AHRS: make comment match code behaviour in airspeed DCM-fallback the comment here was incorrect; we do not need EKF3 to have a good wind estimate to fall back to DCM here. Also restructure the code to make it more obvious that this is what's going on. --- libraries/AP_AHRS/AP_AHRS.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 5912874bf2..987c980cf6 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -807,8 +807,6 @@ bool AP_AHRS::airspeed_sensor_enabled(void) const // if we have an estimate bool AP_AHRS::_airspeed_estimate(float &airspeed_ret) const { - bool ret = false; - #if AP_AIRSPEED_ENABLED if (airspeed_sensor_enabled()) { uint8_t idx = get_active_airspeed_index(); @@ -837,6 +835,8 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret) const // get wind estimates Vector3f wind_vel; + bool have_wind = false; + switch (active_EKF_type()) { case EKFType::NONE: return dcm.airspeed_estimate(get_active_airspeed_index(), airspeed_ret); @@ -853,7 +853,7 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret) const #if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: - ret = EKF3.getWind(wind_vel); + have_wind = EKF3.getWind(wind_vel); break; #endif @@ -865,10 +865,9 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret) const // estimate it via nav velocity and wind estimates Vector3f nav_vel; - float true_airspeed; - if (ret && have_inertial_nav() && get_velocity_NED(nav_vel)) { + if (have_wind && have_inertial_nav() && get_velocity_NED(nav_vel)) { Vector3f true_airspeed_vec = nav_vel - wind_vel; - true_airspeed = true_airspeed_vec.length(); + float true_airspeed = true_airspeed_vec.length(); float gnd_speed = nav_vel.length(); if (_wind_max > 0) { float tas_lim_lower = MAX(0.0f, (gnd_speed - _wind_max)); @@ -878,12 +877,11 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret) const true_airspeed = MAX(0.0f, true_airspeed); } airspeed_ret = true_airspeed / get_EAS2TAS(); - } else { - // fallback to DCM if airspeed estimate if EKF has wind but no velocity estimate - ret = dcm.airspeed_estimate(get_active_airspeed_index(), airspeed_ret); + return true; } - return ret; + // fallback to DCM + return dcm.airspeed_estimate(get_active_airspeed_index(), airspeed_ret); } bool AP_AHRS::_airspeed_estimate_true(float &airspeed_ret) const