From 077401df69bc3659787362e4051d2c184879a3fe Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Tue, 12 Jan 2021 02:02:50 +0000
Subject: [PATCH] SRV_Channel: add upto 32 servo outs

---
 libraries/SRV_Channel/SRV_Channel.h       |  33 ++++---
 libraries/SRV_Channel/SRV_Channel_aux.cpp |  10 +-
 libraries/SRV_Channel/SRV_Channels.cpp    | 106 +++++++++++++++++++++-
 3 files changed, 125 insertions(+), 24 deletions(-)

diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h
index d7dc8d6147..5af85e6b44 100644
--- a/libraries/SRV_Channel/SRV_Channel.h
+++ b/libraries/SRV_Channel/SRV_Channel.h
@@ -29,9 +29,14 @@
 #elif defined(HAL_BUILD_AP_PERIPH)
     #define NUM_SERVO_CHANNELS 0
 #else
-    #define NUM_SERVO_CHANNELS 16
+    #if !HAL_MINIMIZE_FEATURES
+        #define NUM_SERVO_CHANNELS 32
+    #else
+        #define NUM_SERVO_CHANNELS 16
+    #endif
 #endif
 #endif
+static_assert(NUM_SERVO_CHANNELS <= 32, "More than 32 servos not supported");
 
 class SRV_Channels;
 
@@ -319,7 +324,7 @@ private:
     float get_output_norm(void);
 
     // a bitmask type wide enough for NUM_SERVO_CHANNELS
-    typedef uint16_t servo_mask_t;
+    typedef uint32_t servo_mask_t;
 
     // mask of channels where we have a output_pwm value. Cleared when a
     // scaled value is written. 
@@ -383,8 +388,8 @@ public:
     static void set_output_norm(SRV_Channel::Aux_servo_function_t function, float value);
 
     // get output channel mask for a function
-    static uint16_t get_output_channel_mask(SRV_Channel::Aux_servo_function_t function);
-    
+    static uint32_t get_output_channel_mask(SRV_Channel::Aux_servo_function_t function);
+
     // limit slew rate to given limit in percent per second
     static void set_slew_rate(SRV_Channel::Aux_servo_function_t function, float slew_rate, uint16_t range, float dt);
 
@@ -434,8 +439,8 @@ public:
     static void copy_radio_in_out(SRV_Channel::Aux_servo_function_t function, bool do_input_output=false);
 
     // copy radio_in to servo_out by channel mask
-    static void copy_radio_in_out_mask(uint16_t mask);
-    
+    static void copy_radio_in_out_mask(uint32_t mask);
+
     // setup failsafe for an auxiliary channel function, by pwm
     static void set_failsafe_pwm(SRV_Channel::Aux_servo_function_t function, uint16_t pwm);
 
@@ -456,7 +461,7 @@ public:
     static void enable_aux_servos(void);
 
     // enable channels by mask
-    static void enable_by_mask(uint16_t mask);
+    static void enable_by_mask(uint32_t mask);
 
     // return the current function for a channel
     static SRV_Channel::Aux_servo_function_t channel_function(uint8_t channel);
@@ -520,14 +525,14 @@ public:
     static void push();
 
     // disable PWM 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; }
-    static uint16_t get_disabled_channel_mask() { return disabled_mask; }
+    static void set_disabled_channel_mask(uint32_t mask) { disabled_mask = mask; }
+    static uint32_t get_disabled_channel_mask() { return disabled_mask; }
 
     // add to mask of outputs which use digital (non-PWM) output and optionally can reverse thrust, such as DShot
-    static void set_digital_outputs(uint16_t dig_mask, uint16_t rev_mask);
+    static void set_digital_outputs(uint32_t dig_mask, uint32_t rev_mask);
 
     // return true if all of the outputs in mask are digital
-    static bool have_digital_outputs(uint16_t mask) { return mask != 0 && (mask & digital_mask) == mask; }
+    static bool have_digital_outputs(uint32_t mask) { return mask != 0 && (mask & digital_mask) == mask; }
 
     // return true if any of the outputs are digital
     static bool have_digital_outputs() { return digital_mask != 0; }
@@ -606,14 +611,14 @@ private:
 #endif  // AP_FETTEC_ONEWIRE_ENABLED
 
     // mask of disabled channels
-    static uint16_t disabled_mask;
+    static uint32_t disabled_mask;
 
     // mask of outputs which use a digital output protocol, not
     // PWM (eg. DShot)
-    static uint16_t digital_mask;
+    static uint32_t digital_mask;
     
     // mask of outputs which are digitally reversible (eg. DShot-3D)
-    static uint16_t reversible_mask;
+    static uint32_t reversible_mask;
 
     SRV_Channel obj_channels[NUM_SERVO_CHANNELS];
 
diff --git a/libraries/SRV_Channel/SRV_Channel_aux.cpp b/libraries/SRV_Channel/SRV_Channel_aux.cpp
index 82e19ec1fb..ed1cd81479 100644
--- a/libraries/SRV_Channel/SRV_Channel_aux.cpp
+++ b/libraries/SRV_Channel/SRV_Channel_aux.cpp
@@ -244,7 +244,7 @@ void SRV_Channels::enable_aux_servos()
     set TRIM to either 1000 or 1500 depending on whether the channel
     is reversible
 */
-void SRV_Channels::set_digital_outputs(uint16_t dig_mask, uint16_t rev_mask) {
+void SRV_Channels::set_digital_outputs(uint32_t dig_mask, uint32_t rev_mask) {
     digital_mask |= dig_mask;
     reversible_mask |= rev_mask;
 
@@ -284,7 +284,7 @@ void SRV_Channels::set_digital_outputs(uint16_t dig_mask, uint16_t rev_mask) {
 }
 
 /// enable output channels using a channel mask
-void SRV_Channels::enable_by_mask(uint16_t mask)
+void SRV_Channels::enable_by_mask(uint32_t mask)
 {
     for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
         if (mask & (1U<<i)) {
@@ -378,7 +378,7 @@ SRV_Channels::copy_radio_in_out(SRV_Channel::Aux_servo_function_t function, bool
   copy radio_in to radio_out for a channel mask
  */
 void
-SRV_Channels::copy_radio_in_out_mask(uint16_t mask)
+SRV_Channels::copy_radio_in_out_mask(uint32_t mask)
 {
     for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
         if ((1U<<i) & mask) {
@@ -587,7 +587,7 @@ float SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::Aux_servo_functi
 /*
   get mask of output channels for a function
  */
-uint16_t SRV_Channels::get_output_channel_mask(SRV_Channel::Aux_servo_function_t function)
+uint32_t SRV_Channels::get_output_channel_mask(SRV_Channel::Aux_servo_function_t function)
 {
     if (!initialised) {
         update_aux_servo_function();
@@ -824,7 +824,7 @@ void SRV_Channels::upgrade_parameters(void)
 // set RC output frequency on a function output
 void SRV_Channels::set_rc_frequency(SRV_Channel::Aux_servo_function_t function, uint16_t frequency_hz)
 {
-    uint16_t mask = 0;
+    uint32_t mask = 0;
     for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
         SRV_Channel &c = channels[i];
         if (c.function == function) {
diff --git a/libraries/SRV_Channel/SRV_Channels.cpp b/libraries/SRV_Channel/SRV_Channels.cpp
index 5c8dbb5a50..a26aa0bb86 100644
--- a/libraries/SRV_Channel/SRV_Channels.cpp
+++ b/libraries/SRV_Channel/SRV_Channels.cpp
@@ -65,9 +65,9 @@ uint16_t SRV_Channels::override_counter[NUM_SERVO_CHANNELS];
 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;
+uint32_t SRV_Channels::disabled_mask;
+uint32_t SRV_Channels::digital_mask;
+uint32_t SRV_Channels::reversible_mask;
 
 bool SRV_Channels::disabled_passthrough;
 bool SRV_Channels::initialised;
@@ -236,12 +236,108 @@ const AP_Param::GroupInfo SRV_Channels::var_info[] = {
 
     // @Param: _GPIO_MASK
     // @DisplayName: Servo GPIO mask
-    // @Description: This sets a bitmask of outputs which will be available as GPIOs. Any auxiliary output with either the function set to -1 or with the corresponding bit set in this mask will be available for use as a GPIO pin
-    // @Bitmask: 0:Servo 1, 1:Servo 2, 2:Servo 3, 3:Servo 4, 4:Servo 5, 5:Servo 6, 6:Servo 7, 7:Servo 8, 8:Servo 9, 9:Servo 10, 10:Servo 11, 11:Servo 12, 12:Servo 13, 13:Servo 14, 14:Servo 15, 15:Servo 16
+    // @Description: This sets a bitmask of outputs which will be available as GPIOs. Any auxillary output with either the function set to -1 or with the corresponding bit set in this mask will be available for use as a GPIO pin
+    // @Bitmask: 0:Servo 1, 1:Servo 2, 2:Servo 3, 3:Servo 4, 4:Servo 5, 5:Servo 6, 6:Servo 7, 7:Servo 8, 8:Servo 9, 9:Servo 10, 10:Servo 11, 11:Servo 12, 12:Servo 13, 13:Servo 14, 14:Servo 15, 15:Servo 16, 16:Servo 17, 17:Servo 18, 18:Servo 19, 19:Servo 20, 20:Servo 21, 21:Servo 22, 22:Servo 23, 23:Servo 24, 24:Servo 25, 25:Servo 26, 26:Servo 27, 27:Servo 28, 28:Servo 29, 29:Servo 30, 30:Servo 31, 31:Servo 32
     // @User: Advanced
     // @RebootRequired: True
     AP_GROUPINFO("_GPIO_MASK",  26, SRV_Channels, gpio_mask, 0),
     
+#if (NUM_SERVO_CHANNELS >= 17)
+    // @Group: 17_
+    // @Path: SRV_Channel.cpp
+    AP_SUBGROUPINFO(obj_channels[16], "17_",  27, SRV_Channels, SRV_Channel),
+#endif
+
+#if (NUM_SERVO_CHANNELS >= 18)
+    // @Group: 18_
+    // @Path: SRV_Channel.cpp
+    AP_SUBGROUPINFO(obj_channels[17], "18_", 28, SRV_Channels, SRV_Channel),
+#endif
+
+#if (NUM_SERVO_CHANNELS >= 19)
+    // @Group: 19_
+    // @Path: SRV_Channel.cpp
+    AP_SUBGROUPINFO(obj_channels[18], "19_",  29, SRV_Channels, SRV_Channel),
+#endif
+
+#if (NUM_SERVO_CHANNELS >= 20)
+    // @Group: 20_
+    // @Path: SRV_Channel.cpp
+    AP_SUBGROUPINFO(obj_channels[19], "20_",  30, SRV_Channels, SRV_Channel),
+#endif
+
+#if (NUM_SERVO_CHANNELS >= 21)
+    // @Group: 21_
+    // @Path: SRV_Channel.cpp
+    AP_SUBGROUPINFO(obj_channels[20], "21_",  31, SRV_Channels, SRV_Channel),
+#endif
+
+#if (NUM_SERVO_CHANNELS >= 22)
+    // @Group: 22_
+    // @Path: SRV_Channel.cpp
+    AP_SUBGROUPINFO(obj_channels[21], "22_",  32, SRV_Channels, SRV_Channel),
+#endif
+
+#if (NUM_SERVO_CHANNELS >= 23)
+    // @Group: 23_
+    // @Path: SRV_Channel.cpp
+    AP_SUBGROUPINFO(obj_channels[22], "23_",  33, SRV_Channels, SRV_Channel),
+#endif
+
+#if (NUM_SERVO_CHANNELS >= 24)
+    // @Group: 24_
+    // @Path: SRV_Channel.cpp
+    AP_SUBGROUPINFO(obj_channels[23], "24_",  34, SRV_Channels, SRV_Channel),
+#endif
+
+#if (NUM_SERVO_CHANNELS >= 25)
+    // @Group: 25_
+    // @Path: SRV_Channel.cpp
+    AP_SUBGROUPINFO(obj_channels[24], "25_",  35, SRV_Channels, SRV_Channel),
+#endif
+
+#if (NUM_SERVO_CHANNELS >= 26)
+    // @Group: 26_
+    // @Path: SRV_Channel.cpp
+    AP_SUBGROUPINFO(obj_channels[25], "26_",  36, SRV_Channels, SRV_Channel),
+#endif
+
+#if (NUM_SERVO_CHANNELS >= 27)
+    // @Group: 27_
+    // @Path: SRV_Channel.cpp
+    AP_SUBGROUPINFO(obj_channels[26], "27_",  37, SRV_Channels, SRV_Channel),
+#endif
+
+#if (NUM_SERVO_CHANNELS >= 28)
+    // @Group: 28_
+    // @Path: SRV_Channel.cpp
+    AP_SUBGROUPINFO(obj_channels[27], "28_",  38, SRV_Channels, SRV_Channel),
+#endif
+
+#if (NUM_SERVO_CHANNELS >= 29)
+    // @Group: 29_
+    // @Path: SRV_Channel.cpp
+    AP_SUBGROUPINFO(obj_channels[28], "29_",  39, SRV_Channels, SRV_Channel),
+#endif
+
+#if (NUM_SERVO_CHANNELS >= 30)
+    // @Group: 30_
+    // @Path: SRV_Channel.cpp
+    AP_SUBGROUPINFO(obj_channels[29], "30_",  40, SRV_Channels, SRV_Channel),
+#endif
+
+#if (NUM_SERVO_CHANNELS >= 31)
+    // @Group: 31_
+    // @Path: SRV_Channel.cpp
+    AP_SUBGROUPINFO(obj_channels[30], "31_",  41, SRV_Channels, SRV_Channel),
+#endif
+
+#if (NUM_SERVO_CHANNELS >= 32)
+    // @Group: 32_
+    // @Path: SRV_Channel.cpp
+    AP_SUBGROUPINFO(obj_channels[31], "32_",  42, SRV_Channels, SRV_Channel),
+#endif
+
     AP_GROUPEND
 };