mirror of https://github.com/ArduPilot/ardupilot
AP_AHRS: Fall back to DCM airspeed estimate if EKF velocity not available
This commit is contained in:
parent
a50be71b41
commit
1a3258a039
|
@ -688,18 +688,23 @@ bool AP_AHRS_NavEKF::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;
|
float true_airspeed;
|
||||||
if (ret && have_inertial_nav() && get_velocity_NED(nav_vel)) {
|
if (ret && have_inertial_nav()) {
|
||||||
Vector3f true_airspeed_vec = nav_vel - wind_vel;
|
if (get_velocity_NED(nav_vel)) {
|
||||||
true_airspeed = true_airspeed_vec.length();
|
Vector3f true_airspeed_vec = nav_vel - wind_vel;
|
||||||
float gnd_speed = nav_vel.length();
|
true_airspeed = true_airspeed_vec.length();
|
||||||
if (_wind_max > 0) {
|
float gnd_speed = nav_vel.length();
|
||||||
float tas_lim_lower = MAX(0.0f, (gnd_speed - _wind_max));
|
if (_wind_max > 0) {
|
||||||
float tas_lim_upper = MAX(tas_lim_lower, (gnd_speed + _wind_max));
|
float tas_lim_lower = MAX(0.0f, (gnd_speed - _wind_max));
|
||||||
true_airspeed = constrain_float(true_airspeed, tas_lim_lower, tas_lim_upper);
|
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();
|
||||||
} else {
|
} else {
|
||||||
true_airspeed = MAX(0.0f, true_airspeed);
|
// 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);
|
||||||
}
|
}
|
||||||
airspeed_ret = true_airspeed / get_EAS2TAS();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
return ret;
|
return ret;
|
||||||
|
|
Loading…
Reference in New Issue