diff --git a/libraries/AP_RPM/RPM_SITL.cpp b/libraries/AP_RPM/RPM_SITL.cpp
index 3b4269810a..f553bfacbe 100644
--- a/libraries/AP_RPM/RPM_SITL.cpp
+++ b/libraries/AP_RPM/RPM_SITL.cpp
@@ -34,10 +34,17 @@ void AP_RPM_SITL::update(void)
     if (sitl == nullptr) {
         return;
     }
-    if (instance == 0) {
-        state.rate_rpm = sitl->state.rpm[0];
-    } else {
-        state.rate_rpm = sitl->state.rpm[1];
+    const uint32_t motor_mask = sitl->state.motor_mask;
+    uint8_t count = 0;
+    // find the motor with the corresponding index
+    for (uint8_t i=0; i<32; i++) {
+        if (motor_mask & (1U<<i)) {
+            if (count == instance) {
+                state.rate_rpm = sitl->state.rpm[i];
+                break;
+            }
+            count++;
+        }
     }
     state.rate_rpm *= ap_rpm._params[state.instance].scaling;
     state.signal_quality = 0.5f;