aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SIM_Vicon: correct heartbeating

Browse Source 
the heartbeat was packed but never sent
This commit is contained in:
Peter Barker 2024-03-07 13:38:01 +11:00 committed by Andrew Tridgell
parent 298ef64836
commit 28488cbfd6
1 changed files with 20 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/SITL/SIM_Vicon.cpp
View File

@ -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