From c55244ded39cecfdfa9ab6856fce8a963be3d402 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 16 May 2024 14:35:47 +1200 Subject: [PATCH 1/4] core: clean up CommandSender timeouts This is unlikely to cause problems as we generally don't remove systems but we should nevertheless try to clean this up. --- src/mavsdk/core/mavlink_command_sender.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/mavsdk/core/mavlink_command_sender.cpp b/src/mavsdk/core/mavlink_command_sender.cpp index 652613fa0..d37bf7018 100644 --- a/src/mavsdk/core/mavlink_command_sender.cpp +++ b/src/mavsdk/core/mavlink_command_sender.cpp @@ -25,7 +25,15 @@ MavlinkCommandSender::MavlinkCommandSender(SystemImpl& system_impl) : _system_im MavlinkCommandSender::~MavlinkCommandSender() { + if (_command_debugging) { + LogDebug() << "CommandSender destroyed"; + } _system_impl.unregister_all_mavlink_message_handlers(this); + + LockedQueue::Guard work_queue_guard(_work_queue); + for (const auto& work : _work_queue) { + _system_impl.unregister_timeout_handler(work->timeout_cookie); + } } MavlinkCommandSender::Result @@ -285,6 +293,9 @@ void MavlinkCommandSender::receive_command_ack(mavlink_message_t message) void MavlinkCommandSender::receive_timeout(const CommandIdentification& identification) { + if (_command_debugging) { + LogDebug() << "Got timeout!"; + } bool found_command = false; CommandResultCallback temp_callback = nullptr; std::pair temp_result{Result::UnknownError, NAN}; From 9405585066997c630751132d41f36547b14a0f5d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 16 May 2024 14:38:07 +1200 Subject: [PATCH 2/4] core: don't ever block waiting for ack This caused quite a bit of trouble, so it's probably best to just send the command of, and only react to it if we happen to get an ack back. If not, no big deal, and let's not lockup everything else because of that future promise. --- src/mavsdk/core/system_impl.cpp | 57 ++++++++------------------------- src/mavsdk/core/system_impl.h | 5 +-- 2 files changed, 14 insertions(+), 48 deletions(-) diff --git a/src/mavsdk/core/system_impl.cpp b/src/mavsdk/core/system_impl.cpp index bfa1980c8..047c34960 100644 --- a/src/mavsdk/core/system_impl.cpp +++ b/src/mavsdk/core/system_impl.cpp @@ -74,21 +74,6 @@ void SystemImpl::init(uint8_t system_id, uint8_t comp_id) [this](const mavlink_message_t& message) { process_autopilot_version(message); }, this); - // register_mavlink_command_handler( - // MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES, - // [this](const MavlinkCommandReceiver::CommandLong& command) { - // return process_autopilot_version_request(command); - // }, - // this); - - //// TO-DO! - // register_mavlink_command_handler( - // MAV_CMD_REQUEST_MESSAGE, - // [this](const MavlinkCommandReceiver::CommandLong& command) { - // return make_command_ack_message(command, MAV_RESULT::MAV_RESULT_UNSUPPORTED); - // }, - // this); - add_new_component(comp_id); } @@ -313,17 +298,6 @@ void SystemImpl::system_thread() } } -// std::optional -// SystemImpl::process_autopilot_version_request(const MavlinkCommandReceiver::CommandLong& command) -//{ -// if (_should_send_autopilot_version) { -// send_autopilot_version(); -// return make_command_ack_message(command, MAV_RESULT::MAV_RESULT_ACCEPTED); -// } -// -// return {}; -//} - std::string SystemImpl::component_name(uint8_t component_id) { switch (component_id) { @@ -495,22 +469,6 @@ bool SystemImpl::queue_message( } void SystemImpl::send_autopilot_version_request() -{ - auto prom = std::promise(); - auto fut = prom.get_future(); - - send_autopilot_version_request_async( - [&prom](MavlinkCommandSender::Result result, float) { prom.set_value(result); }); - - if (fut.get() == MavlinkCommandSender::Result::Unsupported) { - _old_message_520_supported = false; - LogWarn() - << "Trying alternative command REQUEST_MESSAGE instead of REQUEST_AUTOPILOT_CAPABILITIES next."; - } -} - -void SystemImpl::send_autopilot_version_request_async( - const MavlinkCommandSender::CommandResultCallback& callback) { MavlinkCommandSender::CommandLong command{}; command.target_component_id = get_autopilot_id(); @@ -525,7 +483,18 @@ void SystemImpl::send_autopilot_version_request_async( command.params.maybe_param1 = {static_cast(MAVLINK_MSG_ID_AUTOPILOT_VERSION)}; } - send_command_async(command, callback); + send_command_async(command, [this](MavlinkCommandSender::Result result, float) { + receive_autopilot_version_request_ack(result); + }); +} + +void SystemImpl::receive_autopilot_version_request_ack(MavlinkCommandSender::Result result) +{ + if (result == MavlinkCommandSender::Result::Unsupported) { + _old_message_520_supported = false; + LogWarn() + << "Trying alternative command REQUEST_MESSAGE instead of REQUEST_AUTOPILOT_CAPABILITIES next."; + } } void SystemImpl::set_connected() @@ -567,7 +536,7 @@ void SystemImpl::set_connected() if (enable_needed) { if (has_autopilot()) { - send_autopilot_version_request_async(nullptr); + send_autopilot_version_request(); } std::lock_guard lock(_plugin_impls_mutex); diff --git a/src/mavsdk/core/system_impl.h b/src/mavsdk/core/system_impl.h index 68462e025..0cb958a7a 100644 --- a/src/mavsdk/core/system_impl.h +++ b/src/mavsdk/core/system_impl.h @@ -281,8 +281,6 @@ class SystemImpl { const std::string& filename, int linenumber, const std::function& func); void send_autopilot_version_request(); - void send_autopilot_version_request_async( - const MavlinkCommandSender::CommandResultCallback& callback); MavlinkMissionTransferClient& mission_transfer_client() { return _mission_transfer_client; }; @@ -307,8 +305,7 @@ class SystemImpl { void set_connected(); void set_disconnected(); - std::optional - process_autopilot_version_request(const MavlinkCommandReceiver::CommandLong& command); + void receive_autopilot_version_request_ack(MavlinkCommandSender::Result result); static std::string component_name(uint8_t component_id); static System::ComponentType component_type(uint8_t component_id); From 2dc8e765407f8dffc1169546df1e0824351a11ed Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 16 May 2024 14:40:09 +1200 Subject: [PATCH 3/4] core: potentially fix timeout handler I have seen curious cases where timeouts just didn't seem to trigger as the they should have. My guess was that the void * pointer to the location of the shared_ptr in the map was just a bad idea in the first place. So I changed it to just a uint64_t handle which is easier to debug and should always be unique. I also changed from a std::map to a std::vector, however, the jury is out on what is actually faster for our usage. --- src/mavsdk/core/timeout_handler.cpp | 71 +++++++++++++++-------------- src/mavsdk/core/timeout_handler.h | 11 +++-- 2 files changed, 44 insertions(+), 38 deletions(-) diff --git a/src/mavsdk/core/timeout_handler.cpp b/src/mavsdk/core/timeout_handler.cpp index a7cc972e5..8028c9471 100644 --- a/src/mavsdk/core/timeout_handler.cpp +++ b/src/mavsdk/core/timeout_handler.cpp @@ -6,21 +6,19 @@ TimeoutHandler::TimeoutHandler(Time& time) : _time(time) {} void TimeoutHandler::add(std::function callback, double duration_s, void** cookie) { - auto new_timeout = std::make_shared(); - new_timeout->callback = callback; - new_timeout->time = _time.steady_time_in_future(duration_s); - new_timeout->duration_s = duration_s; - - void* new_cookie = static_cast(new_timeout.get()); - - { - std::lock_guard lock(_timeouts_mutex); - _timeouts.insert(std::pair>(new_cookie, new_timeout)); - } + std::lock_guard lock(_timeouts_mutex); + auto new_timeout = Timeout{}; + new_timeout.callback = callback; + new_timeout.time = _time.steady_time_in_future(duration_s); + new_timeout.duration_s = duration_s; + new_timeout.cookie = _next_cookie++; + _timeouts.push_back(new_timeout); if (cookie != nullptr) { - *cookie = new_cookie; + // TODO: change this API in the future + *cookie = reinterpret_cast(new_timeout.cookie); } + _iterator_invalidated = true; } void TimeoutHandler::refresh(const void* cookie) @@ -31,10 +29,12 @@ void TimeoutHandler::refresh(const void* cookie) std::lock_guard lock(_timeouts_mutex); - auto it = _timeouts.find(const_cast(cookie)); + auto it = std::find_if(_timeouts.begin(), _timeouts.end(), [&](const Timeout& timeout) { + return timeout.cookie == reinterpret_cast(cookie); + }); if (it != _timeouts.end()) { - auto future_time = _time.steady_time_in_future(it->second->duration_s); - it->second->time = future_time; + auto future_time = _time.steady_time_in_future(it->duration_s); + it->time = future_time; } } @@ -46,46 +46,47 @@ void TimeoutHandler::remove(const void* cookie) std::lock_guard lock(_timeouts_mutex); - auto it = _timeouts.find(const_cast(cookie)); + auto it = std::find_if(_timeouts.begin(), _timeouts.end(), [&](auto& timeout) { + return timeout.cookie == reinterpret_cast(cookie); + }); + if (it != _timeouts.end()) { - _timeouts.erase(const_cast(cookie)); + _timeouts.erase(it); _iterator_invalidated = true; } } void TimeoutHandler::run_once() { - _timeouts_mutex.lock(); + std::unique_lock lock(_timeouts_mutex); auto now = _time.steady_time(); for (auto it = _timeouts.begin(); it != _timeouts.end(); /* no ++it */) { // If time is passed, call timeout callback. - if (it->second->time < now) { - if (it->second->callback) { - // Get a copy for the callback because we will remove it. - std::function callback = it->second->callback; + if (it->time < now) { + // Get a copy for the callback because we will remove it. + auto callback = it->callback; - // Self-destruct before calling to avoid locking issues. - _timeouts.erase(it++); + // Self-destruct before calling to avoid locking issues. + it = _timeouts.erase(it); - // Unlock while we callback because it might in turn want to add timeouts. - _timeouts_mutex.unlock(); + if (callback) { + // Unlock while we call back because it might in turn want to add timeouts. + lock.unlock(); callback(); - _timeouts_mutex.lock(); - } + lock.lock(); + // We start over if anyone has messed with this while we called the callback. + if (_iterator_invalidated) { + _iterator_invalidated = false; + it = _timeouts.begin(); + } + } } else { ++it; } - - // We start over if anyone has messed with this while we called the callback. - if (_iterator_invalidated) { - _iterator_invalidated = false; - it = _timeouts.begin(); - } } - _timeouts_mutex.unlock(); } } // namespace mavsdk diff --git a/src/mavsdk/core/timeout_handler.h b/src/mavsdk/core/timeout_handler.h index 2fc5b9d0a..5f9d4d97b 100644 --- a/src/mavsdk/core/timeout_handler.h +++ b/src/mavsdk/core/timeout_handler.h @@ -1,10 +1,12 @@ #pragma once +#include "mavsdk_time.h" + +#include #include #include #include -#include -#include "mavsdk_time.h" +#include namespace mavsdk { @@ -30,13 +32,16 @@ class TimeoutHandler { std::function callback{}; SteadyTimePoint time{}; double duration_s{0.0}; + uint64_t cookie{0}; }; - std::unordered_map> _timeouts{}; + std::vector _timeouts{}; std::mutex _timeouts_mutex{}; bool _iterator_invalidated{false}; Time& _time; + + uint64_t _next_cookie{1}; }; } // namespace mavsdk From 683d2ac95bf4d92e9de0a50e151bb0449c095245 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 16 May 2024 14:48:04 +1200 Subject: [PATCH 4/4] gimbal: prevent segfault without gimbal v2 forced It turns out this needed to be locked after all. Signed-off-by: Julian Oes --- src/mavsdk/plugins/gimbal/gimbal_impl.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/mavsdk/plugins/gimbal/gimbal_impl.cpp b/src/mavsdk/plugins/gimbal/gimbal_impl.cpp index 484b35d89..246121c3d 100644 --- a/src/mavsdk/plugins/gimbal/gimbal_impl.cpp +++ b/src/mavsdk/plugins/gimbal/gimbal_impl.cpp @@ -220,8 +220,7 @@ Gimbal::ControlHandle GimbalImpl::subscribe_control(const Gimbal::ControlCallbac if (result.first) { wait_for_protocol_async([=]() { - // We don't lock here because we don't expect the protocol to change once it has been - // set. + std::lock_guard lock(_mutex); _gimbal_protocol->control_async([this](Gimbal::ControlStatus status) { _control_subscriptions.queue( status, [this](const auto& func) { _system_impl->call_user_callback(func); }); @@ -254,8 +253,7 @@ Gimbal::AttitudeHandle GimbalImpl::subscribe_attitude(const Gimbal::AttitudeCall if (result.first) { wait_for_protocol_async([=]() { - // We don't lock here because we don't expect the protocol to change once it has been - // set. + std::lock_guard lock(_mutex); _gimbal_protocol->attitude_async([this](Gimbal::Attitude attitude) { _attitude_subscriptions.queue( attitude, [this](const auto& func) { _system_impl->call_user_callback(func); });