Update mixer to use servo count from results of pwm mapping.

Also ensure that only aux channels for which there are available servos
are used when forwarding channels.

Removed magic number usage.

src/main/drivers/pwm_mapping.c

@@ -17,7 +17,7 @@
 
 #include <stdbool.h>
 #include <stdint.h>
-
+#include <string.h>
 #include <stdlib.h>
 
 #include "platform.h"
@@ -143,14 +143,16 @@ static const uint16_t * const hardwareMaps[] = {
     airPPM,
 };
 
-void pwmInit(drv_pwm_config_t *init)
+pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
 {
     int i = 0;
     const uint16_t *setup;
 
     int channelIndex = 0;
-    int servoIndex = 0;
-    int motorIndex = 0;
+
+    static pwmOutputConfiguration_t pwmOutputConfiguration;
+
+    memset(&pwmOutputConfiguration, 0, sizeof(pwmOutputConfiguration));
 
     // this is pretty hacky shit, but it will do for now. array of 4 config maps, [ multiPWM multiPPM airPWM airPPM ]
     if (init->airplane)
@@ -265,14 +267,16 @@ void pwmInit(drv_pwm_config_t *init)
             channelIndex++;
         } else if (type == TYPE_M) {
             if (init->motorPwmRate > 500) {
-            	pwmBrushedMotorConfig(&timerHardware[timerIndex], motorIndex, init->motorPwmRate, init->idlePulse);
+            	pwmBrushedMotorConfig(&timerHardware[timerIndex], pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse);
             } else {
-            	pwmBrushlessMotorConfig(&timerHardware[timerIndex], motorIndex, init->motorPwmRate, init->idlePulse);
+            	pwmBrushlessMotorConfig(&timerHardware[timerIndex], pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse);
             }
-            motorIndex++;
+            pwmOutputConfiguration.motorCount++;
         } else if (type == TYPE_S) {
-            pwmServoConfig(&timerHardware[timerIndex], servoIndex, init->servoPwmRate, init->servoCenterPulse);
-            servoIndex++;
+            pwmServoConfig(&timerHardware[timerIndex], pwmOutputConfiguration.servoCount, init->servoPwmRate, init->servoCenterPulse);
+            pwmOutputConfiguration.servoCount++;
         }
     }
+
+    return &pwmOutputConfiguration;
 }

src/main/drivers/pwm_mapping.h

@@ -54,6 +54,12 @@ typedef struct drv_pwm_config_t {
     uint16_t servoCenterPulse;
 } drv_pwm_config_t;
 
+
+typedef struct pwmOutputConfiguration_s {
+    uint8_t servoCount;
+    uint8_t motorCount;
+} pwmOutputConfiguration_t;
+
 // This indexes into the read-only hardware definition structure, timerHardware_t, as well as into pwmPorts structure with dynamic data.
 enum {
     PWM1 = 0,

src/main/flight/mixer.c

@@ -27,6 +27,7 @@
 #include "drivers/gpio.h"
 #include "drivers/timer.h"
 #include "drivers/pwm_output.h"
+#include "drivers/pwm_mapping.h"
 
 #include "rx/rx.h"
 #include "io/gimbal.h"
@@ -39,6 +40,7 @@
 #include "config/runtime_config.h"
 #include "config/config.h"
 
+#define AUX_FORWARD_CHANNEL_TO_SERVO_COUNT 4
 
 static uint8_t numberMotor = 0;
 int16_t motor[MAX_SUPPORTED_MOTORS];
@@ -47,6 +49,8 @@ int16_t servo[MAX_SUPPORTED_SERVOS] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500
 
 static int useServo;
 
+static uint8_t servoCount;
+
 static servoParam_t *servoConf;
 static mixerConfig_t *mixerConfig;
 static flight3DConfig_t *flight3DConfig;
@@ -239,12 +243,14 @@ int servoDirection(int nr, int lr)
         return 1;
 }
 
-void mixerInit(MultiType mixerConfiguration, motorMixer_t *customMixers)
+void mixerInit(MultiType mixerConfiguration, motorMixer_t *customMixers, pwmOutputConfiguration_t *pwmOutputConfiguration)
 {
     int i;
 
     currentMixerConfiguration = mixerConfiguration;
 
+    servoCount = pwmOutputConfiguration->servoCount;
+
     // enable servos for mixes that require them. note, this shifts motor counts.
     useServo = mixers[mixerConfiguration].useServo;
     // if we want camstab/trig, that also enables servos, even if mixer doesn't
@@ -536,15 +542,20 @@ void mixTable(void)
 
     // forward AUX1-4 to servo outputs (not constrained)
     if (gimbalConfig->gimbal_flags & GIMBAL_FORWARDAUX) {
-        int offset = 0;
         // offset servos based off number already used in mixer types
         // airplane and servo_tilt together can't be used
-        if (currentMixerConfiguration == MULTITYPE_AIRPLANE || currentMixerConfiguration == MULTITYPE_FLYING_WING)
-            offset = 4;
-        else if (mixers[currentMixerConfiguration].useServo)
-            offset = 2;
-        for (i = 0; i < 4; i++)
-            pwmWriteServo(i + offset, rcData[AUX1 + i]);
+        int8_t firstServo = servoCount - AUX_FORWARD_CHANNEL_TO_SERVO_COUNT;
+
+        // start forwarding from this channel
+        uint8_t channelOffset = AUX1;
+
+        int8_t servoOffset;
+        for (servoOffset = 0; servoOffset < AUX_FORWARD_CHANNEL_TO_SERVO_COUNT; servoOffset++) {
+            if (firstServo + servoOffset < 0) {
+                continue; // there are not enough servos to forward all the AUX channels.
+            }
+            pwmWriteServo(firstServo + servoOffset, rcData[channelOffset++]);
+        }
     }
 
     maxMotor = motor[0];

src/main/flight/mixer.h

@@ -94,7 +94,6 @@ extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
 extern int16_t servo[MAX_SUPPORTED_SERVOS];
 
 bool isMixerUsingServos(void);
-void mixerInit(MultiType mixerConfiguration, motorMixer_t *customMixers);
 void writeAllMotors(int16_t mc);
 void mixerLoadMix(int index, motorMixer_t *customMixers);
 void mixerResetMotors(void);

src/main/main.c

@@ -78,7 +78,8 @@ void timerInit(void);
 void initTelemetry(void);
 void serialInit(serialConfig_t *initialSerialConfig);
 failsafe_t* failsafeInit(rxConfig_t *intialRxConfig);
-void pwmInit(drv_pwm_config_t *init);
+pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init);
+void mixerInit(MultiType mixerConfiguration, motorMixer_t *customMixers, pwmOutputConfiguration_t *pwmOutputConfiguration);
 void rxInit(rxConfig_t *rxConfig, failsafe_t *failsafe);
 void buzzerInit(failsafe_t *initialFailsafe);
 void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig, gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile);
@@ -163,8 +164,6 @@ void init(void)
         compassInit();
 #endif
 
-    mixerInit(masterConfig.mixerConfiguration, masterConfig.customMixer);
-
     timerInit();
 
     serialInit(&masterConfig.serialConfig);
@@ -195,7 +194,9 @@ void init(void)
         pwm_params.idlePulse = 0; // brushed motors
     pwm_params.servoCenterPulse = masterConfig.rxConfig.midrc;
 
-    pwmInit(&pwm_params);
+    pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params);
+
+    mixerInit(masterConfig.mixerConfiguration, masterConfig.customMixer, pwmOutputConfiguration);
 
     failsafe = failsafeInit(&masterConfig.rxConfig);
     buzzerInit(failsafe);