Support pan and tilt on fixed wing. Fixes #1006

The general idea is that nothing calls pwmWriteServo except for
writeServos() and that writeServos() knows which servos it has used.

src/main/flight/mixer.c

@@ -431,21 +431,14 @@ void mixerResetMotors(void)
 
 #ifdef USE_SERVOS
 
-STATIC_UNIT_TESTED void forwardAuxChannelsToServos(void)
+STATIC_UNIT_TESTED void forwardAuxChannelsToServos(uint8_t firstServoIndex)
 {
-    // offset servos based off number already used in mixer types
-    // airplane and servo_tilt together can't be used
-    int8_t firstServo = servoCount - AUX_FORWARD_CHANNEL_TO_SERVO_COUNT;
-
     // start forwarding from this channel
     uint8_t channelOffset = AUX1;
 
-    int8_t servoOffset;
+    uint8_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++]);
+        pwmWriteServo(firstServoIndex + servoOffset, rcData[channelOffset++]);
     }
 }
 
@@ -460,48 +453,52 @@ void writeServos(void)
     if (!useServo)
         return;
 
+    uint8_t servoIndex = 0;
+
     switch (currentMixerMode) {
         case MIXER_BI:
-            pwmWriteServo(0, servo[SERVO_BIPLANE_LEFT]);
-            pwmWriteServo(1, servo[SERVO_BIPLANE_RIGHT]);
+            pwmWriteServo(servoIndex++, servo[SERVO_BIPLANE_LEFT]);
+            pwmWriteServo(servoIndex++, servo[SERVO_BIPLANE_RIGHT]);
             break;
 
         case MIXER_TRI:
             if (mixerConfig->tri_unarmed_servo) {
                 // if unarmed flag set, we always move servo
-                pwmWriteServo(0, servo[SERVO_RUDDER]);
+                pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]);
             } else {
                 // otherwise, only move servo when copter is armed
                 if (ARMING_FLAG(ARMED))
-                    pwmWriteServo(0, servo[SERVO_RUDDER]);
+                    pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]);
                 else
-                    pwmWriteServo(0, 0); // kill servo signal completely.
+                    pwmWriteServo(servoIndex++, 0); // kill servo signal completely.
             }
             break;
 
         case MIXER_FLYING_WING:
-            pwmWriteServo(0, servo[SERVO_FLAPPERON_1]);
-            pwmWriteServo(1, servo[SERVO_FLAPPERON_2]);
+            pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_1]);
+            pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_2]);
             break;
 
         case MIXER_GIMBAL:
             updateGimbalServos();
+            servoIndex += 2;
             break;
 
         case MIXER_DUALCOPTER:
-            pwmWriteServo(0, servo[SERVO_DUALCOPTER_LEFT]);
-            pwmWriteServo(1, servo[SERVO_DUALCOPTER_RIGHT]);
+            pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_LEFT]);
+            pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_RIGHT]);
             break;
 
         case MIXER_AIRPLANE:
             for (int i = SERVO_PLANE_INDEX_MIN; i <= SERVO_PLANE_INDEX_MAX; i++) {
-                pwmWriteServo(i - SERVO_PLANE_INDEX_MIN, servo[i]);
+                pwmWriteServo(servoIndex++, servo[i]);
+
             }
             break;
 
         case MIXER_SINGLECOPTER:
             for (int i = SERVO_SINGLECOPTER_INDEX_MIN; i <= SERVO_SINGLECOPTER_INDEX_MAX; i++) {
-                pwmWriteServo(i - SERVO_SINGLECOPTER_INDEX_MIN, servo[i]);
+                pwmWriteServo(servoIndex++, servo[i]);
             }
             break;
 
@@ -509,9 +506,16 @@ void writeServos(void)
             // Two servos for SERVO_TILT, if enabled
             if (feature(FEATURE_SERVO_TILT)) {
                 updateGimbalServos();
+                servoIndex += 2;
             }
             break;
     }
+
+    // forward AUX1-4 to servo outputs (not constrained)
+    if (gimbalConfig->gimbal_flags & GIMBAL_FORWARDAUX) {
+        forwardAuxChannelsToServos(servoIndex);
+        servoIndex += AUX_FORWARD_CHANNEL_TO_SERVO_COUNT;
+    }
 }
 #endif
 
@@ -710,10 +714,6 @@ void mixTable(void)
             servo[i] = constrain(servo[i], servoConf[i].min, servoConf[i].max); // limit the values
         }
     }
