From 13c217c0608b47832dc6028af7408a5dd3cb2e57 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 9 Feb 2016 13:58:05 +1100 Subject: [PATCH] AP_TECS: make demanded airspeed during landing stages clearer --- libraries/AP_TECS/AP_TECS.cpp | 28 +++++++++++++++------------- 1 file changed, 15 insertions(+), 13 deletions(-) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 956a8afff1..13a2b5e12f 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -299,7 +299,6 @@ 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; @@ -309,21 +308,24 @@ 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; } - if (_ahrs.airspeed_sensor_enabled()) { - if ((_flight_stage == FLIGHT_LAND_APPROACH || _flight_stage == FLIGHT_LAND_FINAL) && _landAirspeed >= 0) { - _TAS_dem = _landAirspeed * EAS2TAS; - if (_TASmin > _TAS_dem) { - _TASmin = _TAS_dem; - } - } else if (_flight_stage == FLIGHT_LAND_PREFLARE && aparm.land_pre_flare_airspeed > 0) { - _TAS_dem = aparm.land_pre_flare_airspeed * EAS2TAS; - if (_TASmin > _TAS_dem) { - _TASmin = _TAS_dem; - } - } + if (_TASmin > _TAS_dem) { + _TASmin = _TAS_dem; } // Reset states of time since last update is too large