diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 2fbd96e51c..008beaeb52 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -430,34 +430,7 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
 		get_message_interval((int)roundf(cmd_mavlink.param1));
 
 	} else if (cmd_mavlink.command == MAV_CMD_REQUEST_MESSAGE) {
-		uint16_t message_id = (uint16_t)roundf(cmd_mavlink.param1);
-		bool stream_found = false;
-
-		for (const auto stream : _mavlink->get_streams()) {
-			if (stream->get_id() == message_id) {
-				stream_found = stream->request_message(vehicle_command.param1, vehicle_command.param2, vehicle_command.param3,
-								       vehicle_command.param4, vehicle_command.param5, vehicle_command.param6, vehicle_command.param7);
-				break;
-			}
-		}
-
-		if (!stream_found) {
-			MavlinkStream *stream = create_mavlink_stream(message_id, _mavlink);
-
-			if (stream == nullptr) {
-				result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
-
-			} else {
-				bool message_sent = stream->request_message(vehicle_command.param1, vehicle_command.param2, vehicle_command.param3,
-						    vehicle_command.param4, vehicle_command.param5, vehicle_command.param6, vehicle_command.param7);
-
-				if (!message_sent) {
-					result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
-				}
-
-				delete stream;
-			}
-		}
+		result = handle_request_message_command(vehicle_command);
 
 	} else if (cmd_mavlink.command == MAV_CMD_SET_CAMERA_ZOOM) {
 		struct actuator_controls_s actuator_controls = {};
@@ -521,6 +494,43 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
 	}
 }
 
+uint8_t MavlinkReceiver::handle_request_message_command(const vehicle_command_s &vehicle_command)
+{
+	uint16_t message_id = (uint16_t)roundf(vehicle_command.param1);
+	bool stream_found = false;
+
+	uint8_t result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
+
+	for (const auto stream : _mavlink->get_streams()) {
+		if (stream->get_id() == message_id) {
+			stream_found = stream->request_message(vehicle_command.param1, vehicle_command.param2, vehicle_command.param3,
+							       vehicle_command.param4, vehicle_command.param5, vehicle_command.param6, vehicle_command.param7);
+			break;
+		}
+	}
+
+	if (!stream_found) {
+		MavlinkStream *stream = create_mavlink_stream(message_id, _mavlink);
+
+		if (stream == nullptr) {
+			result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
+
+		} else {
+			bool message_sent = stream->request_message(vehicle_command.param1, vehicle_command.param2, vehicle_command.param3,
+					    vehicle_command.param4, vehicle_command.param5, vehicle_command.param6, vehicle_command.param7);
+
+			if (!message_sent) {
+				result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
+			}
+
+			delete stream;
+		}
+	}
+
+	return result;
+}
+
+
 void
 MavlinkReceiver::handle_message_command_ack(mavlink_message_t *msg)
 {
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 475800aee8..d1f021d85d 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -129,6 +129,8 @@ private:
 	void handle_message_command_both(mavlink_message_t *msg, const T &cmd_mavlink,
 					 const vehicle_command_s &vehicle_command);
 
+	uint8_t handle_request_message_command(const vehicle_command_s &vehicle_command);
+
 	void handle_message(mavlink_message_t *msg);
 
 	void handle_message_adsb_vehicle(mavlink_message_t *msg);