Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: make comment match code behaviour in airspeed DCM-fallback

Browse Source 
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.
This commit is contained in:
Peter Barker 2023-09-18 16:09:01 +10:00 committed by Andrew Tridgell
parent c30bceb2a0
commit 0b4007d22c
1 changed files with 8 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -807,8 +807,6 @@ bool AP_AHRS::airspeed_sensor_enabled(void) const
// 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) const
{ {
bool ret = false;
#if AP_AIRSPEED_ENABLED #if AP_AIRSPEED_ENABLED
if (airspeed_sensor_enabled()) { if (airspeed_sensor_enabled()) {
uint8_t idx = get_active_airspeed_index(); uint8_t idx = get_active_airspeed_index();
@ -837,6 +835,8 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret) const
// get wind estimates // get wind estimates
Vector3f wind_vel; Vector3f wind_vel;
bool have_wind = false;
switch (active_EKF_type()) { switch (active_EKF_type()) {
case EKFType::NONE: case EKFType::NONE:
return dcm.airspeed_estimate(get_active_airspeed_index(), airspeed_ret); 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 #if HAL_NAVEKF3_AVAILABLE
case EKFType::THREE: case EKFType::THREE:
ret = EKF3.getWind(wind_vel); have_wind = EKF3.getWind(wind_vel);
break; break;
#endif #endif
@ -865,10 +865,9 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret) const
// estimate it via nav velocity and wind estimates // estimate it via nav velocity and wind estimates
Vector3f nav_vel; Vector3f nav_vel;
float true_airspeed; if (have_wind && have_inertial_nav() && get_velocity_NED(nav_vel)) {
if (ret && have_inertial_nav() && get_velocity_NED(nav_vel)) {
Vector3f true_airspeed_vec = nav_vel - wind_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(); float gnd_speed = nav_vel.length();
if (_wind_max > 0) { if (_wind_max > 0) {
float tas_lim_lower = MAX(0.0f, (gnd_speed - _wind_max)); 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); true_airspeed = MAX(0.0f, true_airspeed);
} }
airspeed_ret = true_airspeed / get_EAS2TAS(); airspeed_ret = true_airspeed / get_EAS2TAS();
} else { return true;
// 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 ret; // fallback to DCM
return dcm.airspeed_estimate(get_active_airspeed_index(), airspeed_ret);
} }
bool AP_AHRS::_airspeed_estimate_true(float &airspeed_ret) const bool AP_AHRS::_airspeed_estimate_true(float &airspeed_ret) const