From f67eb6989d78c0c093fc934ae8a1c2b998846006 Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Fri, 16 Aug 2024 09:42:48 +0300 Subject: [PATCH] mavlink: Fix ESC_STATUS sending for batches > 1 The indexing was wrong for esc_status sending for ESCs 4-> Signed-off-by: Jukka Laitinen --- src/modules/mavlink/streams/ESC_STATUS.hpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/streams/ESC_STATUS.hpp b/src/modules/mavlink/streams/ESC_STATUS.hpp index 1e8911577823..54c11cbd8ebb 100644 --- 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);