diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 439c03d80a05e..f617c918c6680 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -209,6 +209,13 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) case MAV_CMD_NAV_DELAY: mode_auto.do_nav_delay(cmd); break; + + case MAV_CMD_DO_PARACHUTE: + if (cmd.p1 == PARACHUTE_RELEASE) { + // stop motors to avoid parachute tangling + plane.arming.disarm(AP_Arming::Method::PARACHUTE_RELEASE, false); + } + break; default: // unable to use the command, allow the vehicle to try the next command diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index f49814aca9b90..6bcb43209ccd4 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -140,8 +140,6 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) { #if PARACHUTE == ENABLED parachute_release(); - //stop motors to avoid parachute tangling - plane.arming.disarm(AP_Arming::Method::PARACHUTE_RELEASE, false); #endif } else if (g.fs_action_long == FS_ACTION_LONG_GLIDE) { set_mode(mode_fbwa, reason); @@ -183,8 +181,6 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) { #if PARACHUTE == ENABLED parachute_release(); - //stop motors to avoid parachute tangling - plane.arming.disarm(AP_Arming::Method::PARACHUTE_RELEASE, false); #endif } else if (g.fs_action_long == FS_ACTION_LONG_GLIDE) { set_mode(mode_fbwa, reason); diff --git a/ArduPlane/parachute.cpp b/ArduPlane/parachute.cpp index 61cec7ae0abc4..3c293f9ef2dfe 100644 --- a/ArduPlane/parachute.cpp +++ b/ArduPlane/parachute.cpp @@ -31,6 +31,9 @@ void Plane::parachute_release() // release parachute parachute.release(); + // stop motors to avoid parachute tangling + plane.arming.disarm(AP_Arming::Method::PARACHUTE_RELEASE, false); + #if AP_LANDINGGEAR_ENABLED // deploy landing gear g2.landing_gear.set_position(AP_LandingGear::LandingGear_Deploy); diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 22d6f60c364fa..7bb6491c8948d 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -434,6 +434,9 @@ bool AP_Mission::start_command(const Mission_Command& cmd) return start_command_camera(cmd); #endif case MAV_CMD_DO_PARACHUTE: +#if APM_BUILD_TYPE(APM_BUILD_ArduPlane) + _cmd_start_fn(cmd); +#endif return start_command_parachute(cmd); case MAV_CMD_DO_SEND_SCRIPT_MESSAGE: return start_command_do_scripting(cmd);