diff --git a/test/mavsdk_tests/CMakeLists.txt b/test/mavsdk_tests/CMakeLists.txt
index 3b4be475e3..ab115ce187 100644
--- a/test/mavsdk_tests/CMakeLists.txt
+++ b/test/mavsdk_tests/CMakeLists.txt
@@ -36,6 +36,8 @@ if(MAVSDK_FOUND)
         ${CMAKE_THREAD_LIBS_INIT}
     )
 
+    target_include_directories(mavsdk_tests PUBLIC ${CMAKE_BINARY_DIR}/..)
+
     target_compile_options(mavsdk_tests
         PRIVATE
             -Wall
diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp
index 5811d796d4..305842c3b4 100644
--- a/test/mavsdk_tests/autopilot_tester.cpp
+++ b/test/mavsdk_tests/autopilot_tester.cpp
@@ -76,6 +76,7 @@ void AutopilotTester::connect(const std::string uri)
 	_offboard.reset(new Offboard(system));
 	_param.reset(new Param(system));
 	_telemetry.reset(new Telemetry(system));
+	_mavlink_passthrough.reset(new MavlinkPassthrough(system));
 }
 
 void AutopilotTester::wait_until_ready()
@@ -643,6 +644,22 @@ void AutopilotTester::execute_rtl_when_reaching_mission_sequence(int sequence_nu
 	execute_rtl();
 }
 
+void AutopilotTester::send_custom_mavlink_command(const MavlinkPassthrough::CommandInt &command)
+{
+	_mavlink_passthrough->send_command_int(command);
+}
+
+void AutopilotTester::send_custom_mavlink_message(mavlink_message_t &message)
+{
+	_mavlink_passthrough->send_message(message);
+}
+
+void AutopilotTester::add_mavlink_message_callback(uint16_t message_id,
+		std::function< void(const mavlink_message_t &)> callback)
+{
+	_mavlink_passthrough->subscribe_message_async(message_id, std::move(callback));
+}
+
 std::array<float, 3> AutopilotTester::get_current_position_ned()
 {
 	mavsdk::Telemetry::PositionVelocityNed position_velocity_ned = _telemetry->position_velocity_ned();
diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h
index 22a9225628..3e2ebaae72 100644
--- a/test/mavsdk_tests/autopilot_tester.h
+++ b/test/mavsdk_tests/autopilot_tester.h
@@ -39,6 +39,7 @@
 #include <mavsdk/plugins/failure/failure.h>
 #include <mavsdk/plugins/info/info.h>
 #include <mavsdk/plugins/manual_control/manual_control.h>
+#include <mavsdk/plugins/mavlink_passthrough/mavlink_passthrough.h>
 #include <mavsdk/plugins/mission/mission.h>
 #include <mavsdk/plugins/mission_raw/mission_raw.h>
 #include <mavsdk/plugins/offboard/offboard.h>
@@ -48,6 +49,7 @@
 #include <atomic>
 #include <chrono>
 #include <ctime>
+#include <functional>
 #include <iostream>
 #include <memory>
 #include <optional>
@@ -143,6 +145,9 @@ public:
 	void stop_checking_altitude();
 	void check_current_altitude(float target_rel_altitude_m, float max_distance_m = 1.5f);
 	void execute_rtl_when_reaching_mission_sequence(int sequence_number);
+	void send_custom_mavlink_command(const MavlinkPassthrough::CommandInt &command);
+	void send_custom_mavlink_message(mavlink_message_t &message);
+	void add_mavlink_message_callback(uint16_t message_id, std::function< void(const mavlink_message_t &)> callback);
 
 	// Blocking call to get the drone's current position in NED frame
 	std::array<float, 3> get_current_position_ned();
@@ -156,8 +161,11 @@ protected:
 	mavsdk::Param *getParams() const { return _param.get();}
 	mavsdk::Telemetry *getTelemetry() const { return _telemetry.get();}
 	mavsdk::ManualControl *getManualControl() const { return _manual_control.get();}
+	MavlinkPassthrough *getMavlinkPassthrough() const { return _mavlink_passthrough.get();}
 	std::shared_ptr<System> get_system() { return _mavsdk.systems().at(0);}
-	Telemetry::GroundTruth getHome()
+	mavsdk::geometry::CoordinateTransformation get_coordinate_transformation();
+
+	const Telemetry::GroundTruth &getHome()
 	{
 		// Check if home was stored before it is accessed
 		CHECK(_home.absolute_altitude_m != NAN);
@@ -192,7 +200,6 @@ protected:
 	}
 
 private:
-	mavsdk::geometry::CoordinateTransformation get_coordinate_transformation();
 	mavsdk::Mission::MissionItem create_mission_item(
 		const mavsdk::geometry::CoordinateTransformation::LocalCoordinate &local_coordinate,
 		const MissionOptions &mission_options,
@@ -274,6 +281,7 @@ private:
 	std::unique_ptr<mavsdk::Failure> _failure{};
 	std::unique_ptr<mavsdk::Info> _info{};
 	std::unique_ptr<mavsdk::ManualControl> _manual_control{};
+	std::unique_ptr<MavlinkPassthrough> _mavlink_passthrough;
 	std::unique_ptr<mavsdk::Mission> _mission{};
 	std::unique_ptr<mavsdk::MissionRaw> _mission_raw{};
 	std::unique_ptr<mavsdk::Offboard> _offboard{};