From ab5ffab454cbc51fddd2745619910ed0ad396d12 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 8 May 2019 09:16:20 +0200 Subject: [PATCH 1/4] core: allow unregistering one message handler Instead of always unregistering all handlers for a cookie, we would like to be able to unregister single handlers. --- core/system_impl.cpp | 15 +++++++++++++++ core/system_impl.h | 1 + 2 files changed, 16 insertions(+) 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 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 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 callback, double duration_s, void **cookie); From 3bb599678e6165360735e58e684fb78982570818 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 8 May 2019 09:17:26 +0200 Subject: [PATCH 2/4] mavlink_passthrough: support unregistering The API docs say that nullptr can be used to unregister. Therefore, we should not add nullptr to the mavlink handlers but unregister properly. --- plugins/mavlink_passthrough/mavlink_passthrough_impl.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/plugins/mavlink_passthrough/mavlink_passthrough_impl.cpp b/plugins/mavlink_passthrough/mavlink_passthrough_impl.cpp index 160bc2dedc..a8731fefdc 100644 --- a/plugins/mavlink_passthrough/mavlink_passthrough_impl.cpp +++ b/plugins/mavlink_passthrough/mavlink_passthrough_impl.cpp @@ -38,7 +38,11 @@ MavlinkPassthrough::Result MavlinkPassthroughImpl::send_message(mavlink_message_ void MavlinkPassthroughImpl::subscribe_message_async( uint16_t message_id, std::function callback) { - _parent->register_mavlink_message_handler(message_id, callback, this); + if (callback == nullptr) { + _parent->unregister_mavlink_message_handler(message_id, this); + } else { + _parent->register_mavlink_message_handler(message_id, callback, this); + } } uint8_t MavlinkPassthroughImpl::get_our_sysid() const From 85e57ae67c464b2c59ba6b222e854c407fb4f366 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 8 May 2019 09:41:40 +0200 Subject: [PATCH 3/4] mavlink_passthrough: use thread pool for callbacks This resolves the case where an API user tries to unregister while still in the callback. In that case the callback gets erased from the mavlink_handler_table and this causes the caller's scope to be destroyed. We can prevent this by copying callback and the message argument and add it to the user callback queue. Presumably, this can have performance implications but I think we want to make sure not to crash first. --- plugins/mavlink_passthrough/mavlink_passthrough_impl.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/plugins/mavlink_passthrough/mavlink_passthrough_impl.cpp b/plugins/mavlink_passthrough/mavlink_passthrough_impl.cpp index a8731fefdc..57394d0a7c 100644 --- a/plugins/mavlink_passthrough/mavlink_passthrough_impl.cpp +++ b/plugins/mavlink_passthrough/mavlink_passthrough_impl.cpp @@ -41,7 +41,13 @@ void MavlinkPassthroughImpl::subscribe_message_async( if (callback == nullptr) { _parent->unregister_mavlink_message_handler(message_id, this); } else { - _parent->register_mavlink_message_handler(message_id, callback, this); + 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); } } From 9e0bec3ca6acf2c81cb611483beab4fcc3b79159 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 8 May 2019 09:45:20 +0200 Subject: [PATCH 4/4] mavlink_passthrough: also test unregistering --- integration_tests/mavlink_passthrough.cpp | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) 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 prom; std::future 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(); + } }; });