Skip to content

Commit

Permalink
Support Mission Download In Mission Raw Server (#2463)
Browse files Browse the repository at this point in the history 
* Add mission download support (air to ground) in mission raw server

* Removed unused private variable

* Point proto back to mavlink root

---------

Co-authored-by: Jon Reeves <[email protected]>
  • Loading branch information
@jonathanreeves
jonathanreeves and Jon Reeves authored Dec 18, 2024
1 parent f09fa8d commit c7b7bd4
Show file tree
Hide file tree
Showing 9 changed files with 1,398 additions and 209 deletions.
2 changes: 1 addition & 1 deletion proto
Submodule proto updated 1 files
+3 −3 protos/mission_raw_server/mission_raw_server.proto
366 changes: 357 additions & 9 deletions src/mavsdk/core/mavlink_mission_transfer_server.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<ReceiveIncomingMission>(
_sender,
_message_handler,
Expand All @@ -56,6 +48,31 @@ MavlinkMissionTransferServer::receive_incoming_items_async(
return std::weak_ptr<WorkItem>(ptr);
}

std::weak_ptr<MavlinkMissionTransferServer::WorkItem>
MavlinkMissionTransferServer::send_outgoing_items_async(
uint8_t type,
const std::vector<ItemInt>& items,
uint8_t target_system,
uint8_t target_component,
ResultCallback callback)
{
auto ptr = std::make_shared<SendOutgoingMission>(
_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<WorkItem>(ptr);
}

void MavlinkMissionTransferServer::do_work()
{
LockedQueue<WorkItem>::Guard work_queue_guard(_work_queue);
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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<ItemInt>& 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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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<int>(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<std::mutex> 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
Loading

0 comments on commit c7b7bd4

Please sign in to comment.