Merge pull request #2189 from c-devine/pr-mavlink-passthrough-unsubsc…

…ribe Update to MavlinkPassthrough unsubscribe_message
mavlink · Dec 3, 2023 · 1560060 · 1560060
2 parents 9a6416b + ae676cf
commit 1560060
Show file tree

Hide file tree

Showing 5 changed files with 14 additions and 10 deletions.
diff --git a/examples/gimbal_device_tester/gimbal_device_tester.cpp b/examples/gimbal_device_tester/gimbal_device_tester.cpp
@@ -721,7 +721,8 @@ bool test_device_information(MavlinkPassthrough& mavlink_passthrough, AttitudeDa
                 });
 
             // We only need it once.
-            mavlink_passthrough.unsubscribe_message(handle);
+            mavlink_passthrough.unsubscribe_message(
+                MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION, handle);
             prom.set_value();
         });
 

diff --git a/...sdk/plugins/mavlink_passthrough/include/plugins/mavlink_passthrough/mavlink_passthrough.h b/...sdk/plugins/mavlink_passthrough/include/plugins/mavlink_passthrough/mavlink_passthrough.h
@@ -215,8 +215,11 @@ class MavlinkPassthrough : public PluginBase {
 
     /**
      * @brief Unsubscribe from subscribe_message.
+     *
+     * @param message_id The MAVLink message ID.
+     * @param handle The handle returned from subscribe_message.
      */
-    void unsubscribe_message(MessageHandle handle);
+    void unsubscribe_message(uint16_t message_id, MessageHandle handle);
 
     /**
      * @brief Get our own system ID.

diff --git a/src/mavsdk/plugins/mavlink_passthrough/mavlink_passthrough.cpp b/src/mavsdk/plugins/mavlink_passthrough/mavlink_passthrough.cpp
@@ -63,9 +63,9 @@ MavlinkPassthrough::subscribe_message(uint16_t message_id, const MessageCallback
     return _impl->subscribe_message(message_id, callback);
 }
 
-void MavlinkPassthrough::unsubscribe_message(MessageHandle handle)
+void MavlinkPassthrough::unsubscribe_message(uint16_t message_id, MessageHandle handle)
 {
-    _impl->unsubscribe_message(handle);
+    _impl->unsubscribe_message(message_id, handle);
 }
 
 std::ostream& operator<<(std::ostream& str, MavlinkPassthrough::Result const& result)

diff --git a/src/mavsdk/plugins/mavlink_passthrough/mavlink_passthrough_impl.cpp b/src/mavsdk/plugins/mavlink_passthrough/mavlink_passthrough_impl.cpp
@@ -207,12 +207,12 @@ MavlinkPassthrough::MessageHandle MavlinkPassthroughImpl::subscribe_message(
     return _message_subscriptions[message_id].subscribe(callback);
 }
 
-void MavlinkPassthroughImpl::unsubscribe_message(MavlinkPassthrough::MessageHandle handle)
+void MavlinkPassthroughImpl::unsubscribe_message(
+    uint16_t message_id, MavlinkPassthrough::MessageHandle handle)
 {
-    // We don't know which subscription holds the handle, so we have to go
-    // through all of them.
-    for (auto& subscription : _message_subscriptions) {
-        subscription.second.unsubscribe(handle);
+    auto it = _message_subscriptions.find(message_id);
+    if (it != _message_subscriptions.end()) {
+        it->second.unsubscribe(handle);
     }
 }
 

diff --git a/src/mavsdk/plugins/mavlink_passthrough/mavlink_passthrough_impl.h b/src/mavsdk/plugins/mavlink_passthrough/mavlink_passthrough_impl.h
@@ -40,7 +40,7 @@ class MavlinkPassthroughImpl : public PluginImplBase {
     MavlinkPassthrough::MessageHandle
     subscribe_message(uint16_t message_id, const MavlinkPassthrough::MessageCallback& callback);
 
-    void unsubscribe_message(MavlinkPassthrough::MessageHandle handle);
+    void unsubscribe_message(uint16_t message_id, MavlinkPassthrough::MessageHandle handle);
 
     uint8_t get_our_sysid() const;
     uint8_t get_our_compid() const;