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

AP_Periph: fixed non-contiguous mask for ESC telem output 

this fixes the case where a peripheral node has an ESCs that are not
starting at the first channel. Reported here:

https://discuss.ardupilot.org/t/using-matekl431-adapters-for-pwm-and-dshot/85781/22

tested on MatekL431-Periph
This commit is contained in:
Andrew Tridgell 2022-06-10 14:55:54 +10:00
parent e55fa6ad92
commit 5a36d7cf0a
1 changed files with 4 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
Tools/AP_Periph/can.cpp
View File

@ -1573,12 +1573,10 @@ void AP_Periph_FW::hwesc_telem_update()
*/
void AP_Periph_FW::esc_telem_update()
{
const uint8_t mask = esc_telem.get_active_esc_mask();
const uint8_t num_escs = esc_telem.get_num_active_escs();
for (uint8_t i=0; i<num_escs; i++) {
if (((1U<<i) & mask) == 0) {
continue;
}
uint32_t mask = esc_telem.get_active_esc_mask();
while (mask != 0) {
int8_t i = __builtin_ffs(mask) - 1;
mask &= ~(1U<<i);
const float nan = nanf("");
uavcan_equipment_esc_Status pkt {};
const auto *channel = SRV_Channels::srv_channel(i);