diff --git a/core/include/system.h b/core/include/system.h index 45b937b1e2..fd42014e09 100644 --- a/core/include/system.h +++ b/core/include/system.h @@ -1,9 +1,20 @@ #pragma once #include +#include namespace dronecode_sdk { +/** + * @brief Component Types + */ +enum ComponentType { UNKNOWN = 0, AUTOPILOT, CAMERA, GIMBAL }; + +/** + * @brief type for component discovery callback + */ +typedef std::function discover_callback_t; + class SystemImpl; class DronecodeSDKImpl; class PluginImplBase; @@ -76,6 +87,14 @@ class System { */ uint64_t get_uuid() const; + /** + * @brief Register a callback to be called when a component is discovered. + * + * @param callback a function of type void(ComponentType) which will be called with the + * component type of the new component. + */ + void register_component_discovered_callback(discover_callback_t callback) const; + /** * @brief Copy constructor (object is not copyable). */ diff --git a/core/system.cpp b/core/system.cpp index c794dc7fa4..18b1adcf59 100644 --- a/core/system.cpp +++ b/core/system.cpp @@ -53,4 +53,9 @@ uint64_t System::get_uuid() const return _system_impl->get_uuid(); } +void System::register_component_discovered_callback(discover_callback_t callback) const +{ + return _system_impl->register_component_discovered_callback(callback); +} + } // namespace dronecode_sdk diff --git a/core/system_impl.cpp b/core/system_impl.cpp index 4b5adb7146..8023f59120 100644 --- a/core/system_impl.cpp +++ b/core/system_impl.cpp @@ -25,6 +25,8 @@ SystemImpl::SystemImpl(DronecodeSDKImpl &parent, uint8_t system_id, uint8_t comp _call_every_handler(_time), _thread_pool(3) { + component_discovered_callback = nullptr; + _system_thread = new std::thread(&SystemImpl::system_thread, this); register_mavlink_message_handler( @@ -328,10 +330,33 @@ std::string SystemImpl::component_name(uint8_t component_id) } } +ComponentType SystemImpl::component_type(uint8_t component_id) +{ + switch (component_id) { + case MAV_COMP_ID_AUTOPILOT1: + return AUTOPILOT; + case MAV_COMP_ID_CAMERA: + case MAV_COMP_ID_CAMERA2: + case MAV_COMP_ID_CAMERA3: + case MAV_COMP_ID_CAMERA4: + case MAV_COMP_ID_CAMERA5: + case MAV_COMP_ID_CAMERA6: + return CAMERA; + case MAV_COMP_ID_GIMBAL: + return GIMBAL; + default: + return UNKNOWN; + } +} + void SystemImpl::add_new_component(uint8_t component_id) { 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."; } } @@ -341,6 +366,18 @@ size_t SystemImpl::total_components() const return _components.size(); } +void SystemImpl::register_component_discovered_callback(discover_callback_t callback) +{ + component_discovered_callback = callback; + + if (total_components() > 0) { + for (const auto &elem : _components) { + const ComponentType type = component_type(elem); + call_user_callback([this, type]() { component_discovered_callback(type); }); + } + } +} + bool SystemImpl::is_standalone() const { return !has_autopilot(); diff --git a/core/system_impl.h b/core/system_impl.h index 37d8ae33bb..db56a52061 100644 --- a/core/system_impl.h +++ b/core/system_impl.h @@ -7,6 +7,7 @@ #include "timeout_handler.h" #include "call_every_handler.h" #include "thread_pool.h" +#include "system.h" #include #include #include @@ -97,6 +98,9 @@ class SystemImpl { void add_new_component(uint8_t component_id); size_t total_components() const; + void register_component_discovered_callback(discover_callback_t callback); + discover_callback_t component_discovered_callback; + uint8_t get_autopilot_id() const; std::vector get_camera_ids() const; uint8_t get_gimbal_id() const; @@ -190,6 +194,7 @@ class SystemImpl { void set_disconnected(); static std::string component_name(uint8_t component_id); + static ComponentType component_type(uint8_t component_id); void system_thread(); void send_heartbeat(); diff --git a/example/takeoff_land/takeoff_and_land.cpp b/example/takeoff_land/takeoff_and_land.cpp index a05547115e..3332f0af01 100644 --- a/example/takeoff_land/takeoff_and_land.cpp +++ b/example/takeoff_land/takeoff_and_land.cpp @@ -5,6 +5,7 @@ #include #include +#include #include #include #include @@ -29,6 +30,12 @@ void usage(std::string bin_name) << "For example, to connect to the simulator use URL: udp://:14540" << std::endl; } +void component_discovered(ComponentType component_type) +{ + std::cout << NORMAL_CONSOLE_TEXT << "Discovered a component with type " + << unsigned(component_type) << std::endl; +} + int main(int argc, char **argv) { DronecodeSDK dc; @@ -51,6 +58,11 @@ int main(int argc, char **argv) return 1; } + // We don't need to specify the UUID if it's only one system anyway. + // If there were multiple, we could specify it with: + // dc.system(uint64_t uuid); + System &system = dc.system(); + std::cout << "Waiting to discover system..." << std::endl; dc.register_on_discover([&discovered_system](uint64_t uuid) { std::cout << "Discovered system with UUID: " << uuid << std::endl; @@ -67,10 +79,9 @@ int main(int argc, char **argv) return 1; } - // We don't need to specify the UUID if it's only one system anyway. - // If there were multiple, we could specify it with: - // dc.system(uint64_t uuid); - System &system = dc.system(); + // Register a callback so we get told when components (camera, gimbal) etc + // are found. + system.register_component_discovered_callback(component_discovered); auto telemetry = std::make_shared(system); auto action = std::make_shared(system);