From f3d38c756901bb7d13719d8ae0a5bfffd57606a1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marek=20S=2E=20=C5=81ukasiewicz?= Date: Sat, 13 Dec 2025 21:44:41 +0100 Subject: [PATCH] Minor MAVLink fixes --- src/marshconnector.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/marshconnector.cpp b/src/marshconnector.cpp index 3f66679..4d348c4 100644 --- a/src/marshconnector.cpp +++ b/src/marshconnector.cpp @@ -26,6 +26,7 @@ #include "mavlink/mavlink_types.h" #include #include +#include using namespace godot; @@ -403,10 +404,11 @@ Error MarshConnector::send_param(Parameter index) { } mavlink_param_value_t param_value; - memset(param_value.param_id, 0, sizeof(param_value.param_id)); CharString nameBuffer = parameter_names[index].ascii(); const char *name = nameBuffer.get_data(); - strcpy_s(param_value.param_id, name); + + // strncpy will write nulls after data until reaching target count + strncpy(param_value.param_id, name, sizeof(param_value.param_id)); param_value.param_value = parameters[index]; param_value.param_type = MAV_PARAM_TYPE_REAL32; param_value.param_count = PARAM_COUNT; @@ -447,9 +449,8 @@ void MarshConnector::handle_heartbeat(mavlink_message_t message) { // subscribe to messages not sent to visualisation node by default mavlink_command_long_t command; - command.target_system = 1; - command.target_component = - MAV_COMP_ID_USER1; // HACK: Should get it from received HEARTBEAT + command.target_system = message.sysid; + command.target_component = message.compid; command.command = MAV_CMD_SET_MESSAGE_INTERVAL; command.confirmation = 0; command.param1 = static_cast(MAVLINK_MSG_ID_MANUAL_CONTROL); @@ -458,7 +459,7 @@ void MarshConnector::handle_heartbeat(mavlink_message_t message) { command.param4 = 0; command.param5 = 0; command.param6 = 0; - command.param7 = 1; // Address of requestor + command.param7 = 1; // Address of requester mavlink_message_t message_sent; mavlink_msg_command_long_encode_chan(