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

AP_BLHeli: convert servo indexing to FMU indexing for ESC telemetry

This commit is contained in:
Andy Piper 2021-05-24 09:57:32 +01:00 committed by Andrew Tridgell
parent f4c18e0f9c
commit f8495df0af
2 changed files with 12 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

@ -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

3
libraries/AP_BLHeli/AP_BLHeli.h
View File

@ -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;