Merge pull request #10165 from iNavFlight/mmosca-sbus-output

Allow SBUS servo protocol to output 18 channels, expand mixer to up to 24 channels
iNavFlight · Jun 23, 2024 · 41a6913 · 41a6913
2 parents e8b1520 + 47bc883
commit 41a6913
Show file tree

Hide file tree

Showing 7 changed files with 49 additions and 18 deletions.
diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h
@@ -24,13 +24,12 @@
 
 #if defined(TARGET_MOTOR_COUNT)
 #define MAX_MOTORS  TARGET_MOTOR_COUNT
-#define MAX_SERVOS  16
-
 #else
 #define MAX_MOTORS  12
-#define MAX_SERVOS  16
 #endif
 
+#define MAX_SERVOS  18
+
 #define PWM_TIMER_HZ    1000000
 
 #define PULSE_1MS   (1000)      // 1ms pulse width

diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h
@@ -26,18 +26,28 @@ typedef enum rc_alias {
     PITCH,
     YAW,
     THROTTLE,
-    AUX1,
-    AUX2,
-    AUX3,
-    AUX4,
-    AUX5,
-    AUX6,
-    AUX7,
-    AUX8,
-    AUX9,
-    AUX10,
-    AUX11,
-    AUX12
+    AUX1, // 5
+    AUX2, // 6
+    AUX3, // 7
+    AUX4, // 8
+    AUX5, // 9
+    AUX6, // 10
+    AUX7, // 11
+    AUX8, // 12
+    AUX9, // 13
+    AUX10, // 14
+    AUX11, // 15
+    AUX12, // 16
+    AUX13, // 17
+    AUX14, // 18
+#ifdef USE_24CHANNELS
+    AUX15, // 19
+    AUX16, // 20
+    AUX17, // 21
+    AUX18, // 22
+    AUX19, // 23
+    AUX20, // 24
+#endif
 } rc_alias_e;
 
 typedef enum {

diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c
@@ -347,6 +347,16 @@ void servoMixer(float dT)
     input[INPUT_RC_CH14]     = GET_RX_CHANNEL_INPUT(AUX10);
     input[INPUT_RC_CH15]     = GET_RX_CHANNEL_INPUT(AUX11);
     input[INPUT_RC_CH16]     = GET_RX_CHANNEL_INPUT(AUX12);
+    input[INPUT_RC_CH17]     = GET_RX_CHANNEL_INPUT(AUX13);
+    input[INPUT_RC_CH18]     = GET_RX_CHANNEL_INPUT(AUX14);
+#ifdef USE_24CHANNELS
+    input[INPUT_RC_CH19]     = GET_RX_CHANNEL_INPUT(AUX15);
+    input[INPUT_RC_CH20]     = GET_RX_CHANNEL_INPUT(AUX16);
+    input[INPUT_RC_CH21]     = GET_RX_CHANNEL_INPUT(AUX17);
+    input[INPUT_RC_CH22]     = GET_RX_CHANNEL_INPUT(AUX18);
+    input[INPUT_RC_CH23]     = GET_RX_CHANNEL_INPUT(AUX19);
+    input[INPUT_RC_CH24]     = GET_RX_CHANNEL_INPUT(AUX20);
+#endif
 #undef GET_RX_CHANNEL_INPUT
 
 #ifdef USE_HEADTRACKER

diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h
@@ -20,7 +20,7 @@
 #include "config/parameter_group.h"
 #include "programming/logic_condition.h"
 
-#define MAX_SUPPORTED_SERVOS 16
+#define MAX_SUPPORTED_SERVOS 18
 
 // These must be consecutive
 typedef enum {
@@ -66,6 +66,16 @@ typedef enum {
     INPUT_HEADTRACKER_PAN           = 39,
     INPUT_HEADTRACKER_TILT          = 40,
     INPUT_HEADTRACKER_ROLL          = 41,
+    INPUT_RC_CH17                   = 42,
+    INPUT_RC_CH18                   = 43,
+#ifdef USE_24CHANNELS
+    INPUT_RC_CH19                   = 44,
+    INPUT_RC_CH20                   = 45,
+    INPUT_RC_CH21                   = 46,
+    INPUT_RC_CH22                   = 47,
+    INPUT_RC_CH23                   = 48,
+    INPUT_RC_CH24                   = 49,
+#endif
     INPUT_SOURCE_COUNT
 } inputSource_e;
 

diff --git a/src/main/io/servo_sbus.c b/src/main/io/servo_sbus.c
@@ -109,6 +109,8 @@ void sbusServoUpdate(uint8_t index, uint16_t value)
         case 13: sbusFrame.channels.chan13 = sbusEncodeChannelValue(value); break;
         case 14: sbusFrame.channels.chan14 = sbusEncodeChannelValue(value); break;
         case 15: sbusFrame.channels.chan15 = sbusEncodeChannelValue(value); break;
+        case 16: sbusFrame.channels.flags = value > PWM_RANGE_MIDDLE ? (sbusFrame.channels.flags | SBUS_FLAG_CHANNEL_17) : (sbusFrame.channels.flags & ~SBUS_FLAG_CHANNEL_17) ; break;
+        case 17: sbusFrame.channels.flags = value > PWM_RANGE_MIDDLE ? (sbusFrame.channels.flags | SBUS_FLAG_CHANNEL_18) : (sbusFrame.channels.flags & ~SBUS_FLAG_CHANNEL_18) ; break;
         default:
             break;
     }

diff --git a/src/main/rx/sbus_channels.c b/src/main/rx/sbus_channels.c
@@ -28,8 +28,6 @@
 
 #include "rx/sbus_channels.h"
 
-#define SBUS_FLAG_CHANNEL_17        (1 << 0)
-#define SBUS_FLAG_CHANNEL_18        (1 << 1)
 
 #define SBUS_DIGITAL_CHANNEL_MIN 173
 #define SBUS_DIGITAL_CHANNEL_MAX 1812

diff --git a/src/main/rx/sbus_channels.h b/src/main/rx/sbus_channels.h
@@ -22,6 +22,8 @@
 
 #define SBUS_MAX_CHANNEL 18
 
+#define SBUS_FLAG_CHANNEL_17        (1 << 0)
+#define SBUS_FLAG_CHANNEL_18        (1 << 1)
 #define SBUS_FLAG_SIGNAL_LOSS       (1 << 2)
 #define SBUS_FLAG_FAILSAFE_ACTIVE   (1 << 3)