From 37fc2a6940cd7224f6da7be599e501e7e56c85f4 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 17 Sep 2021 18:52:35 +0900 Subject: [PATCH] Copter: guided takeoff supports autoyaw allows both pilot controlled and externally controlled yaw control --- ArduCopter/takeoff.cpp | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index 526d269b0e..bd92ea673c 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -111,6 +111,9 @@ void Mode::auto_takeoff_run() if (!copter.failsafe.radio && copter.flightmode->use_pilot_yaw()) { // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + if (!is_zero(target_yaw_rate)) { + auto_yaw.set_mode(AUTO_YAW_HOLD); + } } // aircraft stays in landed state until rotor speed runup has finished @@ -153,8 +156,17 @@ void Mode::auto_takeoff_run() // run the vertical position controller and set output throttle copter.pos_control->update_z_controller(); - // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control->input_thrust_vector_rate_heading(thrustvector, target_yaw_rate); + // call attitude controller + if (auto_yaw.mode() == AUTO_YAW_HOLD) { + // roll & pitch from position controller, yaw rate from pilot + attitude_control->input_thrust_vector_rate_heading(thrustvector, target_yaw_rate); + } else if (auto_yaw.mode() == AUTO_YAW_RATE) { + // roll & pitch from position controller, yaw rate from mavlink command or mission item + attitude_control->input_thrust_vector_rate_heading(thrustvector, auto_yaw.rate_cds()); + } else { + // roll & pitch from position controller, yaw heading from GCS or auto_heading() + attitude_control->input_thrust_vector_heading(thrustvector, auto_yaw.yaw(), auto_yaw.rate_cds()); + } } void Mode::auto_takeoff_set_start_alt(void)