Merge pull request #10276 from iNavFlight/mmosca-sbus-output-improvements

Allow number of servos exceed pwm outputs, if servo protocol is sbus_pwm

Author: mmosca

src/main/drivers/pwm_mapping.c

@@ -42,6 +42,7 @@
 #include "sensors/rangefinder.h"
 
 #include "io/serial.h"
+#include "io/servo_sbus.h"
 
 enum {
     MAP_TO_NONE,
@@ -442,15 +443,21 @@ static void pwmInitServos(timMotorServoHardware_t * timOutputs)
         return;
     }
 
+
     // If mixer requests more servos than we have timer outputs - throw an error
-    if (servoCount > timOutputs->maxTimServoCount) {
+    uint16_t maxServos = timOutputs->maxTimServoCount;
+    if (servoConfig()->servo_protocol == SERVO_TYPE_SBUS_PWM) {
+        maxServos = MAX(SERVO_SBUS_MAX_SERVOS, timOutputs->maxTimServoCount);
+    }
+
+    if (servoCount > maxServos) {
         pwmInitError = PWM_INIT_ERROR_NOT_ENOUGH_SERVO_OUTPUTS;
         LOG_ERROR(PWM, "Too many servos. Mixer requested %d, timer outputs %d", servoCount, timOutputs->maxTimServoCount);
         return;
     }
 
     // Configure individual servo outputs
-    for (int idx = 0; idx < servoCount; idx++) {
+    for (int idx = 0; idx < MIN(servoCount, timOutputs->maxTimServoCount); idx++) {
         const timerHardware_t *timHw = timOutputs->timServos[idx];
 
         if (!pwmServoConfig(timHw, idx, servoConfig()->servoPwmRate, servoConfig()->servoCenterPulse, feature(FEATURE_PWM_OUTPUT_ENABLE))) {

src/main/drivers/pwm_mapping.h

@@ -21,6 +21,7 @@
 #include "flight/mixer.h"
 #include "flight/mixer_profile.h"
 #include "flight/servos.h"
+#include "common/maths.h"
 
 #if defined(TARGET_MOTOR_COUNT)
 #define MAX_MOTORS  TARGET_MOTOR_COUNT

src/main/drivers/pwm_output.c

@@ -638,7 +638,7 @@ ioTag_t pwmGetMotorPinTag(int motorIndex)
 
 static void pwmServoWriteStandard(uint8_t index, uint16_t value)
 {
-    if (servos[index]) {
+    if (index < MAX_SERVOS && servos[index]) {
         *servos[index]->ccr = value;
     }
 }

src/main/fc/rc_controls.h

@@ -40,13 +40,23 @@ typedef enum rc_alias {
     AUX12, // 16
     AUX13, // 17
     AUX14, // 18
-#ifdef USE_24CHANNELS
+#ifdef USE_34CHANNELS
     AUX15, // 19
     AUX16, // 20
     AUX17, // 21
     AUX18, // 22
     AUX19, // 23
     AUX20, // 24
+    AUX21, // 25
+    AUX22, // 26
+    AUX23, // 27
+    AUX24, // 28
+    AUX25, // 29
+    AUX26, // 30
+    AUX27, // 31
+    AUX28, // 32
+    AUX29, // 33
+    AUX30, // 34
 #endif
 } rc_alias_e;
 

src/main/flight/servos.c

@@ -349,13 +349,23 @@ void servoMixer(float dT)
     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
+#ifdef USE_34CHANNELS
     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);
+    input[INPUT_RC_CH25]     = GET_RX_CHANNEL_INPUT(AUX21);
+    input[INPUT_RC_CH26]     = GET_RX_CHANNEL_INPUT(AUX22);
+    input[INPUT_RC_CH27]     = GET_RX_CHANNEL_INPUT(AUX23);
+    input[INPUT_RC_CH28]     = GET_RX_CHANNEL_INPUT(AUX24);
+    input[INPUT_RC_CH29]     = GET_RX_CHANNEL_INPUT(AUX25);
+    input[INPUT_RC_CH30]     = GET_RX_CHANNEL_INPUT(AUX26);
+    input[INPUT_RC_CH31]     = GET_RX_CHANNEL_INPUT(AUX27);
+    input[INPUT_RC_CH32]     = GET_RX_CHANNEL_INPUT(AUX28);
+    input[INPUT_RC_CH33]     = GET_RX_CHANNEL_INPUT(AUX29);
+    input[INPUT_RC_CH34]     = GET_RX_CHANNEL_INPUT(AUX30);
 #endif
 #undef GET_RX_CHANNEL_INPUT
 

src/main/flight/servos.h

@@ -68,7 +68,6 @@ typedef enum {
     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,
@@ -85,7 +84,6 @@ typedef enum {
     INPUT_RC_CH32                   = 57,
     INPUT_RC_CH33                   = 58,
     INPUT_RC_CH34                   = 59,
-#endif
     INPUT_SOURCE_COUNT
 } inputSource_e;
 

src/main/io/servo_sbus.h

@@ -24,6 +24,8 @@
 
 #pragma once
 
+#define SERVO_SBUS_MAX_SERVOS   18
+
 bool sbusServoInitialize(void);
 void sbusServoUpdate(uint8_t index, uint16_t value);
 void sbusServoSendUpdate(void);

src/main/rx/jetiexbus.c

@@ -64,7 +64,7 @@
 #define JETIEXBUS_OPTIONS (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO)
 #define JETIEXBUS_MIN_FRAME_GAP     1000
 
-#ifdef USE_24CHANNELS
+#ifdef USE_34CHANNELS
 #define JETIEXBUS_CHANNEL_COUNT 24
 #else
 #define JETIEXBUS_CHANNEL_COUNT 16

src/main/target/common.h

@@ -206,7 +206,7 @@
 #define USE_SERIALRX_SUMD
 #define USE_TELEMETRY_HOTT
 #define USE_HOTT_TEXTMODE
-#define USE_24CHANNELS
+#define USE_34CHANNELS
 #define MAX_MIXER_PROFILE_COUNT 2
 #define USE_SMARTPORT_MASTER
 #elif !defined(STM32F7)