diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index f01378f0c6..9267138d57 100755 --- a/src/main/flight/mixer.c +++ b/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)) { diff --git a/src/test/unit/flight_mixer_unittest.cc b/src/test/unit/flight_mixer_unittest.cc index d014d7c66b..f4981f5196 100644 --- a/src/test/unit/flight_mixer_unittest.cc +++ b/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) {