Browse Source

AP_AHRS_DCM: limit measured airspeed according to WIND_MAX

zr-v5.1
Peter Barker 5 years ago committed by Peter Barker
parent
commit
e55057ad5d
  1. 28
      libraries/AP_AHRS/AP_AHRS_DCM.cpp

28
libraries/AP_AHRS/AP_AHRS_DCM.cpp

@ -1048,23 +1048,19 @@ bool AP_AHRS_DCM::get_position(struct Location &loc) const
// return an airspeed estimate if available // return an airspeed estimate if available
bool AP_AHRS_DCM::airspeed_estimate(float &airspeed_ret) const bool AP_AHRS_DCM::airspeed_estimate(float &airspeed_ret) const
{ {
bool ret = false;
if (airspeed_sensor_enabled()) { if (airspeed_sensor_enabled()) {
airspeed_ret = _airspeed->get_airspeed(); airspeed_ret = _airspeed->get_airspeed();
return true; } else if (_flags.wind_estimation && have_gps()) {
} // estimate it via GPS speed and wind
airspeed_ret = _last_airspeed;
if (!_flags.wind_estimation) { } else {
return false; // give the last estimate, but return false. This is used by
} // dead-reckoning code
// estimate it via GPS speed and wind
if (have_gps()) {
airspeed_ret = _last_airspeed; airspeed_ret = _last_airspeed;
ret = true; return false;
} }
if (ret && _wind_max > 0 && AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D) { if (_wind_max > 0 && AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
// constrain the airspeed by the ground speed // constrain the airspeed by the ground speed
// and AHRS_WIND_MAX // and AHRS_WIND_MAX
const float gnd_speed = AP::gps().ground_speed(); const float gnd_speed = AP::gps().ground_speed();
@ -1074,12 +1070,8 @@ bool AP_AHRS_DCM::airspeed_estimate(float &airspeed_ret) const
gnd_speed + _wind_max); gnd_speed + _wind_max);
airspeed_ret = true_airspeed / get_EAS2TAS(); airspeed_ret = true_airspeed / get_EAS2TAS();
} }
if (!ret) {
// give the last estimate, but return false. This is used by return true;
// dead-reckoning code
airspeed_ret = _last_airspeed;
}
return ret;
} }
bool AP_AHRS_DCM::set_home(const Location &loc) bool AP_AHRS_DCM::set_home(const Location &loc)

Loading…
Cancel
Save