|
|
|
@ -1048,23 +1048,19 @@ bool AP_AHRS_DCM::get_position(struct Location &loc) const
@@ -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
@@ -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) |
|
|
|
|