From dc3f6fe69de6dd2285cb7ac3eaf887019c6d1e21 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 18 Aug 2022 17:24:56 +1000
Subject: [PATCH] AP_ESC_TELEM: allow for non-continguous ESC telem motor sets

this fixes ESC telem for quadplanes with motors at 5-8 or 9-12
---
 libraries/AP_ESC_Telem/AP_ESC_Telem_SITL.cpp | 39 +++++++++++---------
 1 file changed, 22 insertions(+), 17 deletions(-)

diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem_SITL.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem_SITL.cpp
index 2889ef8987..220c899823 100644
--- a/libraries/AP_ESC_Telem/AP_ESC_Telem_SITL.cpp
+++ b/libraries/AP_ESC_Telem/AP_ESC_Telem_SITL.cpp
@@ -37,7 +37,26 @@ void AP_ESC_Telem_SITL::update()
     }
 
 #if HAL_WITH_ESC_TELEM
-    for (uint8_t i = 0; i < sitl->state.num_motors; i++) {
+
+    if (AP_HAL::millis64() < 6000) {
+        // this prevents us sending blank data at startup, which triggers
+        // ESC telem messages for all channels
+        return;
+    }
+    uint32_t mask = sitl->state.motor_mask;
+    uint8_t bit;
+    while ((bit = __builtin_ffs(mask)) != 0) {
+        uint8_t motor = bit-1;
+        mask &= ~(1U<<motor);
+
+        if (is_zero(sitl->throttle)) {
+            if (!is_zero(sitl->esc_rpm_armed) && hal.util->get_soft_armed()) {
+                update_rpm(motor, sitl->esc_rpm_armed);
+            }
+        } else {
+            update_rpm(motor, sitl->state.rpm[motor]);
+        }
+
         // some fake values so that is_telemetry_active() returns true
         TelemetryData t {
             .temperature_cdeg = 32,
@@ -46,27 +65,13 @@ void AP_ESC_Telem_SITL::update()
             .consumption_mah = 1.0f,
         };
 
-        update_telem_data(i, t,
+        update_telem_data(motor, t,
             AP_ESC_Telem_Backend::TelemetryType::CURRENT
                 | AP_ESC_Telem_Backend::TelemetryType::VOLTAGE
                 | AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION
                 | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE);
     }
-
-    if (is_zero(sitl->throttle)) {
-        if (!is_zero(sitl->esc_rpm_armed) && hal.util->get_soft_armed()) {
-            for (uint8_t i = 0; i < sitl->state.num_motors; i++) {
-                update_rpm(i, sitl->esc_rpm_armed);
-            }
-        }
-        return;
-    }
-
-    for (uint8_t i = 0; i < sitl->state.num_motors; i++) {
-        update_rpm(i, MAX(sitl->state.rpm[sitl->state.vtol_motor_start+i], sitl->esc_rpm_armed));
-    }
 #endif
-
 }
 
-#endif
\ No newline at end of file
+#endif