AR_Motors: added MOT_REV_DELAY

adds an optional delay when reversing motors for ESCs that need it

Author: tridge

libraries/AR_Motors/AP_MotorsUGV.cpp

@@ -118,6 +118,13 @@ const AP_Param::GroupInfo AP_MotorsUGV::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("THST_ASYM", 14, AP_MotorsUGV, _thrust_asymmetry, 1.0f),
 
+    // @Param: REV_DELAY
+    // @DisplayName: Motor reversal delay
+    // @Description: For reversible motors that need a delay before they can reverse. When greater than zero the throttle will go to zero for this amount of time before outputting the new throttle.
+    // @Units: s
+    // @User: Standard
+    AP_GROUPINFO("REV_DELAY", 15, AP_MotorsUGV, _reverse_delay, 0),
+    
     AP_GROUPEND
 };
 
@@ -1028,6 +1035,23 @@ void AP_MotorsUGV::output_throttle(SRV_Channel::Function function, float throttl
     }
 #endif  // AP_RELAY_ENABLED
 
+    if (_reverse_delay > 0) {
+        switch (function) {
+        case SRV_Channel::k_throttle:
+            rev_delay_throttle.output(function, throttle, _reverse_delay);
+            return;
+        case SRV_Channel::k_throttleLeft:
+            rev_delay_throttleLeft.output(function, throttle * 10, _reverse_delay);
+            return;
+        case SRV_Channel::k_throttleRight:
+            rev_delay_throttleRight.output(function, throttle * 10, _reverse_delay);
+            return;
+        default:
+            // fall through to other non-delayed outputs
+            break;
+        }
+    }
+
     // output to servo channel
     switch (function) {
         case SRV_Channel::k_throttle:
@@ -1172,6 +1196,25 @@ bool AP_MotorsUGV::is_digital_pwm_type() const
     return false;
 }
 
+/*
+  handle delay on reversal for a throttle
+ */
+void AP_MotorsUGV::ReverseThrottle::output(SRV_Channel::Function function, float throttle, float delay)
+{
+    const uint32_t now_ms = AP_HAL::millis();
+    if (is_zero(throttle)) {
+        // pass through, no change, don't update the last throttle
+    } else if (throttle * last_throttle < 0 &&
+        now_ms - last_output_ms < delay*1000) {
+        // sign change, add pause
+        throttle = 0;
+    } else {
+        last_output_ms = now_ms;
+        last_throttle = throttle;
+    }
+    SRV_Channels::set_output_scaled(function, throttle);
+}
+
 namespace AP {
     AP_MotorsUGV *motors_ugv()
     {

libraries/AR_Motors/AP_MotorsUGV.h

@@ -209,6 +209,7 @@ class AP_MotorsUGV {
     AP_Float _vector_angle_max;  // angle between steering's middle position and maximum position when using vectored thrust.  zero to disable vectored thrust
     AP_Float _speed_scale_base;  // speed above which steering is scaled down when using regular steering/throttle vehicles.  zero to disable speed scaling
     AP_Float _steering_throttle_mix; // Steering vs Throttle priorisation.  Higher numbers prioritise steering, lower numbers prioritise throttle.  Only valid for Skid Steering vehicles
+    AP_Float _reverse_delay; // delay in seconds when reversing motor
 
     // internal variables
     float   _steering;  // requested steering as a value from -4500 to +4500
@@ -231,6 +232,17 @@ class AP_MotorsUGV {
     float   _lateral_factor[AP_MOTORS_NUM_MOTORS_MAX];
     uint8_t   _motors_num;
 
+    /*
+      3 reversal handling structures, for k_throttle, k_throttleLeft and k_throttleRight
+     */
+    struct ReverseThrottle {
+        float last_throttle;
+        uint32_t last_output_ms;
+
+        // output with delay for reversal
+        void output(SRV_Channel::Function function, float throttle, float delay);
+    } rev_delay_throttle, rev_delay_throttleLeft, rev_delay_throttleRight;
+
     static AP_MotorsUGV *_singleton;
 };