12401616260

Committed 18 Dec 2024 09:26PM UTC coverage: 44.627% (+1.0%) from 43.59%

Build # 12401616260

Build Type

push

github

Committed by

web-flow

Commit Message

Support Mission Download In Mission Raw Server (#2463)

* 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 <jon.reeves@elroyair.com>

Run Details

667 of 723 new or added lines in 3 files covered. (92.25%)

3 existing lines in 3 files now uncovered.

14584 of 32680 relevant lines covered (44.63%)

284.67 hits per line

Source File
Press 'n' to go to next uncovered line, 'b' for previous

91.71

/src/mavsdk/core/mavlink_mission_transfer_server.cpp

#include <algorithm>
#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::WorkItem>
MavlinkMissionTransferServer::receive_incoming_items_async(
    uint8_t type,
    uint32_t mission_count,
    uint8_t target_system,
    uint8_t target_component,
    ResultAndItemsCallback callback)
{
    auto ptr = std::make_shared<ReceiveIncomingMission>(
        _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<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);
    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<WorkItem>::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<std::mutex> lock(_mutex);
    return _started;
}

bool MavlinkMissionTransferServer::WorkItem::is_done()
{
    std::lock_guard<std::mutex> 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<std::mutex> lock(_mutex);

    _items.clear();

    _started = true;
    _retries_done = 0;
    _cookie = _timeout_handler.add([this]() { process_timeout(); }, _timeout_s);
    process_mission_count();
}

void MavlinkMissionTransferServer::ReceiveIncomingMission::cancel()
{
    std::lock_guard<std::mutex> 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,
                0);
            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,
                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::ReceiveIncomingMission::process_mission_count()
{
    if (_mission_count == 0) {
        send_ack_and_finish();
        _timeout_handler.remove(_cookie);
        return;
    }

    _timeout_handler.refresh(_cookie);
    _next_sequence = 0;
    _retries_done = 0;
    _expected_count = _mission_count;
    request_item();
}

void MavlinkMissionTransferServer::ReceiveIncomingMission::process_mission_item_int(
    const mavlink_message_t& message)
{
    std::lock_guard<std::mutex> 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<std::mutex> lock(_mutex);

    if (_retries_done >= retries) {
        callback_and_reset(Result::Timeout);
        return;
    }

    _cookie = _timeout_handler.add([this]() { process_timeout(); }, _timeout_s);
    request_item();
}

void MavlinkMissionTransferServer::ReceiveIncomingMission::callback_and_reset(Result result)
{
    if (_callback) {
        _callback(result, _items);
    }
    _callback = nullptr;
    _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

1	#include <algorithm>
2	#include "mavlink_mission_transfer_server.h"
3	#include "log.h"
4	#include "unused.h"
5
6	namespace mavsdk {
7
8	MavlinkMissionTransferServer::MavlinkMissionTransferServer(	123✔
9	Sender& sender,
10	MavlinkMessageHandler& message_handler,
11	TimeoutHandler& timeout_handler,
12	TimeoutSCallback timeout_s_callback) :	123✔
13	_sender(sender),	123✔
14	_message_handler(message_handler),	123✔
15	_timeout_handler(timeout_handler),	123✔
16	_timeout_s_callback(std::move(timeout_s_callback))	123✔
17	{
18	if (const char* env_p = std::getenv("MAVSDK_MISSION_TRANSFER_DEBUGGING")) {	123✔
19	if (std::string(env_p) == "1") {	×
20	LogDebug() << "Mission transfer debugging is on.";	×
21	_debugging = true;	×
22	}
23	}
24	}	123✔
25
26	std::weak_ptr<MavlinkMissionTransferServer::WorkItem>
27	MavlinkMissionTransferServer::receive_incoming_items_async(	6✔
28	uint8_t type,
29	uint32_t mission_count,
30	uint8_t target_system,
31	uint8_t target_component,
32	ResultAndItemsCallback callback)
33	{
34	auto ptr = std::make_shared<ReceiveIncomingMission>(	6✔
35	_sender,
36	_message_handler,
37	_timeout_handler,
38	type,
39	_timeout_s_callback(),	12✔
40	callback,
41	mission_count,
42	target_system,
43	target_component,
44	_debugging);	12✔
45
46	_work_queue.push_back(ptr);	6✔
47
48	return std::weak_ptr<WorkItem>(ptr);	6✔
49	}	6✔
50
51	std::weak_ptr<MavlinkMissionTransferServer::WorkItem>
52	MavlinkMissionTransferServer::send_outgoing_items_async(	31✔
53	uint8_t type,
54	const std::vector<ItemInt>& items,
55	uint8_t target_system,
56	uint8_t target_component,
57	ResultCallback callback)
58	{
59	auto ptr = std::make_shared<SendOutgoingMission>(	31✔
60	_sender,
61	_message_handler,
62	_timeout_handler,
63	type,
64	items,
65	_timeout_s_callback(),	62✔
66	callback,
67	target_system,
68	target_component,
69	_debugging);	62✔
70
71	_work_queue.push_back(ptr);	31✔
72
73	return std::weak_ptr<WorkItem>(ptr);	31✔
74	}	31✔
75
76	void MavlinkMissionTransferServer::do_work()	20,171✔
77	{
78	LockedQueue<WorkItem>::Guard work_queue_guard(_work_queue);	20,171✔
79	auto work = work_queue_guard.get_front();	19,724✔
80
81	if (!work) {	19,896✔
82	return;	19,360✔
83	}
84
85	if (!work->has_started()) {	65✔
86	work->start();	37✔
87	}
88	if (work->is_done()) {	65✔
89	work_queue_guard.pop_front();	33✔
90	}
91	}	39,054✔
92
93	bool MavlinkMissionTransferServer::is_idle()	31✔
94	{
95	LockedQueue<WorkItem>::Guard work_queue_guard(_work_queue);	31✔
96	return (work_queue_guard.get_front() == nullptr);	31✔
97	}	31✔
98
99	MavlinkMissionTransferServer::WorkItem::WorkItem(	37✔
100	Sender& sender,
101	MavlinkMessageHandler& message_handler,
102	TimeoutHandler& timeout_handler,
103	uint8_t type,
104	double timeout_s,
105	bool debugging) :	37✔
106	_sender(sender),	37✔
107	_message_handler(message_handler),	37✔
108	_timeout_handler(timeout_handler),	37✔
109	_type(type),	37✔
110	_timeout_s(timeout_s),	37✔
111	_debugging(debugging)	37✔
112	{}	37✔
113
114	MavlinkMissionTransferServer::WorkItem::~WorkItem() {}	37✔
115
116	bool MavlinkMissionTransferServer::WorkItem::has_started()	65✔
117	{
118	std::lock_guard<std::mutex> lock(_mutex);	65✔
119	return _started;	130✔
120	}	65✔
121
122	bool MavlinkMissionTransferServer::WorkItem::is_done()	65✔
123	{
124	std::lock_guard<std::mutex> lock(_mutex);	65✔
125	return _done;	130✔
126	}	65✔
127
128	MavlinkMissionTransferServer::ReceiveIncomingMission::ReceiveIncomingMission(	6✔
129	Sender& sender,
130	MavlinkMessageHandler& message_handler,
131	TimeoutHandler& timeout_handler,
132	uint8_t type,
133	double timeout_s,
134	ResultAndItemsCallback callback,
135	uint32_t mission_count,
136	uint8_t target_system_id,
137	uint8_t target_component_id,
138	bool debugging) :	6✔
139	WorkItem(sender, message_handler, timeout_handler, type, timeout_s, debugging),
140	_callback(callback),	6✔
141	_mission_count(mission_count),	6✔
142	_target_system_id(target_system_id),	6✔
143	_target_component_id(target_component_id)	6✔
144	{}	6✔
145
146	MavlinkMissionTransferServer::ReceiveIncomingMission::~ReceiveIncomingMission()	6✔
147	{
148	_message_handler.unregister_all(this);	6✔
149	_timeout_handler.remove(_cookie);	6✔
150	}	6✔
151
152	void MavlinkMissionTransferServer::ReceiveIncomingMission::start()	6✔
153	{
154	_message_handler.register_one(	6✔
155	MAVLINK_MSG_ID_MISSION_ITEM_INT,
156	[this](const mavlink_message_t& message) { process_mission_item_int(message); },	8✔
157	this);
158
159	std::lock_guard<std::mutex> lock(_mutex);	6✔
160
161	_items.clear();	6✔
162
163	_started = true;	6✔
164	_retries_done = 0;	6✔
165	_cookie = _timeout_handler.add([this]() { process_timeout(); }, _timeout_s);	8✔
166	process_mission_count();	6✔
167	}	6✔
168
169	void MavlinkMissionTransferServer::ReceiveIncomingMission::cancel()	1✔
170	{
171	std::lock_guard<std::mutex> lock(_mutex);	1✔
172
173	_timeout_handler.remove(_cookie);	1✔
174	send_cancel_and_finish();	1✔
175	}	1✔
176
177	void MavlinkMissionTransferServer::ReceiveIncomingMission::request_item()	23✔
178	{
179	if (!_sender.queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	23✔
180	mavlink_message_t message;
181	mavlink_msg_mission_request_int_pack_chan(	21✔
182	mavlink_address.system_id,	21✔
183	mavlink_address.component_id,	21✔
184	channel,
185	&message,
186	_target_system_id,	21✔
187	_target_component_id,	21✔
188	_next_sequence,	21✔
189	_type);	21✔
190	return message;	21✔
191	})) {
192	_timeout_handler.remove(_cookie);	×
193	callback_and_reset(Result::ConnectionError);	×
194	return;	×
195	}
196
197	++_retries_done;	23✔
198	}
199
200	void MavlinkMissionTransferServer::ReceiveIncomingMission::send_ack_and_finish()	2✔
201	{
202	if (!_sender.queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	2✔
203	mavlink_message_t message;
204	mavlink_msg_mission_ack_pack_chan(	2✔
205	mavlink_address.system_id,	2✔
206	mavlink_address.component_id,	2✔
207	channel,
208	&message,
209	_target_system_id,	2✔
210	_target_component_id,	2✔
211	MAV_MISSION_ACCEPTED,
212	_type,	2✔
213	0);
214	return message;	2✔
215	})) {
216	callback_and_reset(Result::ConnectionError);	×
217	return;	×
218	}
219
220	// We do not wait on anything coming back after this.
221	callback_and_reset(Result::Success);	2✔
222	}
223
224	void MavlinkMissionTransferServer::ReceiveIncomingMission::send_cancel_and_finish()	1✔
225	{
226	if (!_sender.queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	1✔
227	mavlink_message_t message;
228	mavlink_msg_mission_ack_pack_chan(	1✔
229	mavlink_address.system_id,	1✔
230	mavlink_address.component_id,	1✔
231	channel,
232	&message,
233	_target_system_id,	1✔
234	_target_component_id,	1✔
235	MAV_MISSION_OPERATION_CANCELLED,
236	_type,	1✔
237	0);
238	return message;	1✔
239	})) {
240	callback_and_reset(Result::ConnectionError);	×
241	return;	×
242	}
243
244	// We do not wait on anything coming back after this.
245	callback_and_reset(Result::Cancelled);	1✔
246	}
247
248	void MavlinkMissionTransferServer::ReceiveIncomingMission::process_mission_count()	6✔
249	{
250	if (_mission_count == 0) {	6✔
251	send_ack_and_finish();	1✔
252	_timeout_handler.remove(_cookie);	1✔
253	return;	1✔
254	}
255
256	_timeout_handler.refresh(_cookie);	5✔
257	_next_sequence = 0;	5✔
258	_retries_done = 0;	5✔
259	_expected_count = _mission_count;	5✔
260	request_item();	5✔
261	}
262
263	void MavlinkMissionTransferServer::ReceiveIncomingMission::process_mission_item_int(	8✔
264	const mavlink_message_t& message)
265	{
266	std::lock_guard<std::mutex> lock(_mutex);	8✔
267	_timeout_handler.refresh(_cookie);	8✔
268
269	mavlink_mission_item_int_t item_int;	8✔
270	mavlink_msg_mission_item_int_decode(&message, &item_int);	8✔
271
272	_items.push_back(ItemInt{	16✔
273	item_int.seq,	8✔
274	item_int.frame,	8✔
275	item_int.command,	8✔
276	item_int.current,	8✔
277	item_int.autocontinue,	8✔
278	item_int.param1,	8✔
279	item_int.param2,	8✔
280	item_int.param3,	8✔
281	item_int.param4,	8✔
282	item_int.x,	8✔
283	item_int.y,	8✔
284	item_int.z,	8✔
285	item_int.mission_type});	8✔
286
287	if (_next_sequence + 1 == _expected_count) {	8✔
288	_timeout_handler.remove(_cookie);	1✔
289	send_ack_and_finish();	1✔
290
291	} else {
292	_next_sequence = item_int.seq + 1;	7✔
293	_retries_done = 0;	7✔
294	request_item();	7✔
295	}
296	}	8✔
297
298	void MavlinkMissionTransferServer::ReceiveIncomingMission::process_timeout()	13✔
299	{
300	std::lock_guard<std::mutex> lock(_mutex);	13✔
301
302	if (_retries_done >= retries) {	13✔
303	callback_and_reset(Result::Timeout);	2✔
304	return;	2✔
305	}
306
307	_cookie = _timeout_handler.add([this]() { process_timeout(); }, _timeout_s);	22✔
308	request_item();	11✔
309	}	13✔
310
311	void MavlinkMissionTransferServer::ReceiveIncomingMission::callback_and_reset(Result result)	5✔
312	{
313	if (_callback) {	5✔
314	_callback(result, _items);	5✔
315	}
316	_callback = nullptr;	5✔
317	_done = true;	5✔
318	}	5✔
319
320	MavlinkMissionTransferServer::SendOutgoingMission::SendOutgoingMission(	31✔
321	Sender& sender,
322	MavlinkMessageHandler& message_handler,
323	TimeoutHandler& timeout_handler,
324	uint8_t type,
325	const std::vector<ItemInt>& items,
326	double timeout_s,
327	ResultCallback callback,
328	uint8_t target_system_id,
329	uint8_t target_component_id,
330	bool debugging) :	31✔
331	WorkItem(sender, message_handler, timeout_handler, type, timeout_s, debugging),
332	_items(items),	31✔
333	_callback(callback),	31✔
334	_target_system_id(target_system_id),	31✔
335	_target_component_id(target_component_id)	31✔
336	{
337	_message_handler.register_one(	31✔
338	MAVLINK_MSG_ID_MISSION_REQUEST,
339	[this](const mavlink_message_t& message) { process_mission_request(message); },	1✔
340	this);
341
342	_message_handler.register_one(	31✔
343	MAVLINK_MSG_ID_MISSION_REQUEST_INT,
344	[this](const mavlink_message_t& message) { process_mission_request_int(message); },	33✔
345	this);
346
347	_message_handler.register_one(	31✔
348	MAVLINK_MSG_ID_MISSION_ACK,
349	[this](const mavlink_message_t& message) { process_mission_ack(message); },	22✔
350	this);
351	}	31✔
352
353	MavlinkMissionTransferServer::SendOutgoingMission::~SendOutgoingMission()	31✔
354	{
355	_message_handler.unregister_all(this);	31✔
356	_timeout_handler.remove(_cookie);	31✔
357	}	31✔
358
359	void MavlinkMissionTransferServer::SendOutgoingMission::start()	31✔
360	{
361	std::lock_guard<std::mutex> lock(_mutex);	31✔
362
363	_started = true;	31✔
364	_retries_done = 0;	31✔
365	_step = Step::SendCount;	31✔
366	_cookie = _timeout_handler.add([this]() { process_timeout(); }, _timeout_s);	34✔
367
368	_next_sequence = 0;	31✔
369
370	send_count();	31✔
371	}	31✔
372
373	void MavlinkMissionTransferServer::SendOutgoingMission::cancel()	1✔
374	{
375	std::lock_guard<std::mutex> lock(_mutex);	1✔
376
377	_timeout_handler.remove(_cookie);	1✔
378	send_cancel_and_finish();	1✔
379	}	1✔
380
381	void MavlinkMissionTransferServer::SendOutgoingMission::send_count()	36✔
382	{
383	if (!_sender.queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	36✔
384	mavlink_message_t message;
385	mavlink_msg_mission_count_pack_chan(	16✔
386	mavlink_address.system_id,	8✔
387	mavlink_address.component_id,	8✔
388	channel,
389	&message,
390	_target_system_id,	16✔
391	_target_component_id,	8✔
392	_items.size(),	8✔
393	_type,	8✔
394	0);
395	return message;	8✔
396	})) {
397	_timeout_handler.remove(_cookie);	4✔
398	callback_and_reset(Result::ConnectionError);	4✔
399	return;	4✔
400	}
401
402	if (_debugging) {	32✔
NEW 403	LogDebug() << "Sending send_count, count: " << _items.size()	×
NEW 404	<< ", retries: " << _retries_done;	×
405	}
406
407	++_retries_done;	32✔
408	}
409
410	void MavlinkMissionTransferServer::SendOutgoingMission::send_cancel_and_finish()	1✔
411	{
412	if (!_sender.queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	1✔
413	mavlink_message_t message;
414	mavlink_msg_mission_ack_pack_chan(	1✔
415	mavlink_address.system_id,	1✔
416	mavlink_address.component_id,	1✔
417	channel,
418	&message,
419	_target_system_id,	1✔
420	_target_component_id,	1✔
421	MAV_MISSION_OPERATION_CANCELLED,
422	_type,	1✔
423	0);
424	return message;	1✔
425	})) {
NEW 426	callback_and_reset(Result::ConnectionError);	×
NEW 427	return;	×
428	}
429
430	// We do not wait on anything coming back after this.
431	callback_and_reset(Result::Cancelled);	1✔
432	}
433
434	void MavlinkMissionTransferServer::SendOutgoingMission::process_mission_request(	1✔
435	const mavlink_message_t& request_message)
436	{
437	mavlink_mission_request_t request;	1✔
438	mavlink_msg_mission_request_decode(&request_message, &request);	1✔
439
440	std::lock_guard<std::mutex> lock(_mutex);	1✔
441
442	// We only support int, so we nack this and thus tell the autopilot to use int.
443	if (!_sender.queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	1✔
444	mavlink_message_t message;
445	mavlink_msg_mission_ack_pack_chan(	1✔
446	mavlink_address.system_id,	1✔
447	mavlink_address.component_id,	1✔
448	channel,
449	&message,
450	request_message.sysid,	1✔
451	request_message.compid,	1✔
452	MAV_MISSION_UNSUPPORTED,
453	_type,	1✔
454	0);
455	return message;	1✔
456	})) {
NEW 457	_timeout_handler.remove(_cookie);	×
NEW 458	callback_and_reset(Result::ConnectionError);	×
NEW 459	return;	×
460	}
461	_timeout_handler.refresh(_cookie);	1✔
462	}	1✔
463
464	void MavlinkMissionTransferServer::SendOutgoingMission::process_mission_request_int(	33✔
465	const mavlink_message_t& message)
466	{
467	std::lock_guard<std::mutex> lock(_mutex);	33✔
468
469	mavlink_mission_request_int_t request_int;	33✔
470	mavlink_msg_mission_request_int_decode(&message, &request_int);	33✔
471
472	_step = Step::SendItems;	33✔
473
474	if (_debugging) {	33✔
NEW 475	LogDebug() << "Process mission_request_int, seq: " << request_int.seq	×
NEW 476	<< ", next expected sequence: " << _next_sequence;	×
477	}
478
479	if (_next_sequence < request_int.seq) {	33✔
480	// We should not go back to a previous one.
481	// TODO: figure out if we should error here.
NEW 482	LogWarn() << "mission_request_int: sequence incorrect";	×
NEW 483	return;	×
484
485	} else if (_next_sequence > request_int.seq) {	33✔
486	// We have already sent that one before.
487	if (_retries_done >= retries) {	6✔
488	LogWarn() << "mission_request_int: retries exceeded";	1✔
489	_timeout_handler.remove(_cookie);	1✔
490	callback_and_reset(Result::Timeout);	1✔
491	return;	1✔
492	}
493
494	} else {
495	// Correct one, sending it the first time.
496	_retries_done = 0;	27✔
497	}
498
499	_timeout_handler.refresh(_cookie);	32✔
500
501	_next_sequence = request_int.seq;	32✔
502
503	send_mission_item();	32✔
504	}	33✔
505
506	void MavlinkMissionTransferServer::SendOutgoingMission::send_mission_item()	32✔
507	{
508	if (_next_sequence >= _items.size()) {	32✔
NEW 509	LogErr() << "send_mission_item: sequence out of bounds";	×
NEW 510	return;	×
511	}
512
513	if (_debugging) {	32✔
NEW 514	LogDebug() << "Sending mission_item_int seq: " << _next_sequence	×
NEW 515	<< ", retry: " << _retries_done;	×
516	}
517
518	if (!_sender.queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	32✔
519	mavlink_message_t message;
520	mavlink_msg_mission_item_int_pack_chan(	372✔
521	mavlink_address.system_id,	31✔
522	mavlink_address.component_id,	31✔
523	channel,
524	&message,
525	_target_system_id,	372✔
526	_target_component_id,	31✔
527	_next_sequence,	31✔
528	_items[_next_sequence].frame,	31✔
529	_items[_next_sequence].command,	31✔
530	_items[_next_sequence].current,	31✔
531	_items[_next_sequence].autocontinue,	31✔
532	_items[_next_sequence].param1,	31✔
533	_items[_next_sequence].param2,	31✔
534	_items[_next_sequence].param3,	31✔
535	_items[_next_sequence].param4,	31✔
536	_items[_next_sequence].x,	31✔
537	_items[_next_sequence].y,	31✔
538	_items[_next_sequence].z,	31✔
539	_type);	31✔
540	return message;	31✔
541	})) {
NEW 542	_timeout_handler.remove(_cookie);	×
NEW 543	callback_and_reset(Result::ConnectionError);	×
NEW 544	return;	×
545	}
546
547	++_next_sequence;	32✔
548
549	++_retries_done;	32✔
550	}
551
552	void MavlinkMissionTransferServer::SendOutgoingMission::process_mission_ack(	22✔
553	const mavlink_message_t& message)
554	{
555	std::lock_guard<std::mutex> lock(_mutex);	22✔
556
557	mavlink_mission_ack_t mission_ack;	22✔
558	mavlink_msg_mission_ack_decode(&message, &mission_ack);	22✔
559
560	if (_debugging) {	22✔
NEW 561	LogDebug() << "Received mission_ack type: " << static_cast<int>(mission_ack.type);	×
562	}
563
564	_timeout_handler.remove(_cookie);	22✔
565
566	switch (mission_ack.type) {	22✔
567	case MAV_MISSION_ERROR:	1✔
568	callback_and_reset(Result::ProtocolError);	1✔
569	return;	1✔
570	case MAV_MISSION_UNSUPPORTED_FRAME:	1✔
571	callback_and_reset(Result::UnsupportedFrame);	1✔
572	return;	1✔
573	case MAV_MISSION_UNSUPPORTED:	1✔
574	callback_and_reset(Result::Unsupported);	1✔
575	return;	1✔
576	case MAV_MISSION_NO_SPACE:	1✔
577	callback_and_reset(Result::TooManyMissionItems);	1✔
578	return;	1✔
579	case MAV_MISSION_INVALID:	8✔
580	// FALLTHROUGH
581	case MAV_MISSION_INVALID_PARAM1:
582	// FALLTHROUGH
583	case MAV_MISSION_INVALID_PARAM2:
584	// FALLTHROUGH
585	case MAV_MISSION_INVALID_PARAM3:
586	// FALLTHROUGH
587	case MAV_MISSION_INVALID_PARAM4:
588	// FALLTHROUGH
589	case MAV_MISSION_INVALID_PARAM5_X:
590	// FALLTHROUGH
591	case MAV_MISSION_INVALID_PARAM6_Y:
592	// FALLTHROUGH
593	case MAV_MISSION_INVALID_PARAM7:
594	callback_and_reset(Result::InvalidParam);	8✔
595	return;	8✔
596	case MAV_MISSION_INVALID_SEQUENCE:	1✔
597	callback_and_reset(Result::InvalidSequence);	1✔
598	return;	1✔
599	case MAV_MISSION_DENIED:	1✔
600	callback_and_reset(Result::Denied);	1✔
601	return;	1✔
602	case MAV_MISSION_OPERATION_CANCELLED:	1✔
603	callback_and_reset(Result::Cancelled);	1✔
604	return;	1✔
605	}
606
607	if (_next_sequence == _items.size()) {	7✔
608	callback_and_reset(Result::Success);	5✔
609	} else {
610	callback_and_reset(Result::ProtocolError);	2✔
611	}
612	}	22✔
613
614	void MavlinkMissionTransferServer::SendOutgoingMission::process_timeout()	11✔
615	{
616	std::lock_guard<std::mutex> lock(_mutex);	11✔
617
618	if (_debugging) {	11✔
NEW 619	LogDebug() << "Timeout triggered, retries: " << _retries_done;	×
620	}
621
622	if (_retries_done >= retries) {	11✔
623	LogWarn() << "timeout: retries exceeded";	2✔
624	callback_and_reset(Result::Timeout);	2✔
625	return;	2✔
626	}
627
628	switch (_step) {	9✔
629	case Step::SendCount:	5✔
630	_cookie = _timeout_handler.add([this]() { process_timeout(); }, _timeout_s);	9✔
631	send_count();	5✔
632	break;	5✔
633
634	case Step::SendItems:	4✔
635	// When waiting for items requested we should wait longer than
636	// just our timeout, otherwise we give up too quickly.
637	++_retries_done;	4✔
638	_cookie = _timeout_handler.add([this]() { process_timeout(); }, _timeout_s);	8✔
639	break;	4✔
640	}
641	}	11✔
642
643	void MavlinkMissionTransferServer::SendOutgoingMission::callback_and_reset(Result result)	30✔
644	{
645	if (_callback) {	30✔
646	_callback(result);	26✔
647	}
648	_callback = nullptr;	30✔
649	_done = true;	30✔
650	}	30✔
651
652	} // namespace mavsdk

mavlink / MAVSDK / 12401616260

Source File Press 'n' to go to next uncovered line, 'b' for previous

Source File
Press 'n' to go to next uncovered line, 'b' for previous