From 66b9e60a49994a06b278f5715da0a3c5c02f11f1 Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Wed, 9 Oct 2024 19:08:49 +1100 Subject: [PATCH] MAV_CMD_SET_MESSAGE_INTERVAL reject non-zero values for unused params (#23643) * MAV_CMD_SET_MESSAGE_INTERVAL reject non-zero values for unused params * Remove data rate * Replace rounding with addition of 0.5/0.5f --- src/modules/mavlink/mavlink_receiver.cpp | 13 ++++++++----- src/modules/mavlink/mavlink_receiver.h | 4 ++-- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index ab283812e5..f655a5d7d9 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -593,12 +593,13 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const result = handle_request_message_command(MAVLINK_MSG_ID_STORAGE_INFORMATION); } else if (cmd_mavlink.command == MAV_CMD_SET_MESSAGE_INTERVAL) { - if (set_message_interval((int)roundf(cmd_mavlink.param1), cmd_mavlink.param2, cmd_mavlink.param3)) { + if (set_message_interval((int)(cmd_mavlink.param1 + 0.5f), cmd_mavlink.param2, cmd_mavlink.param3, cmd_mavlink.param4, + (int)(vehicle_command.param5 + 0.5), (int)(vehicle_command.param6 + 0.5), (int)(vehicle_command.param7 + 0.5f))) { result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED; } } else if (cmd_mavlink.command == MAV_CMD_GET_MESSAGE_INTERVAL) { - get_message_interval((int)roundf(cmd_mavlink.param1)); + get_message_interval((int)(cmd_mavlink.param1 + 0.5f)); } else if (cmd_mavlink.command == MAV_CMD_REQUEST_MESSAGE) { @@ -2273,14 +2274,16 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) } int -MavlinkReceiver::set_message_interval(int msgId, float interval, int data_rate) +MavlinkReceiver::set_message_interval(int msgId, float interval, float param3, float param4, int param5, int param6, + int response_target) { if (msgId == MAVLINK_MSG_ID_HEARTBEAT) { return PX4_ERROR; } - if (data_rate > 0) { - _mavlink.set_data_rate(data_rate); + if ((int)(param3 + 0.5f) || (int)(param4 + 0.5f) || param5 || param6 || response_target) { + // At least one of the unsupported params is non-zero + return PX4_ERROR; } // configure_stream wants a rate (msgs/second), so convert here. diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 39fe7c33f9..d6ed4b2f82 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -230,11 +230,11 @@ private: * * @param msgId The ID of the message interval to be set. * @param interval The interval in usec to send the message. - * @param data_rate The total link data rate in bytes per second. * * @return PX4_OK on success, PX4_ERROR on fail. */ - int set_message_interval(int msgId, float interval, int data_rate = -1); + int set_message_interval(int msgId, float interval, float param3 = 0.0f, float param4 = 0.0f, int param5 = 0, + int param6 = 0, int response_target = 0); void get_message_interval(int msgId); bool evaluate_target_ok(int command, int target_system, int target_component);