From fdb842814bb5fae41254db013a003260ac40f5f9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 20 Oct 2023 11:59:05 +1300 Subject: [PATCH] Split mission transfer into client and server This was weirdly mixed up in the same class before and required another thread in the server plugin due to locking problems. Signed-off-by: Julian Oes --- src/mavsdk/core/CMakeLists.txt | 6 +- src/mavsdk/core/flight_mode.h | 2 +- ...pp => mavlink_mission_transfer_client.cpp} | 338 ++--------- ...er.h => mavlink_mission_transfer_client.h} | 66 +-- ... mavlink_mission_transfer_client_test.cpp} | 530 +++--------------- .../core/mavlink_mission_transfer_server.cpp | 302 ++++++++++ .../core/mavlink_mission_transfer_server.h | 186 ++++++ .../mavlink_mission_transfer_server_test.cpp | 414 ++++++++++++++ src/mavsdk/core/mocks/sender_mock.h | 1 - src/mavsdk/core/server_component_impl.cpp | 4 +- src/mavsdk/core/server_component_impl.h | 6 +- src/mavsdk/core/system_impl.cpp | 6 +- src/mavsdk/core/system_impl.h | 6 +- src/mavsdk/plugins/geofence/geofence_impl.cpp | 53 +- src/mavsdk/plugins/geofence/geofence_impl.h | 4 +- src/mavsdk/plugins/mission/mission_impl.cpp | 100 ++-- src/mavsdk/plugins/mission/mission_impl.h | 22 +- .../plugins/mission_raw/mission_raw_impl.cpp | 64 +-- .../plugins/mission_raw/mission_raw_impl.h | 15 +- .../mission_raw_server_impl.cpp | 293 +++++----- .../mission_raw_server_impl.h | 21 +- 21 files changed, 1347 insertions(+), 1092 deletions(-) rename src/mavsdk/core/{mavlink_mission_transfer.cpp => mavlink_mission_transfer_client.cpp} (74%) rename src/mavsdk/core/{mavlink_mission_transfer.h => mavlink_mission_transfer_client.h} (81%) rename src/mavsdk/core/{mavlink_mission_transfer_test.cpp => mavlink_mission_transfer_client_test.cpp} (77%) create mode 100644 src/mavsdk/core/mavlink_mission_transfer_server.cpp create mode 100644 src/mavsdk/core/mavlink_mission_transfer_server.h create mode 100644 src/mavsdk/core/mavlink_mission_transfer_server_test.cpp diff --git a/src/mavsdk/core/CMakeLists.txt b/src/mavsdk/core/CMakeLists.txt index efe8a8abe0..4bba3621ab 100644 --- a/src/mavsdk/core/CMakeLists.txt +++ b/src/mavsdk/core/CMakeLists.txt @@ -30,7 +30,8 @@ target_sources(mavsdk mavlink_command_sender.cpp mavlink_ftp_client.cpp mavlink_ftp_server.cpp - mavlink_mission_transfer.cpp + mavlink_mission_transfer_client.cpp + mavlink_mission_transfer_server.cpp mavlink_parameter_cache.cpp mavlink_parameter_client.cpp mavlink_parameter_server.cpp @@ -154,7 +155,8 @@ list(APPEND UNIT_TEST_SOURCES ${PROJECT_SOURCE_DIR}/mavsdk/core/mavsdk_test.cpp ${PROJECT_SOURCE_DIR}/mavsdk/core/mavsdk_time_test.cpp ${PROJECT_SOURCE_DIR}/mavsdk/core/mavlink_channels_test.cpp - ${PROJECT_SOURCE_DIR}/mavsdk/core/mavlink_mission_transfer_test.cpp + ${PROJECT_SOURCE_DIR}/mavsdk/core/mavlink_mission_transfer_client_test.cpp + ${PROJECT_SOURCE_DIR}/mavsdk/core/mavlink_mission_transfer_server_test.cpp ${PROJECT_SOURCE_DIR}/mavsdk/core/mavlink_statustext_handler_test.cpp ${PROJECT_SOURCE_DIR}/mavsdk/core/ringbuffer_test.cpp ${PROJECT_SOURCE_DIR}/mavsdk/core/safe_queue_test.cpp diff --git a/src/mavsdk/core/flight_mode.h b/src/mavsdk/core/flight_mode.h index e0206f1352..54198b040b 100644 --- a/src/mavsdk/core/flight_mode.h +++ b/src/mavsdk/core/flight_mode.h @@ -1,7 +1,7 @@ #pragma once #include -#include "mavlink_mission_transfer.h" // FIXME: remove this dependency +#include "mavlink_include.h" #include "autopilot.h" namespace mavsdk { diff --git a/src/mavsdk/core/mavlink_mission_transfer.cpp b/src/mavsdk/core/mavlink_mission_transfer_client.cpp similarity index 74% rename from src/mavsdk/core/mavlink_mission_transfer.cpp rename to src/mavsdk/core/mavlink_mission_transfer_client.cpp index 7f5fee8b60..bca0c55e41 100644 --- a/src/mavsdk/core/mavlink_mission_transfer.cpp +++ b/src/mavsdk/core/mavlink_mission_transfer_client.cpp @@ -1,11 +1,11 @@ #include -#include "mavlink_mission_transfer.h" +#include "mavlink_mission_transfer_client.h" #include "log.h" #include "unused.h" namespace mavsdk { -MavlinkMissionTransfer::MavlinkMissionTransfer( +MavlinkMissionTransferClient::MavlinkMissionTransferClient( Sender& sender, MavlinkMessageHandler& message_handler, TimeoutHandler& timeout_handler, @@ -23,7 +23,8 @@ MavlinkMissionTransfer::MavlinkMissionTransfer( } } -std::weak_ptr MavlinkMissionTransfer::upload_items_async( +std::weak_ptr +MavlinkMissionTransferClient::upload_items_async( uint8_t type, uint8_t target_system_id, const std::vector& items, @@ -55,7 +56,8 @@ std::weak_ptr MavlinkMissionTransfer::upload_i return std::weak_ptr(ptr); } -std::weak_ptr MavlinkMissionTransfer::download_items_async( +std::weak_ptr +MavlinkMissionTransferClient::download_items_async( uint8_t type, uint8_t target_system_id, ResultAndItemsCallback callback, @@ -85,40 +87,7 @@ std::weak_ptr MavlinkMissionTransfer::download return std::weak_ptr(ptr); } -std::weak_ptr -MavlinkMissionTransfer::receive_incoming_items_async( - uint8_t type, - uint32_t mission_count, - uint8_t target_system, - uint8_t target_component, - ResultAndItemsCallback callback) -{ - if (!_int_messages_supported) { - if (callback) { - LogErr() << "Int messages are not supported."; - callback(Result::IntMessagesNotSupported, {}); - } - return {}; - } - - auto ptr = std::make_shared( - _sender, - _message_handler, - _timeout_handler, - type, - _timeout_s_callback(), - callback, - mission_count, - target_system, - target_component, - _debugging); - - _work_queue.push_back(ptr); - - return std::weak_ptr(ptr); -} - -void MavlinkMissionTransfer::clear_items_async( +void MavlinkMissionTransferClient::clear_items_async( uint8_t type, uint8_t target_system_id, ResultCallback callback) { auto ptr = std::make_shared( @@ -134,7 +103,7 @@ void MavlinkMissionTransfer::clear_items_async( _work_queue.push_back(ptr); } -void MavlinkMissionTransfer::set_current_item_async( +void MavlinkMissionTransferClient::set_current_item_async( int current, uint8_t target_system_id, ResultCallback callback) { auto ptr = std::make_shared( @@ -150,7 +119,7 @@ void MavlinkMissionTransfer::set_current_item_async( _work_queue.push_back(ptr); } -void MavlinkMissionTransfer::do_work() +void MavlinkMissionTransferClient::do_work() { LockedQueue::Guard work_queue_guard(_work_queue); auto work = work_queue_guard.get_front(); @@ -167,13 +136,13 @@ void MavlinkMissionTransfer::do_work() } } -bool MavlinkMissionTransfer::is_idle() +bool MavlinkMissionTransferClient::is_idle() { LockedQueue::Guard work_queue_guard(_work_queue); return (work_queue_guard.get_front() == nullptr); } -MavlinkMissionTransfer::WorkItem::WorkItem( +MavlinkMissionTransferClient::WorkItem::WorkItem( Sender& sender, MavlinkMessageHandler& message_handler, TimeoutHandler& timeout_handler, @@ -188,21 +157,21 @@ MavlinkMissionTransfer::WorkItem::WorkItem( _debugging(debugging) {} -MavlinkMissionTransfer::WorkItem::~WorkItem() {} +MavlinkMissionTransferClient::WorkItem::~WorkItem() {} -bool MavlinkMissionTransfer::WorkItem::has_started() +bool MavlinkMissionTransferClient::WorkItem::has_started() { std::lock_guard lock(_mutex); return _started; } -bool MavlinkMissionTransfer::WorkItem::is_done() +bool MavlinkMissionTransferClient::WorkItem::is_done() { std::lock_guard lock(_mutex); return _done; } -MavlinkMissionTransfer::UploadWorkItem::UploadWorkItem( +MavlinkMissionTransferClient::UploadWorkItem::UploadWorkItem( Sender& sender, MavlinkMessageHandler& message_handler, TimeoutHandler& timeout_handler, @@ -235,13 +204,13 @@ MavlinkMissionTransfer::UploadWorkItem::UploadWorkItem( this); } -MavlinkMissionTransfer::UploadWorkItem::~UploadWorkItem() +MavlinkMissionTransferClient::UploadWorkItem::~UploadWorkItem() { _message_handler.unregister_all(this); _timeout_handler.remove(_cookie); } -void MavlinkMissionTransfer::UploadWorkItem::start() +void MavlinkMissionTransferClient::UploadWorkItem::start() { std::lock_guard lock(_mutex); @@ -286,7 +255,7 @@ void MavlinkMissionTransfer::UploadWorkItem::start() send_count(); } -void MavlinkMissionTransfer::UploadWorkItem::cancel() +void MavlinkMissionTransferClient::UploadWorkItem::cancel() { std::lock_guard lock(_mutex); @@ -294,7 +263,7 @@ void MavlinkMissionTransfer::UploadWorkItem::cancel() send_cancel_and_finish(); } -void MavlinkMissionTransfer::UploadWorkItem::send_count() +void MavlinkMissionTransferClient::UploadWorkItem::send_count() { if (!_sender.queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) { mavlink_message_t message; @@ -322,7 +291,7 @@ void MavlinkMissionTransfer::UploadWorkItem::send_count() ++_retries_done; } -void MavlinkMissionTransfer::UploadWorkItem::send_cancel_and_finish() +void MavlinkMissionTransferClient::UploadWorkItem::send_cancel_and_finish() { if (!_sender.queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) { mavlink_message_t message; @@ -345,7 +314,7 @@ void MavlinkMissionTransfer::UploadWorkItem::send_cancel_and_finish() callback_and_reset(Result::Cancelled); } -void MavlinkMissionTransfer::UploadWorkItem::process_mission_request( +void MavlinkMissionTransferClient::UploadWorkItem::process_mission_request( const mavlink_message_t& request_message) { mavlink_mission_request_t request; @@ -393,7 +362,7 @@ void MavlinkMissionTransfer::UploadWorkItem::process_mission_request( } } -void MavlinkMissionTransfer::UploadWorkItem::process_mission_request_int( +void MavlinkMissionTransferClient::UploadWorkItem::process_mission_request_int( const mavlink_message_t& message) { std::lock_guard lock(_mutex); @@ -438,7 +407,7 @@ void MavlinkMissionTransfer::UploadWorkItem::process_mission_request_int( send_mission_item(); } -void MavlinkMissionTransfer::UploadWorkItem::send_mission_item() +void MavlinkMissionTransferClient::UploadWorkItem::send_mission_item() { if (_next_sequence >= _items.size()) { LogErr() << "send_mission_item: sequence out of bounds"; @@ -484,7 +453,8 @@ void MavlinkMissionTransfer::UploadWorkItem::send_mission_item() ++_retries_done; } -void MavlinkMissionTransfer::UploadWorkItem::process_mission_ack(const mavlink_message_t& message) +void MavlinkMissionTransferClient::UploadWorkItem::process_mission_ack( + const mavlink_message_t& message) { std::lock_guard lock(_mutex); @@ -546,7 +516,7 @@ void MavlinkMissionTransfer::UploadWorkItem::process_mission_ack(const mavlink_m } } -void MavlinkMissionTransfer::UploadWorkItem::process_timeout() +void MavlinkMissionTransferClient::UploadWorkItem::process_timeout() { std::lock_guard lock(_mutex); @@ -575,7 +545,7 @@ void MavlinkMissionTransfer::UploadWorkItem::process_timeout() } } -void MavlinkMissionTransfer::UploadWorkItem::callback_and_reset(Result result) +void MavlinkMissionTransferClient::UploadWorkItem::callback_and_reset(Result result) { if (_callback) { _callback(result); @@ -584,14 +554,14 @@ void MavlinkMissionTransfer::UploadWorkItem::callback_and_reset(Result result) _done = true; } -void MavlinkMissionTransfer::UploadWorkItem::update_progress(float progress) +void MavlinkMissionTransferClient::UploadWorkItem::update_progress(float progress) { if (_progress_callback != nullptr) { _progress_callback(progress); } } -MavlinkMissionTransfer::DownloadWorkItem::DownloadWorkItem( +MavlinkMissionTransferClient::DownloadWorkItem::DownloadWorkItem( Sender& sender, MavlinkMessageHandler& message_handler, TimeoutHandler& timeout_handler, @@ -617,13 +587,13 @@ MavlinkMissionTransfer::DownloadWorkItem::DownloadWorkItem( this); } -MavlinkMissionTransfer::DownloadWorkItem::~DownloadWorkItem() +MavlinkMissionTransferClient::DownloadWorkItem::~DownloadWorkItem() { _message_handler.unregister_all(this); _timeout_handler.remove(_cookie); } -void MavlinkMissionTransfer::DownloadWorkItem::start() +void MavlinkMissionTransferClient::DownloadWorkItem::start() { update_progress(0.0f); @@ -636,7 +606,7 @@ void MavlinkMissionTransfer::DownloadWorkItem::start() request_list(); } -void MavlinkMissionTransfer::DownloadWorkItem::cancel() +void MavlinkMissionTransferClient::DownloadWorkItem::cancel() { std::lock_guard lock(_mutex); @@ -644,7 +614,7 @@ void MavlinkMissionTransfer::DownloadWorkItem::cancel() send_cancel_and_finish(); } -void MavlinkMissionTransfer::DownloadWorkItem::request_list() +void MavlinkMissionTransferClient::DownloadWorkItem::request_list() { if (!_sender.queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) { mavlink_message_t message; @@ -666,7 +636,7 @@ void MavlinkMissionTransfer::DownloadWorkItem::request_list() ++_retries_done; } -void MavlinkMissionTransfer::DownloadWorkItem::request_item() +void MavlinkMissionTransferClient::DownloadWorkItem::request_item() { if (!_sender.queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) { mavlink_message_t message; @@ -689,7 +659,7 @@ void MavlinkMissionTransfer::DownloadWorkItem::request_item() ++_retries_done; } -void MavlinkMissionTransfer::DownloadWorkItem::send_ack_and_finish() +void MavlinkMissionTransferClient::DownloadWorkItem::send_ack_and_finish() { if (!_sender.queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) { mavlink_message_t message; @@ -712,7 +682,7 @@ void MavlinkMissionTransfer::DownloadWorkItem::send_ack_and_finish() callback_and_reset(Result::Success); } -void MavlinkMissionTransfer::DownloadWorkItem::send_cancel_and_finish() +void MavlinkMissionTransferClient::DownloadWorkItem::send_cancel_and_finish() { if (!_sender.queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) { mavlink_message_t message; @@ -735,7 +705,7 @@ void MavlinkMissionTransfer::DownloadWorkItem::send_cancel_and_finish() callback_and_reset(Result::Cancelled); } -void MavlinkMissionTransfer::DownloadWorkItem::process_mission_count( +void MavlinkMissionTransferClient::DownloadWorkItem::process_mission_count( const mavlink_message_t& message) { std::lock_guard lock(_mutex); @@ -757,7 +727,7 @@ void MavlinkMissionTransfer::DownloadWorkItem::process_mission_count( request_item(); } -void MavlinkMissionTransfer::DownloadWorkItem::process_mission_item_int( +void MavlinkMissionTransferClient::DownloadWorkItem::process_mission_item_int( const mavlink_message_t& message) { std::lock_guard lock(_mutex); @@ -799,7 +769,7 @@ void MavlinkMissionTransfer::DownloadWorkItem::process_mission_item_int( } } -void MavlinkMissionTransfer::DownloadWorkItem::process_timeout() +void MavlinkMissionTransferClient::DownloadWorkItem::process_timeout() { std::lock_guard lock(_mutex); @@ -821,7 +791,7 @@ void MavlinkMissionTransfer::DownloadWorkItem::process_timeout() } } -void MavlinkMissionTransfer::DownloadWorkItem::callback_and_reset(Result result) +void MavlinkMissionTransferClient::DownloadWorkItem::callback_and_reset(Result result) { if (_callback) { _callback(result, _items); @@ -830,205 +800,14 @@ void MavlinkMissionTransfer::DownloadWorkItem::callback_and_reset(Result result) _done = true; } -void MavlinkMissionTransfer::DownloadWorkItem::update_progress(float progress) +void MavlinkMissionTransferClient::DownloadWorkItem::update_progress(float progress) { if (_progress_callback != nullptr) { _progress_callback(progress); } } -MavlinkMissionTransfer::ReceiveIncomingMission::ReceiveIncomingMission( - Sender& sender, - MavlinkMessageHandler& message_handler, - TimeoutHandler& timeout_handler, - uint8_t type, - double timeout_s, - ResultAndItemsCallback callback, - uint32_t mission_count, - uint8_t target_system_id, - uint8_t target_component_id, - bool debugging) : - WorkItem(sender, message_handler, timeout_handler, type, timeout_s, debugging), - _callback(callback), - _mission_count(mission_count), - _target_system_id(target_system_id), - _target_component_id(target_component_id) -{} - -MavlinkMissionTransfer::ReceiveIncomingMission::~ReceiveIncomingMission() -{ - _message_handler.unregister_all(this); - _timeout_handler.remove(_cookie); -} - -void MavlinkMissionTransfer::ReceiveIncomingMission::start() -{ - _message_handler.register_one( - MAVLINK_MSG_ID_MISSION_ITEM_INT, - [this](const mavlink_message_t& message) { process_mission_item_int(message); }, - this); - - std::lock_guard lock(_mutex); - - _items.clear(); - - _started = true; - _retries_done = 0; - _timeout_handler.add([this]() { process_timeout(); }, _timeout_s, &_cookie); - process_mission_count(); -} - -void MavlinkMissionTransfer::ReceiveIncomingMission::cancel() -{ - std::lock_guard lock(_mutex); - - _timeout_handler.remove(_cookie); - send_cancel_and_finish(); -} - -void MavlinkMissionTransfer::ReceiveIncomingMission::request_item() -{ - if (!_sender.queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) { - mavlink_message_t message; - mavlink_msg_mission_request_int_pack_chan( - mavlink_address.system_id, - mavlink_address.component_id, - channel, - &message, - _target_system_id, - _target_component_id, - _next_sequence, - _type); - return message; - })) { - _timeout_handler.remove(_cookie); - callback_and_reset(Result::ConnectionError); - return; - } - - ++_retries_done; -} - -void MavlinkMissionTransfer::ReceiveIncomingMission::send_ack_and_finish() -{ - if (!_sender.queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) { - mavlink_message_t message; - mavlink_msg_mission_ack_pack_chan( - mavlink_address.system_id, - mavlink_address.component_id, - channel, - &message, - _target_system_id, - _target_component_id, - MAV_MISSION_ACCEPTED, - _type); - return message; - })) { - callback_and_reset(Result::ConnectionError); - return; - } - - // We do not wait on anything coming back after this. - callback_and_reset(Result::Success); -} - -void MavlinkMissionTransfer::ReceiveIncomingMission::send_cancel_and_finish() -{ - if (!_sender.queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) { - mavlink_message_t message; - mavlink_msg_mission_ack_pack_chan( - mavlink_address.system_id, - mavlink_address.component_id, - channel, - &message, - _target_system_id, - _target_component_id, - MAV_MISSION_OPERATION_CANCELLED, - _type); - return message; - })) { - callback_and_reset(Result::ConnectionError); - return; - } - - // We do not wait on anything coming back after this. - callback_and_reset(Result::Cancelled); -} - -void MavlinkMissionTransfer::ReceiveIncomingMission::process_mission_count() -{ - if (_mission_count == 0) { - send_ack_and_finish(); - _timeout_handler.remove(_cookie); - return; - } - - _timeout_handler.refresh(_cookie); - _next_sequence = 0; - _step = Step::RequestItem; - _retries_done = 0; - _expected_count = _mission_count; - request_item(); -} - -void MavlinkMissionTransfer::ReceiveIncomingMission::process_mission_item_int( - const mavlink_message_t& message) -{ - std::lock_guard lock(_mutex); - _timeout_handler.refresh(_cookie); - - mavlink_mission_item_int_t item_int; - mavlink_msg_mission_item_int_decode(&message, &item_int); - - _items.push_back(ItemInt{ - item_int.seq, - item_int.frame, - item_int.command, - item_int.current, - item_int.autocontinue, - item_int.param1, - item_int.param2, - item_int.param3, - item_int.param4, - item_int.x, - item_int.y, - item_int.z, - item_int.mission_type}); - - if (_next_sequence + 1 == _expected_count) { - _timeout_handler.remove(_cookie); - send_ack_and_finish(); - - } else { - _next_sequence = item_int.seq + 1; - _retries_done = 0; - request_item(); - } -} - -void MavlinkMissionTransfer::ReceiveIncomingMission::process_timeout() -{ - std::lock_guard lock(_mutex); - - if (_retries_done >= retries) { - callback_and_reset(Result::Timeout); - return; - } - - _timeout_handler.add([this]() { process_timeout(); }, _timeout_s, &_cookie); - request_item(); -} - -void MavlinkMissionTransfer::ReceiveIncomingMission::callback_and_reset(Result result) -{ - if (_callback) { - _callback(result, _items); - } - _callback = nullptr; - _done = true; -} - -MavlinkMissionTransfer::ClearWorkItem::ClearWorkItem( +MavlinkMissionTransferClient::ClearWorkItem::ClearWorkItem( Sender& sender, MavlinkMessageHandler& message_handler, TimeoutHandler& timeout_handler, @@ -1047,13 +826,13 @@ MavlinkMissionTransfer::ClearWorkItem::ClearWorkItem( this); } -MavlinkMissionTransfer::ClearWorkItem::~ClearWorkItem() +MavlinkMissionTransferClient::ClearWorkItem::~ClearWorkItem() { _message_handler.unregister_all(this); _timeout_handler.remove(_cookie); } -void MavlinkMissionTransfer::ClearWorkItem::start() +void MavlinkMissionTransferClient::ClearWorkItem::start() { std::lock_guard lock(_mutex); @@ -1063,7 +842,7 @@ void MavlinkMissionTransfer::ClearWorkItem::start() send_clear(); } -void MavlinkMissionTransfer::ClearWorkItem::cancel() +void MavlinkMissionTransferClient::ClearWorkItem::cancel() { std::lock_guard lock(_mutex); @@ -1071,7 +850,7 @@ void MavlinkMissionTransfer::ClearWorkItem::cancel() // This is presumably not used or exposed because it is quick. } -void MavlinkMissionTransfer::ClearWorkItem::send_clear() +void MavlinkMissionTransferClient::ClearWorkItem::send_clear() { if (!_sender.queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) { mavlink_message_t message; @@ -1093,7 +872,7 @@ void MavlinkMissionTransfer::ClearWorkItem::send_clear() ++_retries_done; } -void MavlinkMissionTransfer::ClearWorkItem::process_timeout() +void MavlinkMissionTransferClient::ClearWorkItem::process_timeout() { std::lock_guard lock(_mutex); @@ -1106,7 +885,8 @@ void MavlinkMissionTransfer::ClearWorkItem::process_timeout() send_clear(); } -void MavlinkMissionTransfer::ClearWorkItem::process_mission_ack(const mavlink_message_t& message) +void MavlinkMissionTransferClient::ClearWorkItem::process_mission_ack( + const mavlink_message_t& message) { std::lock_guard lock(_mutex); @@ -1160,7 +940,7 @@ void MavlinkMissionTransfer::ClearWorkItem::process_mission_ack(const mavlink_me } } -void MavlinkMissionTransfer::ClearWorkItem::callback_and_reset(Result result) +void MavlinkMissionTransferClient::ClearWorkItem::callback_and_reset(Result result) { if (_callback) { _callback(result); @@ -1169,12 +949,12 @@ void MavlinkMissionTransfer::ClearWorkItem::callback_and_reset(Result result) _done = true; } -void MavlinkMissionTransfer::set_int_messages_supported(bool supported) +void MavlinkMissionTransferClient::set_int_messages_supported(bool supported) { _int_messages_supported = supported; } -MavlinkMissionTransfer::SetCurrentWorkItem::SetCurrentWorkItem( +MavlinkMissionTransferClient::SetCurrentWorkItem::SetCurrentWorkItem( Sender& sender, MavlinkMessageHandler& message_handler, TimeoutHandler& timeout_handler, @@ -1195,13 +975,13 @@ MavlinkMissionTransfer::SetCurrentWorkItem::SetCurrentWorkItem( this); } -MavlinkMissionTransfer::SetCurrentWorkItem::~SetCurrentWorkItem() +MavlinkMissionTransferClient::SetCurrentWorkItem::~SetCurrentWorkItem() { _message_handler.unregister_all(this); _timeout_handler.remove(_cookie); } -void MavlinkMissionTransfer::SetCurrentWorkItem::start() +void MavlinkMissionTransferClient::SetCurrentWorkItem::start() { std::lock_guard lock(_mutex); @@ -1218,7 +998,7 @@ void MavlinkMissionTransfer::SetCurrentWorkItem::start() send_current_mission_item(); } -void MavlinkMissionTransfer::SetCurrentWorkItem::cancel() +void MavlinkMissionTransferClient::SetCurrentWorkItem::cancel() { std::lock_guard lock(_mutex); @@ -1226,7 +1006,7 @@ void MavlinkMissionTransfer::SetCurrentWorkItem::cancel() // This is presumably not used or exposed because it is quick. } -void MavlinkMissionTransfer::SetCurrentWorkItem::send_current_mission_item() +void MavlinkMissionTransferClient::SetCurrentWorkItem::send_current_mission_item() { if (!_sender.queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) { mavlink_message_t message; @@ -1248,7 +1028,7 @@ void MavlinkMissionTransfer::SetCurrentWorkItem::send_current_mission_item() ++_retries_done; } -void MavlinkMissionTransfer::SetCurrentWorkItem::process_mission_current( +void MavlinkMissionTransferClient::SetCurrentWorkItem::process_mission_current( const mavlink_message_t& message) { std::lock_guard lock(_mutex); @@ -1269,7 +1049,7 @@ void MavlinkMissionTransfer::SetCurrentWorkItem::process_mission_current( } } -void MavlinkMissionTransfer::SetCurrentWorkItem::process_timeout() +void MavlinkMissionTransferClient::SetCurrentWorkItem::process_timeout() { std::lock_guard lock(_mutex); @@ -1282,7 +1062,7 @@ void MavlinkMissionTransfer::SetCurrentWorkItem::process_timeout() send_current_mission_item(); } -void MavlinkMissionTransfer::SetCurrentWorkItem::callback_and_reset(Result result) +void MavlinkMissionTransferClient::SetCurrentWorkItem::callback_and_reset(Result result) { if (_callback) { _callback(result); diff --git a/src/mavsdk/core/mavlink_mission_transfer.h b/src/mavsdk/core/mavlink_mission_transfer_client.h similarity index 81% rename from src/mavsdk/core/mavlink_mission_transfer.h rename to src/mavsdk/core/mavlink_mission_transfer_client.h index 33bc9a4dc2..6ac379d682 100644 --- a/src/mavsdk/core/mavlink_mission_transfer.h +++ b/src/mavsdk/core/mavlink_mission_transfer_client.h @@ -16,7 +16,7 @@ namespace mavsdk { -class MavlinkMissionTransfer { +class MavlinkMissionTransferClient { public: enum class Result { Success, @@ -149,54 +149,6 @@ class MavlinkMissionTransfer { uint8_t _target_system_id; }; - class ReceiveIncomingMission : public WorkItem { - public: - explicit ReceiveIncomingMission( - Sender& sender, - MavlinkMessageHandler& message_handler, - TimeoutHandler& timeout_handler, - uint8_t type, - double timeout_s, - ResultAndItemsCallback callback, - uint32_t mission_count, - uint8_t target_system_id, - uint8_t target_component_id, - bool debugging); - ~ReceiveIncomingMission() override; - - void start() override; - void cancel() override; - - ReceiveIncomingMission(const ReceiveIncomingMission&) = delete; - ReceiveIncomingMission(ReceiveIncomingMission&&) = delete; - ReceiveIncomingMission& operator=(const ReceiveIncomingMission&) = delete; - ReceiveIncomingMission& operator=(ReceiveIncomingMission&&) = delete; - - private: - void request_item(); - void send_ack_and_finish(); - void send_cancel_and_finish(); - void process_mission_count(); - void process_mission_item_int(const mavlink_message_t& message); - void process_timeout(); - void callback_and_reset(Result result); - - enum class Step { - RequestList, - RequestItem, - } _step{Step::RequestList}; - - std::vector _items{}; - ResultAndItemsCallback _callback{nullptr}; - void* _cookie{nullptr}; - std::size_t _next_sequence{0}; - std::size_t _expected_count{0}; - unsigned _retries_done{0}; - uint32_t _mission_count{0}; - uint8_t _target_system_id{0}; - uint8_t _target_component_id{0}; - }; - class DownloadWorkItem : public WorkItem { public: explicit DownloadWorkItem( @@ -316,13 +268,13 @@ class MavlinkMissionTransfer { static constexpr unsigned retries = 5; - explicit MavlinkMissionTransfer( + explicit MavlinkMissionTransferClient( Sender& sender, MavlinkMessageHandler& message_handler, TimeoutHandler& timeout_handler, TimeoutSCallback get_timeout_s_callback); - ~MavlinkMissionTransfer() = default; + ~MavlinkMissionTransferClient() = default; std::weak_ptr upload_items_async( uint8_t type, @@ -337,14 +289,6 @@ class MavlinkMissionTransfer { ResultAndItemsCallback callback, ProgressCallback progress_callback = nullptr); - // Server-side - std::weak_ptr receive_incoming_items_async( - uint8_t type, - uint32_t mission_count, - uint8_t target_system, - uint8_t target_component, - ResultAndItemsCallback callback); - void clear_items_async(uint8_t type, uint8_t target_system_id, ResultCallback callback); void set_current_item_async(int current, uint8_t target_system_id, ResultCallback callback); @@ -355,8 +299,8 @@ class MavlinkMissionTransfer { void set_int_messages_supported(bool supported); // Non-copyable - MavlinkMissionTransfer(const MavlinkMissionTransfer&) = delete; - const MavlinkMissionTransfer& operator=(const MavlinkMissionTransfer&) = delete; + MavlinkMissionTransferClient(const MavlinkMissionTransferClient&) = delete; + const MavlinkMissionTransferClient& operator=(const MavlinkMissionTransferClient&) = delete; private: Sender& _sender; diff --git a/src/mavsdk/core/mavlink_mission_transfer_test.cpp b/src/mavsdk/core/mavlink_mission_transfer_client_test.cpp similarity index 77% rename from src/mavsdk/core/mavlink_mission_transfer_test.cpp rename to src/mavsdk/core/mavlink_mission_transfer_client_test.cpp index 3875702480..15cf478ae0 100644 --- a/src/mavsdk/core/mavlink_mission_transfer_test.cpp +++ b/src/mavsdk/core/mavlink_mission_transfer_client_test.cpp @@ -3,7 +3,7 @@ #include #include -#include "mavlink_mission_transfer.h" +#include "mavlink_mission_transfer_client.h" #include "mocks/sender_mock.h" #include "unused.h" @@ -15,8 +15,8 @@ using ::testing::Return; using ::testing::Truly; using MockSender = NiceMock; -using Result = MavlinkMissionTransfer::Result; -using ItemInt = MavlinkMissionTransfer::ItemInt; +using Result = MavlinkMissionTransferClient::Result; +using ItemInt = MavlinkMissionTransferClient::ItemInt; static MavlinkAddress own_address{42, 16}; static uint8_t channel{0}; @@ -29,9 +29,9 @@ static constexpr double timeout_s = 0.5; EXPECT_FALSE(called); \ called = true -class MavlinkMissionTransferTest : public ::testing::Test { +class MavlinkMissionTransferClientTest : public ::testing::Test { protected: - MavlinkMissionTransferTest() : + MavlinkMissionTransferClientTest() : ::testing::Test(), timeout_handler(time), mmt(mock_sender, message_handler, timeout_handler, []() { return timeout_s; }) @@ -49,7 +49,7 @@ class MavlinkMissionTransferTest : public ::testing::Test { MavlinkMessageHandler message_handler; FakeTime time; TimeoutHandler timeout_handler; - MavlinkMissionTransfer mmt; + MavlinkMissionTransferClient mmt; }; ItemInt make_item(uint8_t type, uint16_t sequence) @@ -73,7 +73,21 @@ ItemInt make_item(uint8_t type, uint16_t sequence) return item; } -TEST_F(MavlinkMissionTransferTest, UploadMissionDoesComplainAboutNoItems) +mavlink_message_t make_mission_count(unsigned count) +{ + mavlink_message_t message; + mavlink_msg_mission_count_pack( + own_address.system_id, + own_address.component_id, + &message, + target_address.system_id, + target_address.component_id, + count, + MAV_MISSION_TYPE_MISSION); + return message; +} + +TEST_F(MavlinkMissionTransferClientTest, UploadMissionDoesComplainAboutNoItems) { std::vector items; @@ -94,7 +108,7 @@ TEST_F(MavlinkMissionTransferTest, UploadMissionDoesComplainAboutNoItems) EXPECT_TRUE(mmt.is_idle()); } -TEST_F(MavlinkMissionTransferTest, UploadMissionDoesComplainAboutWrongSequence) +TEST_F(MavlinkMissionTransferClientTest, UploadMissionDoesComplainAboutWrongSequence) { std::vector items; items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 0)); @@ -117,7 +131,8 @@ TEST_F(MavlinkMissionTransferTest, UploadMissionDoesComplainAboutWrongSequence) EXPECT_TRUE(mmt.is_idle()); } -TEST_F(MavlinkMissionTransferTest, UploadMissionDoesComplainAboutInconsistentMissionTypesInAPI) +TEST_F( + MavlinkMissionTransferClientTest, UploadMissionDoesComplainAboutInconsistentMissionTypesInAPI) { std::vector items; items.push_back(make_item(MAV_MISSION_TYPE_FENCE, 0)); @@ -141,7 +156,8 @@ TEST_F(MavlinkMissionTransferTest, UploadMissionDoesComplainAboutInconsistentMis EXPECT_TRUE(mmt.is_idle()); } -TEST_F(MavlinkMissionTransferTest, UploadMissionDoesComplainAboutInconsistentMissionTypesInItems) +TEST_F( + MavlinkMissionTransferClientTest, UploadMissionDoesComplainAboutInconsistentMissionTypesInItems) { std::vector items; items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 0)); @@ -165,7 +181,7 @@ TEST_F(MavlinkMissionTransferTest, UploadMissionDoesComplainAboutInconsistentMis EXPECT_TRUE(mmt.is_idle()); } -TEST_F(MavlinkMissionTransferTest, UploadMissionDoesComplainAboutNoCurrent) +TEST_F(MavlinkMissionTransferClientTest, UploadMissionDoesComplainAboutNoCurrent) { std::vector items; items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 0)); @@ -191,7 +207,7 @@ TEST_F(MavlinkMissionTransferTest, UploadMissionDoesComplainAboutNoCurrent) EXPECT_TRUE(mmt.is_idle()); } -TEST_F(MavlinkMissionTransferTest, UploadMissionDoesComplainAboutTwoCurrents) +TEST_F(MavlinkMissionTransferClientTest, UploadMissionDoesComplainAboutTwoCurrents) { std::vector items; items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 0)); @@ -217,7 +233,7 @@ TEST_F(MavlinkMissionTransferTest, UploadMissionDoesComplainAboutTwoCurrents) EXPECT_TRUE(mmt.is_idle()); } -TEST_F(MavlinkMissionTransferTest, UploadMissionDoesNotCrashIfCallbackIsNull) +TEST_F(MavlinkMissionTransferClientTest, UploadMissionDoesNotCrashIfCallbackIsNull) { ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(false)); @@ -241,7 +257,7 @@ TEST_F(MavlinkMissionTransferTest, UploadMissionDoesNotCrashIfCallbackIsNull) EXPECT_TRUE(mmt.is_idle()); } -TEST_F(MavlinkMissionTransferTest, UploadMissionReturnsConnectionErrorWhenSendMessageFails) +TEST_F(MavlinkMissionTransferClientTest, UploadMissionReturnsConnectionErrorWhenSendMessageFails) { ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(false)); @@ -270,7 +286,8 @@ TEST_F(MavlinkMissionTransferTest, UploadMissionReturnsConnectionErrorWhenSendMe EXPECT_TRUE(mmt.is_idle()); } -bool is_correct_mission_send_count(uint8_t type, unsigned count, const mavlink_message_t& message) +static bool +is_correct_mission_send_count(uint8_t type, unsigned count, const mavlink_message_t& message) { if (message.msgid != MAVLINK_MSG_ID_MISSION_COUNT) { return false; @@ -287,7 +304,7 @@ bool is_correct_mission_send_count(uint8_t type, unsigned count, const mavlink_m mission_count.count == count && mission_count.mission_type == type); } -TEST_F(MavlinkMissionTransferTest, UploadMissionSendsCount) +TEST_F(MavlinkMissionTransferClientTest, UploadMissionSendsCount) { std::vector items; items.push_back(make_item(MAV_MISSION_TYPE_FENCE, 0)); @@ -311,7 +328,7 @@ TEST_F(MavlinkMissionTransferTest, UploadMissionSendsCount) mmt.do_work(); } -TEST_F(MavlinkMissionTransferTest, UploadMissionResendsCount) +TEST_F(MavlinkMissionTransferClientTest, UploadMissionResendsCount) { std::vector items; items.push_back(make_item(MAV_MISSION_TYPE_FENCE, 0)); @@ -339,7 +356,7 @@ TEST_F(MavlinkMissionTransferTest, UploadMissionResendsCount) timeout_handler.run_once(); } -TEST_F(MavlinkMissionTransferTest, UploadMissionTimeoutAfterSendCount) +TEST_F(MavlinkMissionTransferClientTest, UploadMissionTimeoutAfterSendCount) { std::vector items; items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 0)); @@ -354,7 +371,7 @@ TEST_F(MavlinkMissionTransferTest, UploadMissionTimeoutAfterSendCount) return is_correct_mission_send_count( MAV_MISSION_TYPE_MISSION, items.size(), fun(own_address, channel)); }))) - .Times(MavlinkMissionTransfer::retries); + .Times(MavlinkMissionTransferClient::retries); std::promise prom; auto fut = prom.get_future(); @@ -370,7 +387,7 @@ TEST_F(MavlinkMissionTransferTest, UploadMissionTimeoutAfterSendCount) EXPECT_EQ(fut.wait_for(std::chrono::seconds(0)), std::future_status::timeout); // After the specified retries we should give up with a timeout. - for (unsigned i = 0; i < MavlinkMissionTransfer::retries; ++i) { + for (unsigned i = 0; i < MavlinkMissionTransferClient::retries; ++i) { time.sleep_for(std::chrono::milliseconds(static_cast(timeout_s * 1.1 * 1000.))); timeout_handler.run_once(); } @@ -434,7 +451,7 @@ bool is_the_same_mission_item_int(const ItemInt& item, const mavlink_message_t& mission_item_int.mission_type == item.mission_type); } -TEST_F(MavlinkMissionTransferTest, UploadMissionSendsMissionItems) +TEST_F(MavlinkMissionTransferClientTest, UploadMissionSendsMissionItems) { std::vector items; items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 0)); @@ -485,7 +502,7 @@ TEST_F(MavlinkMissionTransferTest, UploadMissionSendsMissionItems) EXPECT_TRUE(mmt.is_idle()); } -TEST_F(MavlinkMissionTransferTest, UploadMissionResendsMissionItems) +TEST_F(MavlinkMissionTransferClientTest, UploadMissionResendsMissionItems) { std::vector items; items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 0)); @@ -543,7 +560,7 @@ TEST_F(MavlinkMissionTransferTest, UploadMissionResendsMissionItems) EXPECT_TRUE(mmt.is_idle()); } -TEST_F(MavlinkMissionTransferTest, UploadMissionResendsMissionItemsButGivesUpAfterSomeRetries) +TEST_F(MavlinkMissionTransferClientTest, UploadMissionResendsMissionItemsButGivesUpAfterSomeRetries) { std::vector items; items.push_back(make_item(MAV_MISSION_TYPE_FENCE, 0)); @@ -568,9 +585,9 @@ TEST_F(MavlinkMissionTransferTest, UploadMissionResendsMissionItemsButGivesUpAft MavlinkAddress mavlink_address, uint8_t channel)> fun) { return is_the_same_mission_item_int(items[0], fun(own_address, channel)); }))) - .Times(MavlinkMissionTransfer::retries); + .Times(MavlinkMissionTransferClient::retries); - for (unsigned i = 0; i < MavlinkMissionTransfer::retries; ++i) { + for (unsigned i = 0; i < MavlinkMissionTransferClient::retries; ++i) { message_handler.process_message(make_mission_request_int(MAV_MISSION_TYPE_MISSION, 0)); } @@ -585,7 +602,7 @@ TEST_F(MavlinkMissionTransferTest, UploadMissionResendsMissionItemsButGivesUpAft EXPECT_TRUE(mmt.is_idle()); } -TEST_F(MavlinkMissionTransferTest, UploadMissionAckArrivesTooEarly) +TEST_F(MavlinkMissionTransferClientTest, UploadMissionAckArrivesTooEarly) { std::vector items; items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 0)); @@ -623,11 +640,11 @@ TEST_F(MavlinkMissionTransferTest, UploadMissionAckArrivesTooEarly) EXPECT_TRUE(mmt.is_idle()); } -class MavlinkMissionTransferNackTests - : public MavlinkMissionTransferTest, +class MavlinkMissionTransferClientNackTests + : public MavlinkMissionTransferClientTest, public ::testing::WithParamInterface> {}; -TEST_P(MavlinkMissionTransferNackTests, UploadMissionNackAreHandled) +TEST_P(MavlinkMissionTransferClientNackTests, UploadMissionNackAreHandled) { uint8_t mavlink_nack = std::get<0>(GetParam()); Result mavsdk_nack = std::get<1>(GetParam()); @@ -670,8 +687,8 @@ TEST_P(MavlinkMissionTransferNackTests, UploadMissionNackAreHandled) } INSTANTIATE_TEST_SUITE_P( - MavlinkMissionTransferTests, - MavlinkMissionTransferNackTests, + MavlinkMissionTransferClientTests, + MavlinkMissionTransferClientNackTests, ::testing::Values( std::make_pair(MAV_MISSION_ERROR, Result::ProtocolError), std::make_pair(MAV_MISSION_UNSUPPORTED_FRAME, Result::UnsupportedFrame), @@ -689,7 +706,7 @@ INSTANTIATE_TEST_SUITE_P( std::make_pair(MAV_MISSION_DENIED, Result::Denied), std::make_pair(MAV_MISSION_OPERATION_CANCELLED, Result::Cancelled))); -TEST_F(MavlinkMissionTransferTest, UploadMissionTimeoutNotTriggeredDuringTransfer) +TEST_F(MavlinkMissionTransferClientTest, UploadMissionTimeoutNotTriggeredDuringTransfer) { std::vector items; items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 0)); @@ -756,7 +773,7 @@ TEST_F(MavlinkMissionTransferTest, UploadMissionTimeoutNotTriggeredDuringTransfe EXPECT_TRUE(mmt.is_idle()); } -TEST_F(MavlinkMissionTransferTest, UploadMissionTimeoutAfterSendMissionItem) +TEST_F(MavlinkMissionTransferClientTest, UploadMissionTimeoutAfterSendMissionItem) { std::vector items; items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 0)); @@ -791,7 +808,7 @@ TEST_F(MavlinkMissionTransferTest, UploadMissionTimeoutAfterSendMissionItem) EXPECT_EQ(fut.wait_for(std::chrono::milliseconds(50)), std::future_status::timeout); // But multiple do. - for (unsigned i = 0; i < (MavlinkMissionTransfer::retries - 1); ++i) { + for (unsigned i = 0; i < (MavlinkMissionTransferClient::retries - 1); ++i) { time.sleep_for(std::chrono::milliseconds(static_cast(timeout_s * 1000.))); timeout_handler.run_once(); } @@ -806,7 +823,7 @@ TEST_F(MavlinkMissionTransferTest, UploadMissionTimeoutAfterSendMissionItem) EXPECT_TRUE(mmt.is_idle()); } -TEST_F(MavlinkMissionTransferTest, UploadMissionDoesNotCrashOnRandomMessages) +TEST_F(MavlinkMissionTransferClientTest, UploadMissionDoesNotCrashOnRandomMessages) { message_handler.process_message(make_mission_request_int(MAV_MISSION_TYPE_MISSION, 0)); @@ -835,22 +852,7 @@ bool is_correct_mission_ack(uint8_t type, uint8_t result, const mavlink_message_ ack.mission_type == type); } -bool is_correct_autopilot_mission_ack( - uint8_t type, uint8_t result, uint8_t target_component, const mavlink_message_t& message) -{ - if (message.msgid != MAVLINK_MSG_ID_MISSION_ACK) { - return false; - } - - mavlink_mission_ack_t ack; - mavlink_msg_mission_ack_decode(&message, &ack); - return ( - message.sysid == own_address.system_id && message.compid == own_address.component_id && - ack.target_system == target_address.system_id && ack.target_component == target_component && - ack.type == result && ack.mission_type == type); -} - -TEST_F(MavlinkMissionTransferTest, UploadMissionCanBeCancelled) +TEST_F(MavlinkMissionTransferClientTest, UploadMissionCanBeCancelled) { std::vector items; items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 0)); @@ -912,7 +914,7 @@ mavlink_message_t make_mission_request(uint8_t type, int sequence) return message; } -TEST_F(MavlinkMissionTransferTest, UploadMissionNacksNonIntCase) +TEST_F(MavlinkMissionTransferClientTest, UploadMissionNacksNonIntCase) { std::vector items; items.push_back(make_item(MAV_MISSION_TYPE_FENCE, 0)); @@ -963,7 +965,7 @@ TEST_F(MavlinkMissionTransferTest, UploadMissionNacksNonIntCase) EXPECT_TRUE(mmt.is_idle()); } -TEST_F(MavlinkMissionTransferTest, UploadMissionWithProgress) +TEST_F(MavlinkMissionTransferClientTest, UploadMissionWithProgress) { std::vector items; items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 0)); @@ -1017,7 +1019,7 @@ TEST_F(MavlinkMissionTransferTest, UploadMissionWithProgress) EXPECT_TRUE(mmt.is_idle()); } -TEST_F(MavlinkMissionTransferTest, DownloadMissionSendsRequestList) +TEST_F(MavlinkMissionTransferClientTest, DownloadMissionSendsRequestList) { ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); @@ -1065,7 +1067,7 @@ bool is_correct_mission_request_list(uint8_t type, const mavlink_message_t& mess mission_request_list.mission_type == type); } -TEST_F(MavlinkMissionTransferTest, DownloadMissionResendsRequestList) +TEST_F(MavlinkMissionTransferClientTest, DownloadMissionResendsRequestList) { ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); @@ -1092,7 +1094,8 @@ TEST_F(MavlinkMissionTransferTest, DownloadMissionResendsRequestList) timeout_handler.run_once(); } -TEST_F(MavlinkMissionTransferTest, DownloadMissionResendsRequestListButGivesUpAfterSomeRetries) +TEST_F( + MavlinkMissionTransferClientTest, DownloadMissionResendsRequestListButGivesUpAfterSomeRetries) { ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); @@ -1103,7 +1106,7 @@ TEST_F(MavlinkMissionTransferTest, DownloadMissionResendsRequestListButGivesUpAf return is_correct_mission_request_list( MAV_MISSION_TYPE_MISSION, fun(own_address, channel)); }))) - .Times(MavlinkMissionTransfer::retries); + .Times(MavlinkMissionTransferClient::retries); std::promise prom; auto fut = prom.get_future(); @@ -1120,7 +1123,7 @@ TEST_F(MavlinkMissionTransferTest, DownloadMissionResendsRequestListButGivesUpAf EXPECT_EQ(fut.wait_for(std::chrono::seconds(0)), std::future_status::timeout); - for (unsigned i = 0; i < MavlinkMissionTransfer::retries; ++i) { + for (unsigned i = 0; i < MavlinkMissionTransferClient::retries; ++i) { time.sleep_for(std::chrono::milliseconds(static_cast(timeout_s * 1.1 * 1000.))); timeout_handler.run_once(); } @@ -1147,37 +1150,7 @@ bool is_correct_mission_request_int( mission_request_int.seq == sequence && mission_request_int.mission_type == type); } -bool is_correct_autopilot_mission_request_int( - uint8_t type, unsigned sequence, uint8_t target_component, const mavlink_message_t& message) -{ - if (message.msgid != MAVLINK_MSG_ID_MISSION_REQUEST_INT) { - return false; - } - - mavlink_mission_request_int_t mission_request_int; - mavlink_msg_mission_request_int_decode(&message, &mission_request_int); - return ( - message.sysid == own_address.system_id && message.compid == own_address.component_id && - mission_request_int.target_system == target_address.system_id && - mission_request_int.target_component == target_component && - mission_request_int.seq == sequence && mission_request_int.mission_type == type); -} - -mavlink_message_t make_mission_count(unsigned count) -{ - mavlink_message_t message; - mavlink_msg_mission_count_pack( - own_address.system_id, - own_address.component_id, - &message, - target_address.system_id, - target_address.component_id, - count, - MAV_MISSION_TYPE_MISSION); - return message; -} - -TEST_F(MavlinkMissionTransferTest, DownloadMissionSendsMissionRequests) +TEST_F(MavlinkMissionTransferClientTest, DownloadMissionSendsMissionRequests) { ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); @@ -1206,42 +1179,7 @@ TEST_F(MavlinkMissionTransferTest, DownloadMissionSendsMissionRequests) message_handler.process_message(make_mission_count(items.size())); } -TEST_F(MavlinkMissionTransferTest, ReceiveIncomingMissionSendsMissionRequests) -{ - ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); - - std::vector items; - items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 0)); - items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 1)); - - mmt.receive_incoming_items_async( - MAV_MISSION_TYPE_MISSION, - items.size(), - target_address.system_id, - target_address.component_id, - [](Result result, const std::vector& output_items) { - UNUSED(output_items); - UNUSED(result); - EXPECT_TRUE(false); - }); - - EXPECT_CALL( - mock_sender, - queue_message(Truly([](std::function fun) { - return is_correct_autopilot_mission_request_int( - MAV_MISSION_TYPE_MISSION, - 0, - target_address.component_id, - fun(own_address, channel)); - }))); - - mmt.do_work(); - - message_handler.process_message(make_mission_count(items.size())); -} - -TEST_F(MavlinkMissionTransferTest, DownloadMissionResendsMissionRequestsAndTimesOutEventually) +TEST_F(MavlinkMissionTransferClientTest, DownloadMissionResendsMissionRequestsAndTimesOutEventually) { ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); @@ -1264,7 +1202,7 @@ TEST_F(MavlinkMissionTransferTest, DownloadMissionResendsMissionRequestsAndTimes return is_correct_mission_request_int( MAV_MISSION_TYPE_MISSION, 0, fun(own_address, channel)); }))) - .Times(MavlinkMissionTransfer::retries); + .Times(MavlinkMissionTransferClient::retries); std::vector items; items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 0)); @@ -1275,57 +1213,7 @@ TEST_F(MavlinkMissionTransferTest, DownloadMissionResendsMissionRequestsAndTimes EXPECT_EQ(fut.wait_for(std::chrono::seconds(0)), std::future_status::timeout); // After the specified retries we should give up with a timeout. - for (unsigned i = 0; i < MavlinkMissionTransfer::retries; ++i) { - time.sleep_for(std::chrono::milliseconds(static_cast(timeout_s * 1.1 * 1000.))); - timeout_handler.run_once(); - } - - EXPECT_EQ(fut.wait_for(std::chrono::seconds(1)), std::future_status::ready); - - mmt.do_work(); - EXPECT_TRUE(mmt.is_idle()); -} - -TEST_F( - MavlinkMissionTransferTest, ReceiveIncomingMissionResendsMissionRequestsAndTimesOutEventually) -{ - ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); - - std::vector items; - items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 0)); - items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 1)); - - std::promise prom; - auto fut = prom.get_future(); - mmt.receive_incoming_items_async( - MAV_MISSION_TYPE_MISSION, - items.size(), - target_address.system_id, - target_address.component_id, - [&prom](Result result, const std::vector& output_items) { - UNUSED(output_items); - EXPECT_EQ(result, Result::Timeout); - prom.set_value(); - }); - - EXPECT_CALL( - mock_sender, - queue_message(Truly([](std::function fun) { - return is_correct_autopilot_mission_request_int( - MAV_MISSION_TYPE_MISSION, - 0, - target_address.component_id, - fun(own_address, channel)); - }))) - .Times(MavlinkMissionTransfer::retries); - - mmt.do_work(); - - EXPECT_EQ(fut.wait_for(std::chrono::seconds(0)), std::future_status::timeout); - - // After the specified retries we should give up with a timeout. - for (unsigned i = 0; i < MavlinkMissionTransfer::retries; ++i) { + for (unsigned i = 0; i < MavlinkMissionTransferClient::retries; ++i) { time.sleep_for(std::chrono::milliseconds(static_cast(timeout_s * 1.1 * 1000.))); timeout_handler.run_once(); } @@ -1361,7 +1249,7 @@ mavlink_message_t make_mission_item(const std::vector& item_ints, std:: return message; } -TEST_F(MavlinkMissionTransferTest, DownloadMissionSendsAllMissionRequestsAndAck) +TEST_F(MavlinkMissionTransferClientTest, DownloadMissionSendsAllMissionRequestsAndAck) { ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); @@ -1428,87 +1316,7 @@ TEST_F(MavlinkMissionTransferTest, DownloadMissionSendsAllMissionRequestsAndAck) EXPECT_TRUE(mmt.is_idle()); } -TEST_F(MavlinkMissionTransferTest, ReceiveIncomingMissionSendsAllMissionRequestsAndAck) -{ - ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); - - std::vector real_items; - real_items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 0)); - real_items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 1)); - real_items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 2)); - - std::promise prom; - auto fut = prom.get_future(); - mmt.receive_incoming_items_async( - MAV_MISSION_TYPE_MISSION, - real_items.size(), - target_address.system_id, - target_address.component_id, - [&prom, &real_items](Result result, const std::vector& items) { - EXPECT_EQ(result, Result::Success); - EXPECT_EQ(items, real_items); - prom.set_value(); - }); - - EXPECT_CALL( - mock_sender, - queue_message(Truly([](std::function fun) { - return is_correct_autopilot_mission_request_int( - MAV_MISSION_TYPE_MISSION, - 0, - target_address.component_id, - fun(own_address, channel)); - }))); - - mmt.do_work(); - - EXPECT_CALL( - mock_sender, - queue_message(Truly([](std::function fun) { - return is_correct_autopilot_mission_request_int( - MAV_MISSION_TYPE_MISSION, - 1, - target_address.component_id, - fun(own_address, channel)); - }))); - - message_handler.process_message(make_mission_item(real_items, 0)); - - EXPECT_CALL( - mock_sender, - queue_message(Truly([](std::function fun) { - return is_correct_autopilot_mission_request_int( - MAV_MISSION_TYPE_MISSION, - 2, - target_address.component_id, - fun(own_address, channel)); - }))); - - message_handler.process_message(make_mission_item(real_items, 1)); - - EXPECT_CALL( - mock_sender, - queue_message(Truly([](std::function fun) { - return is_correct_autopilot_mission_ack( - MAV_MISSION_TYPE_MISSION, - MAV_MISSION_ACCEPTED, - target_address.component_id, - fun(own_address, channel)); - }))); - - message_handler.process_message(make_mission_item(real_items, 2)); - - EXPECT_EQ(fut.wait_for(std::chrono::seconds(1)), std::future_status::ready); - - mmt.do_work(); - EXPECT_TRUE(mmt.is_idle()); -} - -TEST_F(MavlinkMissionTransferTest, DownloadMissionResendsRequestItemAgainForSecondItem) +TEST_F(MavlinkMissionTransferClientTest, DownloadMissionResendsRequestItemAgainForSecondItem) { ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); @@ -1537,11 +1345,11 @@ TEST_F(MavlinkMissionTransferTest, DownloadMissionResendsRequestItemAgainForSeco return is_correct_mission_request_int( MAV_MISSION_TYPE_MISSION, 0, fun(own_address, channel)); }))) - .Times(MavlinkMissionTransfer::retries - 1); + .Times(MavlinkMissionTransferClient::retries - 1); message_handler.process_message(make_mission_count(items.size())); - for (unsigned i = 0; i < MavlinkMissionTransfer::retries - 2; ++i) { + for (unsigned i = 0; i < MavlinkMissionTransferClient::retries - 2; ++i) { time.sleep_for(std::chrono::milliseconds(static_cast(timeout_s * 1.1 * 1000.))); timeout_handler.run_once(); } @@ -1554,11 +1362,11 @@ TEST_F(MavlinkMissionTransferTest, DownloadMissionResendsRequestItemAgainForSeco return is_correct_mission_request_int( MAV_MISSION_TYPE_MISSION, 1, fun(own_address, channel)); }))) - .Times(MavlinkMissionTransfer::retries); + .Times(MavlinkMissionTransferClient::retries); message_handler.process_message(make_mission_item(items, 0)); - for (unsigned i = 0; i < MavlinkMissionTransfer::retries; ++i) { + for (unsigned i = 0; i < MavlinkMissionTransferClient::retries; ++i) { time.sleep_for(std::chrono::milliseconds(static_cast(timeout_s * 1.1 * 1000.))); timeout_handler.run_once(); } @@ -1569,7 +1377,7 @@ TEST_F(MavlinkMissionTransferTest, DownloadMissionResendsRequestItemAgainForSeco EXPECT_TRUE(mmt.is_idle()); } -TEST_F(MavlinkMissionTransferTest, DownloadMissionDoesntHaveDuplicates) +TEST_F(MavlinkMissionTransferClientTest, DownloadMissionDoesntHaveDuplicates) { ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); @@ -1640,75 +1448,7 @@ TEST_F(MavlinkMissionTransferTest, DownloadMissionDoesntHaveDuplicates) EXPECT_TRUE(mmt.is_idle()); } -TEST_F(MavlinkMissionTransferTest, ReceiveIncomingMissionResendsRequestItemAgainForSecondItem) -{ - ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); - - std::vector items; - items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 0)); - items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 1)); - items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 2)); - - std::promise prom; - auto fut = prom.get_future(); - mmt.receive_incoming_items_async( - MAV_MISSION_TYPE_MISSION, - items.size(), - target_address.system_id, - target_address.component_id, - [&prom](Result result, const std::vector& output_items) { - UNUSED(output_items); - EXPECT_EQ(result, Result::Timeout); - prom.set_value(); - }); - - // We almost exhaust the retries of the first one. - EXPECT_CALL( - mock_sender, - queue_message(Truly([](std::function fun) { - return is_correct_autopilot_mission_request_int( - MAV_MISSION_TYPE_MISSION, - 0, - target_address.component_id, - fun(own_address, channel)); - }))) - .Times(MavlinkMissionTransfer::retries - 1); - - mmt.do_work(); - - for (unsigned i = 0; i < MavlinkMissionTransfer::retries - 2; ++i) { - time.sleep_for(std::chrono::milliseconds(static_cast(timeout_s * 1.1 * 1000.))); - timeout_handler.run_once(); - } - - // This time we go over the retry limit. - EXPECT_CALL( - mock_sender, - queue_message(Truly([](std::function fun) { - return is_correct_autopilot_mission_request_int( - MAV_MISSION_TYPE_MISSION, - 1, - target_address.component_id, - fun(own_address, channel)); - }))) - .Times(MavlinkMissionTransfer::retries); - - message_handler.process_message(make_mission_item(items, 0)); - - for (unsigned i = 0; i < MavlinkMissionTransfer::retries; ++i) { - time.sleep_for(std::chrono::milliseconds(static_cast(timeout_s * 1.1 * 1000.))); - timeout_handler.run_once(); - } - - EXPECT_EQ(fut.wait_for(std::chrono::seconds(1)), std::future_status::ready); - - mmt.do_work(); - EXPECT_TRUE(mmt.is_idle()); -} - -TEST_F(MavlinkMissionTransferTest, DownloadMissionEmptyList) +TEST_F(MavlinkMissionTransferClientTest, DownloadMissionEmptyList) { ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); @@ -1744,47 +1484,7 @@ TEST_F(MavlinkMissionTransferTest, DownloadMissionEmptyList) EXPECT_TRUE(mmt.is_idle()); } -TEST_F(MavlinkMissionTransferTest, ReceiveIncomingMissionEmptyList) -{ - ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); - - std::promise prom; - auto fut = prom.get_future(); - mmt.receive_incoming_items_async( - MAV_MISSION_TYPE_MISSION, - 0, - target_address.system_id, - target_address.component_id, - [&prom](Result result, const std::vector& items) { - UNUSED(items); - EXPECT_EQ(result, Result::Success); - prom.set_value(); - }); - - EXPECT_CALL( - mock_sender, - queue_message(Truly([](std::function fun) { - return is_correct_autopilot_mission_ack( - MAV_MISSION_TYPE_MISSION, - MAV_MISSION_ACCEPTED, - target_address.component_id, - fun(own_address, channel)); - }))); - - mmt.do_work(); - - EXPECT_EQ(fut.wait_for(std::chrono::seconds(1)), std::future_status::ready); - - // We want to be sure a timeout is not still triggered later. - time.sleep_for(std::chrono::milliseconds(static_cast(timeout_s * 1.1 * 1000.))); - timeout_handler.run_once(); - - mmt.do_work(); - EXPECT_TRUE(mmt.is_idle()); -} - -TEST_F(MavlinkMissionTransferTest, DownloadMissionTimeoutNotTriggeredDuringTransfer) +TEST_F(MavlinkMissionTransferClientTest, DownloadMissionTimeoutNotTriggeredDuringTransfer) { ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); @@ -1807,25 +1507,25 @@ TEST_F(MavlinkMissionTransferTest, DownloadMissionTimeoutNotTriggeredDuringTrans // We almost use up the max timeout in each cycle. time.sleep_for(std::chrono::milliseconds( - static_cast(timeout_s * MavlinkMissionTransfer::retries * 0.8 * 1000.))); + static_cast(timeout_s * MavlinkMissionTransferClient::retries * 0.8 * 1000.))); timeout_handler.run_once(); message_handler.process_message(make_mission_count(real_items.size())); time.sleep_for(std::chrono::milliseconds( - static_cast(timeout_s * MavlinkMissionTransfer::retries * 0.8 * 1000.))); + static_cast(timeout_s * MavlinkMissionTransferClient::retries * 0.8 * 1000.))); timeout_handler.run_once(); message_handler.process_message(make_mission_item(real_items, 0)); time.sleep_for(std::chrono::milliseconds( - static_cast(timeout_s * MavlinkMissionTransfer::retries * 0.8 * 1000.))); + static_cast(timeout_s * MavlinkMissionTransferClient::retries * 0.8 * 1000.))); timeout_handler.run_once(); message_handler.process_message(make_mission_item(real_items, 1)); time.sleep_for(std::chrono::milliseconds( - static_cast(timeout_s * MavlinkMissionTransfer::retries * 0.8 * 1000.))); + static_cast(timeout_s * MavlinkMissionTransferClient::retries * 0.8 * 1000.))); timeout_handler.run_once(); message_handler.process_message(make_mission_item(real_items, 2)); @@ -1836,7 +1536,7 @@ TEST_F(MavlinkMissionTransferTest, DownloadMissionTimeoutNotTriggeredDuringTrans EXPECT_TRUE(mmt.is_idle()); } -TEST_F(MavlinkMissionTransferTest, DownloadMissionCanBeCancelled) +TEST_F(MavlinkMissionTransferClientTest, DownloadMissionCanBeCancelled) { ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); @@ -1882,55 +1582,7 @@ TEST_F(MavlinkMissionTransferTest, DownloadMissionCanBeCancelled) EXPECT_TRUE(mmt.is_idle()); } -TEST_F(MavlinkMissionTransferTest, ReceiveIncomingMissionCanBeCancelled) -{ - ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); - - std::vector items; - items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 0)); - items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 1)); - items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 2)); - - std::promise prom; - auto fut = prom.get_future(); - auto transfer = mmt.receive_incoming_items_async( - MAV_MISSION_TYPE_MISSION, - items.size(), - target_address.system_id, - target_address.component_id, - [&prom](Result result, const std::vector& output_items) { - UNUSED(output_items); - EXPECT_EQ(result, Result::Cancelled); - prom.set_value(); - }); - mmt.do_work(); - - message_handler.process_message(make_mission_item(items, 0)); - - EXPECT_CALL( - mock_sender, - queue_message(Truly([](std::function fun) { - return is_correct_autopilot_mission_ack( - MAV_MISSION_TYPE_MISSION, - MAV_MISSION_OPERATION_CANCELLED, - target_address.component_id, - fun(own_address, channel)); - }))); - - auto ptr = transfer.lock(); - EXPECT_TRUE(ptr); - if (ptr) { - ptr->cancel(); - } - - EXPECT_EQ(fut.wait_for(std::chrono::seconds(1)), std::future_status::ready); - - mmt.do_work(); - EXPECT_TRUE(mmt.is_idle()); -} - -TEST_F(MavlinkMissionTransferTest, DownloadMissionWithProgress) +TEST_F(MavlinkMissionTransferClientTest, DownloadMissionWithProgress) { ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); @@ -1994,7 +1646,7 @@ bool is_correct_mission_clear_all(uint8_t type, const mavlink_message_t& message clear_all.mission_type == type); } -TEST_F(MavlinkMissionTransferTest, ClearMissionSendsClear) +TEST_F(MavlinkMissionTransferClientTest, ClearMissionSendsClear) { ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); @@ -2047,7 +1699,7 @@ mavlink_message_t make_mission_current(uint16_t seq) return message; } -TEST_F(MavlinkMissionTransferTest, SetCurrentSendsSetCurrent) +TEST_F(MavlinkMissionTransferClientTest, SetCurrentSendsSetCurrent) { ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); @@ -2074,7 +1726,7 @@ TEST_F(MavlinkMissionTransferTest, SetCurrentSendsSetCurrent) EXPECT_TRUE(mmt.is_idle()); } -TEST_F(MavlinkMissionTransferTest, SetCurrentWithRetransmissionAndTimeout) +TEST_F(MavlinkMissionTransferClientTest, SetCurrentWithRetransmissionAndTimeout) { ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); @@ -2083,7 +1735,7 @@ TEST_F(MavlinkMissionTransferTest, SetCurrentWithRetransmissionAndTimeout) queue_message(Truly( [](std::function fun) { return is_correct_mission_set_current(2, fun(own_address, channel)); }))) - .Times(MavlinkMissionTransfer::retries); + .Times(MavlinkMissionTransferClient::retries); std::promise prom; auto fut = prom.get_future(); @@ -2094,7 +1746,7 @@ TEST_F(MavlinkMissionTransferTest, SetCurrentWithRetransmissionAndTimeout) }); mmt.do_work(); - for (unsigned i = 0; i < MavlinkMissionTransfer::retries; ++i) { + for (unsigned i = 0; i < MavlinkMissionTransferClient::retries; ++i) { time.sleep_for(std::chrono::milliseconds(static_cast(timeout_s * 1.1 * 1000.))); timeout_handler.run_once(); } @@ -2105,7 +1757,7 @@ TEST_F(MavlinkMissionTransferTest, SetCurrentWithRetransmissionAndTimeout) EXPECT_TRUE(mmt.is_idle()); } -TEST_F(MavlinkMissionTransferTest, SetCurrentWithRetransmissionAndSuccess) +TEST_F(MavlinkMissionTransferClientTest, SetCurrentWithRetransmissionAndSuccess) { ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); @@ -2114,7 +1766,7 @@ TEST_F(MavlinkMissionTransferTest, SetCurrentWithRetransmissionAndSuccess) queue_message(Truly( [](std::function fun) { return is_correct_mission_set_current(2, fun(own_address, channel)); }))) - .Times(MavlinkMissionTransfer::retries - 1); + .Times(MavlinkMissionTransferClient::retries - 1); std::promise prom; auto fut = prom.get_future(); @@ -2125,7 +1777,7 @@ TEST_F(MavlinkMissionTransferTest, SetCurrentWithRetransmissionAndSuccess) }); mmt.do_work(); - for (unsigned i = 0; i < MavlinkMissionTransfer::retries - 2; ++i) { + for (unsigned i = 0; i < MavlinkMissionTransferClient::retries - 2; ++i) { time.sleep_for(std::chrono::milliseconds(static_cast(timeout_s * 1.1 * 1000.))); timeout_handler.run_once(); } @@ -2139,7 +1791,7 @@ TEST_F(MavlinkMissionTransferTest, SetCurrentWithRetransmissionAndSuccess) EXPECT_TRUE(mmt.is_idle()); } -TEST_F(MavlinkMissionTransferTest, SetCurrentWithInvalidInput) +TEST_F(MavlinkMissionTransferClientTest, SetCurrentWithInvalidInput) { ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); @@ -2158,7 +1810,7 @@ TEST_F(MavlinkMissionTransferTest, SetCurrentWithInvalidInput) EXPECT_TRUE(mmt.is_idle()); } -TEST_F(MavlinkMissionTransferTest, SetCurrentWithRetransmissionWhenWrong) +TEST_F(MavlinkMissionTransferClientTest, SetCurrentWithRetransmissionWhenWrong) { ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); @@ -2193,7 +1845,7 @@ TEST_F(MavlinkMissionTransferTest, SetCurrentWithRetransmissionWhenWrong) EXPECT_TRUE(mmt.is_idle()); } -TEST_F(MavlinkMissionTransferTest, IntMessagesNotSupported) +TEST_F(MavlinkMissionTransferClientTest, IntMessagesNotSupported) { mmt.set_int_messages_supported(false); diff --git a/src/mavsdk/core/mavlink_mission_transfer_server.cpp b/src/mavsdk/core/mavlink_mission_transfer_server.cpp new file mode 100644 index 0000000000..d3830b887e --- /dev/null +++ b/src/mavsdk/core/mavlink_mission_transfer_server.cpp @@ -0,0 +1,302 @@ +#include +#include "mavlink_mission_transfer_server.h" +#include "log.h" +#include "unused.h" + +namespace mavsdk { + +MavlinkMissionTransferServer::MavlinkMissionTransferServer( + Sender& sender, + MavlinkMessageHandler& message_handler, + TimeoutHandler& timeout_handler, + TimeoutSCallback timeout_s_callback) : + _sender(sender), + _message_handler(message_handler), + _timeout_handler(timeout_handler), + _timeout_s_callback(std::move(timeout_s_callback)) +{ + if (const char* env_p = std::getenv("MAVSDK_MISSION_TRANSFER_DEBUGGING")) { + if (std::string(env_p) == "1") { + LogDebug() << "Mission transfer debugging is on."; + _debugging = true; + } + } +} + +std::weak_ptr +MavlinkMissionTransferServer::receive_incoming_items_async( + uint8_t type, + uint32_t mission_count, + uint8_t target_system, + uint8_t target_component, + ResultAndItemsCallback callback) +{ + if (!_int_messages_supported) { + if (callback) { + LogErr() << "Int messages are not supported."; + callback(Result::IntMessagesNotSupported, {}); + } + return {}; + } + + auto ptr = std::make_shared( + _sender, + _message_handler, + _timeout_handler, + type, + _timeout_s_callback(), + callback, + mission_count, + target_system, + target_component, + _debugging); + + _work_queue.push_back(ptr); + + return std::weak_ptr(ptr); +} + +void MavlinkMissionTransferServer::do_work() +{ + LockedQueue::Guard work_queue_guard(_work_queue); + auto work = work_queue_guard.get_front(); + + if (!work) { + return; + } + + if (!work->has_started()) { + work->start(); + } + if (work->is_done()) { + work_queue_guard.pop_front(); + } +} + +bool MavlinkMissionTransferServer::is_idle() +{ + LockedQueue::Guard work_queue_guard(_work_queue); + return (work_queue_guard.get_front() == nullptr); +} + +MavlinkMissionTransferServer::WorkItem::WorkItem( + Sender& sender, + MavlinkMessageHandler& message_handler, + TimeoutHandler& timeout_handler, + uint8_t type, + double timeout_s, + bool debugging) : + _sender(sender), + _message_handler(message_handler), + _timeout_handler(timeout_handler), + _type(type), + _timeout_s(timeout_s), + _debugging(debugging) +{} + +MavlinkMissionTransferServer::WorkItem::~WorkItem() {} + +bool MavlinkMissionTransferServer::WorkItem::has_started() +{ + std::lock_guard lock(_mutex); + return _started; +} + +bool MavlinkMissionTransferServer::WorkItem::is_done() +{ + std::lock_guard lock(_mutex); + return _done; +} + +MavlinkMissionTransferServer::ReceiveIncomingMission::ReceiveIncomingMission( + Sender& sender, + MavlinkMessageHandler& message_handler, + TimeoutHandler& timeout_handler, + uint8_t type, + double timeout_s, + ResultAndItemsCallback callback, + uint32_t mission_count, + uint8_t target_system_id, + uint8_t target_component_id, + bool debugging) : + WorkItem(sender, message_handler, timeout_handler, type, timeout_s, debugging), + _callback(callback), + _mission_count(mission_count), + _target_system_id(target_system_id), + _target_component_id(target_component_id) +{} + +MavlinkMissionTransferServer::ReceiveIncomingMission::~ReceiveIncomingMission() +{ + _message_handler.unregister_all(this); + _timeout_handler.remove(_cookie); +} + +void MavlinkMissionTransferServer::ReceiveIncomingMission::start() +{ + _message_handler.register_one( + MAVLINK_MSG_ID_MISSION_ITEM_INT, + [this](const mavlink_message_t& message) { process_mission_item_int(message); }, + this); + + std::lock_guard lock(_mutex); + + _items.clear(); + + _started = true; + _retries_done = 0; + _timeout_handler.add([this]() { process_timeout(); }, _timeout_s, &_cookie); + process_mission_count(); +} + +void MavlinkMissionTransferServer::ReceiveIncomingMission::cancel() +{ + std::lock_guard lock(_mutex); + + _timeout_handler.remove(_cookie); + send_cancel_and_finish(); +} + +void MavlinkMissionTransferServer::ReceiveIncomingMission::request_item() +{ + if (!_sender.queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) { + mavlink_message_t message; + mavlink_msg_mission_request_int_pack_chan( + mavlink_address.system_id, + mavlink_address.component_id, + channel, + &message, + _target_system_id, + _target_component_id, + _next_sequence, + _type); + return message; + })) { + _timeout_handler.remove(_cookie); + callback_and_reset(Result::ConnectionError); + return; + } + + ++_retries_done; +} + +void MavlinkMissionTransferServer::ReceiveIncomingMission::send_ack_and_finish() +{ + if (!_sender.queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) { + mavlink_message_t message; + mavlink_msg_mission_ack_pack_chan( + mavlink_address.system_id, + mavlink_address.component_id, + channel, + &message, + _target_system_id, + _target_component_id, + MAV_MISSION_ACCEPTED, + _type); + return message; + })) { + callback_and_reset(Result::ConnectionError); + return; + } + + // We do not wait on anything coming back after this. + callback_and_reset(Result::Success); +} + +void MavlinkMissionTransferServer::ReceiveIncomingMission::send_cancel_and_finish() +{ + if (!_sender.queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) { + mavlink_message_t message; + mavlink_msg_mission_ack_pack_chan( + mavlink_address.system_id, + mavlink_address.component_id, + channel, + &message, + _target_system_id, + _target_component_id, + MAV_MISSION_OPERATION_CANCELLED, + _type); + return message; + })) { + callback_and_reset(Result::ConnectionError); + return; + } + + // We do not wait on anything coming back after this. + callback_and_reset(Result::Cancelled); +} + +void MavlinkMissionTransferServer::ReceiveIncomingMission::process_mission_count() +{ + if (_mission_count == 0) { + send_ack_and_finish(); + _timeout_handler.remove(_cookie); + return; + } + + _timeout_handler.refresh(_cookie); + _next_sequence = 0; + _step = Step::RequestItem; + _retries_done = 0; + _expected_count = _mission_count; + request_item(); +} + +void MavlinkMissionTransferServer::ReceiveIncomingMission::process_mission_item_int( + const mavlink_message_t& message) +{ + std::lock_guard lock(_mutex); + _timeout_handler.refresh(_cookie); + + mavlink_mission_item_int_t item_int; + mavlink_msg_mission_item_int_decode(&message, &item_int); + + _items.push_back(ItemInt{ + item_int.seq, + item_int.frame, + item_int.command, + item_int.current, + item_int.autocontinue, + item_int.param1, + item_int.param2, + item_int.param3, + item_int.param4, + item_int.x, + item_int.y, + item_int.z, + item_int.mission_type}); + + if (_next_sequence + 1 == _expected_count) { + _timeout_handler.remove(_cookie); + send_ack_and_finish(); + + } else { + _next_sequence = item_int.seq + 1; + _retries_done = 0; + request_item(); + } +} + +void MavlinkMissionTransferServer::ReceiveIncomingMission::process_timeout() +{ + std::lock_guard lock(_mutex); + + if (_retries_done >= retries) { + callback_and_reset(Result::Timeout); + return; + } + + _timeout_handler.add([this]() { process_timeout(); }, _timeout_s, &_cookie); + request_item(); +} + +void MavlinkMissionTransferServer::ReceiveIncomingMission::callback_and_reset(Result result) +{ + if (_callback) { + _callback(result, _items); + } + _callback = nullptr; + _done = true; +} + +} // namespace mavsdk diff --git a/src/mavsdk/core/mavlink_mission_transfer_server.h b/src/mavsdk/core/mavlink_mission_transfer_server.h new file mode 100644 index 0000000000..a999add5f7 --- /dev/null +++ b/src/mavsdk/core/mavlink_mission_transfer_server.h @@ -0,0 +1,186 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include "mavlink_address.h" +#include "mavlink_include.h" +#include "mavlink_message_handler.h" +#include "timeout_handler.h" +#include "timeout_s_callback.h" +#include "locked_queue.h" +#include "sender.h" + +namespace mavsdk { + +class MavlinkMissionTransferServer { +public: + enum class Result { + Success, + ConnectionError, + Denied, + TooManyMissionItems, + Timeout, + Unsupported, + UnsupportedFrame, + NoMissionAvailable, + Cancelled, + MissionTypeNotConsistent, + InvalidSequence, + CurrentInvalid, + ProtocolError, + InvalidParam, + IntMessagesNotSupported + }; + + struct ItemInt { + uint16_t seq; + uint8_t frame; + uint16_t command; + uint8_t current; + uint8_t autocontinue; + float param1; + float param2; + float param3; + float param4; + int32_t x; + int32_t y; + float z; + uint8_t mission_type; + + bool operator==(const ItemInt& other) const + { + return ( + seq == other.seq && frame == other.frame && command == other.command && + current == other.current && autocontinue == other.autocontinue && + param1 == other.param1 && param2 == other.param2 && param3 == other.param3 && + param4 == other.param4 && x == other.x && y == other.y && z == other.z && + mission_type == other.mission_type); + } + }; + + using ResultCallback = std::function; + using ResultAndItemsCallback = std::function items)>; + using ProgressCallback = std::function; + + class WorkItem { + public: + explicit WorkItem( + Sender& sender, + MavlinkMessageHandler& message_handler, + TimeoutHandler& timeout_handler, + uint8_t type, + double timeout_s, + bool debugging); + virtual ~WorkItem(); + virtual void start() = 0; + virtual void cancel() = 0; + bool has_started(); + bool is_done(); + + WorkItem(const WorkItem&) = delete; + WorkItem(WorkItem&&) = delete; + WorkItem& operator=(const WorkItem&) = delete; + WorkItem& operator=(WorkItem&&) = delete; + + protected: + Sender& _sender; + MavlinkMessageHandler& _message_handler; + TimeoutHandler& _timeout_handler; + uint8_t _type; + double _timeout_s; + bool _started{false}; + bool _done{false}; + std::mutex _mutex{}; + bool _debugging; + }; + + class ReceiveIncomingMission : public WorkItem { + public: + explicit ReceiveIncomingMission( + Sender& sender, + MavlinkMessageHandler& message_handler, + TimeoutHandler& timeout_handler, + uint8_t type, + double timeout_s, + ResultAndItemsCallback callback, + uint32_t mission_count, + uint8_t target_system_id, + uint8_t target_component_id, + bool debugging); + ~ReceiveIncomingMission() override; + + void start() override; + void cancel() override; + + ReceiveIncomingMission(const ReceiveIncomingMission&) = delete; + ReceiveIncomingMission(ReceiveIncomingMission&&) = delete; + ReceiveIncomingMission& operator=(const ReceiveIncomingMission&) = delete; + ReceiveIncomingMission& operator=(ReceiveIncomingMission&&) = delete; + + private: + void request_item(); + void send_ack_and_finish(); + void send_cancel_and_finish(); + void process_mission_count(); + void process_mission_item_int(const mavlink_message_t& message); + void process_timeout(); + void callback_and_reset(Result result); + + enum class Step { + RequestList, + RequestItem, + } _step{Step::RequestList}; + + std::vector _items{}; + ResultAndItemsCallback _callback{nullptr}; + void* _cookie{nullptr}; + std::size_t _next_sequence{0}; + std::size_t _expected_count{0}; + unsigned _retries_done{0}; + uint32_t _mission_count{0}; + uint8_t _target_system_id{0}; + uint8_t _target_component_id{0}; + }; + static constexpr unsigned retries = 5; + + explicit MavlinkMissionTransferServer( + Sender& sender, + MavlinkMessageHandler& message_handler, + TimeoutHandler& timeout_handler, + TimeoutSCallback get_timeout_s_callback); + + ~MavlinkMissionTransferServer() = default; + + std::weak_ptr receive_incoming_items_async( + uint8_t type, + uint32_t mission_count, + uint8_t target_system, + uint8_t target_component, + ResultAndItemsCallback callback); + + void do_work(); + bool is_idle(); + + void set_int_messages_supported(bool supported); + + // Non-copyable + MavlinkMissionTransferServer(const MavlinkMissionTransferServer&) = delete; + const MavlinkMissionTransferServer& operator=(const MavlinkMissionTransferServer&) = delete; + +private: + Sender& _sender; + MavlinkMessageHandler& _message_handler; + TimeoutHandler& _timeout_handler; + TimeoutSCallback _timeout_s_callback; + + LockedQueue _work_queue{}; + + bool _int_messages_supported{true}; + bool _debugging{false}; +}; + +} // namespace mavsdk diff --git a/src/mavsdk/core/mavlink_mission_transfer_server_test.cpp b/src/mavsdk/core/mavlink_mission_transfer_server_test.cpp new file mode 100644 index 0000000000..80525a0eaa --- /dev/null +++ b/src/mavsdk/core/mavlink_mission_transfer_server_test.cpp @@ -0,0 +1,414 @@ +#include +#include +#include +#include + +#include "mavlink_mission_transfer_server.h" +#include "mocks/sender_mock.h" +#include "unused.h" + +using namespace mavsdk; + +using ::testing::_; +using ::testing::NiceMock; +using ::testing::Return; +using ::testing::Truly; +using MockSender = NiceMock; + +using Result = MavlinkMissionTransferServer::Result; +using ItemInt = MavlinkMissionTransferServer::ItemInt; + +static MavlinkAddress own_address{42, 16}; +static uint8_t channel{0}; +static MavlinkAddress target_address{99, MAV_COMP_ID_AUTOPILOT1}; + +static constexpr double timeout_s = 0.5; + +#define ONCE_ONLY \ + static bool called = false; \ + EXPECT_FALSE(called); \ + called = true + +class MavlinkMissionTransferServerTest : public ::testing::Test { +protected: + MavlinkMissionTransferServerTest() : + ::testing::Test(), + timeout_handler(time), + mmt(mock_sender, message_handler, timeout_handler, []() { return timeout_s; }) + {} + + void SetUp() override + { + ON_CALL(mock_sender, get_own_system_id()).WillByDefault(Return(own_address.system_id)); + ON_CALL(mock_sender, get_own_component_id()) + .WillByDefault(Return(own_address.component_id)); + ON_CALL(mock_sender, autopilot()).WillByDefault(Return(Autopilot::Px4)); + } + + MockSender mock_sender; + MavlinkMessageHandler message_handler; + FakeTime time; + TimeoutHandler timeout_handler; + MavlinkMissionTransferServer mmt; +}; + +static ItemInt make_item(uint8_t type, uint16_t sequence) +{ + ItemInt item; + + item.seq = sequence; + item.frame = MAV_FRAME_MISSION; + item.command = MAV_CMD_NAV_WAYPOINT; + item.current = uint8_t(sequence == 0 ? 1 : 0); + item.autocontinue = 1; + item.param1 = 1.0f; + item.param2 = 2.0f; + item.param3 = 3.0f; + item.param4 = 4.0f; + item.x = 5; + item.y = 6; + item.z = 7.0f; + item.mission_type = type; + + return item; +} + +static mavlink_message_t make_mission_count(unsigned count) +{ + mavlink_message_t message; + mavlink_msg_mission_count_pack( + own_address.system_id, + own_address.component_id, + &message, + target_address.system_id, + target_address.component_id, + count, + MAV_MISSION_TYPE_MISSION); + return message; +} + +bool is_correct_autopilot_mission_request_int( + uint8_t type, unsigned sequence, uint8_t target_component, const mavlink_message_t& message) +{ + if (message.msgid != MAVLINK_MSG_ID_MISSION_REQUEST_INT) { + return false; + } + + mavlink_mission_request_int_t mission_request_int; + mavlink_msg_mission_request_int_decode(&message, &mission_request_int); + return ( + message.sysid == own_address.system_id && message.compid == own_address.component_id && + mission_request_int.target_system == target_address.system_id && + mission_request_int.target_component == target_component && + mission_request_int.seq == sequence && mission_request_int.mission_type == type); +} + +bool is_correct_autopilot_mission_ack( + uint8_t type, uint8_t result, uint8_t target_component, const mavlink_message_t& message) +{ + if (message.msgid != MAVLINK_MSG_ID_MISSION_ACK) { + return false; + } + + mavlink_mission_ack_t ack; + mavlink_msg_mission_ack_decode(&message, &ack); + return ( + message.sysid == own_address.system_id && message.compid == own_address.component_id && + ack.target_system == target_address.system_id && ack.target_component == target_component && + ack.type == result && ack.mission_type == type); +} + +bool is_the_same_mission_item_int(const ItemInt& item, const mavlink_message_t& message) +{ + if (message.msgid != MAVLINK_MSG_ID_MISSION_ITEM_INT) { + return false; + } + mavlink_mission_item_int_t mission_item_int; + mavlink_msg_mission_item_int_decode(&message, &mission_item_int); + + return ( + message.sysid == own_address.system_id && // + message.compid == own_address.component_id && // + mission_item_int.target_system == target_address.system_id && // + mission_item_int.target_component == target_address.component_id && // + mission_item_int.seq == item.seq && // + mission_item_int.frame == item.frame && // + mission_item_int.command == item.command && // + mission_item_int.current == item.current && // + mission_item_int.autocontinue == item.autocontinue && // + mission_item_int.param1 == item.param1 && // + mission_item_int.param2 == item.param2 && // + mission_item_int.param3 == item.param3 && // + mission_item_int.param4 == item.param4 && // + mission_item_int.x == item.x && // + mission_item_int.y == item.y && // + mission_item_int.z == item.z && // + mission_item_int.mission_type == item.mission_type); +} + +TEST_F(MavlinkMissionTransferServerTest, ReceiveIncomingMissionSendsMissionRequests) +{ + ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); + + std::vector items; + items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 0)); + items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 1)); + + mmt.receive_incoming_items_async( + MAV_MISSION_TYPE_MISSION, + items.size(), + target_address.system_id, + target_address.component_id, + [](Result result, const std::vector& output_items) { + UNUSED(output_items); + UNUSED(result); + EXPECT_TRUE(false); + }); + + EXPECT_CALL( + mock_sender, + queue_message(Truly([](std::function fun) { + return is_correct_autopilot_mission_request_int( + MAV_MISSION_TYPE_MISSION, + 0, + target_address.component_id, + fun(own_address, channel)); + }))); + + mmt.do_work(); + + message_handler.process_message(make_mission_count(items.size())); +} + +TEST_F( + MavlinkMissionTransferServerTest, + ReceiveIncomingMissionResendsMissionRequestsAndTimesOutEventually) +{ + ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); + + std::vector items; + items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 0)); + items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 1)); + + std::promise prom; + auto fut = prom.get_future(); + mmt.receive_incoming_items_async( + MAV_MISSION_TYPE_MISSION, + items.size(), + target_address.system_id, + target_address.component_id, + [&prom](Result result, const std::vector& output_items) { + UNUSED(output_items); + EXPECT_EQ(result, Result::Timeout); + prom.set_value(); + }); + + EXPECT_CALL( + mock_sender, + queue_message(Truly([](std::function fun) { + return is_correct_autopilot_mission_request_int( + MAV_MISSION_TYPE_MISSION, + 0, + target_address.component_id, + fun(own_address, channel)); + }))) + .Times(MavlinkMissionTransferServer::retries); + + mmt.do_work(); + + EXPECT_EQ(fut.wait_for(std::chrono::seconds(0)), std::future_status::timeout); + + // After the specified retries we should give up with a timeout. + for (unsigned i = 0; i < MavlinkMissionTransferServer::retries; ++i) { + time.sleep_for(std::chrono::milliseconds(static_cast(timeout_s * 1.1 * 1000.))); + timeout_handler.run_once(); + } + + EXPECT_EQ(fut.wait_for(std::chrono::seconds(1)), std::future_status::ready); + + mmt.do_work(); + EXPECT_TRUE(mmt.is_idle()); +} + +mavlink_message_t make_mission_item(const std::vector& item_ints, std::size_t index) +{ + mavlink_message_t message; + mavlink_msg_mission_item_int_pack( + own_address.system_id, + own_address.component_id, + &message, + target_address.system_id, + target_address.component_id, + index, + item_ints[index].frame, + item_ints[index].command, + item_ints[index].current, + item_ints[index].autocontinue, + item_ints[index].param1, + item_ints[index].param2, + item_ints[index].param3, + item_ints[index].param4, + item_ints[index].x, + item_ints[index].y, + item_ints[index].z, + item_ints[index].mission_type); + return message; +} + +TEST_F(MavlinkMissionTransferServerTest, ReceiveIncomingMissionResendsRequestItemAgainForSecondItem) +{ + ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); + + std::vector items; + items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 0)); + items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 1)); + items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 2)); + + std::promise prom; + auto fut = prom.get_future(); + mmt.receive_incoming_items_async( + MAV_MISSION_TYPE_MISSION, + items.size(), + target_address.system_id, + target_address.component_id, + [&prom](Result result, const std::vector& output_items) { + UNUSED(output_items); + EXPECT_EQ(result, Result::Timeout); + prom.set_value(); + }); + + // We almost exhaust the retries of the first one. + EXPECT_CALL( + mock_sender, + queue_message(Truly([](std::function fun) { + return is_correct_autopilot_mission_request_int( + MAV_MISSION_TYPE_MISSION, + 0, + target_address.component_id, + fun(own_address, channel)); + }))) + .Times(MavlinkMissionTransferServer::retries - 1); + + mmt.do_work(); + + for (unsigned i = 0; i < MavlinkMissionTransferServer::retries - 2; ++i) { + time.sleep_for(std::chrono::milliseconds(static_cast(timeout_s * 1.1 * 1000.))); + timeout_handler.run_once(); + } + + // This time we go over the retry limit. + EXPECT_CALL( + mock_sender, + queue_message(Truly([](std::function fun) { + return is_correct_autopilot_mission_request_int( + MAV_MISSION_TYPE_MISSION, + 1, + target_address.component_id, + fun(own_address, channel)); + }))) + .Times(MavlinkMissionTransferServer::retries); + + message_handler.process_message(make_mission_item(items, 0)); + + for (unsigned i = 0; i < MavlinkMissionTransferServer::retries; ++i) { + time.sleep_for(std::chrono::milliseconds(static_cast(timeout_s * 1.1 * 1000.))); + timeout_handler.run_once(); + } + + EXPECT_EQ(fut.wait_for(std::chrono::seconds(1)), std::future_status::ready); + + mmt.do_work(); + EXPECT_TRUE(mmt.is_idle()); +} + +TEST_F(MavlinkMissionTransferServerTest, ReceiveIncomingMissionEmptyList) +{ + ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); + + std::promise prom; + auto fut = prom.get_future(); + mmt.receive_incoming_items_async( + MAV_MISSION_TYPE_MISSION, + 0, + target_address.system_id, + target_address.component_id, + [&prom](Result result, const std::vector& items) { + UNUSED(items); + EXPECT_EQ(result, Result::Success); + prom.set_value(); + }); + + EXPECT_CALL( + mock_sender, + queue_message(Truly([](std::function fun) { + return is_correct_autopilot_mission_ack( + MAV_MISSION_TYPE_MISSION, + MAV_MISSION_ACCEPTED, + target_address.component_id, + fun(own_address, channel)); + }))); + + mmt.do_work(); + + EXPECT_EQ(fut.wait_for(std::chrono::seconds(1)), std::future_status::ready); + + // We want to be sure a timeout is not still triggered later. + time.sleep_for(std::chrono::milliseconds(static_cast(timeout_s * 1.1 * 1000.))); + timeout_handler.run_once(); + + mmt.do_work(); + EXPECT_TRUE(mmt.is_idle()); +} + +TEST_F(MavlinkMissionTransferServerTest, ReceiveIncomingMissionCanBeCancelled) +{ + ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); + + std::vector items; + items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 0)); + items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 1)); + items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 2)); + + std::promise prom; + auto fut = prom.get_future(); + auto transfer = mmt.receive_incoming_items_async( + MAV_MISSION_TYPE_MISSION, + items.size(), + target_address.system_id, + target_address.component_id, + [&prom](Result result, const std::vector& output_items) { + UNUSED(output_items); + EXPECT_EQ(result, Result::Cancelled); + prom.set_value(); + }); + mmt.do_work(); + + message_handler.process_message(make_mission_item(items, 0)); + + EXPECT_CALL( + mock_sender, + queue_message(Truly([](std::function fun) { + return is_correct_autopilot_mission_ack( + MAV_MISSION_TYPE_MISSION, + MAV_MISSION_OPERATION_CANCELLED, + target_address.component_id, + fun(own_address, channel)); + }))); + + auto ptr = transfer.lock(); + EXPECT_TRUE(ptr); + if (ptr) { + ptr->cancel(); + } + + EXPECT_EQ(fut.wait_for(std::chrono::seconds(1)), std::future_status::ready); + + mmt.do_work(); + EXPECT_TRUE(mmt.is_idle()); +} diff --git a/src/mavsdk/core/mocks/sender_mock.h b/src/mavsdk/core/mocks/sender_mock.h index 0cd1560577..027d731f71 100644 --- a/src/mavsdk/core/mocks/sender_mock.h +++ b/src/mavsdk/core/mocks/sender_mock.h @@ -1,7 +1,6 @@ #include #include #include -#include "mavlink_mission_transfer.h" #include "mavlink_address.h" namespace mavsdk { diff --git a/src/mavsdk/core/server_component_impl.cpp b/src/mavsdk/core/server_component_impl.cpp index 47addb51b7..eeb444ed76 100644 --- a/src/mavsdk/core/server_component_impl.cpp +++ b/src/mavsdk/core/server_component_impl.cpp @@ -9,7 +9,7 @@ ServerComponentImpl::ServerComponentImpl(MavsdkImpl& mavsdk_impl, uint8_t compon _own_component_id(component_id), _our_sender(*this), _mavlink_command_receiver(*this), - _mission_transfer( + _mission_transfer_server( _our_sender, mavsdk_impl.mavlink_message_handler, mavsdk_impl.timeout_handler, @@ -121,7 +121,7 @@ void ServerComponentImpl::unregister_all_mavlink_message_handlers(const void* co void ServerComponentImpl::do_work() { _mavlink_parameter_server.do_work(); - _mission_transfer.do_work(); + _mission_transfer_server.do_work(); } Sender& ServerComponentImpl::sender() diff --git a/src/mavsdk/core/server_component_impl.h b/src/mavsdk/core/server_component_impl.h index f151f7b4f4..a56df7c86f 100644 --- a/src/mavsdk/core/server_component_impl.h +++ b/src/mavsdk/core/server_component_impl.h @@ -4,7 +4,7 @@ #include "mavlink_address.h" #include "mavlink_channels.h" #include "mavlink_command_receiver.h" -#include "mavlink_mission_transfer.h" +#include "mavlink_mission_transfer_server.h" #include "mavlink_parameter_server.h" #include "mavlink_request_message_handler.h" #include "mavlink_ftp_server.h" @@ -142,7 +142,7 @@ class ServerComponentImpl { bool set_uid2(std::string uid2); void send_autopilot_version(); - MavlinkMissionTransfer& mission_transfer() { return _mission_transfer; } + MavlinkMissionTransferServer& mission_transfer_server() { return _mission_transfer_server; } MavlinkParameterServer& mavlink_parameter_server() { return _mavlink_parameter_server; } MavlinkRequestMessageHandler& mavlink_request_message_handler() { @@ -170,7 +170,7 @@ class ServerComponentImpl { OurSender _our_sender; MavlinkCommandReceiver _mavlink_command_receiver; - MavlinkMissionTransfer _mission_transfer; + MavlinkMissionTransferServer _mission_transfer_server; MavlinkParameterServer _mavlink_parameter_server; MavlinkRequestMessageHandler _mavlink_request_message_handler; MavlinkFtpServer _mavlink_ftp_server; diff --git a/src/mavsdk/core/system_impl.cpp b/src/mavsdk/core/system_impl.cpp index 1d7e8cd438..97027ad2c8 100644 --- a/src/mavsdk/core/system_impl.cpp +++ b/src/mavsdk/core/system_impl.cpp @@ -26,7 +26,7 @@ SystemImpl::SystemImpl(MavsdkImpl& mavsdk_impl) : _command_sender(*this), _timesync(*this), _ping(*this), - _mission_transfer( + _mission_transfer_client( _mavsdk_impl.default_server_component_impl().sender(), _mavsdk_impl.mavlink_message_handler, _mavsdk_impl.timeout_handler, @@ -261,7 +261,7 @@ void SystemImpl::process_autopilot_version(const mavlink_message_t& message) mavlink_autopilot_version_t autopilot_version; mavlink_msg_autopilot_version_decode(&message, &autopilot_version); - _mission_transfer.set_int_messages_supported( + _mission_transfer_client.set_int_messages_supported( autopilot_version.capabilities & MAV_PROTOCOL_CAPABILITY_MISSION_INT); } @@ -284,7 +284,7 @@ void SystemImpl::system_thread() } _command_sender.do_work(); _timesync.do_work(); - _mission_transfer.do_work(); + _mission_transfer_client.do_work(); _mavlink_ftp_client.do_work(); if (_mavsdk_impl.time.elapsed_since_s(last_ping_time) >= SystemImpl::_ping_interval_s) { diff --git a/src/mavsdk/core/system_impl.h b/src/mavsdk/core/system_impl.h index 9a2ea7133e..1726fca683 100644 --- a/src/mavsdk/core/system_impl.h +++ b/src/mavsdk/core/system_impl.h @@ -10,7 +10,7 @@ #include "mavlink_command_receiver.h" #include "mavlink_ftp_client.h" #include "mavlink_message_handler.h" -#include "mavlink_mission_transfer.h" +#include "mavlink_mission_transfer_client.h" #include "mavlink_request_message_handler.h" #include "mavlink_statustext_handler.h" #include "request_message.h" @@ -283,7 +283,7 @@ class SystemImpl { const MavlinkCommandSender::CommandResultCallback& callback); void send_flight_information_request(); - MavlinkMissionTransfer& mission_transfer() { return _mission_transfer; }; + MavlinkMissionTransferClient& mission_transfer_client() { return _mission_transfer_client; }; MavlinkFtpClient& mavlink_ftp_client() { return _mavlink_ftp_client; }; @@ -392,7 +392,7 @@ class SystemImpl { Timesync _timesync; Ping _ping; - MavlinkMissionTransfer _mission_transfer; + MavlinkMissionTransferClient _mission_transfer_client; RequestMessage _request_message; MavlinkFtpClient _mavlink_ftp_client; diff --git a/src/mavsdk/plugins/geofence/geofence_impl.cpp b/src/mavsdk/plugins/geofence/geofence_impl.cpp index 63a5681243..5eb6c4f02a 100644 --- a/src/mavsdk/plugins/geofence/geofence_impl.cpp +++ b/src/mavsdk/plugins/geofence/geofence_impl.cpp @@ -41,11 +41,14 @@ void GeofenceImpl::upload_geofence_async( const Geofence::GeofenceData& geofence_data, const Geofence::ResultCallback& callback) { // We can just create these items on the stack because they get copied - // later in the MavlinkMissionTransfer constructor. + // later in the MavlinkMissionTransferClient constructor. const auto items = assemble_items(geofence_data); - _system_impl->mission_transfer().upload_items_async( - MAV_MISSION_TYPE_FENCE, 1, items, [this, callback](MavlinkMissionTransfer::Result result) { + _system_impl->mission_transfer_client().upload_items_async( + MAV_MISSION_TYPE_FENCE, + 1, + items, + [this, callback](MavlinkMissionTransferClient::Result result) { auto converted_result = convert_result(result); _system_impl->get_system_id(), _system_impl->call_user_callback( @@ -64,10 +67,10 @@ Geofence::Result GeofenceImpl::clear_geofence() void GeofenceImpl::clear_geofence_async(const Geofence::ResultCallback& callback) { - _system_impl->mission_transfer().clear_items_async( + _system_impl->mission_transfer_client().clear_items_async( MAV_MISSION_TYPE_FENCE, _system_impl->get_system_id(), - [this, callback](MavlinkMissionTransfer::Result result) { + [this, callback](MavlinkMissionTransferClient::Result result) { auto converted_result = convert_result(result); _system_impl->call_user_callback([callback, converted_result]() { if (callback) { @@ -77,10 +80,10 @@ void GeofenceImpl::clear_geofence_async(const Geofence::ResultCallback& callback }); } -std::vector +std::vector GeofenceImpl::assemble_items(const Geofence::GeofenceData& geofence_data) { - std::vector items; + std::vector items; uint16_t sequence = 0; @@ -95,7 +98,7 @@ GeofenceImpl::assemble_items(const Geofence::GeofenceData& geofence_data) const uint8_t autocontinue = 0; const float param1 = float(polygon.points.size()); - items.push_back(MavlinkMissionTransfer::ItemInt{ + items.push_back(MavlinkMissionTransferClient::ItemInt{ sequence, MAV_FRAME_GLOBAL_INT, command, @@ -121,7 +124,7 @@ GeofenceImpl::assemble_items(const Geofence::GeofenceData& geofence_data) MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION : MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION); - items.push_back(MavlinkMissionTransfer::ItemInt{ + items.push_back(MavlinkMissionTransferClient::ItemInt{ sequence, MAV_FRAME_GLOBAL_INT, command, @@ -141,38 +144,38 @@ GeofenceImpl::assemble_items(const Geofence::GeofenceData& geofence_data) return items; } -Geofence::Result GeofenceImpl::convert_result(MavlinkMissionTransfer::Result result) +Geofence::Result GeofenceImpl::convert_result(MavlinkMissionTransferClient::Result result) { switch (result) { - case MavlinkMissionTransfer::Result::Success: + case MavlinkMissionTransferClient::Result::Success: return Geofence::Result::Success; - case MavlinkMissionTransfer::Result::ConnectionError: + case MavlinkMissionTransferClient::Result::ConnectionError: return Geofence::Result::Error; // FIXME - case MavlinkMissionTransfer::Result::Denied: + case MavlinkMissionTransferClient::Result::Denied: return Geofence::Result::Error; // FIXME - case MavlinkMissionTransfer::Result::TooManyMissionItems: + case MavlinkMissionTransferClient::Result::TooManyMissionItems: return Geofence::Result::TooManyGeofenceItems; - case MavlinkMissionTransfer::Result::Timeout: + case MavlinkMissionTransferClient::Result::Timeout: return Geofence::Result::Timeout; - case MavlinkMissionTransfer::Result::Unsupported: + case MavlinkMissionTransferClient::Result::Unsupported: return Geofence::Result::Error; // FIXME - case MavlinkMissionTransfer::Result::UnsupportedFrame: + case MavlinkMissionTransferClient::Result::UnsupportedFrame: return Geofence::Result::Error; // FIXME - case MavlinkMissionTransfer::Result::NoMissionAvailable: + case MavlinkMissionTransferClient::Result::NoMissionAvailable: return Geofence::Result::InvalidArgument; // FIXME - case MavlinkMissionTransfer::Result::Cancelled: + case MavlinkMissionTransferClient::Result::Cancelled: return Geofence::Result::Error; // FIXME - case MavlinkMissionTransfer::Result::MissionTypeNotConsistent: + case MavlinkMissionTransferClient::Result::MissionTypeNotConsistent: return Geofence::Result::InvalidArgument; // FIXME - case MavlinkMissionTransfer::Result::InvalidSequence: + case MavlinkMissionTransferClient::Result::InvalidSequence: return Geofence::Result::InvalidArgument; // FIXME - case MavlinkMissionTransfer::Result::CurrentInvalid: + case MavlinkMissionTransferClient::Result::CurrentInvalid: return Geofence::Result::InvalidArgument; // FIXME - case MavlinkMissionTransfer::Result::ProtocolError: + case MavlinkMissionTransferClient::Result::ProtocolError: return Geofence::Result::Error; // FIXME - case MavlinkMissionTransfer::Result::InvalidParam: + case MavlinkMissionTransferClient::Result::InvalidParam: return Geofence::Result::InvalidArgument; // FIXME - case MavlinkMissionTransfer::Result::IntMessagesNotSupported: + case MavlinkMissionTransferClient::Result::IntMessagesNotSupported: return Geofence::Result::Error; // FIXME default: return Geofence::Result::Unknown; diff --git a/src/mavsdk/plugins/geofence/geofence_impl.h b/src/mavsdk/plugins/geofence/geofence_impl.h index 1002996d5a..322bc77b1a 100644 --- a/src/mavsdk/plugins/geofence/geofence_impl.h +++ b/src/mavsdk/plugins/geofence/geofence_impl.h @@ -37,10 +37,10 @@ class GeofenceImpl : public PluginImplBase { const GeofenceImpl& operator=(const GeofenceImpl&) = delete; private: - std::vector + std::vector assemble_items(const Geofence::GeofenceData& geofence_data); - static Geofence::Result convert_result(MavlinkMissionTransfer::Result result); + static Geofence::Result convert_result(MavlinkMissionTransferClient::Result result); }; } // namespace mavsdk diff --git a/src/mavsdk/plugins/mission/mission_impl.cpp b/src/mavsdk/plugins/mission/mission_impl.cpp index f5f869b6d3..eca9347c43 100644 --- a/src/mavsdk/plugins/mission/mission_impl.cpp +++ b/src/mavsdk/plugins/mission/mission_impl.cpp @@ -156,11 +156,11 @@ void MissionImpl::upload_mission_async( wait_for_protocol_async([callback, mission_plan, this]() { const auto int_items = convert_to_int_items(mission_plan.mission_items); - _mission_data.last_upload = _system_impl->mission_transfer().upload_items_async( + _mission_data.last_upload = _system_impl->mission_transfer_client().upload_items_async( MAV_MISSION_TYPE_MISSION, _system_impl->get_system_id(), int_items, - [this, callback](MavlinkMissionTransfer::Result result) { + [this, callback](MavlinkMissionTransferClient::Result result) { auto converted_result = convert_result(result); _system_impl->call_user_callback([callback, converted_result]() { if (callback) { @@ -189,11 +189,11 @@ void MissionImpl::upload_mission_with_progress_async( wait_for_protocol_async([callback, mission_plan, this]() { const auto int_items = convert_to_int_items(mission_plan.mission_items); - _mission_data.last_upload = _system_impl->mission_transfer().upload_items_async( + _mission_data.last_upload = _system_impl->mission_transfer_client().upload_items_async( MAV_MISSION_TYPE_MISSION, _system_impl->get_system_id(), int_items, - [this, callback](MavlinkMissionTransfer::Result result) { + [this, callback](MavlinkMissionTransferClient::Result result) { auto converted_result = convert_result(result); _system_impl->call_user_callback([callback, converted_result]() { if (callback) { @@ -247,12 +247,12 @@ void MissionImpl::download_mission_async(const Mission::DownloadMissionCallback& return; } - _mission_data.last_download = _system_impl->mission_transfer().download_items_async( + _mission_data.last_download = _system_impl->mission_transfer_client().download_items_async( MAV_MISSION_TYPE_MISSION, _system_impl->get_system_id(), [this, callback]( - MavlinkMissionTransfer::Result result, - std::vector items) { + MavlinkMissionTransferClient::Result result, + std::vector items) { auto result_and_items = convert_to_result_and_mission_items(result, items); _system_impl->call_user_callback([callback, result_and_items]() { callback(result_and_items.first, result_and_items.second); @@ -275,12 +275,12 @@ void MissionImpl::download_mission_with_progress_async( return; } - _mission_data.last_download = _system_impl->mission_transfer().download_items_async( + _mission_data.last_download = _system_impl->mission_transfer_client().download_items_async( MAV_MISSION_TYPE_MISSION, _system_impl->get_system_id(), [this, callback]( - MavlinkMissionTransfer::Result result, - const std::vector& items) { + MavlinkMissionTransferClient::Result result, + const std::vector& items) { auto result_and_items = convert_to_result_and_mission_items(result, items); _system_impl->call_user_callback([callback, result_and_items]() { if (result_and_items.first == Mission::Result::Success) { @@ -360,10 +360,10 @@ float MissionImpl::acceptance_radius(const MissionItem& item) return acceptance_radius_m; } -std::vector +std::vector MissionImpl::convert_to_int_items(const std::vector& mission_items) { - std::vector int_items; + std::vector int_items; bool last_position_valid = false; // This flag is to protect us from using an invalid x/y. @@ -385,7 +385,7 @@ MissionImpl::convert_to_int_items(const std::vector& mission_items) float z = item.relative_altitude_m; MAV_FRAME frame = MAV_FRAME_GLOBAL_RELATIVE_ALT_INT; - MavlinkMissionTransfer::ItemInt next_item{ + MavlinkMissionTransferClient::ItemInt next_item{ static_cast(int_items.size()), (uint8_t)frame, MAV_CMD_NAV_TAKEOFF, @@ -411,7 +411,7 @@ MissionImpl::convert_to_int_items(const std::vector& mission_items) const int32_t y = int32_t(std::round(item.longitude_deg * 1e7)); const float z = item.relative_altitude_m; - MavlinkMissionTransfer::ItemInt next_item{ + MavlinkMissionTransferClient::ItemInt next_item{ static_cast(int_items.size()), static_cast(MAV_FRAME_GLOBAL_RELATIVE_ALT_INT), static_cast(MAV_CMD_NAV_WAYPOINT), @@ -441,7 +441,7 @@ MissionImpl::convert_to_int_items(const std::vector& mission_items) uint8_t autocontinue = 1; - MavlinkMissionTransfer::ItemInt next_item{ + MavlinkMissionTransferClient::ItemInt next_item{ static_cast(int_items.size()), MAV_FRAME_MISSION, MAV_CMD_DO_CHANGE_SPEED, @@ -497,7 +497,7 @@ MissionImpl::convert_to_int_items(const std::vector& mission_items) uint8_t autocontinue = 1; - MavlinkMissionTransfer::ItemInt next_item{ + MavlinkMissionTransferClient::ItemInt next_item{ static_cast(int_items.size()), MAV_FRAME_MISSION, MAV_CMD_NAV_DELAY, @@ -579,7 +579,7 @@ MissionImpl::convert_to_int_items(const std::vector& mission_items) break; } - MavlinkMissionTransfer::ItemInt next_item{ + MavlinkMissionTransferClient::ItemInt next_item{ static_cast(int_items.size()), MAV_FRAME_MISSION, command, @@ -643,7 +643,7 @@ MissionImpl::convert_to_int_items(const std::vector& mission_items) frame = MAV_FRAME_MISSION; } - MavlinkMissionTransfer::ItemInt next_item{ + MavlinkMissionTransferClient::ItemInt next_item{ static_cast(int_items.size()), (uint8_t)frame, command, @@ -675,7 +675,7 @@ MissionImpl::convert_to_int_items(const std::vector& mission_items) } if (_enable_return_to_launch_after_mission) { - MavlinkMissionTransfer::ItemInt next_item{ + MavlinkMissionTransferClient::ItemInt next_item{ static_cast(int_items.size()), MAV_FRAME_MISSION, MAV_CMD_NAV_RETURN_TO_LAUNCH, @@ -697,8 +697,8 @@ MissionImpl::convert_to_int_items(const std::vector& mission_items) } std::pair MissionImpl::convert_to_result_and_mission_items( - MavlinkMissionTransfer::Result result, - const std::vector& int_items) + MavlinkMissionTransferClient::Result result, + const std::vector& int_items) { std::pair result_pair; @@ -965,10 +965,10 @@ void MissionImpl::clear_mission_async(const Mission::ResultCallback& callback) { reset_mission_progress(); - _system_impl->mission_transfer().clear_items_async( + _system_impl->mission_transfer_client().clear_items_async( MAV_MISSION_TYPE_MISSION, _system_impl->get_system_id(), - [this, callback](MavlinkMissionTransfer::Result result) { + [this, callback](MavlinkMissionTransferClient::Result result) { auto converted_result = convert_result(result); _system_impl->call_user_callback([callback, converted_result]() { if (callback) { @@ -1019,10 +1019,10 @@ void MissionImpl::set_current_mission_item_async( }); } - _system_impl->mission_transfer().set_current_item_async( + _system_impl->mission_transfer_client().set_current_item_async( mavlink_index, _system_impl->get_system_id(), - [this, callback](MavlinkMissionTransfer::Result result) { + [this, callback](MavlinkMissionTransferClient::Result result) { auto converted_result = convert_result(result); _system_impl->call_user_callback([callback, converted_result]() { if (callback) { @@ -1159,38 +1159,38 @@ void MissionImpl::unsubscribe_mission_progress(Mission::MissionProgressHandle ha _mission_data.mission_progress_callbacks.unsubscribe(handle); } -Mission::Result MissionImpl::convert_result(MavlinkMissionTransfer::Result result) +Mission::Result MissionImpl::convert_result(MavlinkMissionTransferClient::Result result) { switch (result) { - case MavlinkMissionTransfer::Result::Success: + case MavlinkMissionTransferClient::Result::Success: return Mission::Result::Success; - case MavlinkMissionTransfer::Result::ConnectionError: + case MavlinkMissionTransferClient::Result::ConnectionError: return Mission::Result::Error; - case MavlinkMissionTransfer::Result::Denied: + case MavlinkMissionTransferClient::Result::Denied: return Mission::Result::Denied; - case MavlinkMissionTransfer::Result::TooManyMissionItems: + case MavlinkMissionTransferClient::Result::TooManyMissionItems: return Mission::Result::TooManyMissionItems; - case MavlinkMissionTransfer::Result::Timeout: + case MavlinkMissionTransferClient::Result::Timeout: return Mission::Result::Timeout; - case MavlinkMissionTransfer::Result::Unsupported: + case MavlinkMissionTransferClient::Result::Unsupported: return Mission::Result::Unsupported; - case MavlinkMissionTransfer::Result::UnsupportedFrame: + case MavlinkMissionTransferClient::Result::UnsupportedFrame: return Mission::Result::Unsupported; - case MavlinkMissionTransfer::Result::NoMissionAvailable: + case MavlinkMissionTransferClient::Result::NoMissionAvailable: return Mission::Result::NoMissionAvailable; - case MavlinkMissionTransfer::Result::Cancelled: + case MavlinkMissionTransferClient::Result::Cancelled: return Mission::Result::TransferCancelled; - case MavlinkMissionTransfer::Result::MissionTypeNotConsistent: + case MavlinkMissionTransferClient::Result::MissionTypeNotConsistent: return Mission::Result::Error; // should not happen - case MavlinkMissionTransfer::Result::InvalidSequence: + case MavlinkMissionTransferClient::Result::InvalidSequence: return Mission::Result::Error; // should not happen - case MavlinkMissionTransfer::Result::CurrentInvalid: + case MavlinkMissionTransferClient::Result::CurrentInvalid: return Mission::Result::Error; // should not happen - case MavlinkMissionTransfer::Result::ProtocolError: + case MavlinkMissionTransferClient::Result::ProtocolError: return Mission::Result::ProtocolError; - case MavlinkMissionTransfer::Result::InvalidParam: + case MavlinkMissionTransferClient::Result::InvalidParam: return Mission::Result::InvalidArgument; - case MavlinkMissionTransfer::Result::IntMessagesNotSupported: + case MavlinkMissionTransferClient::Result::IntMessagesNotSupported: return Mission::Result::IntMessagesNotSupported; default: return Mission::Result::Unknown; @@ -1198,7 +1198,7 @@ Mission::Result MissionImpl::convert_result(MavlinkMissionTransfer::Result resul } void MissionImpl::add_gimbal_items_v1( - std::vector& int_items, + std::vector& int_items, unsigned item_i, float pitch_deg, float yaw_deg) @@ -1211,7 +1211,7 @@ void MissionImpl::add_gimbal_items_v1( uint8_t autocontinue = 1; - MavlinkMissionTransfer::ItemInt next_item{ + MavlinkMissionTransferClient::ItemInt next_item{ static_cast(int_items.size()), MAV_FRAME_MISSION, MAV_CMD_DO_MOUNT_CONFIGURE, @@ -1238,7 +1238,7 @@ void MissionImpl::add_gimbal_items_v1( uint8_t autocontinue = 1; - MavlinkMissionTransfer::ItemInt next_item{ + MavlinkMissionTransferClient::ItemInt next_item{ static_cast(int_items.size()), MAV_FRAME_MISSION, MAV_CMD_DO_MOUNT_CONTROL, @@ -1258,7 +1258,7 @@ void MissionImpl::add_gimbal_items_v1( } void MissionImpl::add_gimbal_items_v2( - std::vector& int_items, + std::vector& int_items, unsigned item_i, float pitch_deg, float yaw_deg) @@ -1278,7 +1278,7 @@ void MissionImpl::add_gimbal_items_v2( yaw_deg = fmod(yaw_deg, 360.f); yaw_deg = yaw_deg > 180.f ? yaw_deg - 360.f : yaw_deg; - MavlinkMissionTransfer::ItemInt next_item{ + MavlinkMissionTransferClient::ItemInt next_item{ static_cast(int_items.size()), MAV_FRAME_MISSION, MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW, @@ -1298,13 +1298,13 @@ void MissionImpl::add_gimbal_items_v2( } void MissionImpl::acquire_gimbal_control_v2( - std::vector& int_items, unsigned item_i) + std::vector& int_items, unsigned item_i) { uint8_t current = ((int_items.size() == 0) ? 1 : 0); uint8_t autocontinue = 1; - MavlinkMissionTransfer::ItemInt next_item{ + MavlinkMissionTransferClient::ItemInt next_item{ static_cast(int_items.size()), MAV_FRAME_MISSION, MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE, @@ -1324,13 +1324,13 @@ void MissionImpl::acquire_gimbal_control_v2( } void MissionImpl::release_gimbal_control_v2( - std::vector& int_items, unsigned item_i) + std::vector& int_items, unsigned item_i) { uint8_t current = ((int_items.size() == 0) ? 1 : 0); uint8_t autocontinue = 1; - MavlinkMissionTransfer::ItemInt next_item{ + MavlinkMissionTransferClient::ItemInt next_item{ static_cast(int_items.size()), MAV_FRAME_MISSION, MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE, diff --git a/src/mavsdk/plugins/mission/mission_impl.h b/src/mavsdk/plugins/mission/mission_impl.h index c77ce9bb14..af682c1765 100644 --- a/src/mavsdk/plugins/mission/mission_impl.h +++ b/src/mavsdk/plugins/mission/mission_impl.h @@ -9,7 +9,7 @@ #include "plugins/mission/mission.h" #include "plugin_impl_base.h" #include "system.h" -#include "mavlink_mission_transfer.h" +#include "mavlink_mission_transfer_client.h" #include "callback_list.h" namespace mavsdk { @@ -89,7 +89,7 @@ class MissionImpl : public PluginImplBase { static float hold_time(const Mission::MissionItem& item); static float acceptance_radius(const Mission::MissionItem& item); - std::vector + std::vector convert_to_int_items(const std::vector& mission_items); void report_progress_locked(); @@ -101,27 +101,27 @@ class MissionImpl : public PluginImplBase { // FIXME: make static std::pair convert_to_result_and_mission_items( - MavlinkMissionTransfer::Result result, - const std::vector& int_items); + MavlinkMissionTransferClient::Result result, + const std::vector& int_items); - static Mission::Result convert_result(MavlinkMissionTransfer::Result result); + static Mission::Result convert_result(MavlinkMissionTransferClient::Result result); void add_gimbal_items_v1( - std::vector& int_items, + std::vector& int_items, unsigned item_i, float pitch_deg, float yaw_deg); void add_gimbal_items_v2( - std::vector& int_items, + std::vector& int_items, unsigned item_i, float pitch_deg, float yaw_deg); void acquire_gimbal_control_v2( - std::vector& int_items, unsigned item_i); + std::vector& int_items, unsigned item_i); void release_gimbal_control_v2( - std::vector& int_items, unsigned item_i); + std::vector& int_items, unsigned item_i); struct MissionData { mutable std::mutex mutex{}; @@ -131,8 +131,8 @@ class MissionImpl : public PluginImplBase { CallbackList mission_progress_callbacks{}; int last_current_reported_mission_item{-1}; int last_total_reported_mission_item{-1}; - std::weak_ptr last_upload{}; - std::weak_ptr last_download{}; + std::weak_ptr last_upload{}; + std::weak_ptr last_download{}; bool gimbal_v2_in_control{false}; } _mission_data{}; diff --git a/src/mavsdk/plugins/mission_raw/mission_raw_impl.cpp b/src/mavsdk/plugins/mission_raw/mission_raw_impl.cpp index dcbfd5f7c7..f3e5836676 100644 --- a/src/mavsdk/plugins/mission_raw/mission_raw_impl.cpp +++ b/src/mavsdk/plugins/mission_raw/mission_raw_impl.cpp @@ -147,11 +147,11 @@ void MissionRawImpl::upload_mission_items_async( const auto int_items = convert_to_int_items(mission_raw); - _last_upload = _system_impl->mission_transfer().upload_items_async( + _last_upload = _system_impl->mission_transfer_client().upload_items_async( type, _system_impl->get_system_id(), int_items, - [this, callback, int_items](MavlinkMissionTransfer::Result result) { + [this, callback, int_items](MavlinkMissionTransferClient::Result result) { auto converted_result = convert_result(result); auto converted_items = convert_items(int_items); _system_impl->call_user_callback([callback, converted_result, converted_items]() { @@ -237,12 +237,12 @@ void MissionRawImpl::download_mission_async(const MissionRaw::DownloadMissionCal return; } - _last_download = _system_impl->mission_transfer().download_items_async( + _last_download = _system_impl->mission_transfer_client().download_items_async( MAV_MISSION_TYPE_MISSION, _system_impl->get_system_id(), [this, callback]( - MavlinkMissionTransfer::Result result, - std::vector items) { + MavlinkMissionTransferClient::Result result, + std::vector items) { auto converted_result = convert_result(result); auto converted_items = convert_items(items); _system_impl->call_user_callback([callback, converted_result, converted_items]() { @@ -262,10 +262,10 @@ MissionRaw::Result MissionRawImpl::cancel_mission_download() } } -MavlinkMissionTransfer::ItemInt +MavlinkMissionTransferClient::ItemInt MissionRawImpl::convert_mission_raw(const MissionRaw::MissionItem transfer_mission_raw) { - MavlinkMissionTransfer::ItemInt new_item_int; + MavlinkMissionTransferClient::ItemInt new_item_int; new_item_int.seq = transfer_mission_raw.seq; new_item_int.frame = transfer_mission_raw.frame; @@ -284,10 +284,10 @@ MissionRawImpl::convert_mission_raw(const MissionRaw::MissionItem transfer_missi return new_item_int; } -std::vector +std::vector MissionRawImpl::convert_to_int_items(const std::vector& mission_raw) { - std::vector int_items; + std::vector int_items; for (const auto& item : mission_raw) { int_items.push_back(convert_mission_raw(item)); @@ -300,7 +300,7 @@ MissionRawImpl::convert_to_int_items(const std::vector& } MissionRaw::MissionItem -MissionRawImpl::convert_item(const MavlinkMissionTransfer::ItemInt& transfer_item) +MissionRawImpl::convert_item(const MavlinkMissionTransferClient::ItemInt& transfer_item) { MissionRaw::MissionItem new_item; @@ -321,8 +321,8 @@ MissionRawImpl::convert_item(const MavlinkMissionTransfer::ItemInt& transfer_ite return new_item; } -std::vector -MissionRawImpl::convert_items(const std::vector& transfer_items) +std::vector MissionRawImpl::convert_items( + const std::vector& transfer_items) { std::vector new_items; new_items.reserve(transfer_items.size()); @@ -427,10 +427,10 @@ void MissionRawImpl::clear_mission_async(const MissionRaw::ResultCallback& callb std::vector mission_items{empty_item}; upload_mission_async(mission_items, callback); } else { - _system_impl->mission_transfer().clear_items_async( + _system_impl->mission_transfer_client().clear_items_async( MAV_MISSION_TYPE_MISSION, _system_impl->get_system_id(), - [this, callback](MavlinkMissionTransfer::Result result) { + [this, callback](MavlinkMissionTransferClient::Result result) { auto converted_result = convert_result(result); _system_impl->call_user_callback([callback, converted_result]() { if (callback) { @@ -463,10 +463,10 @@ void MissionRawImpl::set_current_mission_item_async( }); } - _system_impl->mission_transfer().set_current_item_async( + _system_impl->mission_transfer_client().set_current_item_async( index, _system_impl->get_system_id(), - [this, callback](MavlinkMissionTransfer::Result result) { + [this, callback](MavlinkMissionTransferClient::Result result) { auto converted_result = convert_result(result); _system_impl->call_user_callback([callback, converted_result]() { if (callback) { @@ -557,38 +557,38 @@ MissionRawImpl::import_qgroundcontrol_mission_from_string(const std::string& qgc return MissionImport::parse_json(qgc_plan, _system_impl->autopilot()); } -MissionRaw::Result MissionRawImpl::convert_result(MavlinkMissionTransfer::Result result) +MissionRaw::Result MissionRawImpl::convert_result(MavlinkMissionTransferClient::Result result) { switch (result) { - case MavlinkMissionTransfer::Result::Success: + case MavlinkMissionTransferClient::Result::Success: return MissionRaw::Result::Success; - case MavlinkMissionTransfer::Result::ConnectionError: + case MavlinkMissionTransferClient::Result::ConnectionError: return MissionRaw::Result::Error; - case MavlinkMissionTransfer::Result::Denied: + case MavlinkMissionTransferClient::Result::Denied: return MissionRaw::Result::Denied; - case MavlinkMissionTransfer::Result::TooManyMissionItems: + case MavlinkMissionTransferClient::Result::TooManyMissionItems: return MissionRaw::Result::TooManyMissionItems; - case MavlinkMissionTransfer::Result::Timeout: + case MavlinkMissionTransferClient::Result::Timeout: return MissionRaw::Result::Timeout; - case MavlinkMissionTransfer::Result::Unsupported: + case MavlinkMissionTransferClient::Result::Unsupported: return MissionRaw::Result::Unsupported; - case MavlinkMissionTransfer::Result::UnsupportedFrame: + case MavlinkMissionTransferClient::Result::UnsupportedFrame: return MissionRaw::Result::Unsupported; - case MavlinkMissionTransfer::Result::NoMissionAvailable: + case MavlinkMissionTransferClient::Result::NoMissionAvailable: return MissionRaw::Result::NoMissionAvailable; - case MavlinkMissionTransfer::Result::Cancelled: + case MavlinkMissionTransferClient::Result::Cancelled: return MissionRaw::Result::TransferCancelled; - case MavlinkMissionTransfer::Result::MissionTypeNotConsistent: + case MavlinkMissionTransferClient::Result::MissionTypeNotConsistent: return MissionRaw::Result::MissionTypeNotConsistent; - case MavlinkMissionTransfer::Result::InvalidSequence: + case MavlinkMissionTransferClient::Result::InvalidSequence: return MissionRaw::Result::InvalidSequence; - case MavlinkMissionTransfer::Result::CurrentInvalid: + case MavlinkMissionTransferClient::Result::CurrentInvalid: return MissionRaw::Result::CurrentInvalid; - case MavlinkMissionTransfer::Result::ProtocolError: + case MavlinkMissionTransferClient::Result::ProtocolError: return MissionRaw::Result::ProtocolError; - case MavlinkMissionTransfer::Result::InvalidParam: + case MavlinkMissionTransferClient::Result::InvalidParam: return MissionRaw::Result::InvalidArgument; - case MavlinkMissionTransfer::Result::IntMessagesNotSupported: + case MavlinkMissionTransferClient::Result::IntMessagesNotSupported: return MissionRaw::Result::IntMessagesNotSupported; default: return MissionRaw::Result::Unknown; diff --git a/src/mavsdk/plugins/mission_raw/mission_raw_impl.h b/src/mavsdk/plugins/mission_raw/mission_raw_impl.h index cb1fd3bb02..4378d689a8 100644 --- a/src/mavsdk/plugins/mission_raw/mission_raw_impl.h +++ b/src/mavsdk/plugins/mission_raw/mission_raw_impl.h @@ -92,17 +92,17 @@ class MissionRawImpl : public PluginImplBase { MissionRaw::ResultCallback callback, MavlinkCommandSender::Result result); static MissionRaw::Result command_result_to_mission_result(MavlinkCommandSender::Result result); - std::vector + std::vector convert_to_int_items(const std::vector& mission_raw); - MavlinkMissionTransfer::ItemInt + MavlinkMissionTransferClient::ItemInt convert_mission_raw(const MissionRaw::MissionItem transfer_mission_raw); - static MissionRaw::Result convert_result(MavlinkMissionTransfer::Result result); + static MissionRaw::Result convert_result(MavlinkMissionTransferClient::Result result); MissionRaw::MissionItem static convert_item( - const MavlinkMissionTransfer::ItemInt& transfer_item); + const MavlinkMissionTransferClient::ItemInt& transfer_item); std::vector - convert_items(const std::vector& transfer_items); + convert_items(const std::vector& transfer_items); MissionRaw::Result upload_mission_items(std::vector mission_items, uint8_t type); @@ -111,9 +111,8 @@ class MissionRawImpl : public PluginImplBase { uint8_t type, const MissionRaw::ResultCallback& callback); - // TODO: check if these need a mutex as well. - std::weak_ptr _last_upload{}; - std::weak_ptr _last_download{}; + std::weak_ptr _last_upload{}; + std::weak_ptr _last_download{}; struct { std::mutex mutex{}; diff --git a/src/mavsdk/plugins/mission_raw_server/mission_raw_server_impl.cpp b/src/mavsdk/plugins/mission_raw_server/mission_raw_server_impl.cpp index ee633d8e3d..1e6e6e5df9 100644 --- a/src/mavsdk/plugins/mission_raw_server/mission_raw_server_impl.cpp +++ b/src/mavsdk/plugins/mission_raw_server/mission_raw_server_impl.cpp @@ -19,10 +19,10 @@ MissionRawServerImpl::~MissionRawServerImpl() _server_component_impl->unregister_plugin(this); } -MavlinkMissionTransfer::ItemInt +MavlinkMissionTransferServer::ItemInt convert_mission_raw(const MissionRawServer::MissionItem transfer_mission_raw) { - MavlinkMissionTransfer::ItemInt new_item_int{}; + MavlinkMissionTransferServer::ItemInt new_item_int{}; new_item_int.seq = transfer_mission_raw.seq; new_item_int.frame = transfer_mission_raw.frame; @@ -41,10 +41,10 @@ convert_mission_raw(const MissionRawServer::MissionItem transfer_mission_raw) return new_item_int; } -std::vector +std::vector convert_to_int_items(const std::vector& mission_raw) { - std::vector int_items; + std::vector int_items; for (const auto& item : mission_raw) { int_items.push_back(convert_mission_raw(item)); @@ -53,7 +53,8 @@ convert_to_int_items(const std::vector& mission_r return int_items; } -MissionRawServer::MissionItem convert_item(const MavlinkMissionTransfer::ItemInt& transfer_item) +MissionRawServer::MissionItem +convert_item(const MavlinkMissionTransferServer::ItemInt& transfer_item) { MissionRawServer::MissionItem new_item; @@ -75,7 +76,7 @@ MissionRawServer::MissionItem convert_item(const MavlinkMissionTransfer::ItemInt } std::vector -convert_items(const std::vector& transfer_items) +convert_items(const std::vector& transfer_items) { std::vector new_items; new_items.reserve(transfer_items.size()); @@ -87,38 +88,38 @@ convert_items(const std::vector& transfer_items return new_items; } -MissionRawServer::Result convert_result(MavlinkMissionTransfer::Result result) +MissionRawServer::Result convert_result(MavlinkMissionTransferServer::Result result) { switch (result) { - case MavlinkMissionTransfer::Result::Success: + case MavlinkMissionTransferServer::Result::Success: return MissionRawServer::Result::Success; - case MavlinkMissionTransfer::Result::ConnectionError: + case MavlinkMissionTransferServer::Result::ConnectionError: return MissionRawServer::Result::Error; // FIXME - case MavlinkMissionTransfer::Result::Denied: + case MavlinkMissionTransferServer::Result::Denied: return MissionRawServer::Result::Error; // FIXME - case MavlinkMissionTransfer::Result::TooManyMissionItems: + case MavlinkMissionTransferServer::Result::TooManyMissionItems: return MissionRawServer::Result::TooManyMissionItems; - case MavlinkMissionTransfer::Result::Timeout: + case MavlinkMissionTransferServer::Result::Timeout: return MissionRawServer::Result::Timeout; - case MavlinkMissionTransfer::Result::Unsupported: + case MavlinkMissionTransferServer::Result::Unsupported: return MissionRawServer::Result::Unsupported; - case MavlinkMissionTransfer::Result::UnsupportedFrame: + case MavlinkMissionTransferServer::Result::UnsupportedFrame: return MissionRawServer::Result::Unsupported; - case MavlinkMissionTransfer::Result::NoMissionAvailable: + case MavlinkMissionTransferServer::Result::NoMissionAvailable: return MissionRawServer::Result::NoMissionAvailable; - case MavlinkMissionTransfer::Result::Cancelled: + case MavlinkMissionTransferServer::Result::Cancelled: return MissionRawServer::Result::TransferCancelled; - case MavlinkMissionTransfer::Result::MissionTypeNotConsistent: + case MavlinkMissionTransferServer::Result::MissionTypeNotConsistent: return MissionRawServer::Result::InvalidArgument; // FIXME - case MavlinkMissionTransfer::Result::InvalidSequence: + case MavlinkMissionTransferServer::Result::InvalidSequence: return MissionRawServer::Result::InvalidArgument; // FIXME - case MavlinkMissionTransfer::Result::CurrentInvalid: + case MavlinkMissionTransferServer::Result::CurrentInvalid: return MissionRawServer::Result::InvalidArgument; // FIXME - case MavlinkMissionTransfer::Result::ProtocolError: + case MavlinkMissionTransferServer::Result::ProtocolError: return MissionRawServer::Result::Error; // FIXME - case MavlinkMissionTransfer::Result::InvalidParam: + case MavlinkMissionTransferServer::Result::InvalidParam: return MissionRawServer::Result::InvalidArgument; // FIXME - case MavlinkMissionTransfer::Result::IntMessagesNotSupported: + case MavlinkMissionTransferServer::Result::IntMessagesNotSupported: return MissionRawServer::Result::Unsupported; // FIXME default: return MissionRawServer::Result::Unknown; @@ -129,158 +130,140 @@ void MissionRawServerImpl::init() { _server_component_impl->add_capabilities(MAV_PROTOCOL_CAPABILITY_MISSION_INT); - _thread_mission = std::thread([this] { - while (!_stop_work_thread) { - std::unique_lock lock(_work_mutex); - if (!_work_queue.empty()) { - auto task = _work_queue.front(); - _work_queue.pop(); - lock.unlock(); - task(); - } else { - _wait_for_new_task.wait(lock); - } - } - }); - // Handle Initiate Upload _server_component_impl->register_mavlink_message_handler( MAVLINK_MSG_ID_MISSION_COUNT, - [this](const mavlink_message_t& message) { - LogDebug() << "Receive Mission Count in Server"; - - // Decode the count - _target_system_id = message.sysid; - _target_component_id = message.compid; - mavlink_mission_count_t count; - mavlink_msg_mission_count_decode(&message, &count); - _mission_count = count.count; - - // We need to queue this on a different thread or it will deadlock - add_task([this, target_system_id = message.sysid]() { - // Mission Upload Inbound - if (_last_download.lock()) { - _incoming_mission_callbacks.queue( - MissionRawServer::Result::Busy, - MissionRawServer::MissionPlan{}, - [this](const auto& func) { - _server_component_impl->call_user_callback(func); - }); - return; - } - - _server_component_impl->set_our_current_target_system_id(target_system_id); - - _last_download = - _server_component_impl->mission_transfer().receive_incoming_items_async( - MAV_MISSION_TYPE_MISSION, - _mission_count, - _target_system_id, - _target_component_id, - [this]( - MavlinkMissionTransfer::Result result, - std::vector items) { - _current_mission = items; - auto converted_result = convert_result(result); - auto converted_items = convert_items(items); - _incoming_mission_callbacks.queue( - converted_result, {converted_items}, [this](const auto& func) { - _server_component_impl->call_user_callback(func); - }); - _mission_completed = false; - set_current_seq(0); - }); - }); - }, + [this](const mavlink_message_t& message) { process_mission_count(message); }, this); // Handle Set Current from GCS _server_component_impl->register_mavlink_message_handler( MAVLINK_MSG_ID_MISSION_SET_CURRENT, - [this](const mavlink_message_t& message) { - LogDebug() << "Receive Mission Set Current"; - - mavlink_mission_set_current_t set_current; - mavlink_msg_mission_set_current_decode(&message, &set_current); - - if (_current_mission.size() == 0) { - const char text[50] = "No Mission Loaded"; - _server_component_impl->queue_message( - [&](MavlinkAddress mavlink_address, uint8_t channel) { - mavlink_message_t response_message; - mavlink_msg_statustext_pack_chan( - mavlink_address.system_id, - mavlink_address.component_id, - channel, - &response_message, - MAV_SEVERITY_ERROR, - text, - 0, - 0); - return response_message; - }); - } else if (_current_mission.size() <= set_current.seq) { - const char text[50] = "Unknown Mission seq id"; - _server_component_impl->queue_message( - [&](MavlinkAddress mavlink_address, uint8_t channel) { - mavlink_message_t response_message; - mavlink_msg_statustext_pack_chan( - mavlink_address.system_id, - mavlink_address.component_id, - channel, - &response_message, - MAV_SEVERITY_ERROR, - text, - 0, - 0); - return response_message; - }); - } else { - set_current_seq(set_current.seq); - } - }, + [this](const mavlink_message_t& message) { process_mission_set_current(message); }, this); // Handle Clears _server_component_impl->register_mavlink_message_handler( MAVLINK_MSG_ID_MISSION_CLEAR_ALL, - [this](const mavlink_message_t& message) { - mavlink_mission_clear_all_t clear_all; - mavlink_msg_mission_clear_all_decode(&message, &clear_all); - if (clear_all.mission_type == MAV_MISSION_TYPE_ALL || - clear_all.mission_type == MAV_MISSION_TYPE_MISSION) { - _current_mission.clear(); - _current_seq = 0; - _clear_all_callbacks.queue(clear_all.mission_type, [this](const auto& func) { - _server_component_impl->call_user_callback(func); - }); - } - - // Send the MISSION_ACK - _server_component_impl->queue_message( - [&](MavlinkAddress mavlink_address, uint8_t channel) { - mavlink_message_t response_message; - mavlink_msg_mission_ack_pack_chan( - mavlink_address.system_id, - mavlink_address.component_id, - channel, - &response_message, - message.sysid, - message.compid, - MAV_MISSION_RESULT::MAV_MISSION_ACCEPTED, - clear_all.mission_type); - return response_message; - }); - }, + [this](const mavlink_message_t& message) { process_mission_clear(message); }, this); } void MissionRawServerImpl::deinit() { _server_component_impl->unregister_all_mavlink_message_handlers(this); - _stop_work_thread = true; - _wait_for_new_task.notify_all(); - _thread_mission.join(); +} + +void MissionRawServerImpl::process_mission_count(const mavlink_message_t& message) +{ + UNUSED(message); + + // Decode the count + _target_system_id = message.sysid; + _target_component_id = message.compid; + + mavlink_mission_count_t count; + mavlink_msg_mission_count_decode(&message, &count); + _mission_count = count.count; + + // Mission Upload Inbound + if (_last_download.lock()) { + _incoming_mission_callbacks.queue( + MissionRawServer::Result::Busy, + MissionRawServer::MissionPlan{}, + [this](const auto& func) { _server_component_impl->call_user_callback(func); }); + return; + } + + _last_download = _server_component_impl->mission_transfer_server().receive_incoming_items_async( + MAV_MISSION_TYPE_MISSION, + _mission_count, + _target_system_id, + _target_component_id, + [this]( + MavlinkMissionTransferServer::Result result, + std::vector items) { + _current_mission = items; + auto converted_result = convert_result(result); + auto converted_items = convert_items(items); + _incoming_mission_callbacks.queue( + converted_result, {converted_items}, [this](const auto& func) { + _server_component_impl->call_user_callback(func); + }); + _mission_completed = false; + set_current_seq(0); + }); +} + +void MissionRawServerImpl::process_mission_set_current(const mavlink_message_t& message) +{ + LogDebug() << "Receive Mission Set Current"; + + mavlink_mission_set_current_t set_current; + mavlink_msg_mission_set_current_decode(&message, &set_current); + + if (_current_mission.size() == 0) { + const char text[50] = "No Mission Loaded"; + _server_component_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) { + mavlink_message_t response_message; + mavlink_msg_statustext_pack_chan( + mavlink_address.system_id, + mavlink_address.component_id, + channel, + &response_message, + MAV_SEVERITY_ERROR, + text, + 0, + 0); + return response_message; + }); + } else if (_current_mission.size() <= set_current.seq) { + const char text[50] = "Unknown Mission seq id"; + _server_component_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) { + mavlink_message_t response_message; + mavlink_msg_statustext_pack_chan( + mavlink_address.system_id, + mavlink_address.component_id, + channel, + &response_message, + MAV_SEVERITY_ERROR, + text, + 0, + 0); + return response_message; + }); + } else { + set_current_seq(set_current.seq); + } +} + +void MissionRawServerImpl::process_mission_clear(const mavlink_message_t message) +{ + mavlink_mission_clear_all_t clear_all; + mavlink_msg_mission_clear_all_decode(&message, &clear_all); + if (clear_all.mission_type == MAV_MISSION_TYPE_ALL || + clear_all.mission_type == MAV_MISSION_TYPE_MISSION) { + _current_mission.clear(); + _current_seq = 0; + _clear_all_callbacks.queue(clear_all.mission_type, [this](const auto& func) { + _server_component_impl->call_user_callback(func); + }); + } + + // Send the MISSION_ACK + _server_component_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) { + mavlink_message_t response_message; + mavlink_msg_mission_ack_pack_chan( + mavlink_address.system_id, + mavlink_address.component_id, + channel, + &response_message, + message.sysid, + message.compid, + MAV_MISSION_RESULT::MAV_MISSION_ACCEPTED, + clear_all.mission_type); + return response_message; + }); } MissionRawServer::IncomingMissionHandle MissionRawServerImpl::subscribe_incoming_mission( diff --git a/src/mavsdk/plugins/mission_raw_server/mission_raw_server_impl.h b/src/mavsdk/plugins/mission_raw_server/mission_raw_server_impl.h index f86bf442f1..1f48a75c17 100644 --- a/src/mavsdk/plugins/mission_raw_server/mission_raw_server_impl.h +++ b/src/mavsdk/plugins/mission_raw_server/mission_raw_server_impl.h @@ -37,34 +37,25 @@ class MissionRawServerImpl : public ServerPluginImplBase { uint32_t clear_all() const; private: + void process_mission_count(const mavlink_message_t& message); + void process_mission_set_current(const mavlink_message_t& message); + void process_mission_clear(const mavlink_message_t message); + CallbackList _incoming_mission_callbacks{}; CallbackList _current_item_changed_callbacks{}; CallbackList _clear_all_callbacks{}; - std::thread _thread_mission; std::atomic _target_system_id; std::atomic _target_component_id; std::atomic _mission_count; std::atomic _mission_completed; - std::queue> _work_queue; - std::condition_variable _wait_for_new_task; - std::mutex _work_mutex; - std::atomic _stop_work_thread = false; - - std::vector _current_mission; + std::vector _current_mission; std::size_t _current_seq; - std::weak_ptr _last_download{}; + std::weak_ptr _last_download{}; void set_current_seq(std::size_t seq); - - void add_task(std::function task) - { - std::unique_lock lock(_work_mutex); - _work_queue.push(task); - _wait_for_new_task.notify_one(); - } }; } // namespace mavsdk