DAER/visualisation-marsh
1
0
Fork 0
Code Issues Pull requests Projects Releases 1 Packages Wiki Activity Actions

Change component identification to HEARTBEAT.type

Browse source
This commit is contained in:
Marek S. Łukasiewicz 2025-05-27 13:47:53 +02:00
parent 0514c64f02
commit 48154002cc
3 changed files with 18 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

24
src/marshconnector.cpp
View file

@ -56,6 +56,10 @@ MarshConnector::MarshConnector() {
// Initialize member variables
time_passed = 0.0;
system_id = 1;
component_id =
MAV_COMP_ID_USER1 + (MARSH_TYPE_VISUALISATION - MARSH_TYPE_MANAGER);
heartbeat_timer = memnew(Timer);
add_child(heartbeat_timer);
heartbeat_timer->set_wait_time(1.0);
@ -121,15 +125,15 @@ Error MarshConnector::send_heartbeat() {
// print_line("Sending HEARTBEAT at ", time_passed, " seconds");
mavlink_heartbeat_t heartbeat;
heartbeat.type = MAV_TYPE_HELICOPTER;
heartbeat.type = MARSH_TYPE_VISUALISATION;
heartbeat.autopilot = MAV_AUTOPILOT_INVALID;
heartbeat.base_mode = 0; // none of the flags applicable
heartbeat.custom_mode = 0; // not used so far
heartbeat.system_status = MAV_STATE_ACTIVE;
mavlink_message_t message;
mavlink_msg_heartbeat_encode_chan(1, MARSH_COMP_ID_VISUALISATION,
MAVLINK_COMM_0, &message, &heartbeat);
mavlink_msg_heartbeat_encode_chan(system_id, component_id, MAVLINK_COMM_0,
&message, &heartbeat);
return send_message(message);
}
@ -336,16 +340,17 @@ void MarshConnector::handle_heartbeat(mavlink_message_t message) {
if (message.msgid != MAVLINK_MSG_ID_HEARTBEAT)
return;
// mavlink_heartbeat_t heartbeat;
// mavlink_msg_heartbeat_decode(&message, &heartbeat);
if (message.compid == MARSH_COMP_ID_MANAGER) {
mavlink_heartbeat_t heartbeat;
mavlink_msg_heartbeat_decode(&message, &heartbeat);
if (heartbeat.type == MARSH_TYPE_MANAGER) {
if (!manager_connected) {
print_line("Connected to MARSH Manager");
// subscribe to messages not sent to visualisation node by default
mavlink_command_long_t command;
command.target_system = 1;
command.target_component = MARSH_COMP_ID_MANAGER;
command.target_component =
MAV_COMP_ID_USER1; // HACK: Should get it from received HEARTBEAT
command.command = MAV_CMD_SET_MESSAGE_INTERVAL;
command.confirmation = 0;
command.param1 = static_cast<float>(MAVLINK_MSG_ID_MANUAL_CONTROL);
@ -357,9 +362,8 @@ void MarshConnector::handle_heartbeat(mavlink_message_t message) {
command.param7 = 1; // Address of requestor
mavlink_message_t message_sent;
mavlink_msg_command_long_encode_chan(1, MARSH_COMP_ID_VISUALISATION,
MAVLINK_COMM_0, &message_sent,
&command);
mavlink_msg_command_long_encode_chan(
system_id, component_id, MAVLINK_COMM_0, &message_sent, &command);
Error result = send_message(message_sent);
if (result != OK) {
print_line("Subscribe send result ", result);

3
src/marshconnector.h
View file

@ -103,6 +103,9 @@ private:
"LOCAL_FRAME_LAT", "LOCAL_FRAME_LON",
};
uint8_t system_id;
uint8_t component_id;
// TODO: Interpolate with some delay
Vector3 last_location;
Quaternion last_rotation;