From 8f93b1f6a25186b57c6fd0cd75cd25a56c43dc61 Mon Sep 17 00:00:00 2001 From: Yassine HABIB Date: Thu, 8 Oct 2020 14:50:23 +0200 Subject: [PATCH 1/4] Added Mavlink command reception support --- src/core/mavlink_commands.cpp | 79 +++++++++++++++++++++++++++++++++++ src/core/mavlink_commands.h | 77 ++++++++++++++++++++++++++++++++++ src/core/system_impl.cpp | 17 ++++++++ src/core/system_impl.h | 6 +++ 4 files changed, 179 insertions(+) diff --git a/src/core/mavlink_commands.cpp b/src/core/mavlink_commands.cpp index 05f29b37ba..890ccfb4e6 100644 --- a/src/core/mavlink_commands.cpp +++ b/src/core/mavlink_commands.cpp @@ -20,10 +20,22 @@ MAVLinkCommands::MAVLinkCommands(SystemImpl& parent) : _parent(parent) MAVLINK_MSG_ID_COMMAND_ACK, std::bind(&MAVLinkCommands::receive_command_ack, this, std::placeholders::_1), this); + + _parent.register_mavlink_message_handler( + MAVLINK_MSG_ID_COMMAND_LONG, + std::bind(&MAVLinkCommands::receive_command_long, this, std::placeholders::_1), + this); + + _parent.register_mavlink_message_handler( + MAVLINK_MSG_ID_COMMAND_INT, + std::bind(&MAVLinkCommands::receive_command_int, this, std::placeholders::_1), + this); } MAVLinkCommands::~MAVLinkCommands() { + unregister_all_mavlink_command_handlers(this); + _parent.unregister_all_mavlink_message_handlers(this); } @@ -319,4 +331,71 @@ void MAVLinkCommands::call_callback( _parent.call_user_callback([callback, result, progress]() { callback(result, progress); }); } +void MAVLinkCommands::receive_command_int(mavlink_message_t message) +{ + mavlink_command_int_t command_int; + mavlink_msg_command_int_decode(&message, &command_int); + MAVLinkCommands::Command cmd(command_int); + + std::lock_guard lock(_mavlink_command_handler_table_mutex); + + for (auto it = _mavlink_command_handler_table.begin(); it != _mavlink_command_handler_table.end(); ++it) { + if (it->cmd_id == command_int.command) { + it->callback(cmd); + } + } +} + +void MAVLinkCommands::receive_command_long(mavlink_message_t message) +{ + mavlink_command_long_t command_long; + mavlink_msg_command_long_decode(&message, &command_long); + MAVLinkCommands::Command cmd(command_long); + + std::lock_guard lock(_mavlink_command_handler_table_mutex); + + for (auto it = _mavlink_command_handler_table.begin(); it != _mavlink_command_handler_table.end(); ++it) { + if (it->cmd_id == command_long.command) { + it->callback(cmd); + } + } +} + +void MAVLinkCommands::register_mavlink_command_handler( + uint16_t cmd_id, mavlink_command_handler_t callback, const void* cookie) +{ + std::lock_guard lock(_mavlink_command_handler_table_mutex); + + MAVLinkCommandHandlerTableEntry entry = {cmd_id, callback, cookie}; + _mavlink_command_handler_table.push_back(entry); +} + +void MAVLinkCommands::unregister_mavlink_command_handler(uint16_t cmd_id, const void* cookie) +{ + std::lock_guard lock(_mavlink_command_handler_table_mutex); + + for (auto it = _mavlink_command_handler_table.begin(); it != _mavlink_command_handler_table.end(); + /* no ++it */) { + if (it->cmd_id == cmd_id && it->cookie == cookie) { + it = _mavlink_command_handler_table.erase(it); + } else { + ++it; + } + } +} + +void MAVLinkCommands::unregister_all_mavlink_command_handlers(const void* cookie) +{ + std::lock_guard lock(_mavlink_command_handler_table_mutex); + + for (auto it = _mavlink_command_handler_table.begin(); it != _mavlink_command_handler_table.end(); + /* no ++it */) { + if (it->cookie == cookie) { + it = _mavlink_command_handler_table.erase(it); + } else { + ++it; + } + } +} + } // namespace mavsdk diff --git a/src/core/mavlink_commands.h b/src/core/mavlink_commands.h index fa44fe5313..87a3f7448b 100644 --- a/src/core/mavlink_commands.h +++ b/src/core/mavlink_commands.h @@ -105,6 +105,71 @@ class MAVLinkCommands { // Non-copyable MAVLinkCommands(const MAVLinkCommands&) = delete; const MAVLinkCommands& operator=(const MAVLinkCommands&) = delete; + + struct Command { + uint8_t target_system_id{0}; + uint8_t target_component_id{0}; + uint16_t command{0}; + + // Int + MAV_FRAME frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + bool current = 0; + bool autocontinue = false; + + // Long + uint8_t confirmation = 0; + + // Mixed + struct Params { + float param1 = NAN; + float param2 = NAN; + float param3 = NAN; + float param4 = NAN; + float param5 = NAN; + float param6 = NAN; + float param7 = NAN; + int32_t x = 0; + int32_t y = 0; + float z = NAN; + } params{}; + + Command(mavlink_command_int_t command_int) { + target_system_id = command_int.target_system; + target_component_id = command_int.target_component; + command = command_int.command; + frame = static_cast(command_int.frame); + current = command_int.current; + autocontinue = command_int.autocontinue; + params.param1 = command_int.param1; + params.param2 = command_int.param2; + params.param3 = command_int.param3; + params.param4 = command_int.param4; + params.x = command_int.x; + params.y = command_int.y; + params.z = command_int.z; + } + + Command(mavlink_command_long_t command_long) { + target_system_id = command_long.target_system; + target_component_id = command_long.target_component; + command = command_long.command; + confirmation = command_long.confirmation; + params.param1 = command_long.param1; + params.param2 = command_long.param2; + params.param3 = command_long.param3; + params.param4 = command_long.param4; + params.param5 = command_long.param5; + params.param6 = command_long.param6; + params.param7 = command_long.param7; + } + }; + + typedef std::function mavlink_command_handler_t; + void register_mavlink_command_handler( + uint16_t cmd_id, mavlink_command_handler_t callback, const void* cookie); + + void unregister_mavlink_command_handler(uint16_t cmd_id, const void* cookie); + void unregister_all_mavlink_command_handlers(const void* cookie); private: struct Work { @@ -126,6 +191,18 @@ class MAVLinkCommands { LockedQueue _work_queue{}; void* _timeout_cookie = nullptr; + + void receive_command_int(mavlink_message_t message); + void receive_command_long(mavlink_message_t message); + + struct MAVLinkCommandHandlerTableEntry { + uint16_t cmd_id; + mavlink_command_handler_t callback; + const void* cookie; // This is the identification to unregister. + }; + + std::mutex _mavlink_command_handler_table_mutex{}; + std::vector _mavlink_command_handler_table{}; }; } // namespace mavsdk diff --git a/src/core/system_impl.cpp b/src/core/system_impl.cpp index d8fa03963e..1ead8cf228 100644 --- a/src/core/system_impl.cpp +++ b/src/core/system_impl.cpp @@ -1229,4 +1229,21 @@ void SystemImpl::intercept_outgoing_messages(std::function Date: Thu, 22 Oct 2020 11:07:32 +0200 Subject: [PATCH 2/4] mavlink_commands: split functions and implementations between a MavlinkCommandSender and a MavlinkCommandReceiver class --- src/core/mavlink_commands.cpp | 78 ++++---- src/core/mavlink_commands.h | 172 +++++++++--------- src/core/system_impl.cpp | 77 ++++---- src/core/system_impl.h | 25 +-- src/plugins/action/action_impl.cpp | 66 +++---- src/plugins/action/action_impl.h | 4 +- src/plugins/calibration/calibration_impl.cpp | 60 +++--- src/plugins/calibration/calibration_impl.h | 4 +- src/plugins/camera/camera_impl.cpp | 76 ++++---- src/plugins/camera/camera_impl.h | 32 ++-- src/plugins/failure/failure_impl.cpp | 16 +- src/plugins/failure/failure_impl.h | 2 +- src/plugins/follow_me/follow_me_impl.cpp | 14 +- src/plugins/follow_me/follow_me_impl.h | 2 +- src/plugins/gimbal/gimbal_impl.cpp | 18 +- src/plugins/gimbal/gimbal_impl.h | 4 +- src/plugins/gimbal/gimbal_protocol_v1.cpp | 18 +- .../manual_control/manual_control_impl.cpp | 20 +- .../manual_control/manual_control_impl.h | 4 +- .../mavlink_passthrough_impl.cpp | 24 +-- .../mavlink_passthrough_impl.h | 2 +- src/plugins/mission/mission_impl.cpp | 24 +-- src/plugins/mission/mission_impl.h | 4 +- src/plugins/mission_raw/mission_raw_impl.cpp | 24 +-- src/plugins/mission_raw/mission_raw_impl.h | 4 +- src/plugins/offboard/offboard_impl.cpp | 16 +- src/plugins/offboard/offboard_impl.h | 4 +- src/plugins/telemetry/telemetry_impl.cpp | 16 +- src/plugins/telemetry/telemetry_impl.h | 4 +- src/plugins/tune/tune_impl.h | 2 +- tools/rename-enums-to-camelcase.sh | 16 +- 31 files changed, 427 insertions(+), 405 deletions(-) diff --git a/src/core/mavlink_commands.cpp b/src/core/mavlink_commands.cpp index 890ccfb4e6..bee7ee303e 100644 --- a/src/core/mavlink_commands.cpp +++ b/src/core/mavlink_commands.cpp @@ -14,32 +14,21 @@ namespace mavsdk { // - The queue used does not support going through and checking each and every // item yet. -MAVLinkCommands::MAVLinkCommands(SystemImpl& parent) : _parent(parent) +// MavlinkCommandSender class +MavlinkCommandSender::MavlinkCommandSender(SystemImpl& parent) : _parent(parent) { _parent.register_mavlink_message_handler( MAVLINK_MSG_ID_COMMAND_ACK, - std::bind(&MAVLinkCommands::receive_command_ack, this, std::placeholders::_1), - this); - - _parent.register_mavlink_message_handler( - MAVLINK_MSG_ID_COMMAND_LONG, - std::bind(&MAVLinkCommands::receive_command_long, this, std::placeholders::_1), - this); - - _parent.register_mavlink_message_handler( - MAVLINK_MSG_ID_COMMAND_INT, - std::bind(&MAVLinkCommands::receive_command_int, this, std::placeholders::_1), + std::bind(&MavlinkCommandSender::receive_command_ack, this, std::placeholders::_1), this); } -MAVLinkCommands::~MAVLinkCommands() +MavlinkCommandSender::~MavlinkCommandSender() { - unregister_all_mavlink_command_handlers(this); - _parent.unregister_all_mavlink_message_handlers(this); } -MAVLinkCommands::Result MAVLinkCommands::send_command(const MAVLinkCommands::CommandInt& command) +MavlinkCommandSender::Result MavlinkCommandSender::send_command(const MavlinkCommandSender::CommandInt& command) { // We wrap the async call with a promise and future. auto prom = std::make_shared>(); @@ -59,7 +48,7 @@ MAVLinkCommands::Result MAVLinkCommands::send_command(const MAVLinkCommands::Com return res.get(); } -MAVLinkCommands::Result MAVLinkCommands::send_command(const MAVLinkCommands::CommandLong& command) +MavlinkCommandSender::Result MavlinkCommandSender::send_command(const MavlinkCommandSender::CommandLong& command) { // We wrap the async call with a promise and future. auto prom = std::make_shared>(); @@ -78,7 +67,7 @@ MAVLinkCommands::Result MAVLinkCommands::send_command(const MAVLinkCommands::Com return res.get(); } -void MAVLinkCommands::queue_command_async(const CommandInt& command, CommandResultCallback callback) +void MavlinkCommandSender::queue_command_async(const CommandInt& command, CommandResultCallback callback) { // LogDebug() << "Command " << (int)(command.command) << " to send to " // << (int)(command.target_system_id)<< ", " << (int)(command.target_component_id); @@ -108,7 +97,7 @@ void MAVLinkCommands::queue_command_async(const CommandInt& command, CommandResu _work_queue.push_back(new_work); } -void MAVLinkCommands::queue_command_async( +void MavlinkCommandSender::queue_command_async( const CommandLong& command, CommandResultCallback callback) { // LogDebug() << "Command " << (int)(command.command) << " to send to " @@ -137,7 +126,7 @@ void MAVLinkCommands::queue_command_async( _work_queue.push_back(new_work); } -void MAVLinkCommands::receive_command_ack(mavlink_message_t message) +void MavlinkCommandSender::receive_command_ack(mavlink_message_t message) { mavlink_command_ack_t command_ack; mavlink_msg_command_ack_decode(&message, &command_ack); @@ -211,7 +200,7 @@ void MAVLinkCommands::receive_command_ack(mavlink_message_t message) // case where there is no progress update and we keep trying. _parent.unregister_timeout_handler(_timeout_cookie); _parent.register_timeout_handler( - std::bind(&MAVLinkCommands::receive_timeout, this), + std::bind(&MavlinkCommandSender::receive_timeout, this), work->retries_to_do * work->timeout_s, &_timeout_cookie); // FIXME: We can only call callbacks with promises once, so let's not do it @@ -231,7 +220,7 @@ void MAVLinkCommands::receive_command_ack(mavlink_message_t message) } } -void MAVLinkCommands::receive_timeout() +void MavlinkCommandSender::receive_timeout() { CommandResultCallback temp_callback = nullptr; std::pair temp_result{Result::UnknownError, NAN}; @@ -262,7 +251,7 @@ void MAVLinkCommands::receive_timeout() } else { --work->retries_to_do; _parent.register_timeout_handler( - std::bind(&MAVLinkCommands::receive_timeout, this), + std::bind(&MavlinkCommandSender::receive_timeout, this), work->timeout_s, &_timeout_cookie); } @@ -282,7 +271,7 @@ void MAVLinkCommands::receive_timeout() } } -void MAVLinkCommands::do_work() +void MavlinkCommandSender::do_work() { CommandResultCallback temp_callback = nullptr; std::pair temp_result{Result::UnknownError, NAN}; @@ -307,7 +296,7 @@ void MAVLinkCommands::do_work() } else { work->already_sent = true; _parent.register_timeout_handler( - std::bind(&MAVLinkCommands::receive_timeout, this), + std::bind(&MavlinkCommandSender::receive_timeout, this), work->timeout_s, &_timeout_cookie); } @@ -319,7 +308,7 @@ void MAVLinkCommands::do_work() } } -void MAVLinkCommands::call_callback( +void MavlinkCommandSender::call_callback( const CommandResultCallback& callback, Result result, float progress) { if (!callback) { @@ -330,12 +319,34 @@ void MAVLinkCommands::call_callback( // we lock ourselves out when we send a command in the callback receiving a command result. _parent.call_user_callback([callback, result, progress]() { callback(result, progress); }); } +// MavlinkCommandSender class + +// MavlinkCommandReceiver class +MavlinkCommandReceiver::MavlinkCommandReceiver(SystemImpl& parent) : _parent(parent) +{ + _parent.register_mavlink_message_handler( + MAVLINK_MSG_ID_COMMAND_LONG, + std::bind(&MavlinkCommandReceiver::receive_command_long, this, std::placeholders::_1), + this); + + _parent.register_mavlink_message_handler( + MAVLINK_MSG_ID_COMMAND_INT, + std::bind(&MavlinkCommandReceiver::receive_command_int, this, std::placeholders::_1), + this); +} + +MavlinkCommandReceiver::~MavlinkCommandReceiver() +{ + unregister_all_mavlink_command_handlers(this); + + _parent.unregister_all_mavlink_message_handlers(this); +} -void MAVLinkCommands::receive_command_int(mavlink_message_t message) +void MavlinkCommandReceiver::receive_command_int(mavlink_message_t message) { mavlink_command_int_t command_int; mavlink_msg_command_int_decode(&message, &command_int); - MAVLinkCommands::Command cmd(command_int); + MavlinkCommandReceiver::Command cmd(command_int); std::lock_guard lock(_mavlink_command_handler_table_mutex); @@ -346,11 +357,11 @@ void MAVLinkCommands::receive_command_int(mavlink_message_t message) } } -void MAVLinkCommands::receive_command_long(mavlink_message_t message) +void MavlinkCommandReceiver::receive_command_long(mavlink_message_t message) { mavlink_command_long_t command_long; mavlink_msg_command_long_decode(&message, &command_long); - MAVLinkCommands::Command cmd(command_long); + MavlinkCommandReceiver::Command cmd(command_long); std::lock_guard lock(_mavlink_command_handler_table_mutex); @@ -361,7 +372,7 @@ void MAVLinkCommands::receive_command_long(mavlink_message_t message) } } -void MAVLinkCommands::register_mavlink_command_handler( +void MavlinkCommandReceiver::register_mavlink_command_handler( uint16_t cmd_id, mavlink_command_handler_t callback, const void* cookie) { std::lock_guard lock(_mavlink_command_handler_table_mutex); @@ -370,7 +381,7 @@ void MAVLinkCommands::register_mavlink_command_handler( _mavlink_command_handler_table.push_back(entry); } -void MAVLinkCommands::unregister_mavlink_command_handler(uint16_t cmd_id, const void* cookie) +void MavlinkCommandReceiver::unregister_mavlink_command_handler(uint16_t cmd_id, const void* cookie) { std::lock_guard lock(_mavlink_command_handler_table_mutex); @@ -384,7 +395,7 @@ void MAVLinkCommands::unregister_mavlink_command_handler(uint16_t cmd_id, const } } -void MAVLinkCommands::unregister_all_mavlink_command_handlers(const void* cookie) +void MavlinkCommandReceiver::unregister_all_mavlink_command_handlers(const void* cookie) { std::lock_guard lock(_mavlink_command_handler_table_mutex); @@ -397,5 +408,6 @@ void MAVLinkCommands::unregister_all_mavlink_command_handlers(const void* cookie } } } +// MavlinkCommandReceiver class } // namespace mavsdk diff --git a/src/core/mavlink_commands.h b/src/core/mavlink_commands.h index 87a3f7448b..22d4f3795b 100644 --- a/src/core/mavlink_commands.h +++ b/src/core/mavlink_commands.h @@ -12,10 +12,95 @@ namespace mavsdk { class SystemImpl; -class MAVLinkCommands { +class MavlinkCommandReceiver { public: - explicit MAVLinkCommands(SystemImpl& parent); - ~MAVLinkCommands(); + explicit MavlinkCommandReceiver(SystemImpl& parent); + ~MavlinkCommandReceiver(); + + struct Command { + uint8_t target_system_id{0}; + uint8_t target_component_id{0}; + uint16_t command{0}; + + // Int + MAV_FRAME frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + bool current = 0; + bool autocontinue = false; + + // Long + uint8_t confirmation = 0; + + // Mixed + struct Params { + float param1 = NAN; + float param2 = NAN; + float param3 = NAN; + float param4 = NAN; + float param5 = NAN; + float param6 = NAN; + float param7 = NAN; + int32_t x = 0; + int32_t y = 0; + float z = NAN; + } params{}; + + Command(mavlink_command_int_t command_int) { + target_system_id = command_int.target_system; + target_component_id = command_int.target_component; + command = command_int.command; + frame = static_cast(command_int.frame); + current = command_int.current; + autocontinue = command_int.autocontinue; + params.param1 = command_int.param1; + params.param2 = command_int.param2; + params.param3 = command_int.param3; + params.param4 = command_int.param4; + params.x = command_int.x; + params.y = command_int.y; + params.z = command_int.z; + } + + Command(mavlink_command_long_t command_long) { + target_system_id = command_long.target_system; + target_component_id = command_long.target_component; + command = command_long.command; + confirmation = command_long.confirmation; + params.param1 = command_long.param1; + params.param2 = command_long.param2; + params.param3 = command_long.param3; + params.param4 = command_long.param4; + params.param5 = command_long.param5; + params.param6 = command_long.param6; + params.param7 = command_long.param7; + } + }; + + typedef std::function mavlink_command_handler_t; + void register_mavlink_command_handler( + uint16_t cmd_id, mavlink_command_handler_t callback, const void* cookie); + + void unregister_mavlink_command_handler(uint16_t cmd_id, const void* cookie); + void unregister_all_mavlink_command_handlers(const void* cookie); + + SystemImpl& _parent; + + void receive_command_int(mavlink_message_t message); + void receive_command_long(mavlink_message_t message); + + struct MAVLinkCommandHandlerTableEntry { + uint16_t cmd_id; + mavlink_command_handler_t callback; + const void* cookie; // This is the identification to unregister. + }; + + std::mutex _mavlink_command_handler_table_mutex{}; + std::vector _mavlink_command_handler_table{}; +}; + +class MavlinkCommandSender { +public: + explicit MavlinkCommandSender(SystemImpl& parent); + ~MavlinkCommandSender(); enum class Result { Success = 0, @@ -103,73 +188,8 @@ class MAVLinkCommands { static const int DEFAULT_COMPONENT_ID_AUTOPILOT = MAV_COMP_ID_AUTOPILOT1; // Non-copyable - MAVLinkCommands(const MAVLinkCommands&) = delete; - const MAVLinkCommands& operator=(const MAVLinkCommands&) = delete; - - struct Command { - uint8_t target_system_id{0}; - uint8_t target_component_id{0}; - uint16_t command{0}; - - // Int - MAV_FRAME frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; - bool current = 0; - bool autocontinue = false; - - // Long - uint8_t confirmation = 0; - - // Mixed - struct Params { - float param1 = NAN; - float param2 = NAN; - float param3 = NAN; - float param4 = NAN; - float param5 = NAN; - float param6 = NAN; - float param7 = NAN; - int32_t x = 0; - int32_t y = 0; - float z = NAN; - } params{}; - - Command(mavlink_command_int_t command_int) { - target_system_id = command_int.target_system; - target_component_id = command_int.target_component; - command = command_int.command; - frame = static_cast(command_int.frame); - current = command_int.current; - autocontinue = command_int.autocontinue; - params.param1 = command_int.param1; - params.param2 = command_int.param2; - params.param3 = command_int.param3; - params.param4 = command_int.param4; - params.x = command_int.x; - params.y = command_int.y; - params.z = command_int.z; - } - - Command(mavlink_command_long_t command_long) { - target_system_id = command_long.target_system; - target_component_id = command_long.target_component; - command = command_long.command; - confirmation = command_long.confirmation; - params.param1 = command_long.param1; - params.param2 = command_long.param2; - params.param3 = command_long.param3; - params.param4 = command_long.param4; - params.param5 = command_long.param5; - params.param6 = command_long.param6; - params.param7 = command_long.param7; - } - }; - - typedef std::function mavlink_command_handler_t; - void register_mavlink_command_handler( - uint16_t cmd_id, mavlink_command_handler_t callback, const void* cookie); - - void unregister_mavlink_command_handler(uint16_t cmd_id, const void* cookie); - void unregister_all_mavlink_command_handlers(const void* cookie); + MavlinkCommandSender(const MavlinkCommandSender&) = delete; + const MavlinkCommandSender& operator=(const MavlinkCommandSender&) = delete; private: struct Work { @@ -191,18 +211,6 @@ class MAVLinkCommands { LockedQueue _work_queue{}; void* _timeout_cookie = nullptr; - - void receive_command_int(mavlink_message_t message); - void receive_command_long(mavlink_message_t message); - - struct MAVLinkCommandHandlerTableEntry { - uint16_t cmd_id; - mavlink_command_handler_t callback; - const void* cookie; // This is the identification to unregister. - }; - - std::mutex _mavlink_command_handler_table_mutex{}; - std::vector _mavlink_command_handler_table{}; }; } // namespace mavsdk diff --git a/src/core/system_impl.cpp b/src/core/system_impl.cpp index 1ead8cf228..4e2b054de6 100644 --- a/src/core/system_impl.cpp +++ b/src/core/system_impl.cpp @@ -21,7 +21,8 @@ SystemImpl::SystemImpl(MavsdkImpl& parent, uint8_t system_id, uint8_t comp_id, b Sender(parent.own_address, _target_address), _parent(parent), _params(*this), - _commands(*this), + _send_commands(*this), + _receive_commands(*this), _timesync(*this), _call_every_handler(_time), _mission_transfer(*this, _message_handler, _parent.timeout_handler) @@ -159,7 +160,7 @@ void SystemImpl::process_heartbeat(const mavlink_message_t& message) mavlink_heartbeat_t heartbeat; mavlink_msg_heartbeat_decode(&message, &heartbeat); - if (message.compid == MAVLinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT) { + if (message.compid == MavlinkCommandSender::DEFAULT_COMPONENT_ID_AUTOPILOT) { _armed = ((heartbeat.base_mode & MAV_MODE_FLAG_SAFETY_ARMED) ? true : false); _hitl_enabled = ((heartbeat.base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? true : false); @@ -186,7 +187,7 @@ void SystemImpl::process_heartbeat(const mavlink_message_t& message) void SystemImpl::process_autopilot_version(const mavlink_message_t& message) { // Ignore if they don't come from the autopilot component - if (message.compid != MAVLinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT) { + if (message.compid != MavlinkCommandSender::DEFAULT_COMPONENT_ID_AUTOPILOT) { return; } @@ -241,7 +242,7 @@ void SystemImpl::system_thread() while (!_should_exit) { _call_every_handler.run_once(); _params.do_work(); - _commands.do_work(); + _send_commands.do_work(); _timesync.do_work(); _mission_transfer.do_work(); @@ -457,7 +458,7 @@ void SystemImpl::request_autopilot_version() void SystemImpl::send_autopilot_version_request() { // We don't care about an answer, we mostly care about receiving AUTOPILOT_VERSION. - MAVLinkCommands::CommandLong command{}; + MavlinkCommandSender::CommandLong command{}; command.command = MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES; command.params.param1 = 1.0f; @@ -469,7 +470,7 @@ void SystemImpl::send_autopilot_version_request() void SystemImpl::send_flight_information_request() { // We don't care about an answer, we mostly care about receiving FLIGHT_INFORMATION. - MAVLinkCommands::CommandLong command{}; + MavlinkCommandSender::CommandLong command{}; command.command = MAV_CMD_REQUEST_FLIGHT_INFORMATION; command.params.param1 = 1.0f; @@ -852,7 +853,7 @@ void SystemImpl::subscribe_param_float( cookie); } -std::pair +std::pair SystemImpl::make_command_flight_mode(FlightMode flight_mode, uint8_t component_id) { const uint8_t flag_safety_armed = is_armed() ? MAV_MODE_FLAG_SAFETY_ARMED : 0; @@ -907,11 +908,11 @@ SystemImpl::make_command_flight_mode(FlightMode flight_mode, uint8_t component_i break; default: LogErr() << "Unknown Flight mode."; - MAVLinkCommands::CommandLong empty_command{}; - return std::make_pair<>(MAVLinkCommands::Result::UnknownError, empty_command); + MavlinkCommandSender::CommandLong empty_command{}; + return std::make_pair<>(MavlinkCommandSender::Result::UnknownError, empty_command); } - MAVLinkCommands::CommandLong command{}; + MavlinkCommandSender::CommandLong command{}; command.command = MAV_CMD_DO_SET_MODE; command.params.param1 = float(mode); @@ -919,7 +920,7 @@ SystemImpl::make_command_flight_mode(FlightMode flight_mode, uint8_t component_i command.params.param3 = float(custom_sub_mode); command.target_component_id = component_id; - return std::make_pair<>(MAVLinkCommands::Result::Success, command); + return std::make_pair<>(MavlinkCommandSender::Result::Success, command); } SystemImpl::FlightMode SystemImpl::get_flight_mode() const @@ -971,12 +972,12 @@ SystemImpl::FlightMode SystemImpl::to_flight_mode_from_custom_mode(uint32_t cust } } -MAVLinkCommands::Result SystemImpl::set_flight_mode(FlightMode system_mode, uint8_t component_id) +MavlinkCommandSender::Result SystemImpl::set_flight_mode(FlightMode system_mode, uint8_t component_id) { - std::pair result = + std::pair result = make_command_flight_mode(system_mode, component_id); - if (result.first != MAVLinkCommands::Result::Success) { + if (result.first != MavlinkCommandSender::Result::Success) { return result.first; } @@ -986,10 +987,10 @@ MAVLinkCommands::Result SystemImpl::set_flight_mode(FlightMode system_mode, uint void SystemImpl::set_flight_mode_async( FlightMode system_mode, CommandResultCallback callback, uint8_t component_id) { - std::pair result = + std::pair result = make_command_flight_mode(system_mode, component_id); - if (result.first != MAVLinkCommands::Result::Success) { + if (result.first != MavlinkCommandSender::Result::Success) { if (callback) { callback(result.first, NAN); } @@ -1030,7 +1031,7 @@ void SystemImpl::receive_int_param( uint8_t SystemImpl::get_autopilot_id() const { for (auto compid : _components) - if (compid == MAVLinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT) { + if (compid == MavlinkCommandSender::DEFAULT_COMPONENT_ID_AUTOPILOT) { return compid; } // FIXME: Not sure what should be returned if autopilot is not found @@ -1057,70 +1058,70 @@ uint8_t SystemImpl::get_gimbal_id() const return uint8_t(0); } -MAVLinkCommands::Result SystemImpl::send_command(MAVLinkCommands::CommandLong& command) +MavlinkCommandSender::Result SystemImpl::send_command(MavlinkCommandSender::CommandLong& command) { if (_target_address.system_id == 0 && _components.size() == 0) { - return MAVLinkCommands::Result::NoSystem; + return MavlinkCommandSender::Result::NoSystem; } command.target_system_id = get_system_id(); - return _commands.send_command(command); + return _send_commands.send_command(command); } -MAVLinkCommands::Result SystemImpl::send_command(MAVLinkCommands::CommandInt& command) +MavlinkCommandSender::Result SystemImpl::send_command(MavlinkCommandSender::CommandInt& command) { if (_target_address.system_id == 0 && _components.size() == 0) { - return MAVLinkCommands::Result::NoSystem; + return MavlinkCommandSender::Result::NoSystem; } command.target_system_id = get_system_id(); - return _commands.send_command(command); + return _send_commands.send_command(command); } void SystemImpl::send_command_async( - MAVLinkCommands::CommandLong command, const CommandResultCallback callback) + MavlinkCommandSender::CommandLong command, const CommandResultCallback callback) { if (_target_address.system_id == 0 && _components.size() == 0) { if (callback) { - callback(MAVLinkCommands::Result::NoSystem, NAN); + callback(MavlinkCommandSender::Result::NoSystem, NAN); } return; } command.target_system_id = get_system_id(); - _commands.queue_command_async(command, callback); + _send_commands.queue_command_async(command, callback); } void SystemImpl::send_command_async( - MAVLinkCommands::CommandInt command, const CommandResultCallback callback) + MavlinkCommandSender::CommandInt command, const CommandResultCallback callback) { if (_target_address.system_id == 0 && _components.size() == 0) { if (callback) { - callback(MAVLinkCommands::Result::NoSystem, NAN); + callback(MavlinkCommandSender::Result::NoSystem, NAN); } return; } command.target_system_id = get_system_id(); - _commands.queue_command_async(command, callback); + _send_commands.queue_command_async(command, callback); } -MAVLinkCommands::Result +MavlinkCommandSender::Result SystemImpl::set_msg_rate(uint16_t message_id, double rate_hz, uint8_t component_id) { - MAVLinkCommands::CommandLong command = make_command_msg_rate(message_id, rate_hz, component_id); + MavlinkCommandSender::CommandLong command = make_command_msg_rate(message_id, rate_hz, component_id); return send_command(command); } void SystemImpl::set_msg_rate_async( uint16_t message_id, double rate_hz, CommandResultCallback callback, uint8_t component_id) { - MAVLinkCommands::CommandLong command = make_command_msg_rate(message_id, rate_hz, component_id); + MavlinkCommandSender::CommandLong command = make_command_msg_rate(message_id, rate_hz, component_id); send_command_async(command, callback); } -MAVLinkCommands::CommandLong +MavlinkCommandSender::CommandLong SystemImpl::make_command_msg_rate(uint16_t message_id, double rate_hz, uint8_t component_id) { - MAVLinkCommands::CommandLong command{}; + MavlinkCommandSender::CommandLong command{}; // 0 to request default rate, -1 to stop stream @@ -1230,20 +1231,20 @@ void SystemImpl::intercept_outgoing_messages(std::function + std::pair make_command_flight_mode(FlightMode mode, uint8_t component_id); - MAVLinkCommands::CommandLong + MavlinkCommandSender::CommandLong make_command_msg_rate(uint16_t message_id, double rate_hz, uint8_t component_id); static void receive_float_param( @@ -323,7 +323,8 @@ class SystemImpl : public Sender { static constexpr double _HEARTBEAT_SEND_INTERVAL_S = 1.0; MAVLinkParameters _params; - MAVLinkCommands _commands; + MavlinkCommandSender _send_commands; + MavlinkCommandReceiver _receive_commands; Timesync _timesync; diff --git a/src/plugins/action/action_impl.cpp b/src/plugins/action/action_impl.cpp index 189a2d38d5..33925371bc 100644 --- a/src/plugins/action/action_impl.cpp +++ b/src/plugins/action/action_impl.cpp @@ -48,7 +48,7 @@ void ActionImpl::enable() MAVLINK_MSG_ID_EXTENDED_SYS_STATE, 1.0, nullptr, - MAVLinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT); + MavlinkCommandSender::DEFAULT_COMPONENT_ID_AUTOPILOT); } void ActionImpl::disable() {} @@ -183,14 +183,14 @@ Action::Result ActionImpl::transition_to_multicopter() const void ActionImpl::arm_async(const Action::ResultCallback& callback) const { auto send_arm_command = [this, callback]() { - MAVLinkCommands::CommandLong command{}; + MavlinkCommandSender::CommandLong command{}; command.command = MAV_CMD_COMPONENT_ARM_DISARM; command.params.param1 = 1.0f; // arm command.target_component_id = _parent->get_autopilot_id(); _parent->send_command_async( - command, [this, callback](MAVLinkCommands::Result result, float) { + command, [this, callback](MavlinkCommandSender::Result result, float) { command_result_callback(result, callback); }); }; @@ -199,7 +199,7 @@ void ActionImpl::arm_async(const Action::ResultCallback& callback) const _parent->get_flight_mode() == SystemImpl::FlightMode::ReturnToLaunch) { _parent->set_flight_mode_async( SystemImpl::FlightMode::Hold, - [callback, send_arm_command](MAVLinkCommands::Result result, float) { + [callback, send_arm_command](MavlinkCommandSender::Result result, float) { Action::Result action_result = action_result_from_command_result(result); if (action_result != Action::Result::Success) { if (callback) { @@ -224,47 +224,47 @@ void ActionImpl::disarm_async(const Action::ResultCallback& callback) const return; } - MAVLinkCommands::CommandLong command{}; + MavlinkCommandSender::CommandLong command{}; command.command = MAV_CMD_COMPONENT_ARM_DISARM; command.params.param1 = 0.0f; // disarm command.target_component_id = _parent->get_autopilot_id(); - _parent->send_command_async(command, [this, callback](MAVLinkCommands::Result result, float) { + _parent->send_command_async(command, [this, callback](MavlinkCommandSender::Result result, float) { command_result_callback(result, callback); }); } void ActionImpl::terminate_async(const Action::ResultCallback& callback) const { - MAVLinkCommands::CommandLong command{}; + MavlinkCommandSender::CommandLong command{}; command.command = MAV_CMD_DO_FLIGHTTERMINATION; command.params.param1 = 1; command.target_component_id = _parent->get_autopilot_id(); - _parent->send_command_async(command, [this, callback](MAVLinkCommands::Result result, float) { + _parent->send_command_async(command, [this, callback](MavlinkCommandSender::Result result, float) { command_result_callback(result, callback); }); } void ActionImpl::kill_async(const Action::ResultCallback& callback) const { - MAVLinkCommands::CommandLong command{}; + MavlinkCommandSender::CommandLong command{}; command.command = MAV_CMD_COMPONENT_ARM_DISARM; command.params.param1 = 0.0f; // kill command.params.param2 = 21196.f; // magic number to enforce in-air command.target_component_id = _parent->get_autopilot_id(); - _parent->send_command_async(command, [this, callback](MAVLinkCommands::Result result, float) { + _parent->send_command_async(command, [this, callback](MavlinkCommandSender::Result result, float) { command_result_callback(result, callback); }); } void ActionImpl::reboot_async(const Action::ResultCallback& callback) const { - MAVLinkCommands::CommandLong command{}; + MavlinkCommandSender::CommandLong command{}; command.command = MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN; command.params.param1 = 1.0f; // reboot autopilot @@ -273,14 +273,14 @@ void ActionImpl::reboot_async(const Action::ResultCallback& callback) const command.params.param4 = 1.0f; // reboot gimbal command.target_component_id = _parent->get_autopilot_id(); - _parent->send_command_async(command, [this, callback](MAVLinkCommands::Result result, float) { + _parent->send_command_async(command, [this, callback](MavlinkCommandSender::Result result, float) { command_result_callback(result, callback); }); } void ActionImpl::shutdown_async(const Action::ResultCallback& callback) const { - MAVLinkCommands::CommandLong command{}; + MavlinkCommandSender::CommandLong command{}; command.command = MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN; command.params.param1 = 2.0f; // shutdown autopilot @@ -289,32 +289,32 @@ void ActionImpl::shutdown_async(const Action::ResultCallback& callback) const command.params.param4 = 2.0f; // shutdown gimbal command.target_component_id = _parent->get_autopilot_id(); - _parent->send_command_async(command, [this, callback](MAVLinkCommands::Result result, float) { + _parent->send_command_async(command, [this, callback](MavlinkCommandSender::Result result, float) { command_result_callback(result, callback); }); } void ActionImpl::takeoff_async(const Action::ResultCallback& callback) const { - MAVLinkCommands::CommandLong command{}; + MavlinkCommandSender::CommandLong command{}; command.command = MAV_CMD_NAV_TAKEOFF; command.target_component_id = _parent->get_autopilot_id(); - _parent->send_command_async(command, [this, callback](MAVLinkCommands::Result result, float) { + _parent->send_command_async(command, [this, callback](MavlinkCommandSender::Result result, float) { command_result_callback(result, callback); }); } void ActionImpl::land_async(const Action::ResultCallback& callback) const { - MAVLinkCommands::CommandLong command{}; + MavlinkCommandSender::CommandLong command{}; command.command = MAV_CMD_NAV_LAND; command.params.param4 = NAN; // Don't change yaw. command.target_component_id = _parent->get_autopilot_id(); - _parent->send_command_async(command, [this, callback](MAVLinkCommands::Result result, float) { + _parent->send_command_async(command, [this, callback](MavlinkCommandSender::Result result, float) { command_result_callback(result, callback); }); } @@ -323,7 +323,7 @@ void ActionImpl::return_to_launch_async(const Action::ResultCallback& callback) { _parent->set_flight_mode_async( SystemImpl::FlightMode::ReturnToLaunch, - [this, callback](MAVLinkCommands::Result result, float) { + [this, callback](MavlinkCommandSender::Result result, float) { command_result_callback(result, callback); }); } @@ -335,7 +335,7 @@ void ActionImpl::goto_location_async( const float yaw_deg, const Action::ResultCallback& callback) { - MAVLinkCommands::CommandInt command{}; + MavlinkCommandSender::CommandInt command{}; command.command = MAV_CMD_DO_REPOSITION; command.target_component_id = _parent->get_autopilot_id(); @@ -344,7 +344,7 @@ void ActionImpl::goto_location_async( command.params.y = int32_t(std::round(longitude_deg * 1e7)); command.params.z = altitude_amsl_m; - _parent->send_command_async(command, [this, callback](MAVLinkCommands::Result result, float) { + _parent->send_command_async(command, [this, callback](MavlinkCommandSender::Result result, float) { command_result_callback(result, callback); }); } @@ -365,13 +365,13 @@ void ActionImpl::transition_to_fixedwing_async(const Action::ResultCallback& cal return; } - MAVLinkCommands::CommandLong command{}; + MavlinkCommandSender::CommandLong command{}; command.command = MAV_CMD_DO_VTOL_TRANSITION; command.params.param1 = float(MAV_VTOL_STATE_FW); command.target_component_id = _parent->get_autopilot_id(); - _parent->send_command_async(command, [this, callback](MAVLinkCommands::Result result, float) { + _parent->send_command_async(command, [this, callback](MavlinkCommandSender::Result result, float) { command_result_callback(result, callback); }); } @@ -391,13 +391,13 @@ void ActionImpl::transition_to_multicopter_async(const Action::ResultCallback& c } return; } - MAVLinkCommands::CommandLong command{}; + MavlinkCommandSender::CommandLong command{}; command.command = MAV_CMD_DO_VTOL_TRANSITION; command.params.param1 = float(MAV_VTOL_STATE_MC); command.target_component_id = _parent->get_autopilot_id(); - _parent->send_command_async(command, [this, callback](MAVLinkCommands::Result result, float) { + _parent->send_command_async(command, [this, callback](MavlinkCommandSender::Result result, float) { command_result_callback(result, callback); }); } @@ -537,20 +537,20 @@ std::pair ActionImpl::get_return_to_launch_altitude() con result.second); } -Action::Result ActionImpl::action_result_from_command_result(MAVLinkCommands::Result result) +Action::Result ActionImpl::action_result_from_command_result(MavlinkCommandSender::Result result) { switch (result) { - case MAVLinkCommands::Result::Success: + case MavlinkCommandSender::Result::Success: return Action::Result::Success; - case MAVLinkCommands::Result::NoSystem: + case MavlinkCommandSender::Result::NoSystem: return Action::Result::NoSystem; - case MAVLinkCommands::Result::ConnectionError: + case MavlinkCommandSender::Result::ConnectionError: return Action::Result::ConnectionError; - case MAVLinkCommands::Result::Busy: + case MavlinkCommandSender::Result::Busy: return Action::Result::Busy; - case MAVLinkCommands::Result::CommandDenied: + case MavlinkCommandSender::Result::CommandDenied: return Action::Result::CommandDenied; - case MAVLinkCommands::Result::Timeout: + case MavlinkCommandSender::Result::Timeout: return Action::Result::Timeout; default: return Action::Result::Unknown; @@ -558,7 +558,7 @@ Action::Result ActionImpl::action_result_from_command_result(MAVLinkCommands::Re } void ActionImpl::command_result_callback( - MAVLinkCommands::Result command_result, const Action::ResultCallback& callback) const + MavlinkCommandSender::Result command_result, const Action::ResultCallback& callback) const { Action::Result action_result = action_result_from_command_result(command_result); diff --git a/src/plugins/action/action_impl.h b/src/plugins/action/action_impl.h index 0fd7c87175..1013439e1a 100644 --- a/src/plugins/action/action_impl.h +++ b/src/plugins/action/action_impl.h @@ -83,10 +83,10 @@ class ActionImpl : public PluginImplBase { void process_extended_sys_state(const mavlink_message_t& message); - static Action::Result action_result_from_command_result(MAVLinkCommands::Result result); + static Action::Result action_result_from_command_result(MavlinkCommandSender::Result result); void command_result_callback( - MAVLinkCommands::Result command_result, const Action::ResultCallback& callback) const; + MavlinkCommandSender::Result command_result, const Action::ResultCallback& callback) const; std::atomic _in_air_state_known{false}; std::atomic _in_air{false}; diff --git a/src/plugins/calibration/calibration_impl.cpp b/src/plugins/calibration/calibration_impl.cpp index 44f641a309..87d76e6d23 100644 --- a/src/plugins/calibration/calibration_impl.cpp +++ b/src/plugins/calibration/calibration_impl.cpp @@ -60,9 +60,9 @@ void CalibrationImpl::calibrate_gyro_async(const CalibrationCallback& callback) _state = State::GyroCalibration; _calibration_callback = callback; - MAVLinkCommands::CommandLong command{}; + MavlinkCommandSender::CommandLong command{}; command.command = MAV_CMD_PREFLIGHT_CALIBRATION; - MAVLinkCommands::CommandLong::set_as_reserved(command.params, 0.0f); + MavlinkCommandSender::CommandLong::set_as_reserved(command.params, 0.0f); command.params.param1 = 1.0f; // Gyro command.target_component_id = MAV_COMP_ID_AUTOPILOT1; _parent->send_command_async( @@ -99,9 +99,9 @@ void CalibrationImpl::calibrate_accelerometer_async(const CalibrationCallback& c _state = State::AccelerometerCalibration; _calibration_callback = callback; - MAVLinkCommands::CommandLong command{}; + MavlinkCommandSender::CommandLong command{}; command.command = MAV_CMD_PREFLIGHT_CALIBRATION; - MAVLinkCommands::CommandLong::set_as_reserved(command.params, 0.0f); + MavlinkCommandSender::CommandLong::set_as_reserved(command.params, 0.0f); command.params.param5 = 1.0f; // Accel command.target_component_id = MAV_COMP_ID_AUTOPILOT1; _parent->send_command_async( @@ -127,9 +127,9 @@ void CalibrationImpl::calibrate_magnetometer_async(const CalibrationCallback& ca _state = State::MagnetometerCalibration; _calibration_callback = callback; - MAVLinkCommands::CommandLong command{}; + MavlinkCommandSender::CommandLong command{}; command.command = MAV_CMD_PREFLIGHT_CALIBRATION; - MAVLinkCommands::CommandLong::set_as_reserved(command.params, 0.0f); + MavlinkCommandSender::CommandLong::set_as_reserved(command.params, 0.0f); command.params.param2 = 1.0f; // Mag command.target_component_id = MAV_COMP_ID_AUTOPILOT1; _parent->send_command_async( @@ -155,9 +155,9 @@ void CalibrationImpl::calibrate_level_horizon_async(const CalibrationCallback& c _state = State::AccelerometerCalibration; _calibration_callback = callback; - MAVLinkCommands::CommandLong command{}; + MavlinkCommandSender::CommandLong command{}; command.command = MAV_CMD_PREFLIGHT_CALIBRATION; - MAVLinkCommands::CommandLong::set_as_reserved(command.params, 0.0f); + MavlinkCommandSender::CommandLong::set_as_reserved(command.params, 0.0f); command.params.param5 = 2.0f; // Board Level command.target_component_id = MAV_COMP_ID_AUTOPILOT1; _parent->send_command_async( @@ -183,9 +183,9 @@ void CalibrationImpl::calibrate_gimbal_accelerometer_async(const CalibrationCall _state = State::GimbalAccelerometerCalibration; _calibration_callback = callback; - MAVLinkCommands::CommandLong command{}; + MavlinkCommandSender::CommandLong command{}; command.command = MAV_CMD_PREFLIGHT_CALIBRATION; - MAVLinkCommands::CommandLong::set_as_reserved(command.params, 0.0f); + MavlinkCommandSender::CommandLong::set_as_reserved(command.params, 0.0f); command.params.param5 = 1.0f; // Accel command.target_component_id = MAV_COMP_ID_GIMBAL; _parent->send_command_async( @@ -215,17 +215,17 @@ void CalibrationImpl::cancel() const break; } - MAVLinkCommands::CommandLong command{}; + MavlinkCommandSender::CommandLong command{}; command.command = MAV_CMD_PREFLIGHT_CALIBRATION; // All params 0 signal cancellation of a calibration. - MAVLinkCommands::CommandLong::set_as_reserved(command.params, 0.0f); + MavlinkCommandSender::CommandLong::set_as_reserved(command.params, 0.0f); command.target_component_id = target_component_id; // We don't care about the result, the initial callback should get notified about it. _parent->send_command_async(command, nullptr); } void CalibrationImpl::command_result_callback( - MAVLinkCommands::Result command_result, float progress) + MavlinkCommandSender::Result command_result, float progress) { std::lock_guard lock(_calibration_mutex); @@ -239,23 +239,23 @@ void CalibrationImpl::command_result_callback( // use the progress info. If we get an ack, we need to translate that to // a first progress update, and then parse the statustexts for progress. switch (command_result) { - case MAVLinkCommands::Result::Success: + case MavlinkCommandSender::Result::Success: // Silently ignore. break; - case MAVLinkCommands::Result::NoSystem: + case MavlinkCommandSender::Result::NoSystem: // FALLTHROUGH - case MAVLinkCommands::Result::ConnectionError: + case MavlinkCommandSender::Result::ConnectionError: // FALLTHROUGH - case MAVLinkCommands::Result::Busy: + case MavlinkCommandSender::Result::Busy: // FALLTHROUGH - case MAVLinkCommands::Result::CommandDenied: + case MavlinkCommandSender::Result::CommandDenied: // FALLTHROUGH - case MAVLinkCommands::Result::Unsupported: + case MavlinkCommandSender::Result::Unsupported: // FALLTHROUGH - case MAVLinkCommands::Result::UnknownError: + case MavlinkCommandSender::Result::UnknownError: // FALLTHROUGH - case MAVLinkCommands::Result::Timeout: { + case MavlinkCommandSender::Result::Timeout: { // Report all error cases. const auto timeout_result = calibration_result_from_command_result(command_result); call_callback(_calibration_callback, timeout_result, Calibration::ProgressData()); @@ -264,7 +264,7 @@ void CalibrationImpl::command_result_callback( break; } - case MAVLinkCommands::Result::InProgress: { + case MavlinkCommandSender::Result::InProgress: { const auto progress_result = calibration_result_from_command_result(command_result); Calibration::ProgressData progress_data; progress_data.has_progress = true; @@ -277,22 +277,22 @@ void CalibrationImpl::command_result_callback( } Calibration::Result -CalibrationImpl::calibration_result_from_command_result(MAVLinkCommands::Result result) +CalibrationImpl::calibration_result_from_command_result(MavlinkCommandSender::Result result) { switch (result) { - case MAVLinkCommands::Result::Success: + case MavlinkCommandSender::Result::Success: return Calibration::Result::Success; - case MAVLinkCommands::Result::NoSystem: + case MavlinkCommandSender::Result::NoSystem: return Calibration::Result::NoSystem; - case MAVLinkCommands::Result::ConnectionError: + case MavlinkCommandSender::Result::ConnectionError: return Calibration::Result::ConnectionError; - case MAVLinkCommands::Result::Busy: + case MavlinkCommandSender::Result::Busy: return Calibration::Result::Busy; - case MAVLinkCommands::Result::CommandDenied: + case MavlinkCommandSender::Result::CommandDenied: return Calibration::Result::CommandDenied; - case MAVLinkCommands::Result::Timeout: + case MavlinkCommandSender::Result::Timeout: return Calibration::Result::Timeout; - case MAVLinkCommands::Result::InProgress: + case MavlinkCommandSender::Result::InProgress: return Calibration::Result::Next; default: return Calibration::Result::Unknown; diff --git a/src/plugins/calibration/calibration_impl.h b/src/plugins/calibration/calibration_impl.h index e2543f33ac..6a5f4f33d8 100644 --- a/src/plugins/calibration/calibration_impl.h +++ b/src/plugins/calibration/calibration_impl.h @@ -39,10 +39,10 @@ class CalibrationImpl : public PluginImplBase { const Calibration::ProgressData progress_data); void process_statustext(const mavlink_message_t& message); - void command_result_callback(MAVLinkCommands::Result command_result, float progress); + void command_result_callback(MavlinkCommandSender::Result command_result, float progress); static Calibration::Result - calibration_result_from_command_result(MAVLinkCommands::Result result); + calibration_result_from_command_result(MavlinkCommandSender::Result result); void report_started(); void report_done(); diff --git a/src/plugins/camera/camera_impl.cpp b/src/plugins/camera/camera_impl.cpp index abd6919005..dcf873166d 100644 --- a/src/plugins/camera/camera_impl.cpp +++ b/src/plugins/camera/camera_impl.cpp @@ -183,9 +183,9 @@ Camera::Result CameraImpl::select_camera(const size_t id) return Camera::Result::Success; } -MAVLinkCommands::CommandLong CameraImpl::make_command_request_flight_information() +MavlinkCommandSender::CommandLong CameraImpl::make_command_request_flight_information() { - MAVLinkCommands::CommandLong command_flight_information{}; + MavlinkCommandSender::CommandLong command_flight_information{}; command_flight_information.command = MAV_CMD_REQUEST_FLIGHT_INFORMATION; command_flight_information.params.param1 = 1.0f; // Request it @@ -194,9 +194,9 @@ MAVLinkCommands::CommandLong CameraImpl::make_command_request_flight_information return command_flight_information; } -MAVLinkCommands::CommandLong CameraImpl::make_command_request_camera_info() +MavlinkCommandSender::CommandLong CameraImpl::make_command_request_camera_info() { - MAVLinkCommands::CommandLong command_camera_info{}; + MavlinkCommandSender::CommandLong command_camera_info{}; command_camera_info.command = MAV_CMD_REQUEST_CAMERA_INFORMATION; command_camera_info.params.param1 = 1.0f; // Request it @@ -205,10 +205,10 @@ MAVLinkCommands::CommandLong CameraImpl::make_command_request_camera_info() return command_camera_info; } -MAVLinkCommands::CommandLong +MavlinkCommandSender::CommandLong CameraImpl::make_command_take_photo(float interval_s, float no_of_photos) { - MAVLinkCommands::CommandLong cmd_take_photo{}; + MavlinkCommandSender::CommandLong cmd_take_photo{}; cmd_take_photo.command = MAV_CMD_IMAGE_START_CAPTURE; cmd_take_photo.params.param1 = 0.0f; // Reserved, set to 0 @@ -220,9 +220,9 @@ CameraImpl::make_command_take_photo(float interval_s, float no_of_photos) return cmd_take_photo; } -MAVLinkCommands::CommandLong CameraImpl::make_command_stop_photo() +MavlinkCommandSender::CommandLong CameraImpl::make_command_stop_photo() { - MAVLinkCommands::CommandLong cmd_stop_photo{}; + MavlinkCommandSender::CommandLong cmd_stop_photo{}; cmd_stop_photo.command = MAV_CMD_IMAGE_STOP_CAPTURE; cmd_stop_photo.target_component_id = _camera_id + MAV_COMP_ID_CAMERA; @@ -230,9 +230,9 @@ MAVLinkCommands::CommandLong CameraImpl::make_command_stop_photo() return cmd_stop_photo; } -MAVLinkCommands::CommandLong CameraImpl::make_command_start_video(float capture_status_rate_hz) +MavlinkCommandSender::CommandLong CameraImpl::make_command_start_video(float capture_status_rate_hz) { - MAVLinkCommands::CommandLong cmd_start_video{}; + MavlinkCommandSender::CommandLong cmd_start_video{}; cmd_start_video.command = MAV_CMD_VIDEO_START_CAPTURE; cmd_start_video.params.param1 = 0.f; // Reserved, set to 0 @@ -242,9 +242,9 @@ MAVLinkCommands::CommandLong CameraImpl::make_command_start_video(float capture_ return cmd_start_video; } -MAVLinkCommands::CommandLong CameraImpl::make_command_stop_video() +MavlinkCommandSender::CommandLong CameraImpl::make_command_stop_video() { - MAVLinkCommands::CommandLong cmd_stop_video{}; + MavlinkCommandSender::CommandLong cmd_stop_video{}; cmd_stop_video.command = MAV_CMD_VIDEO_STOP_CAPTURE; cmd_stop_video.params.param1 = 0.f; // Reserved, set to 0 @@ -253,9 +253,9 @@ MAVLinkCommands::CommandLong CameraImpl::make_command_stop_video() return cmd_stop_video; } -MAVLinkCommands::CommandLong CameraImpl::make_command_set_camera_mode(float mavlink_mode) +MavlinkCommandSender::CommandLong CameraImpl::make_command_set_camera_mode(float mavlink_mode) { - MAVLinkCommands::CommandLong cmd_set_camera_mode{}; + MavlinkCommandSender::CommandLong cmd_set_camera_mode{}; cmd_set_camera_mode.command = MAV_CMD_SET_CAMERA_MODE; cmd_set_camera_mode.params.param1 = 0.0f; // Reserved, set to 0 @@ -265,9 +265,9 @@ MAVLinkCommands::CommandLong CameraImpl::make_command_set_camera_mode(float mavl return cmd_set_camera_mode; } -MAVLinkCommands::CommandLong CameraImpl::make_command_request_camera_settings() +MavlinkCommandSender::CommandLong CameraImpl::make_command_request_camera_settings() { - MAVLinkCommands::CommandLong cmd_req_camera_settings{}; + MavlinkCommandSender::CommandLong cmd_req_camera_settings{}; cmd_req_camera_settings.command = MAV_CMD_REQUEST_CAMERA_SETTINGS; cmd_req_camera_settings.params.param1 = 1.f; // Request it @@ -276,9 +276,9 @@ MAVLinkCommands::CommandLong CameraImpl::make_command_request_camera_settings() return cmd_req_camera_settings; } -MAVLinkCommands::CommandLong CameraImpl::make_command_request_camera_capture_status() +MavlinkCommandSender::CommandLong CameraImpl::make_command_request_camera_capture_status() { - MAVLinkCommands::CommandLong cmd_req_camera_cap_stat{}; + MavlinkCommandSender::CommandLong cmd_req_camera_cap_stat{}; cmd_req_camera_cap_stat.command = MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS; cmd_req_camera_cap_stat.params.param1 = 1.0f; // Request it @@ -287,9 +287,9 @@ MAVLinkCommands::CommandLong CameraImpl::make_command_request_camera_capture_sta return cmd_req_camera_cap_stat; } -MAVLinkCommands::CommandLong CameraImpl::make_command_request_storage_info() +MavlinkCommandSender::CommandLong CameraImpl::make_command_request_storage_info() { - MAVLinkCommands::CommandLong cmd_req_storage_info{}; + MavlinkCommandSender::CommandLong cmd_req_storage_info{}; cmd_req_storage_info.command = MAV_CMD_REQUEST_STORAGE_INFORMATION; cmd_req_storage_info.params.param1 = 0.f; // Reserved, set to 0 @@ -299,9 +299,9 @@ MAVLinkCommands::CommandLong CameraImpl::make_command_request_storage_info() return cmd_req_storage_info; } -MAVLinkCommands::CommandLong CameraImpl::make_command_start_video_streaming() +MavlinkCommandSender::CommandLong CameraImpl::make_command_start_video_streaming() { - MAVLinkCommands::CommandLong cmd_start_video_streaming{}; + MavlinkCommandSender::CommandLong cmd_start_video_streaming{}; cmd_start_video_streaming.command = MAV_CMD_VIDEO_START_STREAMING; cmd_start_video_streaming.target_component_id = _camera_id + MAV_COMP_ID_CAMERA; @@ -309,9 +309,9 @@ MAVLinkCommands::CommandLong CameraImpl::make_command_start_video_streaming() return cmd_start_video_streaming; } -MAVLinkCommands::CommandLong CameraImpl::make_command_stop_video_streaming() +MavlinkCommandSender::CommandLong CameraImpl::make_command_stop_video_streaming() { - MAVLinkCommands::CommandLong cmd_stop_video_streaming{}; + MavlinkCommandSender::CommandLong cmd_stop_video_streaming{}; cmd_stop_video_streaming.command = MAV_CMD_VIDEO_STOP_STREAMING; cmd_stop_video_streaming.target_component_id = _camera_id + MAV_COMP_ID_CAMERA; @@ -319,9 +319,9 @@ MAVLinkCommands::CommandLong CameraImpl::make_command_stop_video_streaming() return cmd_stop_video_streaming; } -MAVLinkCommands::CommandLong CameraImpl::make_command_request_video_stream_info() +MavlinkCommandSender::CommandLong CameraImpl::make_command_request_video_stream_info() { - MAVLinkCommands::CommandLong cmd_req_video_stream_info{}; + MavlinkCommandSender::CommandLong cmd_req_video_stream_info{}; cmd_req_video_stream_info.command = MAV_CMD_REQUEST_VIDEO_STREAM_INFORMATION; cmd_req_video_stream_info.params.param2 = 1.0f; @@ -533,20 +533,20 @@ void CameraImpl::video_stream_info_async(const Camera::VideoStreamInfoCallback c } Camera::Result -CameraImpl::camera_result_from_command_result(const MAVLinkCommands::Result command_result) +CameraImpl::camera_result_from_command_result(const MavlinkCommandSender::Result command_result) { switch (command_result) { - case MAVLinkCommands::Result::Success: + case MavlinkCommandSender::Result::Success: return Camera::Result::Success; - case MAVLinkCommands::Result::NoSystem: + case MavlinkCommandSender::Result::NoSystem: // FALLTHROUGH - case MAVLinkCommands::Result::ConnectionError: + case MavlinkCommandSender::Result::ConnectionError: // FALLTHROUGH - case MAVLinkCommands::Result::Busy: + case MavlinkCommandSender::Result::Busy: return Camera::Result::Error; - case MAVLinkCommands::Result::CommandDenied: + case MavlinkCommandSender::Result::CommandDenied: return Camera::Result::Denied; - case MAVLinkCommands::Result::Timeout: + case MavlinkCommandSender::Result::Timeout: return Camera::Result::Timeout; default: return Camera::Result::Unknown; @@ -607,7 +607,7 @@ void CameraImpl::set_mode_async(const Camera::Mode mode, const Camera::ResultCal _parent->send_command_async( cmd_set_camera_mode, - [this, callback, mode](MAVLinkCommands::Result result, float progress) { + [this, callback, mode](MavlinkCommandSender::Result result, float progress) { UNUSED(progress); receive_set_mode_command_result(result, callback, mode); }); @@ -937,7 +937,7 @@ void CameraImpl::check_status() } void CameraImpl::receive_command_result( - MAVLinkCommands::Result command_result, const Camera::ResultCallback& callback) + MavlinkCommandSender::Result command_result, const Camera::ResultCallback& callback) { Camera::Result camera_result = camera_result_from_command_result(command_result); @@ -947,7 +947,7 @@ void CameraImpl::receive_command_result( } void CameraImpl::receive_set_mode_command_result( - const MAVLinkCommands::Result command_result, + const MavlinkCommandSender::Result command_result, const Camera::ResultCallback callback, const Camera::Mode mode) { @@ -959,7 +959,7 @@ void CameraImpl::receive_set_mode_command_result( [temp_callback, camera_result]() { temp_callback(camera_result); }); } - if (command_result == MAVLinkCommands::Result::Success && _camera_definition) { + if (command_result == MavlinkCommandSender::Result::Success && _camera_definition) { // This "parameter" needs to be manually set. { std::lock_guard lock(_mode.mutex); @@ -1516,7 +1516,7 @@ Camera::Result CameraImpl::format_storage() void CameraImpl::format_storage_async(Camera::ResultCallback callback) { - MAVLinkCommands::CommandLong cmd_format{}; + MavlinkCommandSender::CommandLong cmd_format{}; cmd_format.command = MAV_CMD_STORAGE_FORMAT; cmd_format.params.param1 = 1.0f; // storage ID diff --git a/src/plugins/camera/camera_impl.h b/src/plugins/camera/camera_impl.h index d8f6ddfd90..5182f07f1a 100644 --- a/src/plugins/camera/camera_impl.h +++ b/src/plugins/camera/camera_impl.h @@ -100,15 +100,15 @@ class CameraImpl : public PluginImplBase { void manual_disable(); void receive_set_mode_command_result( - const MAVLinkCommands::Result command_result, + const MavlinkCommandSender::Result command_result, const Camera::ResultCallback callback, const Camera::Mode mode); static Camera::Result - camera_result_from_command_result(const MAVLinkCommands::Result command_result); + camera_result_from_command_result(const MavlinkCommandSender::Result command_result); void receive_command_result( - MAVLinkCommands::Result command_result, const Camera::ResultCallback& callback); + MavlinkCommandSender::Result command_result, const Camera::ResultCallback& callback); static bool interval_valid(float interval_s); @@ -148,23 +148,23 @@ class CameraImpl : public PluginImplBase { void request_status(); void request_flight_information(); - MAVLinkCommands::CommandLong make_command_take_photo(float interval_s, float no_of_photos); - MAVLinkCommands::CommandLong make_command_stop_photo(); + MavlinkCommandSender::CommandLong make_command_take_photo(float interval_s, float no_of_photos); + MavlinkCommandSender::CommandLong make_command_stop_photo(); - MAVLinkCommands::CommandLong make_command_request_flight_information(); - MAVLinkCommands::CommandLong make_command_request_camera_info(); - MAVLinkCommands::CommandLong make_command_set_camera_mode(float mavlink_mode); - MAVLinkCommands::CommandLong make_command_request_camera_settings(); - MAVLinkCommands::CommandLong make_command_request_camera_capture_status(); - MAVLinkCommands::CommandLong make_command_request_storage_info(); + MavlinkCommandSender::CommandLong make_command_request_flight_information(); + MavlinkCommandSender::CommandLong make_command_request_camera_info(); + MavlinkCommandSender::CommandLong make_command_set_camera_mode(float mavlink_mode); + MavlinkCommandSender::CommandLong make_command_request_camera_settings(); + MavlinkCommandSender::CommandLong make_command_request_camera_capture_status(); + MavlinkCommandSender::CommandLong make_command_request_storage_info(); - MAVLinkCommands::CommandLong make_command_start_video(float capture_status_rate_hz); - MAVLinkCommands::CommandLong make_command_stop_video(); + MavlinkCommandSender::CommandLong make_command_start_video(float capture_status_rate_hz); + MavlinkCommandSender::CommandLong make_command_stop_video(); - MAVLinkCommands::CommandLong make_command_start_video_streaming(); - MAVLinkCommands::CommandLong make_command_stop_video_streaming(); + MavlinkCommandSender::CommandLong make_command_start_video_streaming(); + MavlinkCommandSender::CommandLong make_command_stop_video_streaming(); - MAVLinkCommands::CommandLong make_command_request_video_stream_info(); + MavlinkCommandSender::CommandLong make_command_request_video_stream_info(); std::unique_ptr _camera_definition{}; diff --git a/src/plugins/failure/failure_impl.cpp b/src/plugins/failure/failure_impl.cpp index 79503bee1e..a82061f74b 100644 --- a/src/plugins/failure/failure_impl.cpp +++ b/src/plugins/failure/failure_impl.cpp @@ -73,7 +73,7 @@ Failure::Result FailureImpl::inject( return Failure::Result::Disabled; } - MAVLinkCommands::CommandLong command{}; + MavlinkCommandSender::CommandLong command{}; command.command = MAV_CMD_INJECT_FAILURE; command.params.param1 = static_cast(failure_unit); @@ -85,20 +85,20 @@ Failure::Result FailureImpl::inject( } Failure::Result -FailureImpl::failure_result_from_command_result(MAVLinkCommands::Result command_result) +FailureImpl::failure_result_from_command_result(MavlinkCommandSender::Result command_result) { switch (command_result) { - case MAVLinkCommands::Result::Success: + case MavlinkCommandSender::Result::Success: return Failure::Result::Success; - case MAVLinkCommands::Result::NoSystem: + case MavlinkCommandSender::Result::NoSystem: return Failure::Result::NoSystem; - case MAVLinkCommands::Result::ConnectionError: + case MavlinkCommandSender::Result::ConnectionError: return Failure::Result::ConnectionError; - case MAVLinkCommands::Result::CommandDenied: + case MavlinkCommandSender::Result::CommandDenied: return Failure::Result::Denied; - case MAVLinkCommands::Result::Unsupported: + case MavlinkCommandSender::Result::Unsupported: return Failure::Result::Unsupported; - case MAVLinkCommands::Result::Timeout: + case MavlinkCommandSender::Result::Timeout: return Failure::Result::Timeout; default: return Failure::Result::Unknown; diff --git a/src/plugins/failure/failure_impl.h b/src/plugins/failure/failure_impl.h index c4c8284c23..59ec51c66e 100644 --- a/src/plugins/failure/failure_impl.h +++ b/src/plugins/failure/failure_impl.h @@ -21,7 +21,7 @@ class FailureImpl : public PluginImplBase { inject(Failure::FailureUnit failure_unit, Failure::FailureType failure_type, int32_t instance); private: - Failure::Result failure_result_from_command_result(MAVLinkCommands::Result command_result); + Failure::Result failure_result_from_command_result(MavlinkCommandSender::Result command_result); enum class EnabledState { Init, diff --git a/src/plugins/follow_me/follow_me_impl.cpp b/src/plugins/follow_me/follow_me_impl.cpp index 5adf9776e3..23f15d6468 100644 --- a/src/plugins/follow_me/follow_me_impl.cpp +++ b/src/plugins/follow_me/follow_me_impl.cpp @@ -231,20 +231,20 @@ bool FollowMeImpl::is_config_ok(const FollowMe::Config& config) const return config_ok; } -FollowMe::Result FollowMeImpl::to_follow_me_result(MAVLinkCommands::Result result) const +FollowMe::Result FollowMeImpl::to_follow_me_result(MavlinkCommandSender::Result result) const { switch (result) { - case MAVLinkCommands::Result::Success: + case MavlinkCommandSender::Result::Success: return FollowMe::Result::Success; - case MAVLinkCommands::Result::NoSystem: + case MavlinkCommandSender::Result::NoSystem: return FollowMe::Result::NoSystem; - case MAVLinkCommands::Result::ConnectionError: + case MavlinkCommandSender::Result::ConnectionError: return FollowMe::Result::ConnectionError; - case MAVLinkCommands::Result::Busy: + case MavlinkCommandSender::Result::Busy: return FollowMe::Result::Busy; - case MAVLinkCommands::Result::CommandDenied: + case MavlinkCommandSender::Result::CommandDenied: return FollowMe::Result::CommandDenied; - case MAVLinkCommands::Result::Timeout: + case MavlinkCommandSender::Result::Timeout: return FollowMe::Result::Timeout; default: return FollowMe::Result::Unknown; diff --git a/src/plugins/follow_me/follow_me_impl.h b/src/plugins/follow_me/follow_me_impl.h index 28626a5792..52eea1062c 100644 --- a/src/plugins/follow_me/follow_me_impl.h +++ b/src/plugins/follow_me/follow_me_impl.h @@ -43,7 +43,7 @@ class FollowMeImpl : public PluginImplBase { enum class ConfigParameter; // Config methods bool is_config_ok(const FollowMe::Config& config) const; - FollowMe::Result to_follow_me_result(MAVLinkCommands::Result result) const; + FollowMe::Result to_follow_me_result(MavlinkCommandSender::Result result) const; bool is_target_location_set() const; void send_target_location(); diff --git a/src/plugins/gimbal/gimbal_impl.cpp b/src/plugins/gimbal/gimbal_impl.cpp index 62aa77f48d..caf48d1908 100644 --- a/src/plugins/gimbal/gimbal_impl.cpp +++ b/src/plugins/gimbal/gimbal_impl.cpp @@ -39,7 +39,7 @@ void GimbalImpl::enable() _parent->register_timeout_handler( [this]() { receive_protocol_timeout(); }, 1.0, &_protocol_cookie); - MAVLinkCommands::CommandLong command{}; + MavlinkCommandSender::CommandLong command{}; command.command = MAV_CMD_REQUEST_MESSAGE; command.params.param1 = static_cast(MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION); command.target_component_id = 0; // any component @@ -127,7 +127,7 @@ void GimbalImpl::wait_for_protocol_async(std::function callback) } void GimbalImpl::receive_command_result( - MAVLinkCommands::Result command_result, const Gimbal::ResultCallback& callback) + MavlinkCommandSender::Result command_result, const Gimbal::ResultCallback& callback) { Gimbal::Result gimbal_result = gimbal_result_from_command_result(command_result); @@ -136,17 +136,17 @@ void GimbalImpl::receive_command_result( } } -Gimbal::Result GimbalImpl::gimbal_result_from_command_result(MAVLinkCommands::Result command_result) +Gimbal::Result GimbalImpl::gimbal_result_from_command_result(MavlinkCommandSender::Result command_result) { switch (command_result) { - case MAVLinkCommands::Result::Success: + case MavlinkCommandSender::Result::Success: return Gimbal::Result::Success; - case MAVLinkCommands::Result::Timeout: + case MavlinkCommandSender::Result::Timeout: return Gimbal::Result::Timeout; - case MAVLinkCommands::Result::NoSystem: - case MAVLinkCommands::Result::ConnectionError: - case MAVLinkCommands::Result::Busy: - case MAVLinkCommands::Result::CommandDenied: + case MavlinkCommandSender::Result::NoSystem: + case MavlinkCommandSender::Result::ConnectionError: + case MavlinkCommandSender::Result::Busy: + case MavlinkCommandSender::Result::CommandDenied: default: return Gimbal::Result::Error; } diff --git a/src/plugins/gimbal/gimbal_impl.h b/src/plugins/gimbal/gimbal_impl.h index 50bcc7919d..80df77bffb 100644 --- a/src/plugins/gimbal/gimbal_impl.h +++ b/src/plugins/gimbal/gimbal_impl.h @@ -35,10 +35,10 @@ class GimbalImpl : public PluginImplBase { float altitude_m, Gimbal::ResultCallback callback); - static Gimbal::Result gimbal_result_from_command_result(MAVLinkCommands::Result command_result); + static Gimbal::Result gimbal_result_from_command_result(MavlinkCommandSender::Result command_result); static void receive_command_result( - MAVLinkCommands::Result command_result, const Gimbal::ResultCallback& callback); + MavlinkCommandSender::Result command_result, const Gimbal::ResultCallback& callback); // Non-copyable GimbalImpl(const GimbalImpl&) = delete; diff --git a/src/plugins/gimbal/gimbal_protocol_v1.cpp b/src/plugins/gimbal/gimbal_protocol_v1.cpp index eeee5856f2..a76007004c 100644 --- a/src/plugins/gimbal/gimbal_protocol_v1.cpp +++ b/src/plugins/gimbal/gimbal_protocol_v1.cpp @@ -11,7 +11,7 @@ GimbalProtocolV1::GimbalProtocolV1(SystemImpl& system_impl) : GimbalProtocolBase Gimbal::Result GimbalProtocolV1::set_pitch_and_yaw(float pitch_deg, float yaw_deg) { const float roll_deg = 0.0f; - MAVLinkCommands::CommandLong command{}; + MavlinkCommandSender::CommandLong command{}; command.command = MAV_CMD_DO_MOUNT_CONTROL; command.params.param1 = pitch_deg; @@ -27,7 +27,7 @@ void GimbalProtocolV1::set_pitch_and_yaw_async( float pitch_deg, float yaw_deg, Gimbal::ResultCallback callback) { const float roll_deg = 0.0f; - MAVLinkCommands::CommandLong command{}; + MavlinkCommandSender::CommandLong command{}; command.command = MAV_CMD_DO_MOUNT_CONTROL; command.params.param1 = pitch_deg; @@ -37,7 +37,7 @@ void GimbalProtocolV1::set_pitch_and_yaw_async( command.target_component_id = _system_impl.get_autopilot_id(); _system_impl.send_command_async( - command, [callback](MAVLinkCommands::Result command_result, float progress) { + command, [callback](MavlinkCommandSender::Result command_result, float progress) { UNUSED(progress); GimbalImpl::receive_command_result(command_result, callback); }); @@ -45,7 +45,7 @@ void GimbalProtocolV1::set_pitch_and_yaw_async( Gimbal::Result GimbalProtocolV1::set_mode(const Gimbal::GimbalMode gimbal_mode) { - MAVLinkCommands::CommandInt command{}; + MavlinkCommandSender::CommandInt command{}; // Correct here would actually be to: // - set yaw stabilize / param4 to 0 as usual @@ -68,7 +68,7 @@ Gimbal::Result GimbalProtocolV1::set_mode(const Gimbal::GimbalMode gimbal_mode) void GimbalProtocolV1::set_mode_async( const Gimbal::GimbalMode gimbal_mode, Gimbal::ResultCallback callback) { - MAVLinkCommands::CommandInt command{}; + MavlinkCommandSender::CommandInt command{}; // Correct here would actually be to: // - set yaw stabilize / param4 to 0 as usual @@ -85,7 +85,7 @@ void GimbalProtocolV1::set_mode_async( command.target_component_id = _system_impl.get_autopilot_id(); _system_impl.send_command_async( - command, [callback](MAVLinkCommands::Result command_result, float progress) { + command, [callback](MavlinkCommandSender::Result command_result, float progress) { UNUSED(progress); GimbalImpl::receive_command_result(command_result, callback); }); @@ -106,7 +106,7 @@ float GimbalProtocolV1::to_float_gimbal_mode(const Gimbal::GimbalMode gimbal_mod Gimbal::Result GimbalProtocolV1::set_roi_location(double latitude_deg, double longitude_deg, float altitude_m) { - MAVLinkCommands::CommandInt command{}; + MavlinkCommandSender::CommandInt command{}; command.command = MAV_CMD_DO_SET_ROI_LOCATION; command.params.x = int32_t(std::round(latitude_deg * 1e7)); @@ -120,7 +120,7 @@ GimbalProtocolV1::set_roi_location(double latitude_deg, double longitude_deg, fl void GimbalProtocolV1::set_roi_location_async( double latitude_deg, double longitude_deg, float altitude_m, Gimbal::ResultCallback callback) { - MAVLinkCommands::CommandInt command{}; + MavlinkCommandSender::CommandInt command{}; command.command = MAV_CMD_DO_SET_ROI_LOCATION; command.params.x = int32_t(std::round(latitude_deg * 1e7)); @@ -129,7 +129,7 @@ void GimbalProtocolV1::set_roi_location_async( command.target_component_id = _system_impl.get_autopilot_id(); _system_impl.send_command_async( - command, [callback](MAVLinkCommands::Result command_result, float progress) { + command, [callback](MavlinkCommandSender::Result command_result, float progress) { UNUSED(progress); GimbalImpl::receive_command_result(command_result, callback); }); diff --git a/src/plugins/manual_control/manual_control_impl.cpp b/src/plugins/manual_control/manual_control_impl.cpp index fa9507d47b..ecc2d12124 100644 --- a/src/plugins/manual_control/manual_control_impl.cpp +++ b/src/plugins/manual_control/manual_control_impl.cpp @@ -29,7 +29,7 @@ void ManualControlImpl::disable() {} void ManualControlImpl::start_position_control_async(const ManualControl::ResultCallback callback) { _parent->set_flight_mode_async( - SystemImpl::FlightMode::Posctl, [this, callback](MAVLinkCommands::Result result, float) { + SystemImpl::FlightMode::Posctl, [this, callback](MavlinkCommandSender::Result result, float) { command_result_callback(result, callback); }); } @@ -47,7 +47,7 @@ ManualControl::Result ManualControlImpl::start_position_control() void ManualControlImpl::start_altitude_control_async(const ManualControl::ResultCallback callback) { _parent->set_flight_mode_async( - SystemImpl::FlightMode::Altctl, [this, callback](MAVLinkCommands::Result result, float) { + SystemImpl::FlightMode::Altctl, [this, callback](MavlinkCommandSender::Result result, float) { command_result_callback(result, callback); }); } @@ -88,20 +88,20 @@ ManualControlImpl::set_manual_control_input(float x, float y, float z, float r) } ManualControl::Result -ManualControlImpl::manual_control_result_from_command_result(MAVLinkCommands::Result result) +ManualControlImpl::manual_control_result_from_command_result(MavlinkCommandSender::Result result) { switch (result) { - case MAVLinkCommands::Result::Success: + case MavlinkCommandSender::Result::Success: return ManualControl::Result::Success; - case MAVLinkCommands::Result::NoSystem: + case MavlinkCommandSender::Result::NoSystem: return ManualControl::Result::NoSystem; - case MAVLinkCommands::Result::ConnectionError: + case MavlinkCommandSender::Result::ConnectionError: return ManualControl::Result::ConnectionError; - case MAVLinkCommands::Result::Busy: + case MavlinkCommandSender::Result::Busy: return ManualControl::Result::Busy; - case MAVLinkCommands::Result::CommandDenied: + case MavlinkCommandSender::Result::CommandDenied: return ManualControl::Result::CommandDenied; - case MAVLinkCommands::Result::Timeout: + case MavlinkCommandSender::Result::Timeout: return ManualControl::Result::Timeout; default: return ManualControl::Result::Unknown; @@ -109,7 +109,7 @@ ManualControlImpl::manual_control_result_from_command_result(MAVLinkCommands::Re } void ManualControlImpl::command_result_callback( - MAVLinkCommands::Result command_result, const ManualControl::ResultCallback& callback) + MavlinkCommandSender::Result command_result, const ManualControl::ResultCallback& callback) { ManualControl::Result action_result = manual_control_result_from_command_result(command_result); diff --git a/src/plugins/manual_control/manual_control_impl.h b/src/plugins/manual_control/manual_control_impl.h index b71993190e..05e320bcff 100644 --- a/src/plugins/manual_control/manual_control_impl.h +++ b/src/plugins/manual_control/manual_control_impl.h @@ -28,9 +28,9 @@ class ManualControlImpl : public PluginImplBase { ManualControl::Result set_manual_control_input(float x, float y, float z, float r); private: - ManualControl::Result manual_control_result_from_command_result(MAVLinkCommands::Result result); + ManualControl::Result manual_control_result_from_command_result(MavlinkCommandSender::Result result); void command_result_callback( - MAVLinkCommands::Result command_result, const ManualControl::ResultCallback& callback); + MavlinkCommandSender::Result command_result, const ManualControl::ResultCallback& callback); }; } // namespace mavsdk diff --git a/src/plugins/mavlink_passthrough/mavlink_passthrough_impl.cpp b/src/plugins/mavlink_passthrough/mavlink_passthrough_impl.cpp index c06ebf78d6..88a4f01218 100644 --- a/src/plugins/mavlink_passthrough/mavlink_passthrough_impl.cpp +++ b/src/plugins/mavlink_passthrough/mavlink_passthrough_impl.cpp @@ -45,7 +45,7 @@ MavlinkPassthrough::Result MavlinkPassthroughImpl::send_message(mavlink_message_ MavlinkPassthrough::Result MavlinkPassthroughImpl::send_command_long(const MavlinkPassthrough::CommandLong& command) { - MAVLinkCommands::CommandLong command_internal{}; + MavlinkCommandSender::CommandLong command_internal{}; command_internal.target_system_id = command.target_sysid; command_internal.target_component_id = command.target_compid; command_internal.command = command.command; @@ -64,7 +64,7 @@ MavlinkPassthroughImpl::send_command_long(const MavlinkPassthrough::CommandLong& MavlinkPassthrough::Result MavlinkPassthroughImpl::send_command_int(const MavlinkPassthrough::CommandInt& command) { - MAVLinkCommands::CommandInt command_internal{}; + MavlinkCommandSender::CommandInt command_internal{}; command_internal.target_system_id = command.target_sysid; command_internal.target_component_id = command.target_compid; command_internal.frame = command.frame; @@ -83,28 +83,28 @@ MavlinkPassthroughImpl::send_command_int(const MavlinkPassthrough::CommandInt& c MavlinkPassthrough::Result MavlinkPassthroughImpl::to_mavlink_passthrough_result_from_mavlink_commands_result( - MAVLinkCommands::Result result) + MavlinkCommandSender::Result result) { switch (result) { - case MAVLinkCommands::Result::Success: + case MavlinkCommandSender::Result::Success: return MavlinkPassthrough::Result::Success; - case MAVLinkCommands::Result::NoSystem: + case MavlinkCommandSender::Result::NoSystem: return MavlinkPassthrough::Result::CommandNoSystem; - case MAVLinkCommands::Result::ConnectionError: + case MavlinkCommandSender::Result::ConnectionError: return MavlinkPassthrough::Result::ConnectionError; - case MAVLinkCommands::Result::Busy: + case MavlinkCommandSender::Result::Busy: return MavlinkPassthrough::Result::CommandBusy; - case MAVLinkCommands::Result::CommandDenied: + case MavlinkCommandSender::Result::CommandDenied: return MavlinkPassthrough::Result::CommandDenied; - case MAVLinkCommands::Result::Unsupported: + case MavlinkCommandSender::Result::Unsupported: return MavlinkPassthrough::Result::CommandUnsupported; - case MAVLinkCommands::Result::Timeout: + case MavlinkCommandSender::Result::Timeout: return MavlinkPassthrough::Result::CommandTimeout; default: // FALLTHROUGH - case MAVLinkCommands::Result::InProgress: // FIXME: currently not expected + case MavlinkCommandSender::Result::InProgress: // FIXME: currently not expected // FALLTHROUGH - case MAVLinkCommands::Result::UnknownError: + case MavlinkCommandSender::Result::UnknownError: return MavlinkPassthrough::Result::Unknown; } } diff --git a/src/plugins/mavlink_passthrough/mavlink_passthrough_impl.h b/src/plugins/mavlink_passthrough/mavlink_passthrough_impl.h index 2b10bea983..4f5be7173e 100644 --- a/src/plugins/mavlink_passthrough/mavlink_passthrough_impl.h +++ b/src/plugins/mavlink_passthrough/mavlink_passthrough_impl.h @@ -37,7 +37,7 @@ class MavlinkPassthroughImpl : public PluginImplBase { private: static MavlinkPassthrough::Result - to_mavlink_passthrough_result_from_mavlink_commands_result(MAVLinkCommands::Result result); + to_mavlink_passthrough_result_from_mavlink_commands_result(MavlinkCommandSender::Result result); }; } // namespace mavsdk diff --git a/src/plugins/mission/mission_impl.cpp b/src/plugins/mission/mission_impl.cpp index 4bec3a30d4..ef890f16b0 100644 --- a/src/plugins/mission/mission_impl.cpp +++ b/src/plugins/mission/mission_impl.cpp @@ -621,7 +621,7 @@ Mission::Result MissionImpl::start_mission() void MissionImpl::start_mission_async(const Mission::ResultCallback& callback) { _parent->set_flight_mode_async( - SystemImpl::FlightMode::Mission, [this, callback](MAVLinkCommands::Result result, float) { + SystemImpl::FlightMode::Mission, [this, callback](MavlinkCommandSender::Result result, float) { report_flight_mode_change(callback, result); }); } @@ -638,13 +638,13 @@ Mission::Result MissionImpl::pause_mission() void MissionImpl::pause_mission_async(const Mission::ResultCallback& callback) { _parent->set_flight_mode_async( - SystemImpl::FlightMode::Hold, [this, callback](MAVLinkCommands::Result result, float) { + SystemImpl::FlightMode::Hold, [this, callback](MavlinkCommandSender::Result result, float) { report_flight_mode_change(callback, result); }); } void MissionImpl::report_flight_mode_change( - Mission::ResultCallback callback, MAVLinkCommands::Result result) + Mission::ResultCallback callback, MavlinkCommandSender::Result result) { if (!callback) { return; @@ -654,24 +654,24 @@ void MissionImpl::report_flight_mode_change( [callback, result]() { callback(command_result_to_mission_result(result)); }); } -Mission::Result MissionImpl::command_result_to_mission_result(MAVLinkCommands::Result result) +Mission::Result MissionImpl::command_result_to_mission_result(MavlinkCommandSender::Result result) { switch (result) { - case MAVLinkCommands::Result::Success: + case MavlinkCommandSender::Result::Success: return Mission::Result::Success; - case MAVLinkCommands::Result::NoSystem: + case MavlinkCommandSender::Result::NoSystem: return Mission::Result::Error; // FIXME - case MAVLinkCommands::Result::ConnectionError: + case MavlinkCommandSender::Result::ConnectionError: return Mission::Result::Error; // FIXME - case MAVLinkCommands::Result::Busy: + case MavlinkCommandSender::Result::Busy: return Mission::Result::Busy; - case MAVLinkCommands::Result::CommandDenied: + case MavlinkCommandSender::Result::CommandDenied: return Mission::Result::Error; // FIXME - case MAVLinkCommands::Result::Timeout: + case MavlinkCommandSender::Result::Timeout: return Mission::Result::Timeout; - case MAVLinkCommands::Result::InProgress: + case MavlinkCommandSender::Result::InProgress: return Mission::Result::Busy; // FIXME - case MAVLinkCommands::Result::UnknownError: + case MavlinkCommandSender::Result::UnknownError: return Mission::Result::Unknown; default: return Mission::Result::Unknown; diff --git a/src/plugins/mission/mission_impl.h b/src/plugins/mission/mission_impl.h index fd0d0119f0..06d61e1b2c 100644 --- a/src/plugins/mission/mission_impl.h +++ b/src/plugins/mission/mission_impl.h @@ -88,8 +88,8 @@ class MissionImpl : public PluginImplBase { void reset_mission_progress(); void - report_flight_mode_change(Mission::ResultCallback callback, MAVLinkCommands::Result result); - static Mission::Result command_result_to_mission_result(MAVLinkCommands::Result result); + report_flight_mode_change(Mission::ResultCallback callback, MavlinkCommandSender::Result result); + static Mission::Result command_result_to_mission_result(MavlinkCommandSender::Result result); // FIXME: make static std::pair convert_to_result_and_mission_items( diff --git a/src/plugins/mission_raw/mission_raw_impl.cpp b/src/plugins/mission_raw/mission_raw_impl.cpp index 9c9840fa4b..7a6698fc4d 100644 --- a/src/plugins/mission_raw/mission_raw_impl.cpp +++ b/src/plugins/mission_raw/mission_raw_impl.cpp @@ -301,7 +301,7 @@ MissionRaw::Result MissionRawImpl::start_mission() void MissionRawImpl::start_mission_async(const MissionRaw::ResultCallback& callback) { _parent->set_flight_mode_async( - SystemImpl::FlightMode::Mission, [this, callback](MAVLinkCommands::Result result, float) { + SystemImpl::FlightMode::Mission, [this, callback](MavlinkCommandSender::Result result, float) { report_flight_mode_change(callback, result); }); } @@ -318,13 +318,13 @@ MissionRaw::Result MissionRawImpl::pause_mission() void MissionRawImpl::pause_mission_async(const MissionRaw::ResultCallback& callback) { _parent->set_flight_mode_async( - SystemImpl::FlightMode::Hold, [this, callback](MAVLinkCommands::Result result, float) { + SystemImpl::FlightMode::Hold, [this, callback](MavlinkCommandSender::Result result, float) { report_flight_mode_change(callback, result); }); } void MissionRawImpl::report_flight_mode_change( - MissionRaw::ResultCallback callback, MAVLinkCommands::Result result) + MissionRaw::ResultCallback callback, MavlinkCommandSender::Result result) { if (!callback) { return; @@ -334,24 +334,24 @@ void MissionRawImpl::report_flight_mode_change( [callback, result]() { callback(command_result_to_mission_result(result)); }); } -MissionRaw::Result MissionRawImpl::command_result_to_mission_result(MAVLinkCommands::Result result) +MissionRaw::Result MissionRawImpl::command_result_to_mission_result(MavlinkCommandSender::Result result) { switch (result) { - case MAVLinkCommands::Result::Success: + case MavlinkCommandSender::Result::Success: return MissionRaw::Result::Success; - case MAVLinkCommands::Result::NoSystem: + case MavlinkCommandSender::Result::NoSystem: return MissionRaw::Result::Error; // FIXME - case MAVLinkCommands::Result::ConnectionError: + case MavlinkCommandSender::Result::ConnectionError: return MissionRaw::Result::Error; // FIXME - case MAVLinkCommands::Result::Busy: + case MavlinkCommandSender::Result::Busy: return MissionRaw::Result::Busy; - case MAVLinkCommands::Result::CommandDenied: + case MavlinkCommandSender::Result::CommandDenied: return MissionRaw::Result::Error; // FIXME - case MAVLinkCommands::Result::Timeout: + case MavlinkCommandSender::Result::Timeout: return MissionRaw::Result::Timeout; - case MAVLinkCommands::Result::InProgress: + case MavlinkCommandSender::Result::InProgress: return MissionRaw::Result::Busy; // FIXME - case MAVLinkCommands::Result::UnknownError: + case MavlinkCommandSender::Result::UnknownError: return MissionRaw::Result::Unknown; default: return MissionRaw::Result::Unknown; diff --git a/src/plugins/mission_raw/mission_raw_impl.h b/src/plugins/mission_raw/mission_raw_impl.h index 5477c92559..ee5eb7a901 100644 --- a/src/plugins/mission_raw/mission_raw_impl.h +++ b/src/plugins/mission_raw/mission_raw_impl.h @@ -62,8 +62,8 @@ class MissionRawImpl : public PluginImplBase { void report_progress_current(); void - report_flight_mode_change(MissionRaw::ResultCallback callback, MAVLinkCommands::Result result); - static MissionRaw::Result command_result_to_mission_result(MAVLinkCommands::Result result); + report_flight_mode_change(MissionRaw::ResultCallback callback, MavlinkCommandSender::Result result); + static MissionRaw::Result command_result_to_mission_result(MavlinkCommandSender::Result result); std::vector convert_to_int_items(const std::vector& mission_raw); diff --git a/src/plugins/offboard/offboard_impl.cpp b/src/plugins/offboard/offboard_impl.cpp index 235fd67718..8bdd707caf 100644 --- a/src/plugins/offboard/offboard_impl.cpp +++ b/src/plugins/offboard/offboard_impl.cpp @@ -107,7 +107,7 @@ bool OffboardImpl::is_active() } void OffboardImpl::receive_command_result( - MAVLinkCommands::Result result, const Offboard::ResultCallback& callback) + MavlinkCommandSender::Result result, const Offboard::ResultCallback& callback) { Offboard::Result offboard_result = offboard_result_from_command_result(result); if (callback) { @@ -608,20 +608,20 @@ void OffboardImpl::stop_sending_setpoints() _mode = Mode::NotActive; } -Offboard::Result OffboardImpl::offboard_result_from_command_result(MAVLinkCommands::Result result) +Offboard::Result OffboardImpl::offboard_result_from_command_result(MavlinkCommandSender::Result result) { switch (result) { - case MAVLinkCommands::Result::Success: + case MavlinkCommandSender::Result::Success: return Offboard::Result::Success; - case MAVLinkCommands::Result::NoSystem: + case MavlinkCommandSender::Result::NoSystem: return Offboard::Result::NoSystem; - case MAVLinkCommands::Result::ConnectionError: + case MavlinkCommandSender::Result::ConnectionError: return Offboard::Result::ConnectionError; - case MAVLinkCommands::Result::Busy: + case MavlinkCommandSender::Result::Busy: return Offboard::Result::Busy; - case MAVLinkCommands::Result::CommandDenied: + case MavlinkCommandSender::Result::CommandDenied: return Offboard::Result::CommandDenied; - case MAVLinkCommands::Result::Timeout: + case MavlinkCommandSender::Result::Timeout: return Offboard::Result::Timeout; default: return Offboard::Result::Unknown; diff --git a/src/plugins/offboard/offboard_impl.h b/src/plugins/offboard/offboard_impl.h index 2804cb71bf..6602e88b44 100644 --- a/src/plugins/offboard/offboard_impl.h +++ b/src/plugins/offboard/offboard_impl.h @@ -50,9 +50,9 @@ class OffboardImpl : public PluginImplBase { void process_heartbeat(const mavlink_message_t& message); void receive_command_result( - MAVLinkCommands::Result result, const Offboard::ResultCallback& callback); + MavlinkCommandSender::Result result, const Offboard::ResultCallback& callback); - static Offboard::Result offboard_result_from_command_result(MAVLinkCommands::Result result); + static Offboard::Result offboard_result_from_command_result(MavlinkCommandSender::Result result); void stop_sending_setpoints(); diff --git a/src/plugins/telemetry/telemetry_impl.cpp b/src/plugins/telemetry/telemetry_impl.cpp index 3af08837de..be698e5381 100644 --- a/src/plugins/telemetry/telemetry_impl.cpp +++ b/src/plugins/telemetry/telemetry_impl.cpp @@ -483,20 +483,20 @@ void TelemetryImpl::set_rate_distance_sensor_async( } Telemetry::Result -TelemetryImpl::telemetry_result_from_command_result(MAVLinkCommands::Result command_result) +TelemetryImpl::telemetry_result_from_command_result(MavlinkCommandSender::Result command_result) { switch (command_result) { - case MAVLinkCommands::Result::Success: + case MavlinkCommandSender::Result::Success: return Telemetry::Result::Success; - case MAVLinkCommands::Result::NoSystem: + case MavlinkCommandSender::Result::NoSystem: return Telemetry::Result::NoSystem; - case MAVLinkCommands::Result::ConnectionError: + case MavlinkCommandSender::Result::ConnectionError: return Telemetry::Result::ConnectionError; - case MAVLinkCommands::Result::Busy: + case MavlinkCommandSender::Result::Busy: return Telemetry::Result::Busy; - case MAVLinkCommands::Result::CommandDenied: + case MavlinkCommandSender::Result::CommandDenied: return Telemetry::Result::CommandDenied; - case MAVLinkCommands::Result::Timeout: + case MavlinkCommandSender::Result::Timeout: return Telemetry::Result::Timeout; default: return Telemetry::Result::Unknown; @@ -504,7 +504,7 @@ TelemetryImpl::telemetry_result_from_command_result(MAVLinkCommands::Result comm } void TelemetryImpl::command_result_callback( - MAVLinkCommands::Result command_result, const Telemetry::ResultCallback& callback) + MavlinkCommandSender::Result command_result, const Telemetry::ResultCallback& callback) { Telemetry::Result action_result = telemetry_result_from_command_result(command_result); diff --git a/src/plugins/telemetry/telemetry_impl.h b/src/plugins/telemetry/telemetry_impl.h index 181b4a710e..8cea972a31 100644 --- a/src/plugins/telemetry/telemetry_impl.h +++ b/src/plugins/telemetry/telemetry_impl.h @@ -194,10 +194,10 @@ class TelemetryImpl : public PluginImplBase { void receive_unix_epoch_timeout(); static Telemetry::Result - telemetry_result_from_command_result(MAVLinkCommands::Result command_result); + telemetry_result_from_command_result(MavlinkCommandSender::Result command_result); static void command_result_callback( - MAVLinkCommands::Result command_result, const Telemetry::ResultCallback& callback); + MavlinkCommandSender::Result command_result, const Telemetry::ResultCallback& callback); static Telemetry::LandedState to_landed_state(mavlink_extended_sys_state_t extended_sys_state); diff --git a/src/plugins/tune/tune_impl.h b/src/plugins/tune/tune_impl.h index b7d4e38ad1..7736339930 100644 --- a/src/plugins/tune/tune_impl.h +++ b/src/plugins/tune/tune_impl.h @@ -32,7 +32,7 @@ class TuneImpl : public PluginImplBase { void report_tune_result(const Tune::ResultCallback& callback, Tune::Result result); void receive_command_result( - MAVLinkCommands::Result command_result, const Tune::ResultCallback& callback); + MavlinkCommandSender::Result command_result, const Tune::ResultCallback& callback); Tune::ResultCallback _result_callback = nullptr; diff --git a/tools/rename-enums-to-camelcase.sh b/tools/rename-enums-to-camelcase.sh index 9566951f29..b9660ba096 100755 --- a/tools/rename-enums-to-camelcase.sh +++ b/tools/rename-enums-to-camelcase.sh @@ -236,14 +236,14 @@ sed -i 's/MAVLinkParameters::Result::CONNECTION_ERROR/MAVLinkParameters::Result: sed -i 's/MAVLinkParameters::Result::WRONG_TYPE/MAVLinkParameters::Result::WrongType/g' $files sed -i 's/MAVLinkParameters::Result::PARAM_NAME_TOO_LONG/MAVLinkParameters::Result::ParamNameTooLong/g' $files -sed -i 's/MAVLinkCommands::Result::SUCCESS/MAVLinkCommands::Result::Success/g' $files -sed -i 's/MAVLinkCommands::Result::NO_SYSTEM/MAVLinkCommands::Result::NoSystem/g' $files -sed -i 's/MAVLinkCommands::Result::CONNECTION_ERROR/MAVLinkCommands::Result::ConnectionError/g' $files -sed -i 's/MAVLinkCommands::Result::BUSY/MAVLinkCommands::Result::Busy/g' $files -sed -i 's/MAVLinkCommands::Result::COMMAND_DENIED/MAVLinkCommands::Result::CommandDenied/g' $files -sed -i 's/MAVLinkCommands::Result::TIMEOUT/MAVLinkCommands::Result::Timeout/g' $files -sed -i 's/MAVLinkCommands::Result::IN_PROGRESS/MAVLinkCommands::Result::InProgress/g' $files -sed -i 's/MAVLinkCommands::Result::UNKNOWN_ERROR/MAVLinkCommands::Result::UnknownError/g' $files +sed -i 's/MavlinkCommandSender::Result::SUCCESS/MavlinkCommandSender::Result::Success/g' $files +sed -i 's/MavlinkCommandSender::Result::NO_SYSTEM/MavlinkCommandSender::Result::NoSystem/g' $files +sed -i 's/MavlinkCommandSender::Result::CONNECTION_ERROR/MavlinkCommandSender::Result::ConnectionError/g' $files +sed -i 's/MavlinkCommandSender::Result::BUSY/MavlinkCommandSender::Result::Busy/g' $files +sed -i 's/MavlinkCommandSender::Result::COMMAND_DENIED/MavlinkCommandSender::Result::CommandDenied/g' $files +sed -i 's/MavlinkCommandSender::Result::TIMEOUT/MavlinkCommandSender::Result::Timeout/g' $files +sed -i 's/MavlinkCommandSender::Result::IN_PROGRESS/MavlinkCommandSender::Result::InProgress/g' $files +sed -i 's/MavlinkCommandSender::Result::UNKNOWN_ERROR/MavlinkCommandSender::Result::UnknownError/g' $files sed -i 's/Color::RED/Color::Red/g' $files sed -i 's/Color::GREEN/Color::Green/g' $files From d62c3566074d9bd527914dfdf7a4d7b085667518 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Thu, 22 Oct 2020 13:42:07 +0200 Subject: [PATCH 3/4] style fix --- src/core/mavlink_commands.cpp | 35 ++-- src/core/mavlink_commands.h | 174 +++++++++--------- src/core/system_impl.cpp | 14 +- src/core/system_impl.h | 15 +- src/plugins/action/action_impl.cpp | 70 ++++--- src/plugins/gimbal/gimbal_impl.cpp | 3 +- src/plugins/gimbal/gimbal_impl.h | 3 +- .../manual_control/manual_control_impl.cpp | 6 +- .../manual_control/manual_control_impl.h | 3 +- .../mavlink_passthrough_impl.cpp | 2 +- src/plugins/mission/mission_impl.cpp | 3 +- src/plugins/mission/mission_impl.h | 4 +- src/plugins/mission_raw/mission_raw_impl.cpp | 6 +- src/plugins/mission_raw/mission_raw_impl.h | 4 +- src/plugins/offboard/offboard_impl.cpp | 3 +- src/plugins/offboard/offboard_impl.h | 3 +- 16 files changed, 189 insertions(+), 159 deletions(-) diff --git a/src/core/mavlink_commands.cpp b/src/core/mavlink_commands.cpp index bee7ee303e..775ff0d893 100644 --- a/src/core/mavlink_commands.cpp +++ b/src/core/mavlink_commands.cpp @@ -14,8 +14,7 @@ namespace mavsdk { // - The queue used does not support going through and checking each and every // item yet. -// MavlinkCommandSender class -MavlinkCommandSender::MavlinkCommandSender(SystemImpl& parent) : _parent(parent) +MavlinkCommandSender::MavlinkCommandSender(SystemImpl& system_impl) : _parent(system_impl) { _parent.register_mavlink_message_handler( MAVLINK_MSG_ID_COMMAND_ACK, @@ -28,7 +27,8 @@ MavlinkCommandSender::~MavlinkCommandSender() _parent.unregister_all_mavlink_message_handlers(this); } -MavlinkCommandSender::Result MavlinkCommandSender::send_command(const MavlinkCommandSender::CommandInt& command) +MavlinkCommandSender::Result +MavlinkCommandSender::send_command(const MavlinkCommandSender::CommandInt& command) { // We wrap the async call with a promise and future. auto prom = std::make_shared>(); @@ -48,7 +48,8 @@ MavlinkCommandSender::Result MavlinkCommandSender::send_command(const MavlinkCom return res.get(); } -MavlinkCommandSender::Result MavlinkCommandSender::send_command(const MavlinkCommandSender::CommandLong& command) +MavlinkCommandSender::Result +MavlinkCommandSender::send_command(const MavlinkCommandSender::CommandLong& command) { // We wrap the async call with a promise and future. auto prom = std::make_shared>(); @@ -67,7 +68,8 @@ MavlinkCommandSender::Result MavlinkCommandSender::send_command(const MavlinkCom return res.get(); } -void MavlinkCommandSender::queue_command_async(const CommandInt& command, CommandResultCallback callback) +void MavlinkCommandSender::queue_command_async( + const CommandInt& command, CommandResultCallback callback) { // LogDebug() << "Command " << (int)(command.command) << " to send to " // << (int)(command.target_system_id)<< ", " << (int)(command.target_component_id); @@ -319,10 +321,8 @@ void MavlinkCommandSender::call_callback( // we lock ourselves out when we send a command in the callback receiving a command result. _parent.call_user_callback([callback, result, progress]() { callback(result, progress); }); } -// MavlinkCommandSender class -// MavlinkCommandReceiver class -MavlinkCommandReceiver::MavlinkCommandReceiver(SystemImpl& parent) : _parent(parent) +MavlinkCommandReceiver::MavlinkCommandReceiver(SystemImpl& system_impl) : _parent(system_impl) { _parent.register_mavlink_message_handler( MAVLINK_MSG_ID_COMMAND_LONG, @@ -342,7 +342,7 @@ MavlinkCommandReceiver::~MavlinkCommandReceiver() _parent.unregister_all_mavlink_message_handlers(this); } -void MavlinkCommandReceiver::receive_command_int(mavlink_message_t message) +void MavlinkCommandReceiver::receive_command_int(const mavlink_message_t& message) { mavlink_command_int_t command_int; mavlink_msg_command_int_decode(&message, &command_int); @@ -350,14 +350,16 @@ void MavlinkCommandReceiver::receive_command_int(mavlink_message_t message) std::lock_guard lock(_mavlink_command_handler_table_mutex); - for (auto it = _mavlink_command_handler_table.begin(); it != _mavlink_command_handler_table.end(); ++it) { + for (auto it = _mavlink_command_handler_table.begin(); + it != _mavlink_command_handler_table.end(); + ++it) { if (it->cmd_id == command_int.command) { it->callback(cmd); } } } -void MavlinkCommandReceiver::receive_command_long(mavlink_message_t message) +void MavlinkCommandReceiver::receive_command_long(const mavlink_message_t& message) { mavlink_command_long_t command_long; mavlink_msg_command_long_decode(&message, &command_long); @@ -365,7 +367,9 @@ void MavlinkCommandReceiver::receive_command_long(mavlink_message_t message) std::lock_guard lock(_mavlink_command_handler_table_mutex); - for (auto it = _mavlink_command_handler_table.begin(); it != _mavlink_command_handler_table.end(); ++it) { + for (auto it = _mavlink_command_handler_table.begin(); + it != _mavlink_command_handler_table.end(); + ++it) { if (it->cmd_id == command_long.command) { it->callback(cmd); } @@ -385,7 +389,8 @@ void MavlinkCommandReceiver::unregister_mavlink_command_handler(uint16_t cmd_id, { std::lock_guard lock(_mavlink_command_handler_table_mutex); - for (auto it = _mavlink_command_handler_table.begin(); it != _mavlink_command_handler_table.end(); + for (auto it = _mavlink_command_handler_table.begin(); + it != _mavlink_command_handler_table.end(); /* no ++it */) { if (it->cmd_id == cmd_id && it->cookie == cookie) { it = _mavlink_command_handler_table.erase(it); @@ -399,7 +404,8 @@ void MavlinkCommandReceiver::unregister_all_mavlink_command_handlers(const void* { std::lock_guard lock(_mavlink_command_handler_table_mutex); - for (auto it = _mavlink_command_handler_table.begin(); it != _mavlink_command_handler_table.end(); + for (auto it = _mavlink_command_handler_table.begin(); + it != _mavlink_command_handler_table.end(); /* no ++it */) { if (it->cookie == cookie) { it = _mavlink_command_handler_table.erase(it); @@ -408,6 +414,5 @@ void MavlinkCommandReceiver::unregister_all_mavlink_command_handlers(const void* } } } -// MavlinkCommandReceiver class } // namespace mavsdk diff --git a/src/core/mavlink_commands.h b/src/core/mavlink_commands.h index 22d4f3795b..21e9544c92 100644 --- a/src/core/mavlink_commands.h +++ b/src/core/mavlink_commands.h @@ -12,94 +12,9 @@ namespace mavsdk { class SystemImpl; -class MavlinkCommandReceiver { -public: - explicit MavlinkCommandReceiver(SystemImpl& parent); - ~MavlinkCommandReceiver(); - - struct Command { - uint8_t target_system_id{0}; - uint8_t target_component_id{0}; - uint16_t command{0}; - - // Int - MAV_FRAME frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; - bool current = 0; - bool autocontinue = false; - - // Long - uint8_t confirmation = 0; - - // Mixed - struct Params { - float param1 = NAN; - float param2 = NAN; - float param3 = NAN; - float param4 = NAN; - float param5 = NAN; - float param6 = NAN; - float param7 = NAN; - int32_t x = 0; - int32_t y = 0; - float z = NAN; - } params{}; - - Command(mavlink_command_int_t command_int) { - target_system_id = command_int.target_system; - target_component_id = command_int.target_component; - command = command_int.command; - frame = static_cast(command_int.frame); - current = command_int.current; - autocontinue = command_int.autocontinue; - params.param1 = command_int.param1; - params.param2 = command_int.param2; - params.param3 = command_int.param3; - params.param4 = command_int.param4; - params.x = command_int.x; - params.y = command_int.y; - params.z = command_int.z; - } - - Command(mavlink_command_long_t command_long) { - target_system_id = command_long.target_system; - target_component_id = command_long.target_component; - command = command_long.command; - confirmation = command_long.confirmation; - params.param1 = command_long.param1; - params.param2 = command_long.param2; - params.param3 = command_long.param3; - params.param4 = command_long.param4; - params.param5 = command_long.param5; - params.param6 = command_long.param6; - params.param7 = command_long.param7; - } - }; - - typedef std::function mavlink_command_handler_t; - void register_mavlink_command_handler( - uint16_t cmd_id, mavlink_command_handler_t callback, const void* cookie); - - void unregister_mavlink_command_handler(uint16_t cmd_id, const void* cookie); - void unregister_all_mavlink_command_handlers(const void* cookie); - - SystemImpl& _parent; - - void receive_command_int(mavlink_message_t message); - void receive_command_long(mavlink_message_t message); - - struct MAVLinkCommandHandlerTableEntry { - uint16_t cmd_id; - mavlink_command_handler_t callback; - const void* cookie; // This is the identification to unregister. - }; - - std::mutex _mavlink_command_handler_table_mutex{}; - std::vector _mavlink_command_handler_table{}; -}; - class MavlinkCommandSender { public: - explicit MavlinkCommandSender(SystemImpl& parent); + explicit MavlinkCommandSender(SystemImpl& system_impl); ~MavlinkCommandSender(); enum class Result { @@ -213,4 +128,91 @@ class MavlinkCommandSender { void* _timeout_cookie = nullptr; }; +class MavlinkCommandReceiver { +public: + explicit MavlinkCommandReceiver(SystemImpl& system_impl); + ~MavlinkCommandReceiver(); + + struct Command { + uint8_t target_system_id{0}; + uint8_t target_component_id{0}; + uint16_t command{0}; + + // Int + MAV_FRAME frame{MAV_FRAME_GLOBAL_RELATIVE_ALT}; + bool current{0}; + bool autocontinue{false}; + + // Long + uint8_t confirmation{0}; + + // Mixed + struct Params { + float param1{NAN}; + float param2{NAN}; + float param3{NAN}; + float param4{NAN}; + float param5{NAN}; + float param6{NAN}; + float param7{NAN}; + int32_t x{0}; + int32_t y{0}; + float z{NAN}; + } params{}; + + Command(mavlink_command_int_t command_int) + { + target_system_id = command_int.target_system; + target_component_id = command_int.target_component; + command = command_int.command; + frame = static_cast(command_int.frame); + current = command_int.current; + autocontinue = command_int.autocontinue; + params.param1 = command_int.param1; + params.param2 = command_int.param2; + params.param3 = command_int.param3; + params.param4 = command_int.param4; + params.x = command_int.x; + params.y = command_int.y; + params.z = command_int.z; + } + + Command(mavlink_command_long_t command_long) + { + target_system_id = command_long.target_system; + target_component_id = command_long.target_component; + command = command_long.command; + confirmation = command_long.confirmation; + params.param1 = command_long.param1; + params.param2 = command_long.param2; + params.param3 = command_long.param3; + params.param4 = command_long.param4; + params.param5 = command_long.param5; + params.param6 = command_long.param6; + params.param7 = command_long.param7; + } + }; + + typedef std::function mavlink_command_handler_t; + void register_mavlink_command_handler( + uint16_t cmd_id, mavlink_command_handler_t callback, const void* cookie); + + void unregister_mavlink_command_handler(uint16_t cmd_id, const void* cookie); + void unregister_all_mavlink_command_handlers(const void* cookie); + + SystemImpl& _parent; + + void receive_command_int(const mavlink_message_t& message); + void receive_command_long(const mavlink_message_t& message); + + struct MAVLinkCommandHandlerTableEntry { + uint16_t cmd_id; + mavlink_command_handler_t callback; + const void* cookie; // This is the identification to unregister. + }; + + std::mutex _mavlink_command_handler_table_mutex{}; + std::vector _mavlink_command_handler_table{}; +}; + } // namespace mavsdk diff --git a/src/core/system_impl.cpp b/src/core/system_impl.cpp index 4e2b054de6..630f7c076d 100644 --- a/src/core/system_impl.cpp +++ b/src/core/system_impl.cpp @@ -972,7 +972,8 @@ SystemImpl::FlightMode SystemImpl::to_flight_mode_from_custom_mode(uint32_t cust } } -MavlinkCommandSender::Result SystemImpl::set_flight_mode(FlightMode system_mode, uint8_t component_id) +MavlinkCommandSender::Result +SystemImpl::set_flight_mode(FlightMode system_mode, uint8_t component_id) { std::pair result = make_command_flight_mode(system_mode, component_id); @@ -1107,14 +1108,16 @@ void SystemImpl::send_command_async( MavlinkCommandSender::Result SystemImpl::set_msg_rate(uint16_t message_id, double rate_hz, uint8_t component_id) { - MavlinkCommandSender::CommandLong command = make_command_msg_rate(message_id, rate_hz, component_id); + MavlinkCommandSender::CommandLong command = + make_command_msg_rate(message_id, rate_hz, component_id); return send_command(command); } void SystemImpl::set_msg_rate_async( uint16_t message_id, double rate_hz, CommandResultCallback callback, uint8_t component_id) { - MavlinkCommandSender::CommandLong command = make_command_msg_rate(message_id, rate_hz, component_id); + MavlinkCommandSender::CommandLong command = + make_command_msg_rate(message_id, rate_hz, component_id); send_command_async(command, callback); } @@ -1230,9 +1233,8 @@ void SystemImpl::intercept_outgoing_messages(std::functionget_autopilot_id(); - _parent->send_command_async(command, [this, callback](MavlinkCommandSender::Result result, float) { - command_result_callback(result, callback); - }); + _parent->send_command_async( + command, [this, callback](MavlinkCommandSender::Result result, float) { + command_result_callback(result, callback); + }); } void ActionImpl::terminate_async(const Action::ResultCallback& callback) const @@ -243,9 +244,10 @@ void ActionImpl::terminate_async(const Action::ResultCallback& callback) const command.params.param1 = 1; command.target_component_id = _parent->get_autopilot_id(); - _parent->send_command_async(command, [this, callback](MavlinkCommandSender::Result result, float) { - command_result_callback(result, callback); - }); + _parent->send_command_async( + command, [this, callback](MavlinkCommandSender::Result result, float) { + command_result_callback(result, callback); + }); } void ActionImpl::kill_async(const Action::ResultCallback& callback) const @@ -257,9 +259,10 @@ void ActionImpl::kill_async(const Action::ResultCallback& callback) const command.params.param2 = 21196.f; // magic number to enforce in-air command.target_component_id = _parent->get_autopilot_id(); - _parent->send_command_async(command, [this, callback](MavlinkCommandSender::Result result, float) { - command_result_callback(result, callback); - }); + _parent->send_command_async( + command, [this, callback](MavlinkCommandSender::Result result, float) { + command_result_callback(result, callback); + }); } void ActionImpl::reboot_async(const Action::ResultCallback& callback) const @@ -273,9 +276,10 @@ void ActionImpl::reboot_async(const Action::ResultCallback& callback) const command.params.param4 = 1.0f; // reboot gimbal command.target_component_id = _parent->get_autopilot_id(); - _parent->send_command_async(command, [this, callback](MavlinkCommandSender::Result result, float) { - command_result_callback(result, callback); - }); + _parent->send_command_async( + command, [this, callback](MavlinkCommandSender::Result result, float) { + command_result_callback(result, callback); + }); } void ActionImpl::shutdown_async(const Action::ResultCallback& callback) const @@ -289,9 +293,10 @@ void ActionImpl::shutdown_async(const Action::ResultCallback& callback) const command.params.param4 = 2.0f; // shutdown gimbal command.target_component_id = _parent->get_autopilot_id(); - _parent->send_command_async(command, [this, callback](MavlinkCommandSender::Result result, float) { - command_result_callback(result, callback); - }); + _parent->send_command_async( + command, [this, callback](MavlinkCommandSender::Result result, float) { + command_result_callback(result, callback); + }); } void ActionImpl::takeoff_async(const Action::ResultCallback& callback) const @@ -301,9 +306,10 @@ void ActionImpl::takeoff_async(const Action::ResultCallback& callback) const command.command = MAV_CMD_NAV_TAKEOFF; command.target_component_id = _parent->get_autopilot_id(); - _parent->send_command_async(command, [this, callback](MavlinkCommandSender::Result result, float) { - command_result_callback(result, callback); - }); + _parent->send_command_async( + command, [this, callback](MavlinkCommandSender::Result result, float) { + command_result_callback(result, callback); + }); } void ActionImpl::land_async(const Action::ResultCallback& callback) const @@ -314,9 +320,10 @@ void ActionImpl::land_async(const Action::ResultCallback& callback) const command.params.param4 = NAN; // Don't change yaw. command.target_component_id = _parent->get_autopilot_id(); - _parent->send_command_async(command, [this, callback](MavlinkCommandSender::Result result, float) { - command_result_callback(result, callback); - }); + _parent->send_command_async( + command, [this, callback](MavlinkCommandSender::Result result, float) { + command_result_callback(result, callback); + }); } void ActionImpl::return_to_launch_async(const Action::ResultCallback& callback) const @@ -344,9 +351,10 @@ void ActionImpl::goto_location_async( command.params.y = int32_t(std::round(longitude_deg * 1e7)); command.params.z = altitude_amsl_m; - _parent->send_command_async(command, [this, callback](MavlinkCommandSender::Result result, float) { - command_result_callback(result, callback); - }); + _parent->send_command_async( + command, [this, callback](MavlinkCommandSender::Result result, float) { + command_result_callback(result, callback); + }); } void ActionImpl::transition_to_fixedwing_async(const Action::ResultCallback& callback) const @@ -371,9 +379,10 @@ void ActionImpl::transition_to_fixedwing_async(const Action::ResultCallback& cal command.params.param1 = float(MAV_VTOL_STATE_FW); command.target_component_id = _parent->get_autopilot_id(); - _parent->send_command_async(command, [this, callback](MavlinkCommandSender::Result result, float) { - command_result_callback(result, callback); - }); + _parent->send_command_async( + command, [this, callback](MavlinkCommandSender::Result result, float) { + command_result_callback(result, callback); + }); } void ActionImpl::transition_to_multicopter_async(const Action::ResultCallback& callback) const @@ -397,9 +406,10 @@ void ActionImpl::transition_to_multicopter_async(const Action::ResultCallback& c command.params.param1 = float(MAV_VTOL_STATE_MC); command.target_component_id = _parent->get_autopilot_id(); - _parent->send_command_async(command, [this, callback](MavlinkCommandSender::Result result, float) { - command_result_callback(result, callback); - }); + _parent->send_command_async( + command, [this, callback](MavlinkCommandSender::Result result, float) { + command_result_callback(result, callback); + }); } Action::Result ActionImpl::taking_off_allowed() const diff --git a/src/plugins/gimbal/gimbal_impl.cpp b/src/plugins/gimbal/gimbal_impl.cpp index caf48d1908..d88e0f911e 100644 --- a/src/plugins/gimbal/gimbal_impl.cpp +++ b/src/plugins/gimbal/gimbal_impl.cpp @@ -136,7 +136,8 @@ void GimbalImpl::receive_command_result( } } -Gimbal::Result GimbalImpl::gimbal_result_from_command_result(MavlinkCommandSender::Result command_result) +Gimbal::Result +GimbalImpl::gimbal_result_from_command_result(MavlinkCommandSender::Result command_result) { switch (command_result) { case MavlinkCommandSender::Result::Success: diff --git a/src/plugins/gimbal/gimbal_impl.h b/src/plugins/gimbal/gimbal_impl.h index 80df77bffb..97bb5c6207 100644 --- a/src/plugins/gimbal/gimbal_impl.h +++ b/src/plugins/gimbal/gimbal_impl.h @@ -35,7 +35,8 @@ class GimbalImpl : public PluginImplBase { float altitude_m, Gimbal::ResultCallback callback); - static Gimbal::Result gimbal_result_from_command_result(MavlinkCommandSender::Result command_result); + static Gimbal::Result + gimbal_result_from_command_result(MavlinkCommandSender::Result command_result); static void receive_command_result( MavlinkCommandSender::Result command_result, const Gimbal::ResultCallback& callback); diff --git a/src/plugins/manual_control/manual_control_impl.cpp b/src/plugins/manual_control/manual_control_impl.cpp index ecc2d12124..7faf7e208f 100644 --- a/src/plugins/manual_control/manual_control_impl.cpp +++ b/src/plugins/manual_control/manual_control_impl.cpp @@ -29,7 +29,8 @@ void ManualControlImpl::disable() {} void ManualControlImpl::start_position_control_async(const ManualControl::ResultCallback callback) { _parent->set_flight_mode_async( - SystemImpl::FlightMode::Posctl, [this, callback](MavlinkCommandSender::Result result, float) { + SystemImpl::FlightMode::Posctl, + [this, callback](MavlinkCommandSender::Result result, float) { command_result_callback(result, callback); }); } @@ -47,7 +48,8 @@ ManualControl::Result ManualControlImpl::start_position_control() void ManualControlImpl::start_altitude_control_async(const ManualControl::ResultCallback callback) { _parent->set_flight_mode_async( - SystemImpl::FlightMode::Altctl, [this, callback](MavlinkCommandSender::Result result, float) { + SystemImpl::FlightMode::Altctl, + [this, callback](MavlinkCommandSender::Result result, float) { command_result_callback(result, callback); }); } diff --git a/src/plugins/manual_control/manual_control_impl.h b/src/plugins/manual_control/manual_control_impl.h index 05e320bcff..277584aea4 100644 --- a/src/plugins/manual_control/manual_control_impl.h +++ b/src/plugins/manual_control/manual_control_impl.h @@ -28,7 +28,8 @@ class ManualControlImpl : public PluginImplBase { ManualControl::Result set_manual_control_input(float x, float y, float z, float r); private: - ManualControl::Result manual_control_result_from_command_result(MavlinkCommandSender::Result result); + ManualControl::Result + manual_control_result_from_command_result(MavlinkCommandSender::Result result); void command_result_callback( MavlinkCommandSender::Result command_result, const ManualControl::ResultCallback& callback); }; diff --git a/src/plugins/mavlink_passthrough/mavlink_passthrough_impl.cpp b/src/plugins/mavlink_passthrough/mavlink_passthrough_impl.cpp index 88a4f01218..bf7c881e29 100644 --- a/src/plugins/mavlink_passthrough/mavlink_passthrough_impl.cpp +++ b/src/plugins/mavlink_passthrough/mavlink_passthrough_impl.cpp @@ -103,7 +103,7 @@ MavlinkPassthroughImpl::to_mavlink_passthrough_result_from_mavlink_commands_resu default: // FALLTHROUGH case MavlinkCommandSender::Result::InProgress: // FIXME: currently not expected - // FALLTHROUGH + // FALLTHROUGH case MavlinkCommandSender::Result::UnknownError: return MavlinkPassthrough::Result::Unknown; } diff --git a/src/plugins/mission/mission_impl.cpp b/src/plugins/mission/mission_impl.cpp index ef890f16b0..05cfe550fe 100644 --- a/src/plugins/mission/mission_impl.cpp +++ b/src/plugins/mission/mission_impl.cpp @@ -621,7 +621,8 @@ Mission::Result MissionImpl::start_mission() void MissionImpl::start_mission_async(const Mission::ResultCallback& callback) { _parent->set_flight_mode_async( - SystemImpl::FlightMode::Mission, [this, callback](MavlinkCommandSender::Result result, float) { + SystemImpl::FlightMode::Mission, + [this, callback](MavlinkCommandSender::Result result, float) { report_flight_mode_change(callback, result); }); } diff --git a/src/plugins/mission/mission_impl.h b/src/plugins/mission/mission_impl.h index 06d61e1b2c..2c775c955a 100644 --- a/src/plugins/mission/mission_impl.h +++ b/src/plugins/mission/mission_impl.h @@ -87,8 +87,8 @@ class MissionImpl : public PluginImplBase { void report_progress(); void reset_mission_progress(); - void - report_flight_mode_change(Mission::ResultCallback callback, MavlinkCommandSender::Result result); + void report_flight_mode_change( + Mission::ResultCallback callback, MavlinkCommandSender::Result result); static Mission::Result command_result_to_mission_result(MavlinkCommandSender::Result result); // FIXME: make static diff --git a/src/plugins/mission_raw/mission_raw_impl.cpp b/src/plugins/mission_raw/mission_raw_impl.cpp index 7a6698fc4d..b1cda4a8a9 100644 --- a/src/plugins/mission_raw/mission_raw_impl.cpp +++ b/src/plugins/mission_raw/mission_raw_impl.cpp @@ -301,7 +301,8 @@ MissionRaw::Result MissionRawImpl::start_mission() void MissionRawImpl::start_mission_async(const MissionRaw::ResultCallback& callback) { _parent->set_flight_mode_async( - SystemImpl::FlightMode::Mission, [this, callback](MavlinkCommandSender::Result result, float) { + SystemImpl::FlightMode::Mission, + [this, callback](MavlinkCommandSender::Result result, float) { report_flight_mode_change(callback, result); }); } @@ -334,7 +335,8 @@ void MissionRawImpl::report_flight_mode_change( [callback, result]() { callback(command_result_to_mission_result(result)); }); } -MissionRaw::Result MissionRawImpl::command_result_to_mission_result(MavlinkCommandSender::Result result) +MissionRaw::Result +MissionRawImpl::command_result_to_mission_result(MavlinkCommandSender::Result result) { switch (result) { case MavlinkCommandSender::Result::Success: diff --git a/src/plugins/mission_raw/mission_raw_impl.h b/src/plugins/mission_raw/mission_raw_impl.h index ee5eb7a901..d99bdff0b4 100644 --- a/src/plugins/mission_raw/mission_raw_impl.h +++ b/src/plugins/mission_raw/mission_raw_impl.h @@ -61,8 +61,8 @@ class MissionRawImpl : public PluginImplBase { void report_progress_current(); - void - report_flight_mode_change(MissionRaw::ResultCallback callback, MavlinkCommandSender::Result result); + void report_flight_mode_change( + MissionRaw::ResultCallback callback, MavlinkCommandSender::Result result); static MissionRaw::Result command_result_to_mission_result(MavlinkCommandSender::Result result); std::vector diff --git a/src/plugins/offboard/offboard_impl.cpp b/src/plugins/offboard/offboard_impl.cpp index 8bdd707caf..a4988963d2 100644 --- a/src/plugins/offboard/offboard_impl.cpp +++ b/src/plugins/offboard/offboard_impl.cpp @@ -608,7 +608,8 @@ void OffboardImpl::stop_sending_setpoints() _mode = Mode::NotActive; } -Offboard::Result OffboardImpl::offboard_result_from_command_result(MavlinkCommandSender::Result result) +Offboard::Result +OffboardImpl::offboard_result_from_command_result(MavlinkCommandSender::Result result) { switch (result) { case MavlinkCommandSender::Result::Success: diff --git a/src/plugins/offboard/offboard_impl.h b/src/plugins/offboard/offboard_impl.h index 6602e88b44..afdca1314e 100644 --- a/src/plugins/offboard/offboard_impl.h +++ b/src/plugins/offboard/offboard_impl.h @@ -52,7 +52,8 @@ class OffboardImpl : public PluginImplBase { void receive_command_result( MavlinkCommandSender::Result result, const Offboard::ResultCallback& callback); - static Offboard::Result offboard_result_from_command_result(MavlinkCommandSender::Result result); + static Offboard::Result + offboard_result_from_command_result(MavlinkCommandSender::Result result); void stop_sending_setpoints(); From b1ab361f2a99684d1c193fe146bf3985c5a296f1 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Thu, 22 Oct 2020 15:57:37 +0200 Subject: [PATCH 4/4] mavlink_commands: make clear separation between command types structure in MavlinkCommandReceiver --- src/core/mavlink_commands.cpp | 63 ++++++++++++++++++++------- src/core/mavlink_commands.h | 70 +++++++++++++++++++----------- src/core/system_impl.cpp | 12 ++++- src/core/system_impl.h | 6 ++- tools/rename-enums-to-camelcase.sh | 16 +++---- 5 files changed, 116 insertions(+), 51 deletions(-) diff --git a/src/core/mavlink_commands.cpp b/src/core/mavlink_commands.cpp index 775ff0d893..66923af0c4 100644 --- a/src/core/mavlink_commands.cpp +++ b/src/core/mavlink_commands.cpp @@ -346,12 +346,12 @@ void MavlinkCommandReceiver::receive_command_int(const mavlink_message_t& messag { mavlink_command_int_t command_int; mavlink_msg_command_int_decode(&message, &command_int); - MavlinkCommandReceiver::Command cmd(command_int); + MavlinkCommandReceiver::CommandInt cmd(command_int); std::lock_guard lock(_mavlink_command_handler_table_mutex); - for (auto it = _mavlink_command_handler_table.begin(); - it != _mavlink_command_handler_table.end(); + for (auto it = _mavlink_command_int_handler_table.begin(); + it != _mavlink_command_int_handler_table.end(); ++it) { if (it->cmd_id == command_int.command) { it->callback(cmd); @@ -363,12 +363,12 @@ void MavlinkCommandReceiver::receive_command_long(const mavlink_message_t& messa { mavlink_command_long_t command_long; mavlink_msg_command_long_decode(&message, &command_long); - MavlinkCommandReceiver::Command cmd(command_long); + MavlinkCommandReceiver::CommandLong cmd(command_long); std::lock_guard lock(_mavlink_command_handler_table_mutex); - for (auto it = _mavlink_command_handler_table.begin(); - it != _mavlink_command_handler_table.end(); + for (auto it = _mavlink_command_long_handler_table.begin(); + it != _mavlink_command_long_handler_table.end(); ++it) { if (it->cmd_id == command_long.command) { it->callback(cmd); @@ -377,23 +377,44 @@ void MavlinkCommandReceiver::receive_command_long(const mavlink_message_t& messa } void MavlinkCommandReceiver::register_mavlink_command_handler( - uint16_t cmd_id, mavlink_command_handler_t callback, const void* cookie) + uint16_t cmd_id, mavlink_command_int_handler_t callback, const void* cookie) { std::lock_guard lock(_mavlink_command_handler_table_mutex); - MAVLinkCommandHandlerTableEntry entry = {cmd_id, callback, cookie}; - _mavlink_command_handler_table.push_back(entry); + MAVLinkCommandIntHandlerTableEntry entry = {cmd_id, callback, cookie}; + _mavlink_command_int_handler_table.push_back(entry); +} + +void MavlinkCommandReceiver::register_mavlink_command_handler( + uint16_t cmd_id, mavlink_command_long_handler_t callback, const void* cookie) +{ + std::lock_guard lock(_mavlink_command_handler_table_mutex); + + MAVLinkCommandLongHandlerTableEntry entry = {cmd_id, callback, cookie}; + _mavlink_command_long_handler_table.push_back(entry); } void MavlinkCommandReceiver::unregister_mavlink_command_handler(uint16_t cmd_id, const void* cookie) { std::lock_guard lock(_mavlink_command_handler_table_mutex); - for (auto it = _mavlink_command_handler_table.begin(); - it != _mavlink_command_handler_table.end(); + // COMMAND_INT + for (auto it = _mavlink_command_int_handler_table.begin(); + it != _mavlink_command_int_handler_table.end(); + /* no ++it */) { + if (it->cmd_id == cmd_id && it->cookie == cookie) { + it = _mavlink_command_int_handler_table.erase(it); + } else { + ++it; + } + } + + // COMMAND_LONG + for (auto it = _mavlink_command_long_handler_table.begin(); + it != _mavlink_command_long_handler_table.end(); /* no ++it */) { if (it->cmd_id == cmd_id && it->cookie == cookie) { - it = _mavlink_command_handler_table.erase(it); + it = _mavlink_command_long_handler_table.erase(it); } else { ++it; } @@ -404,11 +425,23 @@ void MavlinkCommandReceiver::unregister_all_mavlink_command_handlers(const void* { std::lock_guard lock(_mavlink_command_handler_table_mutex); - for (auto it = _mavlink_command_handler_table.begin(); - it != _mavlink_command_handler_table.end(); + // COMMAND_INT + for (auto it = _mavlink_command_int_handler_table.begin(); + it != _mavlink_command_int_handler_table.end(); + /* no ++it */) { + if (it->cookie == cookie) { + it = _mavlink_command_int_handler_table.erase(it); + } else { + ++it; + } + } + + // COMMAND_LONG + for (auto it = _mavlink_command_long_handler_table.begin(); + it != _mavlink_command_long_handler_table.end(); /* no ++it */) { if (it->cookie == cookie) { - it = _mavlink_command_handler_table.erase(it); + it = _mavlink_command_long_handler_table.erase(it); } else { ++it; } diff --git a/src/core/mavlink_commands.h b/src/core/mavlink_commands.h index 21e9544c92..2b74187ed2 100644 --- a/src/core/mavlink_commands.h +++ b/src/core/mavlink_commands.h @@ -133,34 +133,25 @@ class MavlinkCommandReceiver { explicit MavlinkCommandReceiver(SystemImpl& system_impl); ~MavlinkCommandReceiver(); - struct Command { + struct CommandInt { uint8_t target_system_id{0}; uint8_t target_component_id{0}; + MAV_FRAME frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; uint16_t command{0}; - - // Int - MAV_FRAME frame{MAV_FRAME_GLOBAL_RELATIVE_ALT}; bool current{0}; bool autocontinue{false}; - - // Long - uint8_t confirmation{0}; - - // Mixed + // Most of the "Reserved" values in MAVLink spec are NAN. struct Params { - float param1{NAN}; - float param2{NAN}; - float param3{NAN}; - float param4{NAN}; - float param5{NAN}; - float param6{NAN}; - float param7{NAN}; - int32_t x{0}; - int32_t y{0}; - float z{NAN}; + float param1 = NAN; + float param2 = NAN; + float param3 = NAN; + float param4 = NAN; + int32_t x = 0; + int32_t y = 0; + float z = NAN; } params{}; - Command(mavlink_command_int_t command_int) + CommandInt(const mavlink_command_int_t& command_int) { target_system_id = command_int.target_system; target_component_id = command_int.target_component; @@ -176,8 +167,24 @@ class MavlinkCommandReceiver { params.y = command_int.y; params.z = command_int.z; } + }; + + struct CommandLong { + uint8_t target_system_id{0}; + uint8_t target_component_id{0}; + uint16_t command{0}; + uint8_t confirmation{0}; + struct Params { + float param1 = NAN; + float param2 = NAN; + float param3 = NAN; + float param4 = NAN; + float param5 = NAN; + float param6 = NAN; + float param7 = NAN; + } params{}; - Command(mavlink_command_long_t command_long) + CommandLong(const mavlink_command_long_t& command_long) { target_system_id = command_long.target_system; target_component_id = command_long.target_component; @@ -193,9 +200,13 @@ class MavlinkCommandReceiver { } }; - typedef std::function mavlink_command_handler_t; + typedef std::function mavlink_command_int_handler_t; + typedef std::function mavlink_command_long_handler_t; + + void register_mavlink_command_handler( + uint16_t cmd_id, mavlink_command_int_handler_t callback, const void* cookie); void register_mavlink_command_handler( - uint16_t cmd_id, mavlink_command_handler_t callback, const void* cookie); + uint16_t cmd_id, mavlink_command_long_handler_t callback, const void* cookie); void unregister_mavlink_command_handler(uint16_t cmd_id, const void* cookie); void unregister_all_mavlink_command_handlers(const void* cookie); @@ -205,14 +216,21 @@ class MavlinkCommandReceiver { void receive_command_int(const mavlink_message_t& message); void receive_command_long(const mavlink_message_t& message); - struct MAVLinkCommandHandlerTableEntry { + struct MAVLinkCommandIntHandlerTableEntry { + uint16_t cmd_id; + mavlink_command_int_handler_t callback; + const void* cookie; // This is the identification to unregister. + }; + + struct MAVLinkCommandLongHandlerTableEntry { uint16_t cmd_id; - mavlink_command_handler_t callback; + mavlink_command_long_handler_t callback; const void* cookie; // This is the identification to unregister. }; std::mutex _mavlink_command_handler_table_mutex{}; - std::vector _mavlink_command_handler_table{}; + std::vector _mavlink_command_int_handler_table{}; + std::vector _mavlink_command_long_handler_table{}; }; } // namespace mavsdk diff --git a/src/core/system_impl.cpp b/src/core/system_impl.cpp index 630f7c076d..fdc0cf0d9a 100644 --- a/src/core/system_impl.cpp +++ b/src/core/system_impl.cpp @@ -1234,7 +1234,17 @@ void SystemImpl::intercept_outgoing_messages(std::function