From 235c8a2d0c919be8eee9b551e085d07d9b49849b Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 24 May 2021 15:44:29 +0100 Subject: [PATCH] AP_BLHeli: make sure telemetry is requested when there is only one motor --- libraries/AP_BLHeli/AP_BLHeli.cpp | 29 +++++++++++++++-------------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/libraries/AP_BLHeli/AP_BLHeli.cpp b/libraries/AP_BLHeli/AP_BLHeli.cpp index 862e16a692..49d9dfa805 100644 --- a/libraries/AP_BLHeli/AP_BLHeli.cpp +++ b/libraries/AP_BLHeli/AP_BLHeli.cpp @@ -1505,15 +1505,15 @@ void AP_BLHeli::log_bidir_telemetry(void) // ask the next ESC for telemetry uint8_t idx_pos = last_telem_esc; - for (uint8_t idx = (idx_pos + 1) % num_motors; - idx != idx_pos; - idx = (idx_pos + 1) % num_motors) { - uint16_t mask = 1U << motor_map[idx]; - if (SRV_Channels::have_digital_outputs(mask)) { - last_telem_esc = idx; + uint8_t idx = (idx_pos + 1) % num_motors; + for (; idx != idx_pos; idx = (idx_pos + 1) % num_motors) { + if (SRV_Channels::have_digital_outputs(1U << motor_map[idx])) { break; } } + if (SRV_Channels::have_digital_outputs(1U << motor_map[idx])) { + last_telem_esc = idx; + } } /* @@ -1574,17 +1574,18 @@ void AP_BLHeli::update_telemetry(void) if (now - last_telem_request_us >= telem_rate_us) { // ask the next ESC for telemetry uint8_t idx_pos = last_telem_esc; - for (uint8_t idx = (idx_pos + 1) % num_motors; - idx != idx_pos; - idx = (idx_pos + 1) % num_motors) { - uint16_t mask = 1U << motor_map[idx]; - if (SRV_Channels::have_digital_outputs(mask)) { - hal.rcout->set_telem_request_mask(mask); - last_telem_esc = idx; - last_telem_request_us = now; + uint8_t idx = (idx_pos + 1) % num_motors; + for (; idx != idx_pos; idx = (idx_pos + 1) % num_motors) { + if (SRV_Channels::have_digital_outputs(1U << motor_map[idx])) { break; } } + uint16_t mask = 1U << motor_map[idx]; + if (SRV_Channels::have_digital_outputs(mask)) { + hal.rcout->set_telem_request_mask(mask); + last_telem_esc = idx; + last_telem_request_us = now; + } } }