diff --git a/proto b/proto index 0fe3a36908..9a871c7b4e 160000 --- a/proto +++ b/proto @@ -1 +1 @@ -Subproject commit 0fe3a36908ebfce285c2854e82d5709b5400c019 +Subproject commit 9a871c7b4ec53a753e9fc46e950c4433dc2d6bf7 diff --git a/src/mavsdk/core/mavlink_mission_transfer_server.cpp b/src/mavsdk/core/mavlink_mission_transfer_server.cpp index e19862db0e..98671e2c4c 100644 --- a/src/mavsdk/core/mavlink_mission_transfer_server.cpp +++ b/src/mavsdk/core/mavlink_mission_transfer_server.cpp @@ -31,14 +31,6 @@ MavlinkMissionTransferServer::receive_incoming_items_async( 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, @@ -56,6 +48,31 @@ MavlinkMissionTransferServer::receive_incoming_items_async( return std::weak_ptr(ptr); } +std::weak_ptr +MavlinkMissionTransferServer::send_outgoing_items_async( + uint8_t type, + const std::vector& items, + uint8_t target_system, + uint8_t target_component, + ResultCallback callback) +{ + auto ptr = std::make_shared( + _sender, + _message_handler, + _timeout_handler, + type, + items, + _timeout_s_callback(), + callback, + 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); @@ -238,7 +255,6 @@ void MavlinkMissionTransferServer::ReceiveIncomingMission::process_mission_count _timeout_handler.refresh(_cookie); _next_sequence = 0; - _step = Step::RequestItem; _retries_done = 0; _expected_count = _mission_count; request_item(); @@ -301,4 +317,336 @@ void MavlinkMissionTransferServer::ReceiveIncomingMission::callback_and_reset(Re _done = true; } +MavlinkMissionTransferServer::SendOutgoingMission::SendOutgoingMission( + Sender& sender, + MavlinkMessageHandler& message_handler, + TimeoutHandler& timeout_handler, + uint8_t type, + const std::vector& items, + double timeout_s, + ResultCallback callback, + uint8_t target_system_id, + uint8_t target_component_id, + bool debugging) : + WorkItem(sender, message_handler, timeout_handler, type, timeout_s, debugging), + _items(items), + _callback(callback), + _target_system_id(target_system_id), + _target_component_id(target_component_id) +{ + _message_handler.register_one( + MAVLINK_MSG_ID_MISSION_REQUEST, + [this](const mavlink_message_t& message) { process_mission_request(message); }, + this); + + _message_handler.register_one( + MAVLINK_MSG_ID_MISSION_REQUEST_INT, + [this](const mavlink_message_t& message) { process_mission_request_int(message); }, + this); + + _message_handler.register_one( + MAVLINK_MSG_ID_MISSION_ACK, + [this](const mavlink_message_t& message) { process_mission_ack(message); }, + this); +} + +MavlinkMissionTransferServer::SendOutgoingMission::~SendOutgoingMission() +{ + _message_handler.unregister_all(this); + _timeout_handler.remove(_cookie); +} + +void MavlinkMissionTransferServer::SendOutgoingMission::start() +{ + std::lock_guard lock(_mutex); + + _started = true; + _retries_done = 0; + _step = Step::SendCount; + _cookie = _timeout_handler.add([this]() { process_timeout(); }, _timeout_s); + + _next_sequence = 0; + + send_count(); +} + +void MavlinkMissionTransferServer::SendOutgoingMission::cancel() +{ + std::lock_guard lock(_mutex); + + _timeout_handler.remove(_cookie); + send_cancel_and_finish(); +} + +void MavlinkMissionTransferServer::SendOutgoingMission::send_count() +{ + if (!_sender.queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) { + mavlink_message_t message; + mavlink_msg_mission_count_pack_chan( + mavlink_address.system_id, + mavlink_address.component_id, + channel, + &message, + _target_system_id, + _target_component_id, + _items.size(), + _type, + 0); + return message; + })) { + _timeout_handler.remove(_cookie); + callback_and_reset(Result::ConnectionError); + return; + } + + if (_debugging) { + LogDebug() << "Sending send_count, count: " << _items.size() + << ", retries: " << _retries_done; + } + + ++_retries_done; +} + +void MavlinkMissionTransferServer::SendOutgoingMission::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, + 0); + 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::SendOutgoingMission::process_mission_request( + const mavlink_message_t& request_message) +{ + mavlink_mission_request_t request; + mavlink_msg_mission_request_decode(&request_message, &request); + + std::lock_guard lock(_mutex); + + // We only support int, so we nack this and thus tell the autopilot to use int. + 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, + request_message.sysid, + request_message.compid, + MAV_MISSION_UNSUPPORTED, + _type, + 0); + return message; + })) { + _timeout_handler.remove(_cookie); + callback_and_reset(Result::ConnectionError); + return; + } + _timeout_handler.refresh(_cookie); +} + +void MavlinkMissionTransferServer::SendOutgoingMission::process_mission_request_int( + const mavlink_message_t& message) +{ + std::lock_guard lock(_mutex); + + mavlink_mission_request_int_t request_int; + mavlink_msg_mission_request_int_decode(&message, &request_int); + + _step = Step::SendItems; + + if (_debugging) { + LogDebug() << "Process mission_request_int, seq: " << request_int.seq + << ", next expected sequence: " << _next_sequence; + } + + if (_next_sequence < request_int.seq) { + // We should not go back to a previous one. + // TODO: figure out if we should error here. + LogWarn() << "mission_request_int: sequence incorrect"; + return; + + } else if (_next_sequence > request_int.seq) { + // We have already sent that one before. + if (_retries_done >= retries) { + LogWarn() << "mission_request_int: retries exceeded"; + _timeout_handler.remove(_cookie); + callback_and_reset(Result::Timeout); + return; + } + + } else { + // Correct one, sending it the first time. + _retries_done = 0; + } + + _timeout_handler.refresh(_cookie); + + _next_sequence = request_int.seq; + + send_mission_item(); +} + +void MavlinkMissionTransferServer::SendOutgoingMission::send_mission_item() +{ + if (_next_sequence >= _items.size()) { + LogErr() << "send_mission_item: sequence out of bounds"; + return; + } + + if (_debugging) { + LogDebug() << "Sending mission_item_int seq: " << _next_sequence + << ", retry: " << _retries_done; + } + + if (!_sender.queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) { + mavlink_message_t message; + mavlink_msg_mission_item_int_pack_chan( + mavlink_address.system_id, + mavlink_address.component_id, + channel, + &message, + _target_system_id, + _target_component_id, + _next_sequence, + _items[_next_sequence].frame, + _items[_next_sequence].command, + _items[_next_sequence].current, + _items[_next_sequence].autocontinue, + _items[_next_sequence].param1, + _items[_next_sequence].param2, + _items[_next_sequence].param3, + _items[_next_sequence].param4, + _items[_next_sequence].x, + _items[_next_sequence].y, + _items[_next_sequence].z, + _type); + return message; + })) { + _timeout_handler.remove(_cookie); + callback_and_reset(Result::ConnectionError); + return; + } + + ++_next_sequence; + + ++_retries_done; +} + +void MavlinkMissionTransferServer::SendOutgoingMission::process_mission_ack( + const mavlink_message_t& message) +{ + std::lock_guard lock(_mutex); + + mavlink_mission_ack_t mission_ack; + mavlink_msg_mission_ack_decode(&message, &mission_ack); + + if (_debugging) { + LogDebug() << "Received mission_ack type: " << static_cast(mission_ack.type); + } + + _timeout_handler.remove(_cookie); + + switch (mission_ack.type) { + case MAV_MISSION_ERROR: + callback_and_reset(Result::ProtocolError); + return; + case MAV_MISSION_UNSUPPORTED_FRAME: + callback_and_reset(Result::UnsupportedFrame); + return; + case MAV_MISSION_UNSUPPORTED: + callback_and_reset(Result::Unsupported); + return; + case MAV_MISSION_NO_SPACE: + callback_and_reset(Result::TooManyMissionItems); + return; + case MAV_MISSION_INVALID: + // FALLTHROUGH + case MAV_MISSION_INVALID_PARAM1: + // FALLTHROUGH + case MAV_MISSION_INVALID_PARAM2: + // FALLTHROUGH + case MAV_MISSION_INVALID_PARAM3: + // FALLTHROUGH + case MAV_MISSION_INVALID_PARAM4: + // FALLTHROUGH + case MAV_MISSION_INVALID_PARAM5_X: + // FALLTHROUGH + case MAV_MISSION_INVALID_PARAM6_Y: + // FALLTHROUGH + case MAV_MISSION_INVALID_PARAM7: + callback_and_reset(Result::InvalidParam); + return; + case MAV_MISSION_INVALID_SEQUENCE: + callback_and_reset(Result::InvalidSequence); + return; + case MAV_MISSION_DENIED: + callback_and_reset(Result::Denied); + return; + case MAV_MISSION_OPERATION_CANCELLED: + callback_and_reset(Result::Cancelled); + return; + } + + if (_next_sequence == _items.size()) { + callback_and_reset(Result::Success); + } else { + callback_and_reset(Result::ProtocolError); + } +} + +void MavlinkMissionTransferServer::SendOutgoingMission::process_timeout() +{ + std::lock_guard lock(_mutex); + + if (_debugging) { + LogDebug() << "Timeout triggered, retries: " << _retries_done; + } + + if (_retries_done >= retries) { + LogWarn() << "timeout: retries exceeded"; + callback_and_reset(Result::Timeout); + return; + } + + switch (_step) { + case Step::SendCount: + _cookie = _timeout_handler.add([this]() { process_timeout(); }, _timeout_s); + send_count(); + break; + + case Step::SendItems: + // When waiting for items requested we should wait longer than + // just our timeout, otherwise we give up too quickly. + ++_retries_done; + _cookie = _timeout_handler.add([this]() { process_timeout(); }, _timeout_s); + break; + } +} + +void MavlinkMissionTransferServer::SendOutgoingMission::callback_and_reset(Result result) +{ + if (_callback) { + _callback(result); + } + _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 index 5fe48e85bb..6849ed0577 100644 --- a/src/mavsdk/core/mavlink_mission_transfer_server.h +++ b/src/mavsdk/core/mavlink_mission_transfer_server.h @@ -130,11 +130,6 @@ class MavlinkMissionTransferServer { void process_timeout(); void callback_and_reset(Result result); - enum class Step { - RequestList, - RequestItem, - } _step{Step::RequestList}; - std::vector _items{}; ResultAndItemsCallback _callback{nullptr}; TimeoutHandler::Cookie _cookie{}; @@ -147,6 +142,55 @@ class MavlinkMissionTransferServer { }; static constexpr unsigned retries = 5; + class SendOutgoingMission : public WorkItem { + public: + explicit SendOutgoingMission( + Sender& sender, + MavlinkMessageHandler& message_handler, + TimeoutHandler& timeout_handler, + uint8_t type, + const std::vector& items, + double timeout_s, + ResultCallback callback, + uint8_t target_system_id, + uint8_t target_component_id, + bool debugging); + + ~SendOutgoingMission() override; + + void start() override; + void cancel() override; + + SendOutgoingMission(const SendOutgoingMission&) = delete; + SendOutgoingMission(SendOutgoingMission&&) = delete; + SendOutgoingMission& operator=(const SendOutgoingMission&) = delete; + SendOutgoingMission& operator=(SendOutgoingMission&&) = delete; + + private: + void send_count(); + void send_mission_item(); + void send_cancel_and_finish(); + + void process_mission_request(const mavlink_message_t& message); + void process_mission_request_int(const mavlink_message_t& message); + void process_mission_ack(const mavlink_message_t& message); + void process_timeout(); + void callback_and_reset(Result result); + + enum class Step { + SendCount, + SendItems, + } _step{Step::SendCount}; + + std::vector _items{}; + ResultCallback _callback{nullptr}; + TimeoutHandler::Cookie _cookie{}; + std::size_t _next_sequence{0}; + unsigned _retries_done{0}; + uint8_t _target_system_id{0}; + uint8_t _target_component_id{0}; + }; + explicit MavlinkMissionTransferServer( Sender& sender, MavlinkMessageHandler& message_handler, @@ -162,11 +206,16 @@ class MavlinkMissionTransferServer { uint8_t target_component, ResultAndItemsCallback callback); + std::weak_ptr send_outgoing_items_async( + uint8_t type, + const std::vector& items, + uint8_t target_system, + uint8_t target_component, + ResultCallback 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; @@ -179,7 +228,6 @@ class MavlinkMissionTransferServer { LockedQueue _work_queue{}; - bool _int_messages_supported{true}; bool _debugging{false}; }; diff --git a/src/mavsdk/core/mavlink_mission_transfer_server_test.cpp b/src/mavsdk/core/mavlink_mission_transfer_server_test.cpp index 00ecff31a1..e11e344866 100644 --- a/src/mavsdk/core/mavlink_mission_transfer_server_test.cpp +++ b/src/mavsdk/core/mavlink_mission_transfer_server_test.cpp @@ -45,107 +45,211 @@ class MavlinkMissionTransferServerTest : public ::testing::Test { 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; -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; -} + 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; -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, - 0); - return message; -} + return item; + } -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; + 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, + 0); + return message; } - 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); -} + static 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; + } -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_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_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); -} + static 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; + } -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_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); } - 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); -} + + static 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); + } + + static 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; + } + + 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; + } + + mavlink_mission_count_t mission_count; + mavlink_msg_mission_count_decode(&message, &mission_count); + + return ( + message.msgid == MAVLINK_MSG_ID_MISSION_COUNT && + message.sysid == own_address.system_id && message.compid == own_address.component_id && + mission_count.target_system == target_address.system_id && + mission_count.target_component == target_address.component_id && + mission_count.count == count && mission_count.mission_type == type); + } + + static mavlink_message_t make_mission_request_int(uint8_t type, int sequence) + { + mavlink_message_t message; + mavlink_msg_mission_request_int_pack( + own_address.system_id, + own_address.component_id, + &message, + target_address.system_id, + target_address.component_id, + sequence, + type); + return message; + } + + static mavlink_message_t make_mission_ack(uint8_t type, uint8_t result) + { + mavlink_message_t message; + mavlink_msg_mission_ack_pack( + own_address.system_id, + own_address.component_id, + &message, + target_address.system_id, + target_address.component_id, + result, + type, + 0); + return message; + } + + static mavlink_message_t make_mission_request(uint8_t type, int sequence) + { + mavlink_message_t message; + mavlink_msg_mission_request_pack( + target_address.system_id, + target_address.component_id, + &message, + own_address.system_id, + own_address.component_id, + sequence, + type); + return message; + } + + static bool + is_correct_mission_ack(uint8_t type, uint8_t result, 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_address.component_id && ack.type == result && + ack.mission_type == type); + } + + MockSender mock_sender; + MavlinkMessageHandler message_handler; + FakeTime time; + TimeoutHandler timeout_handler; + MavlinkMissionTransferServer mmt; +}; TEST_F(MavlinkMissionTransferServerTest, ReceiveIncomingMissionSendsMissionRequests) { @@ -233,31 +337,6 @@ TEST_F( 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)); @@ -413,3 +492,730 @@ TEST_F(MavlinkMissionTransferServerTest, ReceiveIncomingMissionCanBeCancelled) mmt.do_work(); EXPECT_TRUE(mmt.is_idle()); } + +TEST_F(MavlinkMissionTransferServerTest, SendOutgoingMissionEmptyMission) +{ + std::vector items; + + std::promise prom; + auto fut = prom.get_future(); + + ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); + + mmt.send_outgoing_items_async( + MAV_MISSION_TYPE_MISSION, + items, + target_address.system_id, + target_address.component_id, + [&prom](Result result) { + EXPECT_EQ(result, Result::Success); + ONCE_ONLY; + prom.set_value(); + }); + mmt.do_work(); + + // empty mission should just send a zero count and be done + message_handler.process_message( + make_mission_ack(MAV_MISSION_TYPE_MISSION, MAV_MISSION_ACCEPTED)); + + EXPECT_EQ(fut.wait_for(std::chrono::seconds(1)), std::future_status::ready); + + mmt.do_work(); + EXPECT_TRUE(mmt.is_idle()); +} + +TEST_F(MavlinkMissionTransferServerTest, SendOutgoingMissionDoesNotCrashIfCallbackIsNull) +{ + ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(false)); + + // Catch the empty case + std::vector items; + mmt.send_outgoing_items_async( + MAV_MISSION_TYPE_MISSION, + items, + target_address.system_id, + target_address.component_id, + nullptr); + mmt.do_work(); + + items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 0)); + items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 1)); + + mmt.send_outgoing_items_async( + MAV_MISSION_TYPE_MISSION, + items, + target_address.system_id, + target_address.component_id, + nullptr); + mmt.do_work(); + + // Catch the WrongSequence case as well. + items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 3)); + mmt.send_outgoing_items_async( + MAV_MISSION_TYPE_MISSION, + items, + target_address.system_id, + target_address.component_id, + nullptr); + mmt.do_work(); + + mmt.do_work(); + EXPECT_TRUE(mmt.is_idle()); +} + +TEST_F( + MavlinkMissionTransferServerTest, SendOutgoingMissionReturnsConnectionErrorWhenSendMessageFails) +{ + ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(false)); + + 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.send_outgoing_items_async( + MAV_MISSION_TYPE_MISSION, + items, + target_address.system_id, + target_address.component_id, + [&prom](Result result) { + EXPECT_EQ(result, Result::ConnectionError); + ONCE_ONLY; + prom.set_value(); + }); + 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, SendOutgoingMissionSendsCount) +{ + std::vector items; + items.push_back(make_item(MAV_MISSION_TYPE_FENCE, 0)); + items.push_back(make_item(MAV_MISSION_TYPE_FENCE, 1)); + + ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); + + EXPECT_CALL( + mock_sender, + queue_message(Truly([&items](std::function fun) { + return is_correct_mission_send_count( + MAV_MISSION_TYPE_FENCE, items.size(), fun(own_address, channel)); + }))); + + mmt.send_outgoing_items_async( + MAV_MISSION_TYPE_FENCE, + items, + target_address.system_id, + target_address.component_id, + [](Result result) { + UNUSED(result); + EXPECT_TRUE(false); + }); + mmt.do_work(); +} + +TEST_F(MavlinkMissionTransferServerTest, SendOutgoingMissionResendsCount) +{ + std::vector items; + items.push_back(make_item(MAV_MISSION_TYPE_FENCE, 0)); + items.push_back(make_item(MAV_MISSION_TYPE_FENCE, 1)); + + ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); + + EXPECT_CALL( + mock_sender, + queue_message(Truly([&items](std::function fun) { + return is_correct_mission_send_count( + MAV_MISSION_TYPE_FENCE, items.size(), fun(own_address, channel)); + }))) + .Times(2); + + mmt.send_outgoing_items_async( + MAV_MISSION_TYPE_FENCE, + items, + target_address.system_id, + target_address.component_id, + [](Result result) { + UNUSED(result); + EXPECT_TRUE(false); + }); + mmt.do_work(); + + time.sleep_for(std::chrono::milliseconds(static_cast(timeout_s * 1.1 * 1000.))); + timeout_handler.run_once(); +} + +TEST_F(MavlinkMissionTransferServerTest, SendOutgoingMissionTimeoutAfterSendCount) +{ + std::vector items; + items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 0)); + items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 1)); + + ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); + + EXPECT_CALL( + mock_sender, + queue_message(Truly([&items](std::function fun) { + return is_correct_mission_send_count( + MAV_MISSION_TYPE_MISSION, items.size(), fun(own_address, channel)); + }))) + .Times(MavlinkMissionTransferServer::retries); + + std::promise prom; + auto fut = prom.get_future(); + + mmt.send_outgoing_items_async( + MAV_MISSION_TYPE_MISSION, + items, + target_address.system_id, + target_address.component_id, + [&prom](Result result) { + EXPECT_EQ(result, Result::Timeout); + ONCE_ONLY; + prom.set_value(); + }); + 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); +} + +TEST_F(MavlinkMissionTransferServerTest, SendOutgoingMissionSendsMissionItems) +{ + std::vector items; + items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 0)); + items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 1)); + + ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); + + std::promise prom; + auto fut = prom.get_future(); + + mmt.send_outgoing_items_async( + MAV_MISSION_TYPE_MISSION, + items, + target_address.system_id, + target_address.component_id, + [&prom](Result result) { + EXPECT_EQ(result, Result::Success); + ONCE_ONLY; + prom.set_value(); + }); + mmt.do_work(); + + EXPECT_CALL( + mock_sender, + queue_message(Truly([&items](std::function fun) { + return is_the_same_mission_item_int(items[0], fun(own_address, channel)); + }))); + + message_handler.process_message(make_mission_request_int(MAV_MISSION_TYPE_MISSION, 0)); + + EXPECT_CALL( + mock_sender, + queue_message(Truly([&items](std::function fun) { + return is_the_same_mission_item_int(items[1], fun(own_address, channel)); + }))); + + message_handler.process_message(make_mission_request_int(MAV_MISSION_TYPE_MISSION, 1)); + + message_handler.process_message( + make_mission_ack(MAV_MISSION_TYPE_MISSION, MAV_MISSION_ACCEPTED)); + + // We are finished and should have received the successful result. + EXPECT_EQ(fut.wait_for(std::chrono::seconds(1)), std::future_status::ready); + + // We do not expect a timeout later though. + 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, SendOutgoingMissionResendsMissionItems) +{ + std::vector items; + items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 0)); + items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 1)); + + ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); + + std::promise prom; + auto fut = prom.get_future(); + + mmt.send_outgoing_items_async( + MAV_MISSION_TYPE_MISSION, + items, + target_address.system_id, + target_address.component_id, + [&prom](Result result) { + EXPECT_EQ(result, Result::Success); + ONCE_ONLY; + prom.set_value(); + }); + mmt.do_work(); + + EXPECT_CALL( + mock_sender, + queue_message(Truly([&items](std::function fun) { + return is_the_same_mission_item_int(items[0], fun(own_address, channel)); + }))); + + message_handler.process_message(make_mission_request_int(MAV_MISSION_TYPE_MISSION, 0)); + + EXPECT_CALL( + mock_sender, + queue_message(Truly([&items](std::function fun) { + return is_the_same_mission_item_int(items[0], fun(own_address, channel)); + }))); + + // Request 0 again in case it had not arrived. + message_handler.process_message(make_mission_request_int(MAV_MISSION_TYPE_MISSION, 0)); + + EXPECT_CALL( + mock_sender, + queue_message(Truly([&items](std::function fun) { + return is_the_same_mission_item_int(items[1], fun(own_address, channel)); + }))); + + // Request 1 finally. + message_handler.process_message(make_mission_request_int(MAV_MISSION_TYPE_MISSION, 1)); + + message_handler.process_message( + make_mission_ack(MAV_MISSION_TYPE_MISSION, MAV_MISSION_ACCEPTED)); + + // We are finished and should have received the successful result. + EXPECT_EQ(fut.wait_for(std::chrono::seconds(1)), std::future_status::ready); + + mmt.do_work(); + EXPECT_TRUE(mmt.is_idle()); +} + +TEST_F( + MavlinkMissionTransferServerTest, + SendOutgoingMissionResendsMissionItemsButGivesUpAfterSomeRetries) +{ + std::vector items; + items.push_back(make_item(MAV_MISSION_TYPE_FENCE, 0)); + items.push_back(make_item(MAV_MISSION_TYPE_FENCE, 1)); + + ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); + + std::promise prom; + auto fut = prom.get_future(); + + mmt.send_outgoing_items_async( + MAV_MISSION_TYPE_FENCE, + items, + target_address.system_id, + target_address.component_id, + [&prom](Result result) { + EXPECT_EQ(result, Result::Timeout); + ONCE_ONLY; + prom.set_value(); + }); + mmt.do_work(); + + EXPECT_CALL( + mock_sender, + queue_message(Truly([&items](std::function fun) { + return is_the_same_mission_item_int(items[0], fun(own_address, channel)); + }))) + .Times(MavlinkMissionTransferServer::retries); + + for (unsigned i = 0; i < MavlinkMissionTransferServer::retries; ++i) { + message_handler.process_message(make_mission_request_int(MAV_MISSION_TYPE_MISSION, 0)); + } + + EXPECT_EQ(fut.wait_for(std::chrono::seconds(0)), std::future_status::timeout); + + message_handler.process_message(make_mission_request_int(MAV_MISSION_TYPE_MISSION, 0)); + + // We are finished and should have received the successful result. + EXPECT_EQ(fut.wait_for(std::chrono::seconds(1)), std::future_status::ready); + + mmt.do_work(); + EXPECT_TRUE(mmt.is_idle()); +} + +TEST_F(MavlinkMissionTransferServerTest, SendOutgoingMissionAckArrivesTooEarly) +{ + std::vector items; + items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 0)); + items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 1)); + + ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); + + std::promise prom; + auto fut = prom.get_future(); + + mmt.send_outgoing_items_async( + MAV_MISSION_TYPE_MISSION, + items, + target_address.system_id, + target_address.component_id, + [&prom](Result result) { + EXPECT_EQ(result, Result::ProtocolError); + ONCE_ONLY; + prom.set_value(); + }); + mmt.do_work(); + + EXPECT_CALL( + mock_sender, + queue_message(Truly([&items](std::function fun) { + return is_the_same_mission_item_int(items[0], fun(own_address, channel)); + }))); + + message_handler.process_message(make_mission_request_int(MAV_MISSION_TYPE_MISSION, 0)); + + // Don't request item 1 but already send ack. + message_handler.process_message( + make_mission_ack(MAV_MISSION_TYPE_MISSION, MAV_MISSION_ACCEPTED)); + + EXPECT_EQ(fut.wait_for(std::chrono::seconds(1)), std::future_status::ready); + + mmt.do_work(); + EXPECT_TRUE(mmt.is_idle()); +} + +class MavlinkMissionTransferServerNackTests + : public MavlinkMissionTransferServerTest, + public ::testing::WithParamInterface> {}; + +TEST_P(MavlinkMissionTransferServerNackTests, SendOutgoingMissionNackAreHandled) +{ + uint8_t mavlink_nack = std::get<0>(GetParam()); + Result mavsdk_nack = std::get<1>(GetParam()); + + std::vector items; + items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 0)); + items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 1)); + + ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); + + std::promise prom; + auto fut = prom.get_future(); + + mmt.send_outgoing_items_async( + MAV_MISSION_TYPE_MISSION, + items, + target_address.system_id, + target_address.component_id, + [&prom, &mavsdk_nack](Result result) { + EXPECT_EQ(result, mavsdk_nack); + prom.set_value(); + }); + mmt.do_work(); + + EXPECT_CALL( + mock_sender, + queue_message(Truly([&items](std::function fun) { + return is_the_same_mission_item_int(items[0], fun(own_address, channel)); + }))); + + message_handler.process_message(make_mission_request_int(MAV_MISSION_TYPE_MISSION, 0)); + + // Send nack now. + message_handler.process_message(make_mission_ack(MAV_MISSION_TYPE_MISSION, mavlink_nack)); + + EXPECT_EQ(fut.wait_for(std::chrono::seconds(1)), std::future_status::ready); + + mmt.do_work(); + EXPECT_TRUE(mmt.is_idle()); +} + +INSTANTIATE_TEST_SUITE_P( + MavlinkMissionTransferServerTests, + MavlinkMissionTransferServerNackTests, + ::testing::Values( + std::make_pair(MAV_MISSION_ERROR, Result::ProtocolError), + std::make_pair(MAV_MISSION_UNSUPPORTED_FRAME, Result::UnsupportedFrame), + std::make_pair(MAV_MISSION_UNSUPPORTED, Result::Unsupported), + std::make_pair(MAV_MISSION_NO_SPACE, Result::TooManyMissionItems), + std::make_pair(MAV_MISSION_INVALID, Result::InvalidParam), + std::make_pair(MAV_MISSION_INVALID_PARAM1, Result::InvalidParam), + std::make_pair(MAV_MISSION_INVALID_PARAM2, Result::InvalidParam), + std::make_pair(MAV_MISSION_INVALID_PARAM3, Result::InvalidParam), + std::make_pair(MAV_MISSION_INVALID_PARAM4, Result::InvalidParam), + std::make_pair(MAV_MISSION_INVALID_PARAM5_X, Result::InvalidParam), + std::make_pair(MAV_MISSION_INVALID_PARAM6_Y, Result::InvalidParam), + std::make_pair(MAV_MISSION_INVALID_PARAM7, Result::InvalidParam), + std::make_pair(MAV_MISSION_INVALID_SEQUENCE, Result::InvalidSequence), + std::make_pair(MAV_MISSION_DENIED, Result::Denied), + std::make_pair(MAV_MISSION_OPERATION_CANCELLED, Result::Cancelled))); + +TEST_F(MavlinkMissionTransferServerTest, SendOutgoingMissionTimeoutNotTriggeredDuringTransfer) +{ + 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)); + + ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); + + std::promise prom; + auto fut = prom.get_future(); + + mmt.send_outgoing_items_async( + MAV_MISSION_TYPE_MISSION, + items, + target_address.system_id, + target_address.component_id, + [&prom](Result result) { + EXPECT_EQ(result, Result::Success); + ONCE_ONLY; + prom.set_value(); + }); + mmt.do_work(); + + EXPECT_CALL( + mock_sender, + queue_message(Truly([&items](std::function fun) { + return is_the_same_mission_item_int(items[0], fun(own_address, channel)); + }))); + + message_handler.process_message(make_mission_request_int(MAV_MISSION_TYPE_MISSION, 0)); + + // We almost use up the max timeout in each cycle. + time.sleep_for(std::chrono::milliseconds(static_cast(timeout_s * 0.8 * 1000.))); + timeout_handler.run_once(); + + EXPECT_CALL( + mock_sender, + queue_message(Truly([&items](std::function fun) { + return is_the_same_mission_item_int(items[1], fun(own_address, channel)); + }))); + + message_handler.process_message(make_mission_request_int(MAV_MISSION_TYPE_MISSION, 1)); + + time.sleep_for(std::chrono::milliseconds(static_cast(timeout_s * 0.8 * 1000.))); + timeout_handler.run_once(); + + EXPECT_CALL( + mock_sender, + queue_message(Truly([&items](std::function fun) { + return is_the_same_mission_item_int(items[2], fun(own_address, channel)); + }))); + + message_handler.process_message(make_mission_request_int(MAV_MISSION_TYPE_MISSION, 2)); + + time.sleep_for(std::chrono::milliseconds(static_cast(timeout_s * 0.8 * 1000.))); + timeout_handler.run_once(); + + message_handler.process_message( + make_mission_ack(MAV_MISSION_TYPE_MISSION, MAV_MISSION_ACCEPTED)); + + // We are finished and should have received the successful result. + EXPECT_EQ(fut.wait_for(std::chrono::seconds(1)), std::future_status::ready); + + mmt.do_work(); + EXPECT_TRUE(mmt.is_idle()); +} + +TEST_F(MavlinkMissionTransferServerTest, SendOutgoingMissionTimeoutAfterSendMissionItem) +{ + std::vector items; + items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 0)); + items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 1)); + + ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); + + std::promise prom; + auto fut = prom.get_future(); + + mmt.send_outgoing_items_async( + MAV_MISSION_TYPE_MISSION, + items, + target_address.system_id, + target_address.component_id, + [&prom](Result result) { + EXPECT_EQ(result, Result::Timeout); + ONCE_ONLY; + prom.set_value(); + }); + mmt.do_work(); + + EXPECT_CALL( + mock_sender, + queue_message(Truly([&items](std::function fun) { + return is_the_same_mission_item_int(items[0], fun(own_address, channel)); + }))); + + message_handler.process_message(make_mission_request_int(MAV_MISSION_TYPE_MISSION, 0)); + + // Make sure single timeout does not trigger it yet. + time.sleep_for(std::chrono::milliseconds(static_cast(timeout_s * 1000. + 250))); + timeout_handler.run_once(); + + EXPECT_EQ(fut.wait_for(std::chrono::milliseconds(50)), std::future_status::timeout); + + // But multiple do. + for (unsigned i = 0; i < (MavlinkMissionTransferServer::retries - 1); ++i) { + time.sleep_for(std::chrono::milliseconds(static_cast(timeout_s * 1000.))); + timeout_handler.run_once(); + } + + EXPECT_EQ(fut.wait_for(std::chrono::seconds(1)), std::future_status::ready); + + // Ignore later (wrong) ack. + message_handler.process_message( + make_mission_ack(MAV_MISSION_TYPE_MISSION, MAV_MISSION_ACCEPTED)); + + mmt.do_work(); + EXPECT_TRUE(mmt.is_idle()); +} + +TEST_F(MavlinkMissionTransferServerTest, SendOutgoingMissionDoesNotCrashOnRandomMessages) +{ + message_handler.process_message(make_mission_request_int(MAV_MISSION_TYPE_MISSION, 0)); + + message_handler.process_message( + make_mission_ack(MAV_MISSION_TYPE_MISSION, MAV_MISSION_ACCEPTED)); + + 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, SendOutgoingMissionCanBeCancelled) +{ + std::vector items; + items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 0)); + items.push_back(make_item(MAV_MISSION_TYPE_MISSION, 1)); + + ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); + + std::promise prom; + auto fut = prom.get_future(); + + auto transfer = mmt.send_outgoing_items_async( + MAV_MISSION_TYPE_MISSION, + items, + target_address.system_id, + target_address.component_id, + [&prom](Result result) { + EXPECT_EQ(result, Result::Cancelled); + ONCE_ONLY; + prom.set_value(); + }); + mmt.do_work(); + + message_handler.process_message(make_mission_request_int(MAV_MISSION_TYPE_MISSION, 0)); + + EXPECT_CALL( + mock_sender, + queue_message(Truly([](std::function fun) { + return is_correct_mission_ack( + MAV_MISSION_TYPE_MISSION, + MAV_MISSION_OPERATION_CANCELLED, + fun(own_address, channel)); + }))); + + auto ptr = transfer.lock(); + EXPECT_TRUE(ptr); + if (ptr) { + ptr->cancel(); + } + + // We are finished and should have received the successful result. + EXPECT_EQ(fut.wait_for(std::chrono::seconds(1)), std::future_status::ready); + + // We do not expect a timeout later though. + 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, SendOutgoingMissionNacksNonIntCase) +{ + std::vector items; + items.push_back(make_item(MAV_MISSION_TYPE_FENCE, 0)); + + ON_CALL(mock_sender, queue_message(_)).WillByDefault(Return(true)); + + std::promise prom; + auto fut = prom.get_future(); + + mmt.send_outgoing_items_async( + MAV_MISSION_TYPE_FENCE, + items, + target_address.system_id, + target_address.component_id, + [&prom](Result result) { + EXPECT_EQ(result, Result::Success); + ONCE_ONLY; + prom.set_value(); + }); + mmt.do_work(); + + EXPECT_CALL( + mock_sender, + queue_message(Truly([](std::function fun) { + return is_correct_mission_ack( + MAV_MISSION_TYPE_FENCE, MAV_MISSION_UNSUPPORTED, fun(own_address, channel)); + }))) + .Times(1); + + // First the non-int wrong case comes in. + message_handler.process_message(make_mission_request(MAV_MISSION_TYPE_FENCE, 0)); + + EXPECT_CALL( + mock_sender, + queue_message(Truly([&items](std::function fun) { + return is_the_same_mission_item_int(items[0], fun(own_address, channel)); + }))); + + message_handler.process_message(make_mission_request_int(MAV_MISSION_TYPE_FENCE, 0)); + message_handler.process_message(make_mission_ack(MAV_MISSION_TYPE_FENCE, MAV_MISSION_ACCEPTED)); + + // We are finished and should have received the successful result. + EXPECT_EQ(fut.wait_for(std::chrono::seconds(1)), std::future_status::ready); + + // We do not expect a timeout later though. + 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()); +} diff --git a/src/mavsdk/plugins/mission_raw_server/include/plugins/mission_raw_server/mission_raw_server.h b/src/mavsdk/plugins/mission_raw_server/include/plugins/mission_raw_server/mission_raw_server.h index 998c14e7ea..25c20e69f8 100644 --- a/src/mavsdk/plugins/mission_raw_server/include/plugins/mission_raw_server/mission_raw_server.h +++ b/src/mavsdk/plugins/mission_raw_server/include/plugins/mission_raw_server/mission_raw_server.h @@ -185,13 +185,6 @@ class MissionRawServer : public ServerPluginBase { */ void unsubscribe_incoming_mission(IncomingMissionHandle handle); - /** - * @brief Poll for 'MissionPlan' (blocking). - * - * @return One MissionPlan update. - */ - MissionPlan incoming_mission() const; - /** * @brief Callback type for subscribe_current_item_changed. */ @@ -213,13 +206,6 @@ class MissionRawServer : public ServerPluginBase { */ void unsubscribe_current_item_changed(CurrentItemChangedHandle handle); - /** - * @brief Poll for 'MissionItem' (blocking). - * - * @return One MissionItem update. - */ - MissionItem current_item_changed() const; - /** * @brief Set Current item as completed * @@ -249,13 +235,6 @@ class MissionRawServer : public ServerPluginBase { */ void unsubscribe_clear_all(ClearAllHandle handle); - /** - * @brief Poll for 'uint32_t' (blocking). - * - * @return One uint32_t update. - */ - uint32_t clear_all() const; - /** * @brief Copy constructor. */ diff --git a/src/mavsdk/plugins/mission_raw_server/mission_raw_server.cpp b/src/mavsdk/plugins/mission_raw_server/mission_raw_server.cpp index 5610f05462..5438ae7873 100644 --- a/src/mavsdk/plugins/mission_raw_server/mission_raw_server.cpp +++ b/src/mavsdk/plugins/mission_raw_server/mission_raw_server.cpp @@ -32,11 +32,6 @@ void MissionRawServer::unsubscribe_incoming_mission(IncomingMissionHandle handle _impl->unsubscribe_incoming_mission(handle); } -MissionRawServer::MissionPlan MissionRawServer::incoming_mission() const -{ - return _impl->incoming_mission(); -} - MissionRawServer::CurrentItemChangedHandle MissionRawServer::subscribe_current_item_changed(const CurrentItemChangedCallback& callback) { @@ -48,11 +43,6 @@ void MissionRawServer::unsubscribe_current_item_changed(CurrentItemChangedHandle _impl->unsubscribe_current_item_changed(handle); } -MissionRawServer::MissionItem MissionRawServer::current_item_changed() const -{ - return _impl->current_item_changed(); -} - void MissionRawServer::set_current_item_complete() const { _impl->set_current_item_complete(); @@ -69,11 +59,6 @@ void MissionRawServer::unsubscribe_clear_all(ClearAllHandle handle) _impl->unsubscribe_clear_all(handle); } -uint32_t MissionRawServer::clear_all() const -{ - return _impl->clear_all(); -} - bool operator==(const MissionRawServer::MissionItem& lhs, const MissionRawServer::MissionItem& rhs) { return (rhs.seq == lhs.seq) && (rhs.frame == lhs.frame) && (rhs.command == lhs.command) && 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 285e8164de..cc6f33e27c 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 @@ -136,6 +136,12 @@ void MissionRawServerImpl::init() [this](const mavlink_message_t& message) { process_mission_count(message); }, this); + // Handle Initiate Download + _server_component_impl->register_mavlink_message_handler( + MAVLINK_MSG_ID_MISSION_REQUEST_LIST, + [this](const mavlink_message_t& message) { process_mission_request_list(message); }, + this); + // Handle Set Current from GCS _server_component_impl->register_mavlink_message_handler( MAVLINK_MSG_ID_MISSION_SET_CURRENT, @@ -195,6 +201,41 @@ void MissionRawServerImpl::process_mission_count(const mavlink_message_t& messag }); } +void MissionRawServerImpl::process_mission_request_list(const mavlink_message_t& message) +{ + UNUSED(message); + + _target_system_id = message.sysid; + _target_component_id = message.compid; + + mavlink_mission_request_list_t mission_type; + mavlink_msg_mission_request_list_decode(&message, &mission_type); + + // Mission Download Outbound + if (_last_upload.lock()) { + _outgoing_mission_callbacks.queue( + MissionRawServer::Result::Busy, + MissionRawServer::MissionPlan{}, + [this](const auto& func) { _server_component_impl->call_user_callback(func); }); + return; + } + + _last_upload = _server_component_impl->mission_transfer_server().send_outgoing_items_async( + MAV_MISSION_TYPE_MISSION, + _current_mission, + _target_system_id, + _target_component_id, + [this](MavlinkMissionTransferServer::Result result) { + auto converted_result = convert_result(result); + auto converted_items = convert_items(_current_mission); + _outgoing_mission_callbacks.queue( + converted_result, {converted_items}, [this](const auto& func) { + _server_component_impl->call_user_callback(func); + }); + _mission_completed = false; + }); +} + void MissionRawServerImpl::process_mission_set_current(const mavlink_message_t& message) { LogDebug() << "Receive Mission Set Current"; @@ -279,12 +320,6 @@ void MissionRawServerImpl::unsubscribe_incoming_mission( _incoming_mission_callbacks.unsubscribe(handle); } -MissionRawServer::MissionPlan MissionRawServerImpl::incoming_mission() const -{ - // FIXME: this doesn't look right. - return {}; -} - MissionRawServer::CurrentItemChangedHandle MissionRawServerImpl::subscribe_current_item_changed( const MissionRawServer::CurrentItemChangedCallback& callback) { @@ -308,18 +343,6 @@ void MissionRawServerImpl::unsubscribe_clear_all(MissionRawServer::ClearAllHandl _clear_all_callbacks.unsubscribe(handle); } -uint32_t MissionRawServerImpl::clear_all() const -{ - // TO-DO - return {}; -} - -MissionRawServer::MissionItem MissionRawServerImpl::current_item_changed() const -{ - // TO-DO - return {}; -} - void MissionRawServerImpl::set_current_item_complete() { if (_current_seq + 1 > _current_mission.size()) { 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 1f48a75c17..d3fcf9eb65 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 @@ -32,17 +32,16 @@ class MissionRawServerImpl : public ServerPluginImplBase { void set_current_item_complete(); - MissionRawServer::MissionPlan incoming_mission() const; - MissionRawServer::MissionItem current_item_changed() const; - uint32_t clear_all() const; - private: void process_mission_count(const mavlink_message_t& message); + void process_mission_request_list(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 + _outgoing_mission_callbacks{}; CallbackList _current_item_changed_callbacks{}; CallbackList _clear_all_callbacks{}; std::atomic _target_system_id; @@ -54,6 +53,7 @@ class MissionRawServerImpl : public ServerPluginImplBase { std::size_t _current_seq; std::weak_ptr _last_download{}; + std::weak_ptr _last_upload{}; void set_current_seq(std::size_t seq); }; diff --git a/src/mavsdk_server/src/generated/mission_raw_server/mission_raw_server.pb.cc b/src/mavsdk_server/src/generated/mission_raw_server/mission_raw_server.pb.cc index bbca0db2ad..bd64e474ec 100644 --- a/src/mavsdk_server/src/generated/mission_raw_server/mission_raw_server.pb.cc +++ b/src/mavsdk_server/src/generated/mission_raw_server/mission_raw_server.pb.cc @@ -506,25 +506,25 @@ const char descriptor_table_protodef_mission_5fraw_5fserver_2fmission_5fraw_5fse "ULT_UNSUPPORTED\020\007\022\037\n\033RESULT_NO_MISSION_A" "VAILABLE\020\010\022\"\n\036RESULT_UNSUPPORTED_MISSION" "_CMD\020\013\022\035\n\031RESULT_TRANSFER_CANCELLED\020\014\022\024\n" - "\020RESULT_NO_SYSTEM\020\r\022\017\n\013RESULT_NEXT\020\0162\366\004\n" - "\027MissionRawServerService\022\226\001\n\030SubscribeIn" + "\020RESULT_NO_SYSTEM\020\r\022\017\n\013RESULT_NEXT\020\0162\202\005\n" + "\027MissionRawServerService\022\232\001\n\030SubscribeIn" "comingMission\022>.mavsdk.rpc.mission_raw_s" "erver.SubscribeIncomingMissionRequest\0326." "mavsdk.rpc.mission_raw_server.IncomingMi" - "ssionResponse\"\0000\001\022\237\001\n\033SubscribeCurrentIt" - "emChanged\022A.mavsdk.rpc.mission_raw_serve" - "r.SubscribeCurrentItemChangedRequest\0329.m" - "avsdk.rpc.mission_raw_server.CurrentItem" - "ChangedResponse\"\0000\001\022\233\001\n\026SetCurrentItemCo" - "mplete\022<.mavsdk.rpc.mission_raw_server.S" - "etCurrentItemCompleteRequest\032=.mavsdk.rp" - "c.mission_raw_server.SetCurrentItemCompl" - "eteResponse\"\004\200\265\030\001\022\201\001\n\021SubscribeClearAll\022" - "7.mavsdk.rpc.mission_raw_server.Subscrib" - "eClearAllRequest\032/.mavsdk.rpc.mission_ra" - "w_server.ClearAllResponse\"\0000\001B5\n\034io.mavs" - "dk.mission_raw_serverB\025MissionRawServerP" - "rotob\006proto3" + "ssionResponse\"\004\200\265\030\0000\001\022\243\001\n\033SubscribeCurre" + "ntItemChanged\022A.mavsdk.rpc.mission_raw_s" + "erver.SubscribeCurrentItemChangedRequest" + "\0329.mavsdk.rpc.mission_raw_server.Current" + "ItemChangedResponse\"\004\200\265\030\0000\001\022\233\001\n\026SetCurre" + "ntItemComplete\022<.mavsdk.rpc.mission_raw_" + "server.SetCurrentItemCompleteRequest\032=.m" + "avsdk.rpc.mission_raw_server.SetCurrentI" + "temCompleteResponse\"\004\200\265\030\001\022\205\001\n\021SubscribeC" + "learAll\0227.mavsdk.rpc.mission_raw_server." + "SubscribeClearAllRequest\032/.mavsdk.rpc.mi" + "ssion_raw_server.ClearAllResponse\"\004\200\265\030\0000" + "\001B5\n\034io.mavsdk.mission_raw_serverB\025Missi" + "onRawServerProtob\006proto3" }; static const ::_pbi::DescriptorTable* const descriptor_table_mission_5fraw_5fserver_2fmission_5fraw_5fserver_2eproto_deps[1] = { @@ -534,7 +534,7 @@ static ::absl::once_flag descriptor_table_mission_5fraw_5fserver_2fmission_5fraw PROTOBUF_CONSTINIT const ::_pbi::DescriptorTable descriptor_table_mission_5fraw_5fserver_2fmission_5fraw_5fserver_2eproto = { false, false, - 2092, + 2104, descriptor_table_protodef_mission_5fraw_5fserver_2fmission_5fraw_5fserver_2eproto, "mission_raw_server/mission_raw_server.proto", &descriptor_table_mission_5fraw_5fserver_2fmission_5fraw_5fserver_2eproto_once,