From 8c92ab0680403899037415ad3286ea240bb8511c Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Sat, 4 Jan 2025 10:43:33 +0000
Subject: [PATCH 1/2] AP_BLHeli: normalize ESC index correctly with iomcu

---
 libraries/AP_BLHeli/AP_BLHeli.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/libraries/AP_BLHeli/AP_BLHeli.cpp b/libraries/AP_BLHeli/AP_BLHeli.cpp
index 3b0a32c89896f2..c2856a921505bb 100644
--- a/libraries/AP_BLHeli/AP_BLHeli.cpp
+++ b/libraries/AP_BLHeli/AP_BLHeli.cpp
@@ -1469,7 +1469,7 @@ void AP_BLHeli::read_telemetry_packet(void)
 
     uint8_t normalized_motor_idx = motor_idx - chan_offset;
 #if HAL_WITH_IO_MCU
-    if (AP_BoardConfig::io_dshot()) {
+    if (AP_BoardConfig::io_enabled()) {
         normalized_motor_idx = motor_idx;
     }
 #endif

From dafdb7c9dd51ab9d7775a0eedbc8e7a4d46d6efd Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Thu, 9 Jan 2025 18:25:59 +0000
Subject: [PATCH 2/2] AP_BLHeli: use native motor numbering

---
 libraries/AP_BLHeli/AP_BLHeli.cpp | 11 ++---------
 1 file changed, 2 insertions(+), 9 deletions(-)

diff --git a/libraries/AP_BLHeli/AP_BLHeli.cpp b/libraries/AP_BLHeli/AP_BLHeli.cpp
index c2856a921505bb..797047aa6071bb 100644
--- a/libraries/AP_BLHeli/AP_BLHeli.cpp
+++ b/libraries/AP_BLHeli/AP_BLHeli.cpp
@@ -1466,14 +1466,7 @@ void AP_BLHeli::read_telemetry_packet(void)
     const uint8_t motor_idx = motor_map[last_telem_esc];
     // we have received valid data, mark the ESC as now active
     hal.rcout->set_active_escs_mask(1<<motor_idx);
-
-    uint8_t normalized_motor_idx = motor_idx - chan_offset;
-#if HAL_WITH_IO_MCU
-    if (AP_BoardConfig::io_enabled()) {
-        normalized_motor_idx = motor_idx;
-    }
-#endif
-    update_rpm(normalized_motor_idx, new_rpm);
+    update_rpm(motor_idx, new_rpm);
 
     TelemetryData t {
         .temperature_cdeg = int16_t(buf[0] * 100),
@@ -1482,7 +1475,7 @@ void AP_BLHeli::read_telemetry_packet(void)
         .consumption_mah = float(uint16_t((buf[5]<<8) | buf[6])),
     };
 
-    update_telem_data(normalized_motor_idx, t,
+    update_telem_data(motor_idx, t,
         AP_ESC_Telem_Backend::TelemetryType::CURRENT
             | AP_ESC_Telem_Backend::TelemetryType::VOLTAGE
             | AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION