From 1421ef9e3da54d995abcfbc017b0d543ad54a04a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 26 Oct 2018 11:50:57 +0200 Subject: [PATCH 01/10] core: don't initialize without UUID We already disabled initialization without UUID behind an ifdef, however, we missed one place. This change disables the fallback after 10 heartbeats/seconds as well. This fixes an issue where we call `on_discovery` even though only a UDP bridge and no vehicle is connected. --- core/system_impl.cpp | 2 ++ core/system_impl.h | 3 +-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/core/system_impl.cpp b/core/system_impl.cpp index 8023f59120..10c5909894 100644 --- a/core/system_impl.cpp +++ b/core/system_impl.cpp @@ -187,11 +187,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(); diff --git a/core/system_impl.h b/core/system_impl.h index db56a52061..6946bbf5ce 100644 --- a/core/system_impl.h +++ b/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 _uuid_initialized{false}; - uint8_t _non_autopilot_heartbeats = 0; - bool _supports_mission_int{false}; std::atomic _armed{false}; std::atomic _hitl_enabled{false}; From 08bf77c921787c6a8da1908757190763d06608ec Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 26 Oct 2018 11:53:00 +0200 Subject: [PATCH 02/10] backend: start GRPC server before connecting This changes the initialization logic to first start the GRPC server and then try to connect it all. This is based on the assumption that we can initialize the GRPC server and the plugin it has using the default/dummy system and we don't have to have anything connected yet. Once something connects, the system gets updated and the plugins start to "work". --- backend/src/backend_api.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/backend/src/backend_api.cpp b/backend/src/backend_api.cpp index fbb0f5e841..b988540853 100644 --- a/backend/src/backend_api.cpp +++ b/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); From 76450e0d2c7d1f2ead83798a3ec012ec85935195 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 26 Oct 2018 11:55:57 +0200 Subject: [PATCH 03/10] backend: remove assert because it would trigger We need to remove this assert because we now initialize the GRPC server without having a vehicle connected and therefore, the system list of system UUIDs can be empty. This doesn't matter because the default/dummy system is still valid and the plugins will be ok later. --- backend/src/grpc_server.h | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/backend/src/grpc_server.h b/backend/src/grpc_server.h index 885432c9ed..485613e262 100644 --- a/backend/src/grpc_server.h +++ b/backend/src/grpc_server.h @@ -30,10 +30,7 @@ class GRPCServer { _mission(_dc.system()), _mission_service(_mission), _telemetry(_dc.system()), - _telemetry_service(_telemetry) - { - assert(_dc.system_uuids().size() >= 1); - } + _telemetry_service(_telemetry) {} void run(); void wait(); From a527157fab9509e1204440cc73fc4a5f63c1373b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 26 Oct 2018 12:01:36 +0200 Subject: [PATCH 04/10] version: 0.7.0 Minor version bump because the initialization of the backend changed. --- version | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/version b/version index b616048743..faef31a435 100644 --- a/version +++ b/version @@ -1 +1 @@ -0.6.2 +0.7.0 From 6be656dad6c48386a12401986278b3e74d7632bc Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 26 Oct 2018 12:02:39 +0200 Subject: [PATCH 05/10] backend: style fix --- backend/src/grpc_server.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/backend/src/grpc_server.h b/backend/src/grpc_server.h index 485613e262..de2ddcd622 100644 --- a/backend/src/grpc_server.h +++ b/backend/src/grpc_server.h @@ -30,7 +30,8 @@ class GRPCServer { _mission(_dc.system()), _mission_service(_mission), _telemetry(_dc.system()), - _telemetry_service(_telemetry) {} + _telemetry_service(_telemetry) + {} void run(); void wait(); From 2c01b9e45df674981f5e05c0ca936f801bd75a18 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 26 Oct 2018 14:23:22 +0200 Subject: [PATCH 06/10] core: improved a few printfs --- core/dronecode_sdk_impl.cpp | 6 +++--- core/system_impl.cpp | 11 ++++++++--- 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/core/dronecode_sdk_impl.cpp b/core/dronecode_sdk_impl.cpp index 898f433097..4cc34845d5 100644 --- a/core/dronecode_sdk_impl.cpp +++ b/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; diff --git a/core/system_impl.cpp b/core/system_impl.cpp index 10c5909894..155530f9e9 100644 --- a/core/system_impl.cpp +++ b/core/system_impl.cpp @@ -353,13 +353,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."; } } @@ -498,9 +503,9 @@ void SystemImpl::set_connected() std::lock_guard 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; From 22e078e9fbc01537bba0a87e6ceb9cc42af28754 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 26 Oct 2018 14:23:48 +0200 Subject: [PATCH 07/10] core: don't call on_discovery with dummy system These are check to prevent us from calling on_discovery if we're actually just a dummy system and should wait until something real is connected. --- core/dronecode_sdk_impl.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/core/dronecode_sdk_impl.cpp b/core/dronecode_sdk_impl.cpp index 4cc34845d5..39ac1245b3 100644 --- a/core/dronecode_sdk_impl.cpp +++ b/core/dronecode_sdk_impl.cpp @@ -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()); } } From e54115c5b2b4dd8ae295ae64a1705e4d8938271c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 26 Oct 2018 14:24:48 +0200 Subject: [PATCH 08/10] core: ignore heartbeat from UDP_BRIDGE component This is a hack to prevent a UDP_BRIDGE from telling us we're connected when really we're interested in the drone, not the connection bridge. A proper fix would be to check for specific components when deciding if something is connected or not. --- core/system_impl.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/core/system_impl.cpp b/core/system_impl.cpp index 155530f9e9..1b67af72b5 100644 --- a/core/system_impl.cpp +++ b/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); From 6734af44f0f13d69a0fb6178e23bfb904b0ca266 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 26 Oct 2018 14:27:04 +0200 Subject: [PATCH 09/10] camera: move callback resets into deinit As a general rule we should have all custom things in `init()` and `deinit()` instead of the constructor or destructor. --- plugins/camera/camera_impl.cpp | 40 +++++++++++++++++----------------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/plugins/camera/camera_impl.cpp b/plugins/camera/camera_impl.cpp index 7c70cb8551..c2977afd89 100644 --- a/plugins/camera/camera_impl.cpp +++ b/plugins/camera/camera_impl.cpp @@ -23,26 +23,6 @@ CameraImpl::CameraImpl(System &system) : PluginImplBase(system) CameraImpl::~CameraImpl() { _parent->unregister_plugin(this); - - { - std::lock_guard lock(_status.mutex); - _status.callback = nullptr; - } - - { - std::lock_guard lock(_get_mode.mutex); - _get_mode.callback = nullptr; - } - - { - std::lock_guard lock(_capture_info.mutex); - _capture_info.callback = nullptr; - } - - { - std::lock_guard lock(_video_stream_info.mutex); - _video_stream_info.callback = nullptr; - } } void CameraImpl::init() @@ -91,6 +71,26 @@ void CameraImpl::deinit() { _parent->remove_call_every(_status.call_every_cookie); _parent->unregister_all_mavlink_message_handlers(this); + + { + std::lock_guard lock(_status.mutex); + _status.callback = nullptr; + } + + { + std::lock_guard lock(_get_mode.mutex); + _get_mode.callback = nullptr; + } + + { + std::lock_guard lock(_capture_info.mutex); + _capture_info.callback = nullptr; + } + + { + std::lock_guard lock(_video_stream_info.mutex); + _video_stream_info.callback = nullptr; + } } void CameraImpl::enable() From 66611f91bb0d0e9812c765f879d33dc7b9f31f38 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 26 Oct 2018 14:28:46 +0200 Subject: [PATCH 10/10] camera: fix camera definition not available This fixes the problem where we don't have a camera definition because we never receive the camera information. We need to request the camera information when we're connected, so in `enable()` not when constructed `init()`. --- plugins/camera/camera_impl.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/plugins/camera/camera_impl.cpp b/plugins/camera/camera_impl.cpp index c2977afd89..9e966bca65 100644 --- a/plugins/camera/camera_impl.cpp +++ b/plugins/camera/camera_impl.cpp @@ -61,10 +61,6 @@ 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() @@ -97,6 +93,10 @@ 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); }