diff --git a/libraries/AP_HAL_SITL/SITL_Periph_State.cpp b/libraries/AP_HAL_SITL/SITL_Periph_State.cpp
index 83c13dfc09..c53f84623f 100644
--- a/libraries/AP_HAL_SITL/SITL_Periph_State.cpp
+++ b/libraries/AP_HAL_SITL/SITL_Periph_State.cpp
@@ -226,9 +226,19 @@ void SimMCast::servo_fd_open(void)
  */
 void SimMCast::servo_send(void)
 {
+    const auto *_sitl = AP::sitl();
+    if (_sitl == nullptr) {
+        return;
+    }
     uint16_t out[SITL_NUM_CHANNELS] {};
     hal.rcout->read(out, SITL_NUM_CHANNELS);
-    send(servo_fd, (void*)out, sizeof(out), 0);
+
+    float out_float[SITL_NUM_CHANNELS];
+    const uint32_t mask = uint32_t(_sitl->can_servo_mask.get());
+    for (uint8_t i=0; i<SITL_NUM_CHANNELS; i++) {
+        out_float[i] = (mask & (1U<<i)) ? out[i] : nanf("");
+    }
+    send(servo_fd, (void*)out_float, sizeof(out_float), 0);
 }
 
 /*
diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp
index 56a66c709b..8c3af4c856 100644
--- a/libraries/AP_HAL_SITL/SITL_State.cpp
+++ b/libraries/AP_HAL_SITL/SITL_State.cpp
@@ -692,8 +692,15 @@ void SITL_State::multicast_state_send(void)
 void SITL_State::check_servo_input(void)
 {
     // drain any pending packets
-    while (recv(servo_in_fd, (void*)mc_servo, sizeof(mc_servo), MSG_DONTWAIT) == sizeof(mc_servo)) {
-        // noop
+    float mc_servo_float[SITL_NUM_CHANNELS];
+    // we loop to ensure we drain all packets from all nodes
+    while (recv(servo_in_fd, (void*)mc_servo_float, sizeof(mc_servo_float), MSG_DONTWAIT) == sizeof(mc_servo_float)) {
+        for (uint8_t i=0; i<SITL_NUM_CHANNELS; i++) {
+            // nan means that node is not outputting this channel
+            if (!isnan(mc_servo_float[i])) {
+                mc_servo[i] = uint16_t(mc_servo_float[i]);
+            }
+        }
     }
 }