From 28488cbfd6aa896a2c3bab28f1ccba35f62aeb96 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
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