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};
 };