void writeServos(void)
{
uint8_t servoIndex = 0;
bool zeroServoValue = false;
/*
* in case of tricopters, there might me a need to zero servo output when unarmed
*/
if ((currentMixerMode == MIXER_TRI || currentMixerMode == MIXER_CUSTOM_TRI) && !ARMING_FLAG(ARMED) && !mixerConfig->tri_unarmed_servo) {
zeroServoValue = true;
}
if (mixerUsesServos) {
for (int i = minServoIndex; i <= maxServoIndex; i++) {
if (zeroServoValue) {
pwmWriteServo(servoIndex++, 0);
} else {
pwmWriteServo(servoIndex++, servo[i]);
}
}
}
if (feature(FEATURE_SERVO_TILT)) {
pwmWriteServo(servoIndex++, servo[SERVO_GIMBAL_PITCH]);
pwmWriteServo(servoIndex++, servo[SERVO_GIMBAL_ROLL]);
}
// forward AUX to remaining servo outputs (not constrained)
if (feature(FEATURE_CHANNEL_FORWARDING)) {
forwardAuxChannelsToServos(servoIndex);
servoIndex += MAX_AUX_CHANNEL_COUNT;
}
}
void writeServos(void)
{
uint8_t servoIndex = 0;
switch (currentMixerMode) {
case MIXER_BICOPTER:
pwmWriteServo(servoIndex++, servo[SERVO_BICOPTER_LEFT]);
pwmWriteServo(servoIndex++, servo[SERVO_BICOPTER_RIGHT]);
break;
case MIXER_TRI:
case MIXER_CUSTOM_TRI:
if (mixerConfig->tri_unarmed_servo) {
// if unarmed flag set, we always move servo
pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]);
} else {
// otherwise, only move servo when copter is armed
if (ARMING_FLAG(ARMED))
pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]);
else
pwmWriteServo(servoIndex++, 0); // kill servo signal completely.
}
break;
case MIXER_FLYING_WING:
pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_1]);
pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_2]);
break;
case MIXER_DUALCOPTER:
pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_LEFT]);
pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_RIGHT]);
break;
case MIXER_CUSTOM_AIRPLANE:
case MIXER_AIRPLANE:
for (int i = SERVO_PLANE_INDEX_MIN; i <= SERVO_PLANE_INDEX_MAX; i++) {
pwmWriteServo(servoIndex++, servo[i]);
}
break;
case MIXER_SINGLECOPTER:
for (int i = SERVO_SINGLECOPTER_INDEX_MIN; i <= SERVO_SINGLECOPTER_INDEX_MAX; i++) {
pwmWriteServo(servoIndex++, servo[i]);
}
break;
default:
break;
}
// Two servos for SERVO_TILT, if enabled
if (feature(FEATURE_SERVO_TILT) || currentMixerMode == MIXER_GIMBAL) {
updateGimbalServos(servoIndex);
servoIndex += 2;
}
// forward AUX to remaining servo outputs (not constrained)
if (feature(FEATURE_CHANNEL_FORWARDING)) {
forwardAuxChannelsToServos(servoIndex);
servoIndex += MAX_AUX_CHANNEL_COUNT;
}
}
