AP_ESC_TELEM: solved the logical error in grouping of escs into 4

adityaomar3 · Apr 22, 2024 · 6f61934 · 6f61934
1 parent 8125f47
commit 6f61934
Showing 1 changed file with 5 additions and 2 deletions.
diff --git 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;