From 01888a3085533cdbebc8fd06bc542a0141749744 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 11 Oct 2024 09:03:22 +1300
Subject: [PATCH] mavlink: fix SET_MESSAGE_INTERVAL parsing (#23796)

This fixes the SITL tests that fail in CI because we catch NAN as non
zero after cast to int. To fix this I've added the check whether they
are finite at all.

The checks for param5 and param6 would be a bit trickier because they
can be int or float, so I have omitted them for now.
---
 src/modules/mavlink/mavlink_receiver.cpp | 21 +++++++++++++++------
 src/modules/mavlink/mavlink_receiver.h   |  3 +--
 2 files changed, 16 insertions(+), 8 deletions(-)

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index f655a5d7d9..b573e36fec 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -593,8 +593,8 @@ 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)(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))) {
+		if (set_message_interval(
+			    (int)(cmd_mavlink.param1 + 0.5f), cmd_mavlink.param2, cmd_mavlink.param3, cmd_mavlink.param4, vehicle_command.param7)) {
 			result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED;
 		}
 
@@ -2274,15 +2274,24 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
 }
 
 int
-MavlinkReceiver::set_message_interval(int msgId, float interval, float param3, float param4, int param5, int param6,
-				      int response_target)
+MavlinkReceiver::set_message_interval(int msgId, float interval, float param3, float param4, float param7)
 {
 	if (msgId == MAVLINK_MSG_ID_HEARTBEAT) {
 		return PX4_ERROR;
 	}
 
-	if ((int)(param3 + 0.5f) || (int)(param4 + 0.5f) || param5 || param6 || response_target) {
-		// At least one of the unsupported params is non-zero
+	if (PX4_ISFINITE(param3) && (int)(param3 + 0.5f) != 0) {
+		PX4_ERR("SET_MESSAGE_INTERVAL requested param3 not supported.");
+		return PX4_ERROR;
+	}
+
+	if (PX4_ISFINITE(param4) && (int)(param4 + 0.5f) != 0) {
+		PX4_ERR("SET_MESSAGE_INTERVAL requested param4 not supported.");
+		return PX4_ERROR;
+	}
+
+	if (PX4_ISFINITE(param7) && (int)(param7 + 0.5f) != 0) {
+		PX4_ERR("SET_MESSAGE_INTERVAL response target not supported.");
 		return PX4_ERROR;
 	}
 
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index d6ed4b2f82..040c7d314e 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -233,8 +233,7 @@ private:
 	 *
 	 * @return PX4_OK on success, PX4_ERROR on fail.
 	 */
-	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);
+	int set_message_interval(int msgId, float interval, float param3, float param4, float param7);
 	void get_message_interval(int msgId);
 
 	bool evaluate_target_ok(int command, int target_system, int target_component);