diff --git a/core/system_impl.cpp b/core/system_impl.cpp
index 14061e61fe..c69cf9eeea 100644
--- a/core/system_impl.cpp
+++ b/core/system_impl.cpp
@@ -80,6 +80,21 @@ void SystemImpl::register_mavlink_message_handler(uint16_t msg_id,
     _iterator_invalidated = true;
 }
 
+void SystemImpl::unregister_mavlink_message_handler(uint16_t msg_id, const void *cookie)
+{
+    std::lock_guard<std::mutex> lock(_mavlink_handler_table_mutex);
+
+    for (auto it = _mavlink_handler_table.begin(); it != _mavlink_handler_table.end();
+         /* no ++it */) {
+        if (it->msg_id == msg_id && it->cookie == cookie) {
+            it = _mavlink_handler_table.erase(it);
+            _iterator_invalidated = true;
+        } else {
+            ++it;
+        }
+    }
+}
+
 void SystemImpl::unregister_all_mavlink_message_handlers(const void *cookie)
 {
     std::lock_guard<std::mutex> lock(_mavlink_handler_table_mutex);
diff --git a/core/system_impl.h b/core/system_impl.h
index 49d9e8f720..5f55d3c7eb 100644
--- a/core/system_impl.h
+++ b/core/system_impl.h
@@ -51,6 +51,7 @@ class SystemImpl {
                                           mavlink_message_handler_t callback,
                                           const void *cookie);
 
+    void unregister_mavlink_message_handler(uint16_t msg_id, const void *cookie);
     void unregister_all_mavlink_message_handlers(const void *cookie);
 
     void register_timeout_handler(std::function<void()> callback, double duration_s, void **cookie);
diff --git a/integration_tests/mavlink_passthrough.cpp b/integration_tests/mavlink_passthrough.cpp
index 04e8c5a7df..d85df9b477 100644
--- a/integration_tests/mavlink_passthrough.cpp
+++ b/integration_tests/mavlink_passthrough.cpp
@@ -49,15 +49,25 @@ TEST_F(SitlTest, MavlinkPassthrough)
         std::promise<void> prom;
         std::future<void> fut = prom.get_future();
         unsigned counter = 0;
+        bool stopped = false;
 
         mavlink_passthrough->subscribe_message_async(
-            MAVLINK_MSG_ID_HIGHRES_IMU, [&prom, &counter](const mavlink_message_t &message) {
+            MAVLINK_MSG_ID_HIGHRES_IMU,
+            [&prom, &counter, &stopped, mavlink_passthrough](const mavlink_message_t &message) {
                 mavlink_highres_imu_t highres_imu;
                 mavlink_msg_highres_imu_decode(&message, &highres_imu);
 
-                LogInfo() << "HIGHRES_IMU.temperature: " << highres_imu.temperature << " degrees C";
+                LogInfo() << "HIGHRES_IMU.temperature [1] (" << counter << ")"
+                          << highres_imu.temperature << " degrees C";
                 if (++counter > 100) {
-                    prom.set_value();
+                    EXPECT_FALSE(stopped);
+                    if (!stopped) {
+                        stopped = true;
+                        // Unsubscribe again
+                        mavlink_passthrough->subscribe_message_async(MAVLINK_MSG_ID_HIGHRES_IMU,
+                                                                     nullptr);
+                        prom.set_value();
+                    }
                 };
             });
 
diff --git a/plugins/mavlink_passthrough/mavlink_passthrough_impl.cpp b/plugins/mavlink_passthrough/mavlink_passthrough_impl.cpp
index 160bc2dedc..57394d0a7c 100644
--- a/plugins/mavlink_passthrough/mavlink_passthrough_impl.cpp
+++ b/plugins/mavlink_passthrough/mavlink_passthrough_impl.cpp
@@ -38,7 +38,17 @@ MavlinkPassthrough::Result MavlinkPassthroughImpl::send_message(mavlink_message_
 void MavlinkPassthroughImpl::subscribe_message_async(
     uint16_t message_id, std::function<void(const mavlink_message_t &)> callback)
 {
-    _parent->register_mavlink_message_handler(message_id, callback, this);
+    if (callback == nullptr) {
+        _parent->unregister_mavlink_message_handler(message_id, this);
+    } else {
+        auto temp_callback = callback;
+        _parent->register_mavlink_message_handler(
+            message_id,
+            [this, temp_callback](const mavlink_message_t &message) {
+                _parent->call_user_callback([temp_callback, message]() { temp_callback(message); });
+            },
+            this);
+    }
 }
 
 uint8_t MavlinkPassthroughImpl::get_our_sysid() const