Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: constrain airspeed sensor airspeed using WIND_MAX 

fixes regression from d1d790019c
This commit is contained in:
Peter Barker 2021-08-24 13:38:57 +10:00 committed by Andrew Tridgell
parent b32e0ab8a3
commit 398f731442
1 changed files with 12 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -623,6 +623,18 @@ bool AP_AHRS_NavEKF::airspeed_estimate(float &airspeed_ret) const
bool ret = false;
if (airspeed_sensor_enabled()) {
airspeed_ret = AP::airspeed()->get_airspeed();
if (_wind_max > 0 && AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
// constrain the airspeed by the ground speed
// and AHRS_WIND_MAX
const float gnd_speed = AP::gps().ground_speed();
float true_airspeed = airspeed_ret * get_EAS2TAS();
true_airspeed = constrain_float(true_airspeed,
gnd_speed - _wind_max,
gnd_speed + _wind_max);
airspeed_ret = true_airspeed / get_EAS2TAS();
}
return true;
}