From 9abcd6b7ca29015cb22a998f07c7d36c3055ce2b Mon Sep 17 00:00:00 2001 From: aditya Date: Sun, 1 Oct 2023 00:57:55 +0530 Subject: [PATCH] AP_ESC_TELEM: solved the logical error in grouping of escs into 4 --- libraries/AP_ESC_Telem/AP_ESC_Telem.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index a82a014ded..e6f3bd523c 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -318,8 +318,11 @@ void AP_ESC_Telem::send_esc_telemetry_mavlink(uint8_t mav_chan) const uint32_t now_us = AP_HAL::micros(); // loop through groups of 4 ESCs - const uint8_t esc_offset = constrain_int16(mavlink_offset, 0, ESC_TELEM_MAX_ESCS - 1); - const uint8_t num_idx = ESC_TELEM_MAX_ESCS / 4; + const uint8_t esc_offset = constrain_int16(mavlink_offset, 0, ESC_TELEM_MAX_ESCS-1); + + // ensure we send out partially-full groups: + const uint8_t num_idx = (ESC_TELEM_MAX_ESCS + 3) / 4; + for (uint8_t idx = 0; idx < num_idx; idx++) { const uint8_t i = (next_idx + idx) % num_idx;