diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index 9267138d57..a2af22b6dc 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -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(1, servo[SERVO_GIMBAL_ROLL]);
+    pwmWriteServo(firstServoIndex + 0, servo[SERVO_GIMBAL_PITCH]);
+    pwmWriteServo(firstServoIndex + 1, servo[SERVO_GIMBAL_ROLL]);
 }
 
 void writeServos(void)
@@ -479,11 +479,6 @@ void writeServos(void)
             pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_2]);
             break;
 
-        case MIXER_GIMBAL:
-            updateGimbalServos();
-            servoIndex += 2;
-            break;
-
         case MIXER_DUALCOPTER:
             pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_LEFT]);
             pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_RIGHT]);
@@ -503,14 +498,15 @@ void writeServos(void)
             break;
 
         default:
-            // Two servos for SERVO_TILT, if enabled
-            if (feature(FEATURE_SERVO_TILT)) {
-                updateGimbalServos();
-                servoIndex += 2;
-            }
             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)
     if (gimbalConfig->gimbal_flags & GIMBAL_FORWARDAUX) {
         forwardAuxChannelsToServos(servoIndex);
diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c
index 48577f1ca8..21020c4db8 100644
--- a/src/main/io/serial_msp.c
+++ b/src/main/io/serial_msp.c
@@ -827,7 +827,7 @@ static bool processOutCommand(uint8_t cmdMSP)
         break;
 #ifdef USE_SERVOS
     case MSP_SERVO:
-        s_struct((uint8_t *)&servo, 16);
+        s_struct((uint8_t *)&servo, MAX_SUPPORTED_SERVOS * 2);
         break;
     case MSP_SERVO_CONF:
         headSerialReply(MAX_SUPPORTED_SERVOS * 7);