diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index fe94e93fb0..0d58c45578 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/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 bool AP_AHRS_DCM::airspeed_estimate(float &airspeed_ret) const { - bool ret = false; if (airspeed_sensor_enabled()) { airspeed_ret = _airspeed->get_airspeed(); - return true; - } - - if (!_flags.wind_estimation) { - return false; - } - - // estimate it via GPS speed and wind - if (have_gps()) { + } else if (_flags.wind_estimation && have_gps()) { + // estimate it via GPS speed and wind + airspeed_ret = _last_airspeed; + } else { + // give the last estimate, but return false. This is used by + // dead-reckoning code 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 // and AHRS_WIND_MAX 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); airspeed_ret = true_airspeed / get_EAS2TAS(); } - if (!ret) { - // give the last estimate, but return false. This is used by - // dead-reckoning code - airspeed_ret = _last_airspeed; - } - return ret; + + return true; } bool AP_AHRS_DCM::set_home(const Location &loc)