From 5adf1d9596bf7a3fd0715f68f33db4c328a3b9fa Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 14 Oct 2020 14:29:33 +0900 Subject: [PATCH] Copter: guided mode angle controller sets auto-armed from run method --- ArduCopter/mode_guided.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index ff4f1086ec..95563d0390 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -339,11 +339,6 @@ void ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms_or_thrust, guided_angle_state.update_time_ms = millis(); - // interpret positive climb rate or thrust as triggering take-off - if (motors->armed() && !copter.ap.auto_armed && is_positive(climb_rate_cms_or_thrust)) { - copter.set_auto_armed(true); - } - // log target copter.Log_Write_GuidedTarget(guided_mode, Vector3f(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd), @@ -618,6 +613,11 @@ void ModeGuided::angle_control_run() guided_angle_state.use_thrust = false; } + // interpret positive climb rate or thrust as triggering take-off + if (motors->armed() && is_positive(guided_angle_state.use_thrust ? guided_angle_state.thrust : climb_rate_cms)) { + copter.set_auto_armed(true); + } + // if not armed set throttle to zero and exit immediately if (!motors->armed() || !copter.ap.auto_armed || (copter.ap.land_complete && (guided_angle_state.climb_rate_cms <= 0.0f))) { make_safe_spool_down();