From 0af878703fe4a99fff10e834012c063308fe0d60 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Wed, 13 Apr 2016 09:10:29 -0700 Subject: [PATCH] AP_TECS: move target land_airspeed logic to top layer --- libraries/AP_TECS/AP_TECS.cpp | 14 +------------- 1 file changed, 1 insertion(+), 13 deletions(-) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index d5c281ad40..861a89cd93 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -339,6 +339,7 @@ void AP_TECS::_update_speed(float load_factor) // Convert equivalent airspeeds to true airspeeds float EAS2TAS = _ahrs.get_EAS2TAS(); + _TAS_dem = _EAS_dem * EAS2TAS; _TASmax = aparm.airspeed_max * EAS2TAS; _TASmin = aparm.airspeed_min * EAS2TAS; @@ -348,19 +349,6 @@ void AP_TECS::_update_speed(float load_factor) _TASmin *= load_factor; } - float demanded_airspeed = _EAS_dem; - if (_ahrs.airspeed_sensor_enabled()) { - if ((_flight_stage == FLIGHT_LAND_APPROACH || _flight_stage == FLIGHT_LAND_FINAL) && _landAirspeed >= 0) { - demanded_airspeed = _landAirspeed; - } else if (_flight_stage == FLIGHT_LAND_PREFLARE) { - if (aparm.land_pre_flare_airspeed > 0) { - demanded_airspeed = aparm.land_pre_flare_airspeed; - } else if (_landAirspeed >= 0) { - demanded_airspeed = _landAirspeed; - } - } - } - _TAS_dem = demanded_airspeed * EAS2TAS; if (_TASmax < _TASmin) { _TASmax = _TASmin; }