Refactored mixer / servo init.

src/main/fc/init.c

@@ -581,7 +581,6 @@ void init(void)
 #endif
 
     mixerInit(mixerConfig()->mixerMode);
-    mixerConfigureOutput();
 
     uint16_t idlePulse = motorConfig()->mincommand;
     if (featureIsEnabled(FEATURE_3D)) {
@@ -745,7 +744,6 @@ void init(void)
 
 #ifdef USE_SERVOS
     servosInit();
-    servoConfigureOutput();
     if (isMixerUsingServos()) {
         //pwm_params.useChannelForwarding = featureIsEnabled(FEATURE_CHANNEL_FORWARDING);
         servoDevInit(&servoConfig()->dev);

src/main/flight/mixer.h

@@ -102,8 +102,6 @@ void initEscEndpoints(void);
 void mixerInit(mixerMode_e mixerMode);
 void mixerInitProfile(void);
 
-void mixerConfigureOutput(void);
-
 void mixerResetDisarmedMotors(void);
 void mixTable(timeUs_t currentTimeUs);
 void stopMotors(void);

src/main/flight/mixer_init.c

@@ -317,24 +317,6 @@ void mixerInitProfile(void)
 #endif
 }
 
-void mixerInit(mixerMode_e mixerMode)
-{
-    currentMixerMode = mixerMode;
-
-    mixerRuntime.feature3dEnabled = featureIsEnabled(FEATURE_3D);
-
-    initEscEndpoints();
-#ifdef USE_SERVOS
-    if (mixerIsTricopter()) {
-        mixerTricopterInit();
-    }
-#endif
-
-#ifdef USE_DYN_IDLE
-    mixerRuntime.idleThrottleOffset = motorConfig()->digitalIdleOffsetValue * 0.0001f;
-#endif
-}
-
 #ifdef USE_LAUNCH_CONTROL
 // Create a custom mixer for launch control based on the current settings
 // but disable the front motors. We don't care about roll or yaw because they
@@ -354,7 +336,7 @@ void loadLaunchControlMixer(void)
 
 #ifndef USE_QUAD_MIXER_ONLY
 
-void mixerConfigureOutput(void)
+static void mixerConfigureOutput(void)
 {
     mixerRuntime.motorCount = 0;
 
@@ -401,7 +383,7 @@ void mixerLoadMix(int index, motorMixer_t *customMixers)
     }
 }
 #else
-void mixerConfigureOutput(void)
+static void mixerConfigureOutput(void)
 {
     mixerRuntime.motorCount = QUAD_MOTOR_COUNT;
     for (int i = 0; i < mixerRuntime.motorCount; i++) {
@@ -414,6 +396,26 @@ void mixerConfigureOutput(void)
 }
 #endif // USE_QUAD_MIXER_ONLY
 
+void mixerInit(mixerMode_e mixerMode)
+{
+    currentMixerMode = mixerMode;
+
+    mixerRuntime.feature3dEnabled = featureIsEnabled(FEATURE_3D);
+
+    initEscEndpoints();
+#ifdef USE_SERVOS
+    if (mixerIsTricopter()) {
+        mixerTricopterInit();
+    }
+#endif
+
+#ifdef USE_DYN_IDLE
+    mixerRuntime.idleThrottleOffset = motorConfig()->digitalIdleOffsetValue * 0.0001f;
+#endif
+
+    mixerConfigureOutput();
+}
+
 void mixerResetDisarmedMotors(void)
 {
     // set disarmed motor values

src/main/flight/servos.c

@@ -218,25 +218,6 @@ int servoDirection(int servoIndex, int inputSource)
     }
 }
 
-void servosInit(void)
-{
-    // enable servos for mixes that require them. note, this shifts motor counts.
-    useServo = mixers[getMixerMode()].useServo;
-    // if we want camstab/trig, that also enables servos, even if mixer doesn't
-    if (featureIsEnabled(FEATURE_SERVO_TILT) || featureIsEnabled(FEATURE_CHANNEL_FORWARDING)) {
-        useServo = 1;
-    }
-
-    // give all servos a default command
-    for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
-        servo[i] = DEFAULT_SERVO_MIDDLE;
-    }
-
-    if (mixerIsTricopter()) {
-        servosTricopterInit();
-    }
-}
-
 void loadCustomServoMixer(void)
 {
     // reset settings
@@ -254,7 +235,7 @@ void loadCustomServoMixer(void)
     }
 }
 
-void servoConfigureOutput(void)
+static void servoConfigureOutput(void)
 {
     if (useServo) {
         servoRuleCount = servoMixers[getMixerMode()].servoRuleCount;
@@ -276,6 +257,27 @@ void servoConfigureOutput(void)
 }
 
 
+void servosInit(void)
+{
+    // enable servos for mixes that require them. note, this shifts motor counts.
+    useServo = mixers[getMixerMode()].useServo;
+    // if we want camstab/trig, that also enables servos, even if mixer doesn't
+    if (featureIsEnabled(FEATURE_SERVO_TILT) || featureIsEnabled(FEATURE_CHANNEL_FORWARDING)) {
+        useServo = 1;
+    }
+
+    // give all servos a default command
+    for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
+        servo[i] = DEFAULT_SERVO_MIDDLE;
+    }
+
+    if (mixerIsTricopter()) {
+        servosTricopterInit();
+    }
+
+    servoConfigureOutput();
+}
+
 void servoMixerLoadMix(int index)
 {
     // we're 1-based

src/main/flight/servos.h

@@ -140,7 +140,6 @@ void writeServos(void);
 void servoMixerLoadMix(int index);
 void loadCustomServoMixer(void);
 int servoDirection(int servoIndex, int fromChannel);
-void servoConfigureOutput(void);
 void servosInit(void);
 void servosFilterInit(void);
 void servoMixer(void);