From b8fc52495474b5a7851c00b4f1603f68f3aa0901 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Tue, 23 Feb 2016 14:42:59 -0800 Subject: [PATCH] Plane: enforce expected flight_stage in TECS --- ArduPlane/ArduPlane.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 030b268560..663381200d 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -884,9 +884,18 @@ void Plane::update_alt() } if (auto_throttle_mode && !throttle_suppressed) { + + // set Flight stage for controller. If not in AUTO then assume normal operation. + // this prevents TECS from being stuck in the wrong stage if you switch from + // AUTO to, say, FBWB during an aborted landing + AP_SpdHgtControl::FlightStage fs = flight_stage; + if (control_mode != AUTO) { + fs = AP_SpdHgtControl::FLIGHT_NORMAL; + } + SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(), target_airspeed_cm, - flight_stage, + fs, is_doing_auto_land, distance_beyond_land_wp, auto_state.takeoff_pitch_cd,