-    // forward AUX1-4 to servo outputs (not constrained)
-    if (gimbalConfig->gimbal_flags & GIMBAL_FORWARDAUX) {
-        forwardAuxChannelsToServos();
-    }
 #endif
 
     if (ARMING_FLAG(ARMED)) {

src/test/unit/flight_mixer_unittest.cc

@@ -46,7 +46,7 @@ extern "C" {
     #include "io/rc_controls.h"
 
     extern uint8_t servoCount;
-    void forwardAuxChannelsToServos(void);
+    void forwardAuxChannelsToServos(uint8_t firstServoIndex);
 
     void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers);
     void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration);
@@ -90,7 +90,7 @@ TEST(FlightMixerTest, TestForwardAuxChannelsToServosWithNoServos)
     rcData[AUX4] = TEST_RC_MID;
 
     // when
-    forwardAuxChannelsToServos();
+    forwardAuxChannelsToServos(MAX_SUPPORTED_SERVOS);
 
     // then
     for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
@@ -110,26 +110,20 @@ TEST(FlightMixerTest, TestForwardAuxChannelsToServosWithMaxServos)
     rcData[AUX4] = 2000;
 
     // when
-    forwardAuxChannelsToServos();
+    forwardAuxChannelsToServos(MAX_SUPPORTED_SERVOS);
 
     // then
     uint8_t i;
-    for (i = 0; i < MAX_SUPPORTED_SERVOS - 4; i++) {
-        EXPECT_EQ(servos[i].value, 0);
+    for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
+        EXPECT_EQ(0, servos[i].value);
     }
-
-    // -1 for zero based offset
-    EXPECT_EQ(1000, servos[MAX_SUPPORTED_SERVOS - 3 - 1].value);
-    EXPECT_EQ(1250, servos[MAX_SUPPORTED_SERVOS - 2 - 1].value);
-    EXPECT_EQ(1750, servos[MAX_SUPPORTED_SERVOS - 1 - 1].value);
-    EXPECT_EQ(2000, servos[MAX_SUPPORTED_SERVOS - 0 - 1].value);
 }
 
-TEST(FlightMixerTest, TestForwardAuxChannelsToServosWithLessServosThanAuxChannelsToForward)
+TEST(FlightMixerTest, TestForwardAuxChannelsToServosWithLessRemainingServosThanAuxChannelsToForward)
 {
     // given
     memset(&servos, 0, sizeof(servos));
-    servoCount = 2;
+    servoCount = MAX_SUPPORTED_SERVOS - 2;
 
     rcData[AUX1] = 1000;
     rcData[AUX2] = 1250;
@@ -137,17 +131,17 @@ TEST(FlightMixerTest, TestForwardAuxChannelsToServosWithLessServosThanAuxChannel
     rcData[AUX4] = 2000;
 
     // when
-    forwardAuxChannelsToServos();
+    forwardAuxChannelsToServos(MAX_SUPPORTED_SERVOS - 2);
 
     // then
     uint8_t i;
-    for (i = 2; i < MAX_SUPPORTED_SERVOS; i++) {
-        EXPECT_EQ(servos[i].value, 0);
+    for (i = 0; i < MAX_SUPPORTED_SERVOS - 2; i++) {
+        EXPECT_EQ(0, servos[i].value);
     }
 
     // -1 for zero based offset
-    EXPECT_EQ(1000, servos[0].value);
-    EXPECT_EQ(1250, servos[1].value);
+    EXPECT_EQ(1000, servos[MAX_SUPPORTED_SERVOS - 1 - 1].value);
+    EXPECT_EQ(1250, servos[MAX_SUPPORTED_SERVOS - 0 - 1].value);
 }
 
 TEST(FlightMixerTest, TestTricopterServo)
@@ -317,7 +311,13 @@ void pwmCompleteOneshotMotorUpdate(uint8_t motorCount) {
 }
 
 void pwmWriteServo(uint8_t index, uint16_t value) {
-    servos[index].value = value;
+    // FIXME logic in test, mimic's production code.
+    // Perhaps the solution is to remove the logic from the production code version and assume that
+    // anything calling calling pwmWriteServo always uses a valid index?
+    // See MAX_SERVOS in pwm_output (driver) and MAX_SUPPORTED_SERVOS (flight)
+    if (index < MAX_SERVOS) {
+        servos[index].value = value;
+    }
 }
 
 bool failsafeIsActive(void) {