AP_BLHeli: convert servo indexing to FMU indexing for ESC telemetry

libraries/AP_BLHeli/AP_BLHeli.cpp

@@ -1302,6 +1302,13 @@ void AP_BLHeli::init(void)
         last_control_port = control_port;
     }
 
+#if HAL_WITH_IO_MCU
+    if (AP_BoardConfig::io_enabled()) {
+        // with IOMCU the local (FMU) channels start at 8
+        chan_offset = 8;
+    }
+#endif
+
     uint16_t mask = uint16_t(channel_mask.get());
 
     /*
@@ -1438,7 +1445,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);
-    update_rpm(motor_idx, new_rpm);
+    update_rpm(motor_idx - chan_offset, new_rpm);
 
     TelemetryData t {
         .temperature_cdeg = int16_t(buf[0] * 100),
@@ -1447,7 +1454,7 @@ void AP_BLHeli::read_telemetry_packet(void)
         .consumption_mah = float(uint16_t((buf[5]<<8) | buf[6])),
     };
 
-    update_telem_data(motor_idx, t,
+    update_telem_data(motor_idx - chan_offset, t,
         AP_ESC_Telem_Backend::TelemetryType::CURRENT
             | AP_ESC_Telem_Backend::TelemetryType::VOLTAGE
             | AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION

libraries/AP_BLHeli/AP_BLHeli.h

@@ -245,6 +245,9 @@ private:
     uint8_t motor_map[max_motors];
     uint16_t motor_mask;
 
+    // convert between servo number and FMU channel number for ESC telemetry
+    uint8_t chan_offset;
+
     // when did we last request telemetry?
     uint32_t last_telem_request_us;
     uint8_t last_telem_esc;