gimbal: try to avoid potential lockup on reconnection (#2385)

src/mavsdk/plugins/gimbal/gimbal_impl.cpp

@@ -54,7 +54,8 @@ void GimbalImpl::enable()
         }
     }
 
-    request_gimbal_information();
+    _request_gimbal_information_cookie =
+        _system_impl->add_call_every([this]() { request_gimbal_information(); }, 3.0);
 }
 
 void GimbalImpl::disable()
@@ -75,6 +76,9 @@ void GimbalImpl::receive_protocol_timeout()
 {
     // We did not receive a GIMBAL_MANAGER_INFORMATION in time, so we have to
     // assume Version2 is not available.
+
+    _system_impl->remove_call_every(_request_gimbal_information_cookie);
+
     LogWarn() << "Falling back to Gimbal Version 1";
     std::lock_guard<std::mutex> lock(_mutex);
     _gimbal_protocol.reset(new GimbalProtocolV1(*_system_impl));
@@ -94,6 +98,8 @@ void GimbalImpl::process_gimbal_manager_information(const mavlink_message_t& mes
 
     _protocol_cookie = {};
 
+    _system_impl->remove_call_every(_request_gimbal_information_cookie);
+
     // We need to schedule the construction for later because it wants
     // to register more message subscriptions which blocks.
     // TODO: we should fix this at the callback list level
@@ -282,20 +288,17 @@ Gimbal::Attitude GimbalImpl::attitude()
 
 void GimbalImpl::wait_for_protocol()
 {
-    unsigned counter = 0;
     while (true) {
         {
-            std::lock_guard<std::mutex> lock(_mutex);
-            if (_gimbal_protocol != nullptr) {
-                break;
+            // Try to get lock, if we can't, just keep trying.
+            std::unique_lock<std::mutex> lock(_mutex, std::try_to_lock);
+            if (lock.owns_lock()) {
+                if (_gimbal_protocol != nullptr) {
+                    break;
+                }
             }
         }
-        // Request gimbal information every 3 seconds again
-        if (counter % 30 == 0) {
-            request_gimbal_information();
-        }
         std::this_thread::sleep_for(std::chrono::milliseconds(100));
-        ++counter;
     }
 }
 

src/mavsdk/plugins/gimbal/gimbal_impl.h

@@ -83,6 +83,8 @@ class GimbalImpl : public PluginImplBase {
     std::unique_ptr<GimbalProtocolBase> _gimbal_protocol{nullptr};
     CallbackList<Gimbal::ControlStatus> _control_subscriptions{};
     CallbackList<Gimbal::Attitude> _attitude_subscriptions{};
+
+    CallEveryHandler::Cookie _request_gimbal_information_cookie{};
 };
 
 } // namespace mavsdk