Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_BLHeli: normalize motor index correctly for iomcu running dshot

This commit is contained in:
Andy Piper 2023-05-30 18:23:50 +01:00 committed by Andrew Tridgell
parent 49bdf7a295
commit 9642343e8e
1 changed files with 9 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_BLHeli/AP_BLHeli.cpp
View File

@ -1460,7 +1460,14 @@ 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 - chan_offset, new_rpm);
uint8_t normalized_motor_idx = motor_idx - chan_offset;
#if HAL_WITH_IO_MCU
if (AP_BoardConfig::io_dshot()) {
normalized_motor_idx = motor_idx;
}
#endif
update_rpm(normalized_motor_idx, new_rpm);
TelemetryData t {
.temperature_cdeg = int16_t(buf[0] * 100),
@ -1469,7 +1476,7 @@ void AP_BLHeli::read_telemetry_packet(void)
.consumption_mah = float(uint16_t((buf[5]<<8) | buf[6])),
};
update_telem_data(motor_idx - chan_offset, t,
update_telem_data(normalized_motor_idx, t,
AP_ESC_Telem_Backend::TelemetryType::CURRENT
| AP_ESC_Telem_Backend::TelemetryType::VOLTAGE
| AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION