diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 3c868b9be9..9ab591f997 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -163,7 +163,10 @@ Mavlink::~Mavlink()
 	}
 
 	if (_instance_id >= 0) {
-		mavlink_module_instances[_instance_id] = nullptr;
+		{
+			LockGuard lg{mavlink_module_mutex};
+			mavlink_module_instances[_instance_id] = nullptr;
+		}
 		mavlink_instance_count.fetch_sub(1);
 	}
 
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index c913909657..4bccb35b52 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -3160,7 +3160,7 @@ MavlinkReceiver::run()
 	ssize_t nread = 0;
 	hrt_abstime last_send_update = 0;
 
-	while (!_mavlink.should_exit()) {
+	while (!_should_exit.load()) {
 
 		// check for parameter updates
 		if (_parameter_update_sub.updated()) {
diff --git a/test/mavsdk_tests/test_multicopter_basics.cpp b/test/mavsdk_tests/test_multicopter_basics.cpp
index 52b933ff17..427e69c03f 100644
--- a/test/mavsdk_tests/test_multicopter_basics.cpp
+++ b/test/mavsdk_tests/test_multicopter_basics.cpp
@@ -40,7 +40,7 @@
 TEST_CASE("Takeoff and hold position", "[multicopter][vtol]")
 {
 	const float takeoff_altitude = 10.f;
-	const float altitude_tolerance = 0.1f;
+	const float altitude_tolerance = 0.2f;
 	const int delay_seconds = 60.f;
 
 	AutopilotTester tester;