aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_AHRS: Handle airspeed estimation with have_inertial_nav() == false

Browse Source
This commit is contained in:
Paul Riseborough 2021-02-28 15:43:36 +11:00 committed by Andrew Tridgell
parent e3eea0a54a
commit 9b53d505ea
1 changed files with 13 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -688,23 +688,21 @@ bool AP_AHRS_NavEKF::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()) {
if (get_velocity_NED(nav_vel)) {
Vector3f true_airspeed_vec = nav_vel - wind_vel;
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));
float tas_lim_upper = MAX(tas_lim_lower, (gnd_speed + _wind_max));
true_airspeed = constrain_float(true_airspeed, tas_lim_lower, tas_lim_upper);
} else {
true_airspeed = MAX(0.0f, true_airspeed);
}
airspeed_ret = true_airspeed / get_EAS2TAS();
if (ret && have_inertial_nav() && get_velocity_NED(nav_vel)) {
Vector3f true_airspeed_vec = nav_vel - wind_vel;
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));
float tas_lim_upper = MAX(tas_lim_lower, (gnd_speed + _wind_max));
true_airspeed = constrain_float(true_airspeed, tas_lim_lower, tas_lim_upper);
} else {
// fallback to DCM if airspeed estimate if EKF has wind but no velocity estimate
ret = AP_AHRS_DCM::airspeed_estimate(get_active_airspeed_index(), airspeed_ret);
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 = AP_AHRS_DCM::airspeed_estimate(get_active_airspeed_index(), airspeed_ret);
}
return ret;