From 58b08530125b06ea754206469bfcbec59de39bbc Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Fri, 9 Jul 2021 14:57:54 +0930 Subject: [PATCH] Copter: Guided: use default yaw mode for all gps based sub modes. --- ArduCopter/mode_guided.cpp | 15 +++------------ 1 file changed, 3 insertions(+), 12 deletions(-) diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 94b611985d..a82de8e3b4 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -153,6 +153,9 @@ void ModeGuided::pva_control_start() // initialise velocity controller pos_control->init_z_controller(); pos_control->init_xy_controller(); + + // initialise yaw + auto_yaw.set_mode_to_default(false); } // initialise guided mode's position controller @@ -163,9 +166,6 @@ void ModeGuided::pos_control_start() // initialise position controller pva_control_start(); - - // initialise yaw - auto_yaw.set_mode_to_default(false); } // initialise guided mode's velocity controller @@ -176,9 +176,6 @@ void ModeGuided::accel_control_start() // initialise position controller pva_control_start(); - - // pilot always controls yaw - auto_yaw.set_mode(AUTO_YAW_HOLD); } // initialise guided mode's velocity and acceleration controller @@ -189,9 +186,6 @@ void ModeGuided::velaccel_control_start() // initialise position controller pva_control_start(); - - // pilot always controls yaw - auto_yaw.set_mode(AUTO_YAW_HOLD); } // initialise guided mode's position, velocity and acceleration controller @@ -202,9 +196,6 @@ void ModeGuided::posvelaccel_control_start() // initialise position controller pva_control_start(); - - // pilot always controls yaw - auto_yaw.set_mode(AUTO_YAW_HOLD); } bool ModeGuided::is_taking_off() const