mavlink: fix SET_MESSAGE_INTERVAL parsing (PX4#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.
vanttec · Oct 10, 2024 · 01888a3 · 01888a3
1 parent 9557a2d
commit 01888a3
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 8 deletions.
diff --git 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
@@ -233,8 +233,7 @@ class MavlinkReceiver : public ModuleParams
 	 *
 	 * @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);