Fix backend connection issues

backend/src/backend_api.cpp

@@ -5,8 +5,8 @@
 void runBackend(const int mavlink_listen_port, void (*onServerStarted)(void *), void *context)
 {
     dronecode_sdk::backend::DronecodeSDKBackend backend;
-    backend.connect(mavlink_listen_port);
     backend.startGRPCServer();
+    backend.connect(mavlink_listen_port);
 
     if (onServerStarted != nullptr) {
         onServerStarted(context);

backend/src/grpc_server.h

@@ -31,9 +31,7 @@ class GRPCServer {
         _mission_service(_mission),
         _telemetry(_dc.system()),
         _telemetry_service(_telemetry)
-    {
-        assert(_dc.system_uuids().size() >= 1);
-    }
+    {}
 
     void run();
     void wait();

core/dronecode_sdk_impl.cpp

@@ -215,12 +215,12 @@ System &DronecodeSDKImpl::get_system()
         }
 
         if (_systems.size() > 1) {
-            LogErr() << "Error: more than one system found:";
+            LogErr() << "More than one system found:";
 
             // Just return first system instead of failing.
             return *_systems.begin()->second;
         } else {
-            LogErr() << "Error: no system found.";
+            LogWarn() << "No system found.";
             uint8_t system_id = 0, comp_id = 0;
             make_system_with_component(system_id, comp_id);
             return *_systems[system_id];
@@ -242,7 +242,7 @@ System &DronecodeSDKImpl::get_system(const uint64_t uuid)
 
     // We have not found a system with this UUID.
     // TODO: this is an error condition that we ought to handle properly.
-    LogErr() << "system with UUID: " << uuid << " not found";
+    LogErr() << "System with UUID: " << uuid << " not found";
 
     // Create a dummy
     uint8_t system_id = 0, comp_id = 0;
@@ -322,6 +322,14 @@ void DronecodeSDKImpl::register_on_discover(const DronecodeSDK::event_callback_t
 
     if (callback) {
         for (auto const &connected_system : _systems) {
+            // Ignore dummy system with system ID 0.
+            if (connected_system.first == 0) {
+                continue;
+            }
+            // Ignore system if UUID is not initialized yet.
+            if (connected_system.second->get_uuid() == 0) {
+                continue;
+            }
             callback(connected_system.second->get_uuid());
         }
     }

core/system_impl.cpp

@@ -172,6 +172,12 @@ void SystemImpl::remove_call_every(const void *cookie)
 
 void SystemImpl::process_heartbeat(const mavlink_message_t &message)
 {
+    // FIXME: for now we ignore heartbeats from UDP_BRIDGE because that's just
+    // confusing since it doesn't mean a vehicle is connected.
+    if (message.compid == MAV_COMP_ID_UDP_BRIDGE) {
+        return;
+    }
+
     mavlink_heartbeat_t heartbeat;
     mavlink_msg_heartbeat_decode(&message, &heartbeat);
 
@@ -187,11 +193,13 @@ void SystemImpl::process_heartbeat(const mavlink_message_t &message)
     if (is_autopilot(message.compid) && !have_uuid()) {
         request_autopilot_version();
 
+#if defined(ENABLE_FALLBACK_TO_SYSTEM_ID)
     } else if (!is_autopilot(message.compid) && !have_uuid() && ++_non_autopilot_heartbeats >= 10) {
         // We've received consecutive heartbeats (atleast twice) from a
         // non-autopilot system! Lets not delay for filling UUID anymore.
         _uuid = message.sysid;
         _uuid_initialized = true;
+#endif
     }
 
     set_connected();
@@ -351,13 +359,18 @@ ComponentType SystemImpl::component_type(uint8_t component_id)
 
 void SystemImpl::add_new_component(uint8_t component_id)
 {
+    if (component_id == 0) {
+        return;
+    }
+
     auto res_pair = _components.insert(component_id);
     if (res_pair.second) {
         if (component_discovered_callback != nullptr) {
             const ComponentType type = component_type(component_id);
             call_user_callback([this, type]() { component_discovered_callback(type); });
         }
-        LogDebug() << "Component " << component_name(component_id) << " added.";
+        LogDebug() << "Component " << component_name(component_id) << " (" << int(component_id)
+                   << ") added.";
     }
 }
 
@@ -496,9 +509,9 @@ void SystemImpl::set_connected()
         std::lock_guard<std::mutex> lock(_connection_mutex);
 
         if (!_connected && _uuid_initialized) {
-            LogDebug() << "Found " << _components.size() << " component(s).";
+            LogDebug() << "Discovered " << _components.size() << " component(s) "
+                       << "(UUID: " << _uuid << ")";
 
-            LogDebug() << "Discovered " << _uuid;
             _parent.notify_on_discover(_uuid);
             _connected = true;
 

core/system_impl.h

@@ -229,11 +229,10 @@ class SystemImpl {
 
 #if defined(ENABLE_FALLBACK_TO_SYSTEM_ID)
     int _uuid_retries = 0;
+    uint8_t _non_autopilot_heartbeats = 0;
 #endif
     std::atomic<bool> _uuid_initialized{false};
 
-    uint8_t _non_autopilot_heartbeats = 0;
-
     bool _supports_mission_int{false};
     std::atomic<bool> _armed{false};
     std::atomic<bool> _hitl_enabled{false};

plugins/camera/camera_impl.cpp

@@ -23,26 +23,6 @@ CameraImpl::CameraImpl(System &system) : PluginImplBase(system)
 CameraImpl::~CameraImpl()
 {
     _parent->unregister_plugin(this);
-
-    {
-        std::lock_guard<std::mutex> lock(_status.mutex);
-        _status.callback = nullptr;
-    }
-
-    {
-        std::lock_guard<std::mutex> lock(_get_mode.mutex);
-        _get_mode.callback = nullptr;
-    }
-
-    {
-        std::lock_guard<std::mutex> lock(_capture_info.mutex);
-        _capture_info.callback = nullptr;
-    }
-
-    {
-        std::lock_guard<std::mutex> lock(_video_stream_info.mutex);
-        _video_stream_info.callback = nullptr;
-    }
 }
 
 void CameraImpl::init()
@@ -81,22 +61,42 @@ void CameraImpl::init()
         MAVLINK_MSG_ID_FLIGHT_INFORMATION,
         std::bind(&CameraImpl::process_flight_information, this, _1),
         this);
-
-    auto command_camera_info = make_command_request_camera_info();
-
-    _parent->send_command_async(command_camera_info, nullptr);
 }
 
 void CameraImpl::deinit()
 {
     _parent->remove_call_every(_status.call_every_cookie);
     _parent->unregister_all_mavlink_message_handlers(this);
+
+    {
+        std::lock_guard<std::mutex> lock(_status.mutex);
+        _status.callback = nullptr;
+    }
+
+    {
+        std::lock_guard<std::mutex> lock(_get_mode.mutex);
+        _get_mode.callback = nullptr;
+    }
+
+    {
+        std::lock_guard<std::mutex> lock(_capture_info.mutex);
+        _capture_info.callback = nullptr;
+    }
+
+    {
+        std::lock_guard<std::mutex> lock(_video_stream_info.mutex);
+        _video_stream_info.callback = nullptr;
+    }
 }
 
 void CameraImpl::enable()
 {
     refresh_params();
     request_flight_information();
+
+    auto command_camera_info = make_command_request_camera_info();
+    _parent->send_command_async(command_camera_info, nullptr);
+
     _parent->add_call_every(
         [this]() { request_flight_information(); }, 10.0, &_flight_information_call_every_cookie);
 }

version

@@ -1 +1 @@
-0.6.2
+0.7.0