From 541cdbef3ecc3cb234bed2f39caa62278baef127 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 18 Sep 2021 10:33:58 +0900 Subject: [PATCH] Copter: guided mode remains in takeoff submode longer --- ArduCopter/mode.h | 2 +- ArduCopter/mode_guided.cpp | 9 +++++---- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 9509bb32cf..8b3bffe7bc 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -940,7 +940,7 @@ private: // controls which controller is run (pos or vel): SubMode guided_mode = SubMode::TakeOff; bool send_notification; // used to send one time notification to ground station - + bool takeoff_complete; // true once takeoff has completed (used to trigger retracting of landing gear) }; diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 5e514e24c9..801c347a0f 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -138,6 +138,9 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm) // get initial alt for WP_NAVALT_MIN auto_takeoff_set_start_alt(); + // record takeoff has not completed + takeoff_complete = false; + return true; } @@ -629,14 +632,12 @@ void ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms_or_thrust, void ModeGuided::takeoff_run() { auto_takeoff_run(); - if (wp_nav->reached_wp_destination()) { + if (!takeoff_complete && wp_nav->reached_wp_destination()) { + takeoff_complete = true; #if LANDING_GEAR_ENABLED == ENABLED // optionally retract landing gear copter.landinggear.retract_after_takeoff(); #endif - - // change to velocity control after take off. - init(true); } }