core: prevent lockup on connection destruction (#2412)

When we destroy Mavsdk and clear the list of connections, we likely
end up in a deadlock.

What happens is that:
1. A connection wants to forward a message and is trying to acquire the
   connection mutex.
2. At the same time, the connection is being destroyed, so we are
   waiting for the connection receive thread to be joinable. While the
   connections are being destroyed, we have the connection mutex which
   is blocking 1.

The proposed solution is to:
1. Make it less likely by acquiring the connection mutex properly before
   checking _connections.size() and not for the individual connections.
2. Check the _should_exit flag before trying to acquire the mutex. I
   believe by the time the connections are being cleared, this flag is
   set, and hence the deadlock should not happen, fingers crossed.

Author: julianoes

src/mavsdk/core/mavsdk_impl.cpp

@@ -292,8 +292,6 @@ void MavsdkImpl::forward_message(mavlink_message_t& message, Connection* connect
         (message.msgid != MAVLINK_MSG_ID_HEARTBEAT || forward_heartbeats_enabled);
 
     if (!targeted_only_at_us && heartbeat_check_ok) {
-        std::lock_guard<std::mutex> lock(_connections_mutex);
-
         unsigned successful_emissions = 0;
         for (auto& entry : _connections) {
             // Check whether the connection is not the one from which we received the message.
@@ -319,6 +317,11 @@ void MavsdkImpl::receive_message(mavlink_message_t& message, Connection* connect
                    << static_cast<int>(message.sysid) << "/" << static_cast<int>(message.compid);
     }
 
+    if (_should_exit) {
+        // If we're meant to clean up, let's not try to acquire any more locks but bail.
+        return;
+    }
+
     // This is a low level interface where incoming messages can be tampered
     // with or even dropped.
     {
@@ -332,6 +335,11 @@ void MavsdkImpl::receive_message(mavlink_message_t& message, Connection* connect
         }
     }
 
+    if (_should_exit) {
+        // If we're meant to clean up, let's not try to acquire any more locks but bail.
+        return;
+    }
+
     /** @note: Forward message if option is enabled and multiple interfaces are connected.
      *  Performs message forwarding checks for every messages if message forwarding
      *  is enabled on at least one connection, and in case of a single forwarding connection,
@@ -342,15 +350,20 @@ void MavsdkImpl::receive_message(mavlink_message_t& message, Connection* connect
      * 2. At least 1 forwarding connection.
      * 3. At least 2 forwarding connections or current connection is not forwarding.
      */
-    if (_connections.size() > 1 && mavsdk::Connection::forwarding_connections_count() > 0 &&
-        (mavsdk::Connection::forwarding_connections_count() > 1 ||
-         !connection->should_forward_messages())) {
-        if (_message_logging_on) {
-            LogDebug() << "Forwarding message " << message.msgid << " from "
-                       << static_cast<int>(message.sysid) << "/"
-                       << static_cast<int>(message.compid);
+
+    {
+        std::lock_guard<std::mutex> lock(_connections_mutex);
+
+        if (_connections.size() > 1 && mavsdk::Connection::forwarding_connections_count() > 0 &&
+            (mavsdk::Connection::forwarding_connections_count() > 1 ||
+             !connection->should_forward_messages())) {
+            if (_message_logging_on) {
+                LogDebug() << "Forwarding message " << message.msgid << " from "
+                           << static_cast<int>(message.sysid) << "/"
+                           << static_cast<int>(message.compid);
+            }
+            forward_message(message, connection);
         }
-        forward_message(message, connection);
     }
 
     // Don't ever create a system with sysid 0.