fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 12:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

QURT: HITL: Moved heartbeat send into actuator send thread to avoid race conditions

Browse Source
This commit is contained in:
Eric Katzfey
2025-08-05 17:16:05 -07:00
committed by Ramon Roche
parent 5b9e099fd1
commit 7e5cc020c3
1 changed files with 19 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp
+19 -17
View File
@@ -274,6 +274,7 @@ void send_actuator_data()
int _actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs_sim), 0);
int _vehicle_control_mode_sub_ = orb_subscribe(ORB_ID(vehicle_control_mode));
uint64_t last_heartbeat_timestamp = hrt_absolute_time();
int previous_timestamp = 0;
int previous_uorb_timestamp = 0;
int differential = 0;
@@ -331,6 +332,23 @@ void send_actuator_data()
send_esc_status(hil_act_control);
}
uint64_t timestamp = hrt_absolute_time();
if ((timestamp - last_heartbeat_timestamp) > 1000000) {
mavlink_heartbeat_t hb = {};
mavlink_message_t hb_message = {};
hb.autopilot = 12;
hb.base_mode |= (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) ? 128 : 0;
mavlink_msg_heartbeat_encode(1, 1, &hb_message, &hb);
uint8_t hb_newBuf[MAVLINK_MAX_PACKET_LEN];
uint16_t hb_newBufLen = 0;
hb_newBufLen = mavlink_msg_to_send_buffer(hb_newBuf, &hb_message);
(void) writeResponse(&hb_newBuf, hb_newBufLen);
last_heartbeat_timestamp = timestamp;
heartbeat_sent_counter++;
}
differential = hrt_absolute_time() - previous_timestamp;
px4_usleep(1000);
@@ -392,8 +410,7 @@ void task_main(int argc, char *argv[])
return;
}
uint64_t last_heartbeat_timestamp = hrt_absolute_time();
uint64_t last_imu_update_timestamp = last_heartbeat_timestamp;
uint64_t last_imu_update_timestamp = hrt_absolute_time();
_px4_accel = new PX4Accelerometer(1310988);
_px4_gyro = new PX4Gyroscope(1310988);
@@ -447,21 +464,6 @@ void task_main(int argc, char *argv[])
}
}
if ((timestamp - last_heartbeat_timestamp) > 1000000) {
mavlink_heartbeat_t hb = {};
mavlink_message_t hb_message = {};
hb.autopilot = 12;
hb.base_mode |= (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) ? 128 : 0;
mavlink_msg_heartbeat_encode(1, 1, &hb_message, &hb);
uint8_t hb_newBuf[MAVLINK_MAX_PACKET_LEN];
uint16_t hb_newBufLen = 0;
hb_newBufLen = mavlink_msg_to_send_buffer(hb_newBuf, &hb_message);
(void) writeResponse(&hb_newBuf, hb_newBufLen);
last_heartbeat_timestamp = timestamp;
heartbeat_sent_counter++;
}
bool vehicle_updated = false;
(void) orb_check(_vehicle_status_sub, &vehicle_updated);