Merge pull request #2381 from mavlink/pr-request-old-messages

Switch to new REQUEST_MESSAGE but fall back to deprecated request message commands if required

src/mavsdk/core/CMakeLists.txt

@@ -42,6 +42,7 @@ target_sources(mavsdk
     mavlink_parameter_subscription.cpp
     mavlink_parameter_helper.cpp
     mavlink_receiver.cpp
+    mavlink_request_message.cpp
     mavlink_request_message_handler.cpp
     mavlink_statustext_handler.cpp
     mavlink_message_handler.cpp
@@ -60,7 +61,6 @@ target_sources(mavsdk
     log.cpp
     cli_arg.cpp
     geometry.cpp
-    request_message.cpp
     mavsdk_time.cpp
     timesync.cpp
 )

src/mavsdk/core/mavlink_component_metadata.cpp

@@ -61,7 +61,7 @@ void MavlinkComponentMetadata::request_component(uint32_t compid)
     const std::lock_guard lg{_mavlink_components_mutex};
     if (_mavlink_components.find(compid) == _mavlink_components.end()) {
         _mavlink_components[compid] = MavlinkComponent{};
-        _system_impl.request_message().request(
+        _system_impl.mavlink_request_message().request(
             MAVLINK_MSG_ID_COMPONENT_METADATA, compid, [this](auto&& result, auto&& message) {
                 receive_component_metadata(result, message);
             });

src/mavsdk/core/mavlink_message_handler.cpp

@@ -17,51 +17,121 @@ MavlinkMessageHandler::MavlinkMessageHandler()
 void MavlinkMessageHandler::register_one(
     uint16_t msg_id, const Callback& callback, const void* cookie)
 {
-    std::lock_guard<std::mutex> lock(_mutex);
-
-    Entry entry = {msg_id, {}, callback, cookie};
-    _table.push_back(entry);
+    register_one_impl(msg_id, {}, callback, cookie);
 }
 
 void MavlinkMessageHandler::register_one_with_component_id(
     uint16_t msg_id, uint8_t component_id, const Callback& callback, const void* cookie)
 {
-    std::lock_guard<std::mutex> lock(_mutex);
+    register_one_impl(msg_id, {component_id}, callback, cookie);
+}
 
-    Entry entry = {msg_id, component_id, callback, cookie};
-    _table.push_back(entry);
+void MavlinkMessageHandler::register_one_impl(
+    uint16_t msg_id,
+    std::optional<uint8_t> maybe_component_id,
+    const Callback& callback,
+    const void* cookie)
+{
+    Entry entry = {msg_id, maybe_component_id, callback, cookie};
+
+    std::unique_lock<std::mutex> lock(_mutex, std::try_to_lock);
+    if (lock.owns_lock()) {
+        _table.push_back(entry);
+    } else {
+        std::lock_guard<std::mutex> register_later_lock(_register_later_mutex);
+        _register_later_table.push_back(entry);
+    }
 }
 
 void MavlinkMessageHandler::unregister_one(uint16_t msg_id, const void* cookie)
 {
-    std::lock_guard<std::mutex> lock(_mutex);
+    unregister_impl({msg_id}, cookie);
+}
 
-    for (auto it = _table.begin(); it != _table.end();
-         /* no ++it */) {
-        if (it->msg_id == msg_id && it->cookie == cookie) {
-            it = _table.erase(it);
-        } else {
-            ++it;
-        }
+void MavlinkMessageHandler::unregister_all(const void* cookie)
+{
+    unregister_impl({}, cookie);
+}
+
+void MavlinkMessageHandler::unregister_impl(
+    std::optional<uint16_t> maybe_msg_id, const void* cookie)
+{
+    std::unique_lock<std::mutex> lock(_mutex, std::try_to_lock);
+    if (lock.owns_lock()) {
+        _table.erase(
+            std::remove_if(
+                _table.begin(),
+                _table.end(),
+                [&](auto& entry) {
+                    if (maybe_msg_id) {
+                        return (entry.msg_id == maybe_msg_id.value() && entry.cookie == cookie);
+                    } else {
+                        return (entry.cookie == cookie);
+                    }
+                }),
+            _table.end());
+    } else {
+        std::lock_guard<std::mutex> unregister_later_lock(_unregister_later_mutex);
+        _unregister_later_table.push_back(UnregisterEntry{maybe_msg_id, cookie});
     }
 }
 
-void MavlinkMessageHandler::unregister_all(const void* cookie)
+void MavlinkMessageHandler::check_register_later()
 {
-    std::lock_guard<std::mutex> lock(_mutex);
+    std::lock_guard<std::mutex> _register_later_lock(_register_later_mutex);
+
+    // We could probably just grab the lock here, but it's safer not to
+    // acquire both locks to avoid deadlocks.
+    std::unique_lock<std::mutex> lock(_mutex, std::try_to_lock);
+    if (!lock.owns_lock()) {
+        // Try again later.
+        return;
+    }
 
-    for (auto it = _table.begin(); it != _table.end();
-         /* no ++it */) {
-        if (it->cookie == cookie) {
-            it = _table.erase(it);
-        } else {
-            ++it;
-        }
+    for (const auto& entry : _register_later_table) {
+        _table.push_back(entry);
+    }
+
+    _register_later_table.clear();
+}
+
+void MavlinkMessageHandler::check_unregister_later()
+{
+    std::lock_guard<std::mutex> _unregister_later_lock(_unregister_later_mutex);
+
+    // We could probably just grab the lock here, but it's safer not to
+    // acquire both locks to avoid deadlocks.
+    std::unique_lock<std::mutex> lock(_mutex, std::try_to_lock);
+    if (!lock.owns_lock()) {
+        // Try again later.
+        return;
+    }
+
+    for (const auto& unregister_entry : _unregister_later_table) {
+        _table.erase(
+            std::remove_if(
+                _table.begin(),
+                _table.end(),
+                [&](auto& entry) {
+                    if (unregister_entry.maybe_msg_id) {
+                        return (
+                            entry.msg_id == unregister_entry.maybe_msg_id.value() &&
+                            entry.cookie == unregister_entry.cookie);
+                    } else {
+                        return (entry.cookie == unregister_entry.cookie);
+                    }
+                }),
+            _table.end());
     }
+
+    _unregister_later_table.clear();
 }
 
 void MavlinkMessageHandler::process_message(const mavlink_message_t& message)
 {
+    check_register_later();
+    check_unregister_later();
+
     std::lock_guard<std::mutex> lock(_mutex);
 
     bool forwarded = false;
@@ -97,6 +167,9 @@ void MavlinkMessageHandler::process_message(const mavlink_message_t& message)
 void MavlinkMessageHandler::update_component_id(
     uint16_t msg_id, uint8_t component_id, const void* cookie)
 {
+    check_register_later();
+    check_unregister_later();
+
     std::lock_guard<std::mutex> lock(_mutex);
 
     for (auto& entry : _table) {

src/mavsdk/core/mavlink_message_handler.h

@@ -31,9 +31,31 @@ class MavlinkMessageHandler {
     void update_component_id(uint16_t msg_id, uint8_t cmp_id, const void* cookie);
 
 private:
+    void register_one_impl(
+        uint16_t msg_id,
+        std::optional<uint8_t> maybe_component_id,
+        const Callback& callback,
+        const void* cookie);
+
+    void unregister_impl(std::optional<uint16_t> maybe_msg_id, const void* cookie);
+
+    void check_register_later();
+    void check_unregister_later();
+
     std::mutex _mutex{};
     std::vector<Entry> _table{};
 
+    std::mutex _register_later_mutex{};
+    std::vector<Entry> _register_later_table{};
+
+    struct UnregisterEntry {
+        std::optional<uint16_t> maybe_msg_id;
+        const void* cookie;
+    };
+
+    std::mutex _unregister_later_mutex{};
+    std::vector<UnregisterEntry> _unregister_later_table{};
+
     bool _debugging{false};
 };