From 5b8e768bc0e87c5983ca2746ffa79b678875a6e4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 27 Mar 2019 13:16:57 +0100 Subject: [PATCH 1/4] core: add configuration option This adds a configuration option in order to set the system and component ID of the SDK, as well as correctly set the MAV_TYPE. --- core/dronecode_sdk.cpp | 5 ++ core/dronecode_sdk_impl.cpp | 53 ++++++++++++++++++++++ core/dronecode_sdk_impl.h | 9 ++++ core/include/dronecode_sdk/dronecode_sdk.h | 20 ++++++++ core/system_impl.cpp | 15 ++++++ core/system_impl.h | 13 ++---- 6 files changed, 106 insertions(+), 9 deletions(-) diff --git a/core/dronecode_sdk.cpp b/core/dronecode_sdk.cpp index 4660a12c5c..23cc197b54 100644 --- a/core/dronecode_sdk.cpp +++ b/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 DronecodeSDK::system_uuids() const { return _impl->get_system_uuids(); diff --git a/core/dronecode_sdk_impl.cpp b/core/dronecode_sdk_impl.cpp index 57b5ea488d..70c6cbc21a 100644 --- a/core/dronecode_sdk_impl.cpp +++ b/core/dronecode_sdk_impl.cpp @@ -190,6 +190,11 @@ void DronecodeSDKImpl::add_connection(std::shared_ptr new_connection _connections.push_back(new_connection); } +void DronecodeSDKImpl::set_configuration(DronecodeSDK::Configuration configuration) +{ + _configuration = configuration; +} + std::vector DronecodeSDKImpl::get_system_uuids() const { std::vector 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 bridgin 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 lock(_systems_mutex); diff --git a/core/dronecode_sdk_impl.h b/core/dronecode_sdk_impl.h index 37eb478d8c..233dd988a6 100644 --- a/core/dronecode_sdk_impl.h +++ b/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 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 _configuration{ + DronecodeSDK::Configuration::GroundStation}; + std::atomic _should_exit = {false}; }; diff --git a/core/include/dronecode_sdk/dronecode_sdk.h b/core/include/dronecode_sdk/dronecode_sdk.h index 77b047e2c3..96372f964e 100644 --- a/core/include/dronecode_sdk/dronecode_sdk.h +++ b/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. * diff --git a/core/system_impl.cpp b/core/system_impl.cpp index 0ab0cfe01b..b651a43f19 100644 --- a/core/system_impl.cpp +++ b/core/system_impl.cpp @@ -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; diff --git a/core/system_impl.h b/core/system_impl.h index 23eedfe7d9..49d9e8f720 100644 --- a/core/system_impl.h +++ b/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; } From 38878951b46a42a311e15e692e0b19badaf97a5b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 27 Mar 2019 13:18:43 +0100 Subject: [PATCH 2/4] core: use new system/component ID and MAV_TYPE --- core/mavlink_commands.cpp | 8 ++++---- core/mavlink_parameters.cpp | 20 ++++++++++---------- core/system_impl.cpp | 6 +++--- 3 files changed, 17 insertions(+), 17 deletions(-) diff --git a/core/mavlink_commands.cpp b/core/mavlink_commands.cpp index 64df24dec3..3b38e76893 100644 --- a/core/mavlink_commands.cpp +++ b/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, diff --git a/core/mavlink_parameters.cpp b/core/mavlink_parameters.cpp index cced0b61e5..22e5ab935e 100644 --- a/core/mavlink_parameters.cpp +++ b/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(), diff --git a/core/system_impl.cpp b/core/system_impl.cpp index b651a43f19..14061e61fe 100644 --- a/core/system_impl.cpp +++ b/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, From 0b18ca36c5828d848cfd4af64c5f34d2c2797af7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 27 Mar 2019 13:19:27 +0100 Subject: [PATCH 3/4] plugins: use new system/comonent ID API --- plugins/camera/camera_impl.cpp | 4 +- plugins/follow_me/follow_me_impl.cpp | 4 +- plugins/log_files/log_files_impl.cpp | 12 ++-- .../mavlink_passthrough_impl.cpp | 4 +- plugins/mission/mission_impl.cpp | 68 +++++++++---------- plugins/mission_raw/mission_raw_impl.cpp | 12 ++-- plugins/offboard/offboard_impl.cpp | 16 ++--- 7 files changed, 60 insertions(+), 60 deletions(-) diff --git a/plugins/camera/camera_impl.cpp b/plugins/camera/camera_impl.cpp index 4f0980a2cc..f36eabe4e5 100644 --- a/plugins/camera/camera_impl.cpp +++ b/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, diff --git a/plugins/follow_me/follow_me_impl.cpp b/plugins/follow_me/follow_me_impl.cpp index bb9374be53..39418f40e7 100644 --- a/plugins/follow_me/follow_me_impl.cpp +++ b/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, diff --git a/plugins/log_files/log_files_impl.cpp b/plugins/log_files/log_files_impl.cpp index ee633fd49f..5a1855849b 100644 --- a/plugins/log_files/log_files_impl.cpp +++ b/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, diff --git a/plugins/mavlink_passthrough/mavlink_passthrough_impl.cpp b/plugins/mavlink_passthrough/mavlink_passthrough_impl.cpp index 3612554b50..160bc2dedc 100644 --- a/plugins/mavlink_passthrough/mavlink_passthrough_impl.cpp +++ b/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 diff --git a/plugins/mission/mission_impl.cpp b/plugins/mission/mission_impl.cpp index c05452a0d5..9732b6d75d 100644 --- a/plugins/mission/mission_impl.cpp +++ b/plugins/mission/mission_impl.cpp @@ -72,8 +72,8 @@ void MissionImpl::process_mission_request(const mavlink_message_t &unused) UNUSED(unused); mavlink_message_t message; - mavlink_msg_mission_ack_pack(GCSClient::system_id, - GCSClient::component_id, + mavlink_msg_mission_ack_pack(_parent->get_own_system_id(), + _parent->get_own_component_id(), &message, _parent->get_system_id(), _parent->get_autopilot_id(), @@ -91,8 +91,8 @@ void MissionImpl::process_mission_request_int(const mavlink_message_t &message) mavlink_mission_request_int_t mission_request_int; mavlink_msg_mission_request_int_decode(&message, &mission_request_int); - if (mission_request_int.target_system != GCSClient::system_id && - mission_request_int.target_component != GCSClient::component_id) { + if (mission_request_int.target_system != _parent->get_own_system_id() && + mission_request_int.target_component != _parent->get_own_component_id()) { LogWarn() << "Ignore mission request int that is not for us"; return; } @@ -139,8 +139,8 @@ void MissionImpl::process_mission_ack(const mavlink_message_t &message) mavlink_mission_ack_t mission_ack; mavlink_msg_mission_ack_decode(&message, &mission_ack); - if (mission_ack.target_system != GCSClient::system_id && - mission_ack.target_component != GCSClient::component_id) { + if (mission_ack.target_system != _parent->get_own_system_id() && + mission_ack.target_component != _parent->get_own_component_id()) { LogWarn() << "Ignore mission ack that is not for us"; return; } @@ -279,8 +279,8 @@ void MissionImpl::process_mission_item_int(const mavlink_message_t &message) _parent->unregister_timeout_handler(_timeout_cookie); mavlink_message_t ack_message; - mavlink_msg_mission_ack_pack(GCSClient::system_id, - GCSClient::component_id, + mavlink_msg_mission_ack_pack(_parent->get_own_system_id(), + _parent->get_own_component_id(), &ack_message, _parent->get_system_id(), _parent->get_autopilot_id(), @@ -363,8 +363,8 @@ void MissionImpl::upload_mission_async( void MissionImpl::send_count() { mavlink_message_t message; - mavlink_msg_mission_count_pack(GCSClient::system_id, - GCSClient::component_id, + mavlink_msg_mission_count_pack(_parent->get_own_system_id(), + _parent->get_own_component_id(), &message, _parent->get_system_id(), _parent->get_autopilot_id(), @@ -399,8 +399,8 @@ void MissionImpl::upload_mission_cancel() report_mission_result(_mission_data.result_callback, Mission::Result::CANCELLED); mavlink_message_t message; - mavlink_msg_mission_ack_pack(GCSClient::system_id, - GCSClient::component_id, + mavlink_msg_mission_ack_pack(_parent->get_own_system_id(), + _parent->get_own_component_id(), &message, _parent->get_system_id(), _parent->get_autopilot_id(), @@ -456,8 +456,8 @@ void MissionImpl::download_mission_async( void MissionImpl::request_list() { mavlink_message_t message; - mavlink_msg_mission_request_list_pack(GCSClient::system_id, - GCSClient::component_id, + mavlink_msg_mission_request_list_pack(_parent->get_own_system_id(), + _parent->get_own_component_id(), &message, _parent->get_system_id(), _parent->get_autopilot_id(), @@ -493,8 +493,8 @@ void MissionImpl::download_mission_cancel() Mission::Result::CANCELLED); mavlink_message_t message; - mavlink_msg_mission_ack_pack(GCSClient::system_id, - GCSClient::component_id, + mavlink_msg_mission_ack_pack(_parent->get_own_system_id(), + _parent->get_own_component_id(), &message, _parent->get_system_id(), _parent->get_autopilot_id(), @@ -539,8 +539,8 @@ void MissionImpl::assemble_mavlink_messages() uint8_t current = ((_mission_data.mavlink_mission_item_messages.size() == 0) ? 1 : 0); auto message = std::make_shared(); - mavlink_msg_mission_item_int_pack(GCSClient::system_id, - GCSClient::component_id, + mavlink_msg_mission_item_int_pack(_parent->get_own_system_id(), + _parent->get_own_component_id(), message.get(), _parent->get_system_id(), _parent->get_autopilot_id(), @@ -578,8 +578,8 @@ void MissionImpl::assemble_mavlink_messages() uint8_t autocontinue = 1; auto message_speed = std::make_shared(); - mavlink_msg_mission_item_int_pack(GCSClient::system_id, - GCSClient::component_id, + mavlink_msg_mission_item_int_pack(_parent->get_own_system_id(), + _parent->get_own_component_id(), message_speed.get(), _parent->get_system_id(), _parent->get_autopilot_id(), @@ -615,8 +615,8 @@ void MissionImpl::assemble_mavlink_messages() auto message_gimbal_configure = std::make_shared(); mavlink_msg_mission_item_int_pack( - GCSClient::system_id, - GCSClient::component_id, + _parent->get_own_system_id(), + _parent->get_own_component_id(), message_gimbal_configure.get(), _parent->get_system_id(), _parent->get_autopilot_id(), @@ -650,8 +650,8 @@ void MissionImpl::assemble_mavlink_messages() uint8_t autocontinue = 1; auto message_gimbal = std::make_shared(); - mavlink_msg_mission_item_int_pack(GCSClient::system_id, - GCSClient::component_id, + mavlink_msg_mission_item_int_pack(_parent->get_own_system_id(), + _parent->get_own_component_id(), message_gimbal.get(), _parent->get_system_id(), _parent->get_autopilot_id(), @@ -695,8 +695,8 @@ void MissionImpl::assemble_mavlink_messages() std::shared_ptr message_delay(new mavlink_message_t()); mavlink_msg_mission_item_int_pack( - GCSClient::system_id, - GCSClient::component_id, + _parent->get_own_system_id(), + _parent->get_own_component_id(), message_delay.get(), _parent->get_system_id(), _parent->get_autopilot_id(), @@ -769,8 +769,8 @@ void MissionImpl::assemble_mavlink_messages() } auto message_camera = std::make_shared(); - mavlink_msg_mission_item_int_pack(GCSClient::system_id, - GCSClient::component_id, + mavlink_msg_mission_item_int_pack(_parent->get_own_system_id(), + _parent->get_own_component_id(), message_camera.get(), _parent->get_system_id(), _parent->get_autopilot_id(), @@ -802,8 +802,8 @@ void MissionImpl::assemble_mavlink_messages() if (_enable_return_to_launch_after_mission) { std::shared_ptr message_rtl(new mavlink_message_t()); - mavlink_msg_mission_item_int_pack(GCSClient::system_id, - GCSClient::component_id, + mavlink_msg_mission_item_int_pack(_parent->get_own_system_id(), + _parent->get_own_component_id(), message_rtl.get(), _parent->get_system_id(), _parent->get_autopilot_id(), @@ -974,8 +974,8 @@ void MissionImpl::download_next_mission_item() mavlink_message_t message; { std::lock_guard lock(_mission_data.mutex); - mavlink_msg_mission_request_int_pack(GCSClient::system_id, - GCSClient::component_id, + mavlink_msg_mission_request_int_pack(_parent->get_own_system_id(), + _parent->get_own_component_id(), &message, _parent->get_system_id(), _parent->get_autopilot_id(), @@ -1081,8 +1081,8 @@ void MissionImpl::set_current_mission_item_async(int current, Mission::result_ca } mavlink_message_t message; - mavlink_msg_mission_set_current_pack(GCSClient::system_id, - GCSClient::component_id, + mavlink_msg_mission_set_current_pack(_parent->get_own_system_id(), + _parent->get_own_component_id(), &message, _parent->get_system_id(), _parent->get_autopilot_id(), diff --git a/plugins/mission_raw/mission_raw_impl.cpp b/plugins/mission_raw/mission_raw_impl.cpp index 6c6f83846b..080bec9d62 100644 --- a/plugins/mission_raw/mission_raw_impl.cpp +++ b/plugins/mission_raw/mission_raw_impl.cpp @@ -127,8 +127,8 @@ void MissionRawImpl::request_list() // LogDebug() << "Requesting mission list (" << _mission_download.retries << ")"; mavlink_message_t message; - mavlink_msg_mission_request_list_pack(GCSClient::system_id, - GCSClient::component_id, + mavlink_msg_mission_request_list_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 MissionRawImpl::request_item() // << " (" << _mission_download.retries << ")"; mavlink_message_t message; - mavlink_msg_mission_request_int_pack(GCSClient::system_id, - GCSClient::component_id, + mavlink_msg_mission_request_int_pack(_parent->get_own_system_id(), + _parent->get_own_component_id(), &message, _parent->get_system_id(), _parent->get_autopilot_id(), @@ -291,8 +291,8 @@ void MissionRawImpl::send_ack() // LogDebug() << "Sending ack"; mavlink_message_t message; - mavlink_msg_mission_ack_pack(GCSClient::system_id, - GCSClient::component_id, + mavlink_msg_mission_ack_pack(_parent->get_own_system_id(), + _parent->get_own_component_id(), &message, _parent->get_system_id(), _parent->get_autopilot_id(), diff --git a/plugins/offboard/offboard_impl.cpp b/plugins/offboard/offboard_impl.cpp index 60a46132b7..2737c1941d 100644 --- a/plugins/offboard/offboard_impl.cpp +++ b/plugins/offboard/offboard_impl.cpp @@ -245,8 +245,8 @@ void OffboardImpl::send_position_ned() mavlink_message_t message; mavlink_msg_set_position_target_local_ned_pack( - GCSClient::system_id, - GCSClient::component_id, + _parent->get_own_system_id(), + _parent->get_own_component_id(), &message, static_cast(_parent->get_time().elapsed_s() * 1e3), _parent->get_system_id(), @@ -298,8 +298,8 @@ void OffboardImpl::send_velocity_ned() mavlink_message_t message; mavlink_msg_set_position_target_local_ned_pack( - GCSClient::system_id, - GCSClient::component_id, + _parent->get_own_system_id(), + _parent->get_own_component_id(), &message, static_cast(_parent->get_time().elapsed_s() * 1e3), _parent->get_system_id(), @@ -351,8 +351,8 @@ void OffboardImpl::send_velocity_body() mavlink_message_t message; mavlink_msg_set_position_target_local_ned_pack( - GCSClient::system_id, - GCSClient::component_id, + _parent->get_own_system_id(), + _parent->get_own_component_id(), &message, static_cast(_parent->get_time().elapsed_s() * 1e3), _parent->get_system_id(), @@ -393,8 +393,8 @@ void OffboardImpl::send_attitude_rate() mavlink_message_t message; mavlink_msg_set_attitude_target_pack( - GCSClient::system_id, - GCSClient::component_id, + _parent->get_own_system_id(), + _parent->get_own_component_id(), &message, static_cast(_parent->get_time().elapsed_s() * 1e3), _parent->get_system_id(), From 00750080edf8f1d729ba3a488fdeab90425ea120 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 27 Mar 2019 14:19:19 +0100 Subject: [PATCH 4/4] core: fix slang typo --- core/dronecode_sdk_impl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/dronecode_sdk_impl.cpp b/core/dronecode_sdk_impl.cpp index 70c6cbc21a..635aacc0f4 100644 --- a/core/dronecode_sdk_impl.cpp +++ b/core/dronecode_sdk_impl.cpp @@ -281,7 +281,7 @@ uint8_t DronecodeSDKImpl::get_own_component_id() const return MAV_COMP_ID_MISSIONPLANNER + 1; case DronecodeSDK::Configuration::CompanionComputer: - // It's at least a possibility that we are bridgin MAVLink traffic. + // It's at least a possibility that we are bridging MAVLink traffic. return MAV_COMP_ID_UDP_BRIDGE; default: