First cut of custom motor and servo mixer test.

Note: The test is rather heavyweight, more like an integration test than
a unit test, but will allow for the underlying code to be refactored.

src/main/flight/mixer.c

@@ -53,44 +53,9 @@
 #include "config/runtime_config.h"
 #include "config/config.h"
 
-typedef enum {
-    SERVO_GIMBAL_PITCH = 0,
-    SERVO_GIMBAL_ROLL = 1,
-    SERVO_FLAPS = 2,
-    SERVO_FLAPPERON_1 = 3,
-    SERVO_FLAPPERON_2 = 4,
-    SERVO_RUDDER = 5,
-    SERVO_ELEVATOR = 6,
-    SERVO_THROTTLE = 7, // for internal combustion (IC) planes
-
-    SERVO_BICOPTER_LEFT = 4,
-    SERVO_BICOPTER_RIGHT = 5,
-
-    SERVO_DUALCOPTER_LEFT = 4,
-    SERVO_DUALCOPTER_RIGHT = 5,
-    
-    SERVO_SINGLECOPTER_1 = 3,
-    SERVO_SINGLECOPTER_2 = 4,
-    SERVO_SINGLECOPTER_3 = 5,
-    SERVO_SINGLECOPTER_4 = 6,
-    
-} servoIndex_e;
-
-#define SERVO_PLANE_INDEX_MIN SERVO_FLAPS
-#define SERVO_PLANE_INDEX_MAX SERVO_THROTTLE
-
-#define SERVO_DUALCOPTER_INDEX_MIN SERVO_DUALCOPTER_LEFT
-#define SERVO_DUALCOPTER_INDEX_MAX SERVO_DUALCOPTER_RIGHT
-
-#define SERVO_SINGLECOPTER_INDEX_MIN SERVO_SINGLECOPTER_1
-#define SERVO_SINGLECOPTER_INDEX_MAX SERVO_SINGLECOPTER_4
-
-#define SERVO_FLAPPERONS_MIN SERVO_FLAPPERON_1
-#define SERVO_FLAPPERONS_MAX SERVO_FLAPPERON_2
-
 //#define MIXER_DEBUG
 
-uint8_t motorCount = 0;
+uint8_t motorCount;
 
 int16_t motor[MAX_SUPPORTED_MOTORS];
 int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
