mavlink: Fix ESC_STATUS sending for batches > 1

The indexing was wrong for esc_status sending for ESCs 4-> Signed-off-by: Jukka Laitinen <[email protected]>
tiiuae · Aug 26, 2024 · f67eb69 · f67eb69
1 parent ca47f6f
commit f67eb69
Showing 1 changed file with 5 additions and 4 deletions.
diff --git a/src/modules/mavlink/streams/ESC_STATUS.hpp b/src/modules/mavlink/streams/ESC_STATUS.hpp
@@ -75,10 +75,11 @@ class MavlinkStreamESCStatus : public MavlinkStream
 			for (int batch_number = 0; batch_number < _number_of_batches; batch_number++) {
 				msg.index = batch_number * batch_size;
 
-				for (int esc_index = 0; esc_index < batch_size ; esc_index++) {
-					msg.rpm[esc_index] = esc_status.esc[esc_index].esc_rpm;
-					msg.voltage[esc_index] = esc_status.esc[esc_index].esc_voltage;
-					msg.current[esc_index] = esc_status.esc[esc_index].esc_current;
+				for (int esc_index = 0; esc_index < batch_size
+				     && msg.index + esc_index < esc_status_s::CONNECTED_ESC_MAX; esc_index++) {
+					msg.rpm[esc_index] = esc_status.esc[msg.index + esc_index].esc_rpm;
+					msg.voltage[esc_index] = esc_status.esc[msg.index + esc_index].esc_voltage;
+					msg.current[esc_index] = esc_status.esc[msg.index + esc_index].esc_current;
 				}
 
 				mavlink_msg_esc_status_send_struct(_mavlink->get_channel(), &msg);