From d5a73f9238e873e0426c17ac83792b57a5806476 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 18 Apr 2024 09:39:46 +1200 Subject: [PATCH 1/2] gimbal: fixup subscriptions We finally don't lose the subscriptions any more after a timeout. This is done by doing the subscription after every V2 instantiation in the GimbalImpl class instead of within the V2 class. Signed-off-by: Julian Oes --- src/mavsdk/plugins/gimbal/gimbal_impl.cpp | 24 ++++++++++++++++++++++- src/mavsdk/plugins/gimbal/gimbal_impl.h | 3 +++ 2 files changed, 26 insertions(+), 1 deletion(-) diff --git a/src/mavsdk/plugins/gimbal/gimbal_impl.cpp b/src/mavsdk/plugins/gimbal/gimbal_impl.cpp index ebfcc339c4..5c83eb9491 100644 --- a/src/mavsdk/plugins/gimbal/gimbal_impl.cpp +++ b/src/mavsdk/plugins/gimbal/gimbal_impl.cpp @@ -36,6 +36,8 @@ void GimbalImpl::init() void GimbalImpl::deinit() { + std::lock_guard lock(_mutex); + _gimbal_protocol.reset(nullptr); _system_impl->unregister_all_mavlink_message_handlers(this); } @@ -61,7 +63,7 @@ void GimbalImpl::receive_protocol_timeout() { // We did not receive a GIMBAL_MANAGER_INFORMATION in time, so we have to // assume Version2 is not available. - LogDebug() << "Falling back to Gimbal Version 1"; + LogWarn() << "Falling back to Gimbal Version 1"; _gimbal_protocol.reset(new GimbalProtocolV1(*_system_impl)); _protocol_cookie = nullptr; } @@ -85,6 +87,10 @@ void GimbalImpl::process_gimbal_manager_information(const mavlink_message_t& mes _system_impl->call_user_callback([=]() { _gimbal_protocol.reset(new GimbalProtocolV2( *_system_impl, gimbal_manager_information, message.sysid, message.compid)); + _gimbal_protocol->attitude_async( + [this](auto attitude) { receive_attitude_update(attitude); }); + _gimbal_protocol->control_async( + [this](auto control_status) { receive_control_status_update(control_status); }); }); } } @@ -275,6 +281,22 @@ void GimbalImpl::receive_command_result( } } +void GimbalImpl::receive_attitude_update(Gimbal::Attitude attitude) +{ + std::lock_guard lock(_mutex); + + _attitude_subscriptions.queue( + attitude, [this](const auto& func) { _system_impl->call_user_callback(func); }); +} + +void GimbalImpl::receive_control_status_update(Gimbal::ControlStatus control_status) +{ + std::lock_guard lock(_mutex); + + _control_subscriptions.queue( + control_status, [this](const auto& func) { _system_impl->call_user_callback(func); }); +} + Gimbal::Result GimbalImpl::gimbal_result_from_command_result(MavlinkCommandSender::Result command_result) { diff --git a/src/mavsdk/plugins/gimbal/gimbal_impl.h b/src/mavsdk/plugins/gimbal/gimbal_impl.h index a3c079d05f..7dde8aaa83 100644 --- a/src/mavsdk/plugins/gimbal/gimbal_impl.h +++ b/src/mavsdk/plugins/gimbal/gimbal_impl.h @@ -67,6 +67,9 @@ class GimbalImpl : public PluginImplBase { const GimbalImpl& operator=(const GimbalImpl&) = delete; private: + void receive_attitude_update(Gimbal::Attitude attitude); + void receive_control_status_update(Gimbal::ControlStatus control_status); + std::unique_ptr _gimbal_protocol{nullptr}; void* _protocol_cookie{nullptr}; From 4d16950b673c54a3a1a4d4d75fc6c88834dbc185 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 18 Apr 2024 10:42:59 +1200 Subject: [PATCH 2/2] info: prevent a few potential lockups These happened when a system was re-discovered. Signed-off-by: Julian Oes --- src/mavsdk/plugins/info/info_impl.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/mavsdk/plugins/info/info_impl.cpp b/src/mavsdk/plugins/info/info_impl.cpp index 44241d7cfe..03aee905d6 100644 --- a/src/mavsdk/plugins/info/info_impl.cpp +++ b/src/mavsdk/plugins/info/info_impl.cpp @@ -56,7 +56,7 @@ void InfoImpl::enable() if (!_flight_info_subscriptions.empty()) { // We're hoping to get flight information regularly to update flight time. - _system_impl->set_msg_rate(MAVLINK_MSG_ID_FLIGHT_INFORMATION, 1.0); + _system_impl->set_msg_rate_async(MAVLINK_MSG_ID_FLIGHT_INFORMATION, 1.0, nullptr); } } @@ -321,20 +321,20 @@ void InfoImpl::wait_for_identification() const Info::FlightInformationHandle InfoImpl::subscribe_flight_information(const Info::FlightInformationCallback& callback) { - std::lock_guard lock(_mutex); - // Make sure we get the message regularly. - _system_impl->set_msg_rate(MAVLINK_MSG_ID_FLIGHT_INFORMATION, 1.0); + _system_impl->set_msg_rate_async(MAVLINK_MSG_ID_FLIGHT_INFORMATION, 1.0, nullptr); + + std::lock_guard lock(_mutex); return _flight_info_subscriptions.subscribe(callback); } void InfoImpl::unsubscribe_flight_information(Info::FlightInformationHandle handle) { - std::lock_guard lock(_mutex); - // Reset message to default - _system_impl->set_msg_rate(MAVLINK_MSG_ID_FLIGHT_INFORMATION, 0.0); + _system_impl->set_msg_rate_async(MAVLINK_MSG_ID_FLIGHT_INFORMATION, 0.0, nullptr); + + std::lock_guard lock(_mutex); _flight_info_subscriptions.unsubscribe(handle); }