Add configuration option

core/dronecode_sdk.cpp

@@ -37,6 +37,11 @@ ConnectionResult DronecodeSDK::add_serial_connection(const std::string &dev_path
     return _impl->add_serial_connection(dev_path, baudrate);
 }
 
+void DronecodeSDK::set_configuration(Configuration configuration)
+{
+    _impl->set_configuration(configuration);
+}
+
 std::vector<uint64_t> DronecodeSDK::system_uuids() const
 {
     return _impl->get_system_uuids();

core/dronecode_sdk_impl.cpp

@@ -190,6 +190,11 @@ void DronecodeSDKImpl::add_connection(std::shared_ptr<Connection> new_connection
     _connections.push_back(new_connection);
 }
 
+void DronecodeSDKImpl::set_configuration(DronecodeSDK::Configuration configuration)
+{
+    _configuration = configuration;
+}
+
 std::vector<uint64_t> DronecodeSDKImpl::get_system_uuids() const
 {
     std::vector<uint64_t> uuids = {};
@@ -250,6 +255,54 @@ System &DronecodeSDKImpl::get_system(const uint64_t uuid)
     return *_systems[system_id];
 }
 
+uint8_t DronecodeSDKImpl::get_own_system_id() const
+{
+    switch (_configuration.load()) {
+        case DronecodeSDK::Configuration::GroundStation:
+            // FIXME: This doesn't make much sense actually but it seems to work.
+            return 0;
+
+        case DronecodeSDK::Configuration::CompanionComputer:
+            // FIXME: This should be the same as the drone but we need to
+            // add auto detection for it.
+            return 1;
+
+        default:
+            LogErr() << "Unknown configuration";
+            return 0;
+    }
+}
+
+uint8_t DronecodeSDKImpl::get_own_component_id() const
+{
+    switch (_configuration.load()) {
+        case DronecodeSDK::Configuration::GroundStation:
+            // FIXME: For now we increment by 1 to avoid conflicts with others.
+            return MAV_COMP_ID_MISSIONPLANNER + 1;
+
+        case DronecodeSDK::Configuration::CompanionComputer:
+            // It's at least a possibility that we are bridging MAVLink traffic.
+            return MAV_COMP_ID_UDP_BRIDGE;
+
+        default:
+            LogErr() << "Unknown configuration";
+            return 0;
+    }
+}
+
+uint8_t DronecodeSDKImpl::get_mav_type() const
+{
+    switch (_configuration.load()) {
+        case DronecodeSDK::Configuration::GroundStation:
+            return MAV_TYPE_GCS;
+        case DronecodeSDK::Configuration::CompanionComputer:
+            return MAV_TYPE_ONBOARD_CONTROLLER;
+        default:
+            LogErr() << "Unknown configuration";
+            return 0;
+    }
+}
+
 bool DronecodeSDKImpl::is_connected() const
 {
     std::lock_guard<std::recursive_mutex> lock(_systems_mutex);

core/dronecode_sdk_impl.h

@@ -27,10 +27,16 @@ class DronecodeSDKImpl {
     ConnectionResult add_tcp_connection(const std::string &remote_ip, int remote_port);
     ConnectionResult add_serial_connection(const std::string &dev_path, int baudrate);
 
+    void set_configuration(DronecodeSDK::Configuration configuration);
+
     std::vector<uint64_t> get_system_uuids() const;
     System &get_system();
     System &get_system(uint64_t uuid);
 
+    uint8_t get_own_system_id() const;
+    uint8_t get_own_component_id() const;
+    uint8_t get_mav_type() const;
+
     bool is_connected() const;
     bool is_connected(uint64_t uuid) const;
 
@@ -56,6 +62,9 @@ class DronecodeSDKImpl {
     DronecodeSDK::event_callback_t _on_discover_callback;
     DronecodeSDK::event_callback_t _on_timeout_callback;
 
+    std::atomic<DronecodeSDK::Configuration> _configuration{
+        DronecodeSDK::Configuration::GroundStation};
+
     std::atomic<bool> _should_exit = {false};
 };
 

core/include/dronecode_sdk/dronecode_sdk.h

@@ -114,6 +114,26 @@ class DronecodeSDK {
     ConnectionResult add_serial_connection(const std::string &dev_path,
                                            int baudrate = DEFAULT_SERIAL_BAUDRATE);
 
+    /**
+     * @brief Possible configurations.
+     */
+    enum class Configuration {
+        GroundStation, /**< @brief SDK is used as a ground station. */
+        CompanionComputer /**< @brief SDK is used on a companion computer onboard the system (e.g.
+                             drone). */
+    };
+
+    /**
+     * @brief Set `Configuration` of SDK.
+     *
+     * The default configuration is `Configuration::GroundStation`
+     * The configuration is used in order to set the MAVLink system ID, the
+     * component ID, as well as the MAV_TYPE accordingly.
+     *
+     * @param configuration Configuration chosen.
+     */
+    void set_configuration(Configuration configuration);
+
     /**
      * @brief Get vector of system UUIDs.
      *

core/mavlink_commands.cpp

@@ -73,8 +73,8 @@ void MAVLinkCommands::queue_command_async(const CommandInt &command,
     //  << (int)(command.target_system_id)<< ", " << (int)(command.target_component_id;
 
     Work new_work{};
-    mavlink_msg_command_int_pack(GCSClient::system_id,
-                                 GCSClient::component_id,
+    mavlink_msg_command_int_pack(_parent.get_own_system_id(),
+                                 _parent.get_own_component_id(),
                                  &new_work.mavlink_message,
                                  command.target_system_id,
                                  command.target_component_id,
@@ -102,8 +102,8 @@ void MAVLinkCommands::queue_command_async(const CommandLong &command,
     //  << (int)(command.target_system_id)<< ", " << (int)(command.target_component_id;
 
     Work new_work{};
-    mavlink_msg_command_long_pack(GCSClient::system_id,
-                                  GCSClient::component_id,
+    mavlink_msg_command_long_pack(_parent.get_own_system_id(),
+                                  _parent.get_own_component_id(),
                                   &new_work.mavlink_message,
                                   command.target_system_id,
                                   command.target_component_id,

core/mavlink_parameters.cpp

@@ -162,8 +162,8 @@ void MAVLinkParameters::do_work()
                 work->param_value.get_128_bytes(param_value_buf);
 
                 // FIXME: extended currently always go to the camera component
-                mavlink_msg_param_ext_set_pack(GCSClient::system_id,
-                                               GCSClient::component_id,
+                mavlink_msg_param_ext_set_pack(_parent.get_own_system_id(),
+                                               _parent.get_own_component_id(),
                                                &message,
                                                _parent.get_system_id(),
                                                MAV_COMP_ID_CAMERA,
@@ -172,8 +172,8 @@ void MAVLinkParameters::do_work()
                                                work->param_value.get_mav_param_ext_type());
             } else {
                 // Param set is intended for Autopilot only.
-                mavlink_msg_param_set_pack(GCSClient::system_id,
-                                           GCSClient::component_id,
+                mavlink_msg_param_set_pack(_parent.get_own_system_id(),
+                                           _parent.get_own_component_id(),
                                            &message,
                                            _parent.get_system_id(),
                                            _parent.get_autopilot_id(),
@@ -203,8 +203,8 @@ void MAVLinkParameters::do_work()
         case WorkItem::Type::Get: {
             // LogDebug() << "now getting: " << work->param_name;
             if (work->extended) {
-                mavlink_msg_param_ext_request_read_pack(GCSClient::system_id,
-                                                        GCSClient::component_id,
+                mavlink_msg_param_ext_request_read_pack(_parent.get_own_system_id(),
+                                                        _parent.get_own_component_id(),
                                                         &message,
                                                         _parent.get_system_id(),
                                                         MAV_COMP_ID_CAMERA,
@@ -213,14 +213,14 @@ void MAVLinkParameters::do_work()
 
             } else {
                 // LogDebug() << "request read: "
-                //    << (int)GCSClient::system_id << ":"
-                //    << (int)GCSClient::component_id <<
+                //    << (int)_parent.get_own_system_id() << ":"
+                //    << (int)_parent.get_own_component_id() <<
                 //    " to "
                 //    << (int)_parent.get_system_id() << ":"
                 //    << (int)_parent.get_autopilot_id();
 
-                mavlink_msg_param_request_read_pack(GCSClient::system_id,
-                                                    GCSClient::component_id,
+                mavlink_msg_param_request_read_pack(_parent.get_own_system_id(),
+                                                    _parent.get_own_component_id(),
                                                     &message,
                                                     _parent.get_system_id(),
                                                     _parent.get_autopilot_id(),

core/system_impl.cpp

@@ -457,10 +457,10 @@ void SystemImpl::send_heartbeat()
 {
     mavlink_message_t message;
     // GCSClient is not autopilot!; hence MAV_AUTOPILOT_INVALID.
-    mavlink_msg_heartbeat_pack(GCSClient::system_id,
-                               GCSClient::component_id,
+    mavlink_msg_heartbeat_pack(get_own_system_id(),
+                               get_own_component_id(),
                                &message,
-                               MAV_TYPE_GCS,
+                               get_own_mav_type(),
                                MAV_AUTOPILOT_INVALID,
                                0,
                                0,
@@ -607,6 +607,21 @@ void SystemImpl::set_system_id(uint8_t system_id)
     _system_id = system_id;
 }
 
+uint8_t SystemImpl::get_own_system_id() const
+{
+    return _parent.get_own_system_id();
+}
+
+uint8_t SystemImpl::get_own_component_id() const
+{
+    return _parent.get_own_component_id();
+}
+
+uint8_t SystemImpl::get_own_mav_type() const
+{
+    return _parent.get_mav_type();
+}
+
 MAVLinkParameters::Result SystemImpl::set_param_float(const std::string &name, float value)
 {
     MAVLinkParameters::ParamValue param_value;

core/system_impl.h

@@ -26,15 +26,6 @@ namespace dronecode_sdk {
 class DronecodeSDKImpl;
 class PluginImplBase;
 
-// GCS: Ground Control Station
-// Type that represents DronecodeSDK client application which is a GCS.
-struct GCSClient {
-    static constexpr uint8_t system_id = 0;
-    // FIXME: This is a workaround for now. We should revert it later or add a compid for the SDK.
-    static constexpr uint8_t component_id = MAV_COMP_ID_SYSTEM_CONTROL + 1;
-    static constexpr MAV_TYPE type = MAV_TYPE_GCS;
-};
-
 // This class is the pimpl of System. This is to hide the private methods
 // and functionality from the public library API.
 class SystemImpl {
@@ -112,6 +103,10 @@ class SystemImpl {
 
     void set_system_id(uint8_t system_id);
 
+    uint8_t get_own_system_id() const;
+    uint8_t get_own_component_id() const;
+    uint8_t get_own_mav_type() const;
+
     bool does_support_mission_int() const { return _supports_mission_int; }
 
     bool is_armed() const { return _armed; }

plugins/camera/camera_impl.cpp

@@ -312,8 +312,8 @@ CameraImpl::make_message_set_video_stream_settings(const Camera::VideoStreamSett
 {
     mavlink_message_t msg;
 
-    mavlink_msg_set_video_stream_settings_pack(GCSClient::system_id,
-                                               GCSClient::component_id,
+    mavlink_msg_set_video_stream_settings_pack(_parent->get_own_system_id(),
+                                               _parent->get_own_component_id(),
                                                &msg,
                                                _parent->get_system_id(),
                                                _camera_id + MAV_COMP_ID_CAMERA,

plugins/follow_me/follow_me_impl.cpp

@@ -272,8 +272,8 @@ void FollowMeImpl::send_target_location()
     uint64_t custom_state = 0;
 
     mavlink_message_t msg{};
-    mavlink_msg_follow_target_pack(GCSClient::system_id,
-                                   GCSClient::component_id,
+    mavlink_msg_follow_target_pack(_parent->get_own_system_id(),
+                                   _parent->get_own_component_id(),
                                    &msg,
                                    elapsed_msec,
                                    _estimatation_capabilities,

plugins/log_files/log_files_impl.cpp

@@ -42,8 +42,8 @@ void LogFilesImpl::disable() {}
 void LogFilesImpl::request_end()
 {
     mavlink_message_t msg;
-    mavlink_msg_log_request_end_pack(GCSClient::system_id,
-                                     GCSClient::component_id,
+    mavlink_msg_log_request_end_pack(_parent->get_own_system_id(),
+                                     _parent->get_own_component_id(),
                                      &msg,
                                      _parent->get_system_id(),
                                      MAV_COMP_ID_AUTOPILOT1);
@@ -90,8 +90,8 @@ void LogFilesImpl::request_list_entry(int entry_id)
     }
 
     mavlink_message_t msg;
-    mavlink_msg_log_request_list_pack(GCSClient::system_id,
-                                      GCSClient::component_id,
+    mavlink_msg_log_request_list_pack(_parent->get_own_system_id(),
+                                      _parent->get_own_component_id(),
                                       &msg,
                                       _parent->get_system_id(),
                                       MAV_COMP_ID_AUTOPILOT1,
@@ -380,8 +380,8 @@ void LogFilesImpl::check_missing_log_data()
 void LogFilesImpl::request_log_data(unsigned id, unsigned chunk_id, unsigned bytes_to_get)
 {
     mavlink_message_t msg;
-    mavlink_msg_log_request_data_pack(GCSClient::system_id,
-                                      GCSClient::component_id,
+    mavlink_msg_log_request_data_pack(_parent->get_own_system_id(),
+                                      _parent->get_own_component_id(),
                                       &msg,
                                       _parent->get_system_id(),
                                       MAV_COMP_ID_AUTOPILOT1,

plugins/mavlink_passthrough/mavlink_passthrough_impl.cpp

@@ -43,12 +43,12 @@ void MavlinkPassthroughImpl::subscribe_message_async(
 
 uint8_t MavlinkPassthroughImpl::get_our_sysid() const
 {
-    return GCSClient::system_id;
+    return _parent->get_own_system_id();
 }
 
 uint8_t MavlinkPassthroughImpl::get_our_compid() const
 {
-    return GCSClient::component_id;
+    return _parent->get_own_component_id();
 }
 
 uint8_t MavlinkPassthroughImpl::get_target_sysid() const