From 22517422f934d61247d6af8f702bbc194c7d4018 Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Fri, 18 Nov 2016 17:48:22 -0800
Subject: [PATCH] AP_Motors: add option to disable motor PWM output while
 disarmed

---
 libraries/AP_Motors/AP_MotorsMatrix.cpp      |  9 +++++++--
 libraries/AP_Motors/AP_MotorsMulticopter.cpp | 16 +++++++++++++++-
 libraries/AP_Motors/AP_MotorsMulticopter.h   |  2 ++
 3 files changed, 24 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp
index 816256dd02..a4b9662bf4 100644
--- a/libraries/AP_Motors/AP_MotorsMatrix.cpp
+++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp
@@ -88,15 +88,20 @@ void AP_MotorsMatrix::output_to_motors()
     int16_t motor_out[AP_MOTORS_MAX_NUM_MOTORS];    // final pwm values sent to the motor
 
     switch (_spool_mode) {
-        case SHUT_DOWN:
+        case SHUT_DOWN: {
             // sends minimum values out to the motors
             // set motor output based on thrust requests
             for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
                 if (motor_enabled[i]) {
-                    motor_out[i] = get_pwm_output_min();
+                    if (_disarm_disable_pwm && _disarm_safety_timer == 0 && !armed()) {
+                        motor_out[i] = 0;
+                    } else {
+                        motor_out[i] = get_pwm_output_min();
+                    }
                 }
             }
             break;
+        }
         case SPIN_WHEN_ARMED:
             // sends output to motors when armed but not flying
             for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp
index 3369c47619..4b75b9ecd5 100644
--- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp
+++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp
@@ -137,6 +137,13 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("HOVER_LEARN", 22, AP_MotorsMulticopter, _throttle_hover_learn, HOVER_LEARN_AND_SAVE),
 
+    // @Param: SAFE_DISARM
+    // @DisplayName: Motor PWM output disabled when disarmed
+    // @Description: Disables motor PWM output when disarmed
+    // @Values: 0:PWM enabled while disarmed, 1:PWM disabled while disarmed
+    // @User: Advanced
+    AP_GROUPINFO("SAFE_DISARM", 23, AP_MotorsMulticopter, _disarm_disable_pwm, 0),
+
     AP_GROUPEND
 };
 
@@ -151,7 +158,8 @@ AP_MotorsMulticopter::AP_MotorsMulticopter(uint16_t loop_rate, uint16_t speed_hz
     _batt_timer(0),
     _lift_max(1.0f),
     _throttle_limit(1.0f),
-    _throttle_thrust_max(0.0f)
+    _throttle_thrust_max(0.0f),
+    _disarm_safety_timer(0)
 {
     AP_Param::setup_object_defaults(this, var_info);
 
@@ -385,6 +393,12 @@ void AP_MotorsMulticopter::update_throttle_hover(float dt)
 // run spool logic
 void AP_MotorsMulticopter::output_logic()
 {
+    if (_flags.armed) {
+        _disarm_safety_timer = 100;
+    } else if (_disarm_safety_timer != 0) {
+        _disarm_safety_timer--;
+    }
+
     // force desired and current spool mode if disarmed or not interlocked
     if (!_flags.armed || !_flags.interlock) {
         _spool_desired = DESIRED_SHUT_DOWN;
diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h
index 1a3c2e8d76..c3996ffab7 100644
--- a/libraries/AP_Motors/AP_MotorsMulticopter.h
+++ b/libraries/AP_Motors/AP_MotorsMulticopter.h
@@ -161,6 +161,7 @@ protected:
     AP_Int16            _pwm_max;               // maximum PWM value that will ever be output to the motors (if 0, vehicle's throttle input channel's max pwm used)
     AP_Float            _throttle_hover;        // estimated throttle required to hover throttle in the range 0 ~ 1
     AP_Int8             _throttle_hover_learn;  // enable/disabled hover thrust learning
+    AP_Int8             _disarm_disable_pwm;    // disable PWM output while disarmed
 
     // motor output variables
     bool                motor_enabled[AP_MOTORS_MAX_NUM_MOTORS];    // true if motor is enabled
@@ -180,6 +181,7 @@ protected:
     float               _lift_max;              // maximum lift ratio from battery voltage
     float               _throttle_limit;        // ratio of throttle limit between hover and maximum
     float               _throttle_thrust_max;   // the maximum allowed throttle thrust 0.0 to 1.0 in the range throttle_min to throttle_max
+    uint16_t            _disarm_safety_timer;
 
     // vehicle supplied callback for thrust compensation. Used for tiltrotors and tiltwings
     thrust_compensation_fn_t _thrust_compensation_callback;