Merge pull request #2286 from mavlink/pr-fixups

gimbal/info: fixups

src/mavsdk/plugins/gimbal/gimbal_impl.cpp

@@ -36,6 +36,8 @@ void GimbalImpl::init()
 
 void GimbalImpl::deinit()
 {
+    std::lock_guard<std::mutex> 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<std::mutex> 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<std::mutex> 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)
 {

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<GimbalProtocolBase> _gimbal_protocol{nullptr};
 
     void* _protocol_cookie{nullptr};

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<std::mutex> 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<std::mutex> lock(_mutex);
 
     return _flight_info_subscriptions.subscribe(callback);
 }
 
 void InfoImpl::unsubscribe_flight_information(Info::FlightInformationHandle handle)
 {
-    std::lock_guard<std::mutex> 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<std::mutex> lock(_mutex);
 
     _flight_info_subscriptions.unsubscribe(handle);
 }