From 220fd7caa10b81db964496887de286e887e7e956 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 15 Oct 2022 17:55:08 +1100
Subject: [PATCH] AP_RPM: fixed SITL RPM backend for new motor mask

---
 libraries/AP_RPM/RPM_SITL.cpp | 15 +++++++++++----
 1 file changed, 11 insertions(+), 4 deletions(-)

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;