From ec3e9014e4bc5b9d7768c98c3161d12dadc6ae9f Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Wed, 13 Apr 2016 09:12:26 -0700 Subject: [PATCH] Plane: move target land_airspeed logic to top layer - this is to allow min groundspeed to be enforced which is otherwise lost when TECS assigns a target airspeed at the lower level --- ArduPlane/navigation.cpp | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 817a32601f..9b7e03cab4 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -98,6 +98,31 @@ void Plane::calc_airspeed_errors() ((int32_t)aparm.airspeed_min * 100); } + // Landing airspeed target + if (control_mode == AUTO && ahrs.airspeed_sensor_enabled()) { + float land_airspeed = SpdHgt_Controller->get_land_airspeed(); + switch (flight_stage) { + case AP_SpdHgtControl::FLIGHT_LAND_APPROACH: + if (land_airspeed >= 0) { + target_airspeed_cm = land_airspeed * 100; + } + break; + + case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE: + case AP_SpdHgtControl::FLIGHT_LAND_FINAL: + if (auto_state.land_pre_flare && aparm.land_pre_flare_airspeed > 0) { + // if we just preflared then continue using the pre-flare airspeed during final flare + target_airspeed_cm = aparm.land_pre_flare_airspeed * 100; + } else if (land_airspeed >= 0) { + target_airspeed_cm = land_airspeed * 100; + } + break; + + default: + break; + } + } + // Set target to current airspeed + ground speed undershoot, // but only when this is faster than the target airspeed commanded // above.