From 945af08fd00606663bcc7188734382dd86f0e2ac Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sat, 10 Jul 2021 10:55:19 +0930 Subject: [PATCH] Copter: Guided Angle init Z controller on time out --- ArduCopter/mode_guided.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index dd6fa1a6df..122ebb629a 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -833,7 +833,11 @@ void ModeGuided::angle_control_run() pitch_in = 0.0f; climb_rate_cms = 0.0f; yaw_rate_in = 0.0f; - guided_angle_state.use_thrust = false; + if (guided_angle_state.use_thrust) { + // initialise vertical velocity controller + pos_control->init_z_controller(); + guided_angle_state.use_thrust = false; + } } // interpret positive climb rate or thrust as triggering take-off