Commit 7db62674 authored by Dominic Clifton's avatar Dominic Clifton
Browse files

Allow all models to use servo tilt.

See #1006
parent 889b14df
...@@ -442,10 +442,10 @@ STATIC_UNIT_TESTED void forwardAuxChannelsToServos(uint8_t firstServoIndex) ...@@ -442,10 +442,10 @@ STATIC_UNIT_TESTED void forwardAuxChannelsToServos(uint8_t firstServoIndex)
} }
} }
static void updateGimbalServos(void) static void updateGimbalServos(uint8_t firstServoIndex)
{ {
pwmWriteServo(0, servo[SERVO_GIMBAL_PITCH]); pwmWriteServo(firstServoIndex + 0, servo[SERVO_GIMBAL_PITCH]);
pwmWriteServo(1, servo[SERVO_GIMBAL_ROLL]); pwmWriteServo(firstServoIndex + 1, servo[SERVO_GIMBAL_ROLL]);
} }
void writeServos(void) void writeServos(void)
...@@ -479,11 +479,6 @@ void writeServos(void) ...@@ -479,11 +479,6 @@ void writeServos(void)
pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_2]); pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_2]);
break; break;
case MIXER_GIMBAL:
updateGimbalServos();
servoIndex += 2;
break;
case MIXER_DUALCOPTER: case MIXER_DUALCOPTER:
pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_LEFT]); pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_LEFT]);
pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_RIGHT]); pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_RIGHT]);
...@@ -503,14 +498,15 @@ void writeServos(void) ...@@ -503,14 +498,15 @@ void writeServos(void)
break; break;
default: default:
// Two servos for SERVO_TILT, if enabled
if (feature(FEATURE_SERVO_TILT)) {
updateGimbalServos();
servoIndex += 2;
}
break; break;
} }
// Two servos for SERVO_TILT, if enabled
if (feature(FEATURE_SERVO_TILT)) {
updateGimbalServos(servoIndex);
servoIndex += 2;
}
// forward AUX1-4 to servo outputs (not constrained) // forward AUX1-4 to servo outputs (not constrained)
if (gimbalConfig->gimbal_flags & GIMBAL_FORWARDAUX) { if (gimbalConfig->gimbal_flags & GIMBAL_FORWARDAUX) {
forwardAuxChannelsToServos(servoIndex); forwardAuxChannelsToServos(servoIndex);
......
...@@ -827,7 +827,7 @@ static bool processOutCommand(uint8_t cmdMSP) ...@@ -827,7 +827,7 @@ static bool processOutCommand(uint8_t cmdMSP)
break; break;
#ifdef USE_SERVOS #ifdef USE_SERVOS
case MSP_SERVO: case MSP_SERVO:
s_struct((uint8_t *)&servo, 16); s_struct((uint8_t *)&servo, MAX_SUPPORTED_SERVOS * 2);
break; break;
case MSP_SERVO_CONF: case MSP_SERVO_CONF:
headSerialReply(MAX_SUPPORTED_SERVOS * 7); headSerialReply(MAX_SUPPORTED_SERVOS * 7);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment