AP_Motors: Use safety timer to enable pwm before spool up

libraries/AP_Motors/AP_MotorsMulticopter.cpp

@@ -187,7 +187,7 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = {
 
     // @Param: SLEW_UP_TIME
     // @DisplayName: Output slew time for increasing throttle
-    // @Description: Time in seconds to slew output from zero to full.  For medium size copter such as a Solo, a value of 0.25 is a good starting point.  This is used to limit the rate at which output can change. Range is constrained between 0 and 0.5.
+    // @Description: Time in seconds to slew output from zero to full. This is used to limit the rate at which output can change. Range is constrained between 0 and 0.5.
     // @Range: 0 .5
     // @Units: s
     // @Increment: 0.001
@@ -196,13 +196,22 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = {
 
     // @Param: SLEW_DN_TIME
     // @DisplayName: Output slew time for decreasing throttle
-    // @Description: Time in seconds to slew output from full to zero.  For medium size copter such as a Solo, a value of 0.275 is a good starting point.  This is used to limit the rate at which output can change.  Range is constrained between 0 and 0.5.
+    // @Description: Time in seconds to slew output from full to zero. This is used to limit the rate at which output can change.  Range is constrained between 0 and 0.5.
     // @Range: 0 .5
     // @Units: s
     // @Increment: 0.001
     // @User: Advanced
     AP_GROUPINFO("SLEW_DN_TIME", 41, AP_MotorsMulticopter, _slew_dn_time, AP_MOTORS_SLEW_TIME_DEFAULT),
 
+    // @Param: SAFE_TIME
+    // @DisplayName: Time taken to disable and enable the motor PWM output when disarmed and armed.
+    // @Description: Time taken to disable and enable the motor PWM output when disarmed and armed.
+    // @Range: 0 5
+    // @Units: s
+    // @Increment: 0.001
+    // @User: Advanced
+    AP_GROUPINFO("SAFE_TIME", 42, AP_MotorsMulticopter, _safe_time, AP_MOTORS_SAFE_TIME_DEFAULT),
+
     AP_GROUPEND
 };
 
@@ -386,7 +395,8 @@ int16_t AP_MotorsMulticopter::output_to_pwm(float actuator)
     float pwm_output;
     if (_spool_state == SpoolState::SHUT_DOWN) {
         // in shutdown mode, use PWM 0 or minimum PWM
-        if (_disarm_disable_pwm && _disarm_safety_timer == 0 && !armed()) {
+        if (_disarm_disable_pwm && is_equal(_disarm_safe_timer, 0.0f) && !armed()) {
+//        if (_disarm_disable_pwm && !armed()) {
             pwm_output = 0;
         } else {
             pwm_output = get_pwm_output_min();
@@ -496,9 +506,13 @@ void AP_MotorsMulticopter::update_throttle_hover(float dt)
 void AP_MotorsMulticopter::output_logic()
 {
     if (_flags.armed) {
-        _disarm_safety_timer = 100;
-    } else if (_disarm_safety_timer != 0) {
-        _disarm_safety_timer--;
+        if (_disarm_disable_pwm && (_disarm_safe_timer < _safe_time)) {
+            _disarm_safe_timer += 1.0f/_loop_rate;
+        } else {
+            _disarm_safe_timer = _safe_time;
+        }
+    } else if (_disarm_safe_timer > 0.0f) {
+        _disarm_safe_timer -= 1.0f/_loop_rate;;
     }
 
     // force desired and current spool mode if disarmed or not interlocked
@@ -525,7 +539,7 @@ void AP_MotorsMulticopter::output_logic()
         limit.throttle_upper = true;
 
         // make sure the motors are spooling in the correct direction
-        if (_spool_desired != DesiredSpoolState::SHUT_DOWN) {
+        if (_spool_desired != DesiredSpoolState::SHUT_DOWN && _disarm_safe_timer >= _safe_time.get()) {
             _spool_state = SpoolState::GROUND_IDLE;
             break;
         }
@@ -561,6 +575,7 @@ void AP_MotorsMulticopter::output_logic()
                 _spool_state = SpoolState::SHUT_DOWN;
             }
             break;
+
         case DesiredSpoolState::THROTTLE_UNLIMITED:
             _spin_up_ratio += spool_step;
             // constrain ramp value and update mode
@@ -569,6 +584,7 @@ void AP_MotorsMulticopter::output_logic()
                 _spool_state = SpoolState::SPOOLING_UP;
             }
             break;
+
         case DesiredSpoolState::GROUND_IDLE:
             float spin_up_armed_ratio = 0.0f;
             if (_spin_min > 0.0f) {

libraries/AP_Motors/AP_MotorsMulticopter.h

@@ -23,6 +23,7 @@
 #define AP_MOTORS_BAT_CURR_TC_DEFAULT   5.0f    // Time constant used to limit the maximum current
 #define AP_MOTORS_BATT_VOLT_FILT_HZ     0.5f    // battery voltage filtered at 0.5hz
 #define AP_MOTORS_SLEW_TIME_DEFAULT     0.0f    // slew rate limit for thrust output
+#define AP_MOTORS_SAFE_TIME_DEFAULT     1.0f    // Time for the esc when transitioning between zero pwm to minimum
 
 // spool definition
 #define AP_MOTORS_SPOOL_UP_TIME_DEFAULT 0.5f    // time (in seconds) for throttle to increase from zero to min throttle, and min throttle to full throttle.
@@ -148,6 +149,7 @@ protected:
     AP_Float            _thrust_curve_expo;     // curve used to linearize pwm to thrust conversion.  set to 0 for linear and 1 for second order approximation
     AP_Float            _slew_up_time;          // throttle increase slew limitting
     AP_Float            _slew_dn_time;          // throttle decrease slew limitting
+    AP_Float            _safe_time;             // Time for the esc when transitioning between zero pwm to minimum
     AP_Float            _spin_min;              // throttle out ratio which produces the minimum thrust.  (i.e. 0 ~ 1 ) of the full throttle range
     AP_Float            _spin_max;              // throttle out ratio which produces the maximum thrust.  (i.e. 0 ~ 1 ) of the full throttle range
     AP_Float            _spin_arm;              // throttle out ratio which produces the armed spin rate.  (i.e. 0 ~ 1 ) of the full throttle range
@@ -185,7 +187,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;
+    float               _disarm_safe_timer;     // Timer for the esc when transitioning between zero pwm to minimum
 
     // vehicle supplied callback for thrust compensation. Used for tiltrotors and tiltwings
     thrust_compensation_fn_t _thrust_compensation_callback;