From 28488cbfd6aa896a2c3bab28f1ccba35f62aeb96 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 7 Mar 2024 13:38:01 +1100 Subject: [PATCH] SIM_Vicon: correct heartbeating the heartbeat was packed but never sent --- libraries/SITL/SIM_Vicon.cpp | 30 ++++++++++++++++++++---------- 1 file changed, 20 insertions(+), 10 deletions(-) diff --git a/libraries/SITL/SIM_Vicon.cpp b/libraries/SITL/SIM_Vicon.cpp index f447e52758..6b04a81eb6 100644 --- a/libraries/SITL/SIM_Vicon.cpp +++ b/libraries/SITL/SIM_Vicon.cpp @@ -37,21 +37,31 @@ void Vicon::maybe_send_heartbeat() { const uint32_t now = AP_HAL::millis(); - if (now - last_heartbeat_ms < 100) { + if (now - last_heartbeat_ms < 500) { // we only provide a heartbeat every so often return; } + + uint8_t msg_buf_index; + if (!get_free_msg_buf_index(msg_buf_index)) { + return; + } + last_heartbeat_ms = now; - mavlink_message_t msg; - mavlink_msg_heartbeat_pack(system_id, - component_id, - &msg, - MAV_TYPE_GCS, - MAV_AUTOPILOT_INVALID, - 0, - 0, - 0); + const mavlink_heartbeat_t heartbeat{ + type : MAV_TYPE_GCS, + autopilot : MAV_AUTOPILOT_INVALID, + }; + + mavlink_msg_heartbeat_encode_status( + system_id, + component_id, + &mav_status, + &msg_buf[msg_buf_index].obs_msg, + &heartbeat + ); + msg_buf[msg_buf_index].time_send_us = AP_HAL::millis(); } // get unused index in msg_buf