From 9b438d7aadcb54ea1ea89753f64794784a36ea57 Mon Sep 17 00:00:00 2001 From: david sastre Date: Sun, 3 May 2020 23:54:30 +0200 Subject: [PATCH] quadplane.cpp: shut down motors when parachute released --- ArduPlane/quadplane.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index f6faed8ef3250..be2dff69e780f 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1856,6 +1856,18 @@ void QuadPlane::update(void) */ void QuadPlane::update_throttle_suppression(void) { +#if PARACHUTE == ENABLED + // if parachute has been released shut down motors + if (plane.parachute.release_initiated()) { + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN); + + // set throttle to 0 for correct last_motors_active_ms behaviour + motors->set_throttle(0); + last_motors_active_ms = 0; + return; + } +#endif + // if the motors have been running in the last 2 seconds then // allow them to run now if (AP_HAL::millis() - last_motors_active_ms < 2000) {