From 1858a0544fdd2ed42f2c2c7b8bec0a4120e93433 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 20 Oct 2015 15:40:24 +0900 Subject: [PATCH] Copter: guided takeoff checks auto-armed status This resolves an edge case in which the vehicle could takeoff with auto-armed false --- ArduCopter/control_guided.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/ArduCopter/control_guided.cpp b/ArduCopter/control_guided.cpp index b792399d2d..527270a8d6 100644 --- a/ArduCopter/control_guided.cpp +++ b/ArduCopter/control_guided.cpp @@ -248,6 +248,18 @@ void Copter::guided_run() // called by guided_run at 100hz or more void Copter::guided_takeoff_run() { + // if not auto armed or motors not enabled set throttle to zero and exit immediately + if (!ap.auto_armed || !motors.get_interlock()) { +#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw + // call attitude controller + attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain()); + attitude_control.set_throttle_out(0,false,g.throttle_filt); +#else // multicopters do not stabilize roll/pitch/yaw when disarmed + attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); +#endif + return; + } + // process pilot's yaw input float target_yaw_rate = 0; if (!failsafe.radio) {