@@ -449,6 +414,7 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
 {
     int i;
 
+    motorCount = 0;
     servoCount = pwmOutputConfiguration->servoCount;
 
     if (currentMixerMode == MIXER_CUSTOM || currentMixerMode == MIXER_CUSTOM_TRI || currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
@@ -695,7 +661,7 @@ void StopPwmAllMotors()
 }
 
 #ifndef USE_QUAD_MIXER_ONLY
-static void servoMixer(void)
+STATIC_UNIT_TESTED void servoMixer(void)
 {
     int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500]
     static int16_t currentOutput[MAX_SERVO_RULES];

src/main/flight/mixer.h

@@ -114,6 +114,42 @@ enum {
     INPUT_SOURCE_COUNT
 } inputSource_e;
 
+// target servo channels
+typedef enum {
+    SERVO_GIMBAL_PITCH = 0,
+    SERVO_GIMBAL_ROLL = 1,
+    SERVO_FLAPS = 2,
+    SERVO_FLAPPERON_1 = 3,
+    SERVO_FLAPPERON_2 = 4,
+    SERVO_RUDDER = 5,
+    SERVO_ELEVATOR = 6,
+    SERVO_THROTTLE = 7, // for internal combustion (IC) planes
+
+    SERVO_BICOPTER_LEFT = 4,
+    SERVO_BICOPTER_RIGHT = 5,
+
+    SERVO_DUALCOPTER_LEFT = 4,
+    SERVO_DUALCOPTER_RIGHT = 5,
+
+    SERVO_SINGLECOPTER_1 = 3,
+    SERVO_SINGLECOPTER_2 = 4,
+    SERVO_SINGLECOPTER_3 = 5,
+    SERVO_SINGLECOPTER_4 = 6,
+
+} servoIndex_e; // FIXME rename to servoChannel_e
+
+#define SERVO_PLANE_INDEX_MIN SERVO_FLAPS
+#define SERVO_PLANE_INDEX_MAX SERVO_THROTTLE
+
+#define SERVO_DUALCOPTER_INDEX_MIN SERVO_DUALCOPTER_LEFT
+#define SERVO_DUALCOPTER_INDEX_MAX SERVO_DUALCOPTER_RIGHT
+
+#define SERVO_SINGLECOPTER_INDEX_MIN SERVO_SINGLECOPTER_1
+#define SERVO_SINGLECOPTER_INDEX_MAX SERVO_SINGLECOPTER_4
+
+#define SERVO_FLAPPERONS_MIN SERVO_FLAPPERON_1
+#define SERVO_FLAPPERONS_MAX SERVO_FLAPPERON_2
+
 typedef struct servoMixer_t {
     uint8_t targetChannel;                  // servo that receives the output of the rule
     uint8_t inputSource;                    // input channel for this rule

src/main/io/rc_controls.c

@@ -23,6 +23,8 @@
 
 #include "platform.h"
 
+#include "build_config.h"
+
 #include "common/axis.h"
 #include "common/maths.h"
 
@@ -70,7 +72,10 @@ uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e
 
 
 void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue) {
-#ifdef BLACKBOX
+#ifndef BLACKBOX
+    UNUSED(adjustmentFunction);
+    UNUSED(newValue);
+#else
     if (feature(FEATURE_BLACKBOX)) {
         flightLogEvent_inflightAdjustment_t eventData;
         eventData.adjustmentFunction = adjustmentFunction;
@@ -82,7 +87,10 @@ void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction,
 }
 
 void blackboxLogInflightAdjustmentEventFloat(adjustmentFunction_e adjustmentFunction, float newFloatValue) {
-#ifdef BLACKBOX
+#ifndef BLACKBOX
+    UNUSED(adjustmentFunction);
+    UNUSED(newFloatValue);
+#else
     if (feature(FEATURE_BLACKBOX)) {
         flightLogEvent_inflightAdjustment_t eventData;
         eventData.adjustmentFunction = adjustmentFunction;

src/test/unit/flight_mixer_unittest.cc

@@ -77,6 +77,9 @@ uint8_t lastOneShotUpdateMotorCount;
 
 uint32_t testFeatureMask = 0;
 
+int updatedServoCount;
+int updatedMotorCount;
+
 
 TEST(FlightMixerTest, TestForwardAuxChannelsToServosWithNoServos)
 {
@@ -148,11 +151,14 @@ TEST(FlightMixerTest, TestForwardAuxChannelsToServosWithLessRemainingServosThanA
 TEST(FlightMixerTest, TestTricopterServo)
 {
     // given
+    updatedServoCount = 0;
+
     mixerConfig_t mixerConfig;
     memset(&mixerConfig, 0, sizeof(mixerConfig));
 
     rxConfig_t rxConfig;
     memset(&rxConfig, 0, sizeof(rxConfig));
+    rxConfig.midrc = 1500;
 
     mixerConfig.tri_unarmed_servo = 1;
 
@@ -205,18 +211,20 @@ TEST(FlightMixerTest, TestTricopterServo)
     memset(axisPID, 0, sizeof(axisPID));
     axisPID[YAW] = 0;
 
-
     // when
     mixTable();
     writeServos();
 
     // then
+    EXPECT_EQ(1, updatedServoCount);
     EXPECT_EQ(TEST_SERVO_MID, servos[0].value);
 }
 
 TEST(FlightMixerTest, TestQuadMotors)
 {
     // given
+    updatedMotorCount = 0;
+
     mixerConfig_t mixerConfig;
     memset(&mixerConfig, 0, sizeof(mixerConfig));
 
@@ -274,12 +282,139 @@ TEST(FlightMixerTest, TestQuadMotors)
     writeMotors();
 
     // then
+    EXPECT_EQ(4, updatedMotorCount);
+
     EXPECT_EQ(TEST_MIN_COMMAND, motors[0].value);
     EXPECT_EQ(TEST_MIN_COMMAND, motors[1].value);
     EXPECT_EQ(TEST_MIN_COMMAND, motors[2].value);
     EXPECT_EQ(TEST_MIN_COMMAND, motors[3].value);
 }
 
+
+class CustomMixerTest : public ::testing::Test {
+protected:
+    // given
+    mixerConfig_t mixerConfig;
+
+    rxConfig_t rxConfig;
+
+    servoParam_t servoConf[MAX_SUPPORTED_SERVOS];
+
+    escAndServoConfig_t escAndServoConfig;
+
+    gimbalConfig_t gimbalConfig;
+    motorMixer_t customMotorMixer[MAX_SUPPORTED_MOTORS];
+    servoMixer_t customServoMixer[MAX_SUPPORTED_SERVOS];
+
+    virtual void SetUp() {
+        updatedServoCount = 0;
+        updatedMotorCount = 0;
+
+        memset(&rcData, 0, sizeof(rcData));
+        memset(&rcCommand, 0, sizeof(rcCommand));
+        memset(&mixerConfig, 0, sizeof(mixerConfig));
+        memset(&rxConfig, 0, sizeof(rxConfig));
+        rxConfig.midrc = 1500;
+
+        memset(&servoConf, 0, sizeof(servoConf));
+        for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
+            servoConf[i].min = DEFAULT_SERVO_MIN;
+            servoConf[i].max = DEFAULT_SERVO_MAX;
+            servoConf[i].middle = DEFAULT_SERVO_MIDDLE;
+            servoConf[i].rate = 100;
+            servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED;
+        }
+
+        memset(&escAndServoConfig, 0, sizeof(escAndServoConfig));
+
+        escAndServoConfig.mincommand = TEST_MIN_COMMAND;
+
+        gimbalConfig.mode = GIMBAL_MODE_NORMAL;
+
+        mixerUseConfigs(
+            servoConf,
+            &gimbalConfig,
+            NULL,
+            &escAndServoConfig,
+            &mixerConfig,
+            NULL,
+            &rxConfig
+        );
+
+        memset(&customMotorMixer, 0, sizeof(customMotorMixer));
+
+        memset(&customServoMixer, 0, sizeof(customServoMixer));
+    }
+};
+
+
+TEST_F(CustomMixerTest, TestCustomMixer)
+{
+    // given
+    enum {
+        EXPECTED_SERVOS_TO_MIX_COUNT = 6,
+        EXPECTED_MOTORS_TO_MIX_COUNT = 2
+    };
+
+    servoMixer_t testServoMixer[EXPECTED_SERVOS_TO_MIX_COUNT] = {
+        { SERVO_FLAPS, INPUT_RC_AUX1,  100, 0, 0, 100, 0 },
+        { SERVO_FLAPPERON_1, INPUT_STABILIZED_ROLL,  100, 0, 0, 100, 0 },
+        { SERVO_FLAPPERON_2, INPUT_STABILIZED_ROLL,  100, 0, 0, 100, 0 },
+        { SERVO_RUDDER, INPUT_STABILIZED_YAW,   100, 0, 0, 100, 0 },
+        { SERVO_ELEVATOR, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 },
+        { SERVO_THROTTLE, INPUT_STABILIZED_THROTTLE, 100, 0, 0, 100, 0 },
+    };
+    memcpy(customServoMixer, testServoMixer, sizeof(testServoMixer));
+
+    static const motorMixer_t testMotorMixer[EXPECTED_MOTORS_TO_MIX_COUNT] = {
+        { 1.0f,  0.0f,  0.0f, -1.0f },          // LEFT
+        { 1.0f,  0.0f,  0.0f,  1.0f },          // RIGHT
+    };
+    memcpy(customMotorMixer, testMotorMixer, sizeof(testMotorMixer));
+
+
+    mixerInit(MIXER_CUSTOM_AIRPLANE, customMotorMixer, customServoMixer);
+
+    pwmOutputConfiguration_t pwmOutputConfiguration = {
+            .servoCount = 6,
+            .motorCount = 2
+    };
+
+    mixerUsePWMOutputConfiguration(&pwmOutputConfiguration);
+
+    // and
+    rcCommand[THROTTLE] = 1000;
+
+    // and
+    rcData[AUX1] = 2000;
+
+    // and
+    memset(axisPID, 0, sizeof(axisPID));
+    axisPID[YAW] = 0;
+
+
+    // when
+    mixTable();
+    writeMotors();
+    writeServos();
+
+    // then
+    EXPECT_EQ(EXPECTED_MOTORS_TO_MIX_COUNT, updatedMotorCount);
+
+    EXPECT_EQ(TEST_MIN_COMMAND, motors[0].value);
+    EXPECT_EQ(TEST_MIN_COMMAND, motors[1].value);
+
+    EXPECT_EQ(EXPECTED_SERVOS_TO_MIX_COUNT, updatedServoCount);
+
+    EXPECT_EQ(2000, servos[0].value); // Flaps
+    EXPECT_EQ(TEST_SERVO_MID, servos[1].value);
+    EXPECT_EQ(TEST_SERVO_MID, servos[2].value);
+    EXPECT_EQ(TEST_SERVO_MID, servos[3].value);
+    EXPECT_EQ(TEST_SERVO_MID, servos[4].value);
+    EXPECT_EQ(1000, servos[5].value); // Throttle
+
+}
+
 // STUBS
 
 extern "C" {
@@ -309,6 +444,7 @@ int32_t lowpassFixed(lowpass_t *, int32_t, int16_t) {
 
 void pwmWriteMotor(uint8_t index, uint16_t value) {
     motors[index].value = value;
+    updatedMotorCount++;
 }
 
 void pwmShutdownPulsesForAllMotors(uint8_t motorCount)
@@ -332,6 +468,7 @@ void pwmWriteServo(uint8_t index, uint16_t value) {
     if (index < MAX_SERVOS) {
         servos[index].value = value;
     }
+    updatedServoCount++;
 }
 
 bool failsafeIsActive(void) {