SRV_Channel: support reversible DShot motors

force the PWM MIN/MAX to 1000/2000

libraries/SRV_Channel/SRV_Channel.h

@@ -446,6 +446,16 @@ public:
     // disable output to a set of channels given by a mask. This is used by the AP_BLHeli code
     static void set_disabled_channel_mask(uint16_t mask) { disabled_mask = mask; }
 
+    // add to mask of outputs which can do reverse thrust using digital controls
+    static void set_reversible_mask(uint16_t mask) {
+        reversible_mask |= mask;
+    }
+
+    // add to mask of outputs which use digital (non-PWM) output, such as DShot
+    static void set_digital_mask(uint16_t mask) {
+        digital_mask |= mask;
+    }
+    
 private:
     struct {
         bool k_throttle_reversible:1;
@@ -476,7 +486,14 @@ private:
     static AP_BLHeli *blheli_ptr;
 #endif
     static uint16_t disabled_mask;
+
+    // mask of outputs which use a digital output protocol, not
+    // PWM (eg. DShot)
+    static uint16_t digital_mask;
     
+    // mask of outputs which are digitally reversible (eg. DShot-3D)
+    static uint16_t reversible_mask;
+
     SRV_Channel obj_channels[NUM_SERVO_CHANNELS];
 
     static struct srv_function {

libraries/SRV_Channel/SRV_Channel_aux.cpp

@@ -161,9 +161,30 @@ void SRV_Channels::enable_aux_servos()
     // includes k_none servos, which allows those to get their initial
     // trim value on startup
     for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
+        SRV_Channel &c = channels[i];
         // see if it is a valid function
-        if ((uint8_t)channels[i].function.get() < SRV_Channel::k_nr_aux_servo_functions) {
-            hal.rcout->enable_ch(channels[i].ch_num);
+        if ((uint8_t)c.function.get() < SRV_Channel::k_nr_aux_servo_functions) {
+            hal.rcout->enable_ch(c.ch_num);
+        }
+
+        /*
+          for channels which have been marked as digital output then the
+          MIN/MAX/TRIM values have no meaning for controlling output, as
+          the HAL handles the scaling. We still need to cope with places
+          in the code that may try to set a PWM value however, so to
+          ensure consistency we force the MIN/MAX/TRIM to be consistent
+          across all digital channels. We use a MIN/MAX of 1000/2000, and
+          set TRIM to either 1000 or 1500 depending on whether the channel
+          is reversible
+        */
+        if (digital_mask & (1U<<i)) {
+            c.servo_min.set(1000);
+            c.servo_max.set(2000);
+            if (reversible_mask & (1U<<i)) {
+                c.servo_trim.set(1500);
+            } else {
+                c.servo_trim.set(1000);
+            }
         }
     }
 

libraries/SRV_Channel/SRV_Channels.cpp

@@ -39,6 +39,8 @@ AP_BLHeli *SRV_Channels::blheli_ptr;
 #endif
 
 uint16_t SRV_Channels::disabled_mask;
+uint16_t SRV_Channels::digital_mask;
+uint16_t SRV_Channels::reversible_mask;
 
 bool SRV_Channels::disabled_passthrough;
 bool SRV_Channels::initialised;