16665088909

Committed 01 Aug 2025 02:55AM UTC coverage: 46.166% (-0.1%) from 46.31%

Build # 16665088909

Build Type

push

github

Committed by

web-flow

Commit Message

Merge pull request #2630 from mavlink/pr-segfault-fixes

Stack-use-after-free and thread-safety fixes, CI additions, clang-format-19

Run Details

241 of 320 new or added lines in 32 files covered. (75.31%)

39 existing lines in 10 files now uncovered.

16101 of 34876 relevant lines covered (46.17%)

361.1 hits per line

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

32.13

/src/mavsdk/plugins/mission_raw_server/mission_raw_server_impl.cpp

#include "mission_raw_server_impl.h"
#include "callback_list.tpp"
#include "mavlink_address.h"

namespace mavsdk {

template class CallbackList<MissionRawServer::Result, MissionRawServer::MissionPlan>;
template class CallbackList<MissionRawServer::MissionItem>;
template class CallbackList<uint32_t>;

MissionRawServerImpl::MissionRawServerImpl(std::shared_ptr<ServerComponent> server_component) :
    ServerPluginImplBase(server_component)
{
    _server_component_impl->register_plugin(this);
}

MissionRawServerImpl::~MissionRawServerImpl()
{
    _server_component_impl->unregister_plugin(this);
}

MavlinkMissionTransferServer::ItemInt
convert_mission_raw(const MissionRawServer::MissionItem transfer_mission_raw)
{
    MavlinkMissionTransferServer::ItemInt new_item_int{};

    new_item_int.seq = transfer_mission_raw.seq;
    new_item_int.frame = transfer_mission_raw.frame;
    new_item_int.command = transfer_mission_raw.command;
    new_item_int.current = transfer_mission_raw.current;
    new_item_int.autocontinue = transfer_mission_raw.autocontinue;
    new_item_int.param1 = transfer_mission_raw.param1;
    new_item_int.param2 = transfer_mission_raw.param2;
    new_item_int.param3 = transfer_mission_raw.param3;
    new_item_int.param4 = transfer_mission_raw.param4;
    new_item_int.x = transfer_mission_raw.x;
    new_item_int.y = transfer_mission_raw.y;
    new_item_int.z = transfer_mission_raw.z;
    new_item_int.mission_type = transfer_mission_raw.mission_type;

    return new_item_int;
}

std::vector<MavlinkMissionTransferServer::ItemInt>
convert_to_int_items(const std::vector<MissionRawServer::MissionItem>& mission_raw)
{
    std::vector<MavlinkMissionTransferServer::ItemInt> int_items;

    for (const auto& item : mission_raw) {
        int_items.push_back(convert_mission_raw(item));
    }

    return int_items;
}

MissionRawServer::MissionItem
convert_item(const MavlinkMissionTransferServer::ItemInt& transfer_item)
{
    MissionRawServer::MissionItem new_item;

    new_item.seq = transfer_item.seq;
    new_item.frame = transfer_item.frame;
    new_item.command = transfer_item.command;
    new_item.current = transfer_item.current;
    new_item.autocontinue = transfer_item.autocontinue;
    new_item.param1 = transfer_item.param1;
    new_item.param2 = transfer_item.param2;
    new_item.param3 = transfer_item.param3;
    new_item.param4 = transfer_item.param4;
    new_item.x = transfer_item.x;
    new_item.y = transfer_item.y;
    new_item.z = transfer_item.z;
    new_item.mission_type = transfer_item.mission_type;

    return new_item;
}

std::vector<MissionRawServer::MissionItem>
convert_items(const std::vector<MavlinkMissionTransferServer::ItemInt>& transfer_items)
{
    std::vector<MissionRawServer::MissionItem> new_items;
    new_items.reserve(transfer_items.size());

    for (const auto& transfer_item : transfer_items) {
        new_items.push_back(convert_item(transfer_item));
    }

    return new_items;
}

MissionRawServer::Result convert_result(MavlinkMissionTransferServer::Result result)
{
    switch (result) {
        case MavlinkMissionTransferServer::Result::Success:
            return MissionRawServer::Result::Success;
        case MavlinkMissionTransferServer::Result::ConnectionError:
            return MissionRawServer::Result::Error; // FIXME
        case MavlinkMissionTransferServer::Result::Denied:
            return MissionRawServer::Result::Error; // FIXME
        case MavlinkMissionTransferServer::Result::TooManyMissionItems:
            return MissionRawServer::Result::TooManyMissionItems;
        case MavlinkMissionTransferServer::Result::Timeout:
            return MissionRawServer::Result::Timeout;
        case MavlinkMissionTransferServer::Result::Unsupported:
            return MissionRawServer::Result::Unsupported;
        case MavlinkMissionTransferServer::Result::UnsupportedFrame:
            return MissionRawServer::Result::Unsupported;
        case MavlinkMissionTransferServer::Result::NoMissionAvailable:
            return MissionRawServer::Result::NoMissionAvailable;
        case MavlinkMissionTransferServer::Result::Cancelled:
            return MissionRawServer::Result::TransferCancelled;
        case MavlinkMissionTransferServer::Result::MissionTypeNotConsistent:
            return MissionRawServer::Result::InvalidArgument; // FIXME
        case MavlinkMissionTransferServer::Result::InvalidSequence:
            return MissionRawServer::Result::InvalidArgument; // FIXME
        case MavlinkMissionTransferServer::Result::CurrentInvalid:
            return MissionRawServer::Result::InvalidArgument; // FIXME
        case MavlinkMissionTransferServer::Result::ProtocolError:
            return MissionRawServer::Result::Error; // FIXME
        case MavlinkMissionTransferServer::Result::InvalidParam:
            return MissionRawServer::Result::InvalidArgument; // FIXME
        case MavlinkMissionTransferServer::Result::IntMessagesNotSupported:
            return MissionRawServer::Result::Unsupported; // FIXME
        default:
            return MissionRawServer::Result::Unknown;
    }
}

void MissionRawServerImpl::init()
{
    _server_component_impl->add_capabilities(
        MAV_PROTOCOL_CAPABILITY_MISSION_INT | MAV_PROTOCOL_CAPABILITY_MISSION_FENCE |
        MAV_PROTOCOL_CAPABILITY_MISSION_RALLY);

    // Handle Initiate Upload
    _server_component_impl->register_mavlink_message_handler(
        MAVLINK_MSG_ID_MISSION_COUNT,
        [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,
        [this](const mavlink_message_t& message) { process_mission_set_current(message); },
        this);

    // Handle Clears
    _server_component_impl->register_mavlink_message_handler(
        MAVLINK_MSG_ID_MISSION_CLEAR_ALL,
        [this](const mavlink_message_t& message) { process_mission_clear(message); },
        this);
}

void MissionRawServerImpl::deinit()
{
    _server_component_impl->unregister_all_mavlink_message_handlers(this);
}

void MissionRawServerImpl::process_mission_count(const mavlink_message_t& message)
{
    UNUSED(message);

    // Decode the count
    _target_system_id = message.sysid;
    _target_component_id = message.compid;

    mavlink_mission_count_t count;
    mavlink_msg_mission_count_decode(&message, &count);
    _mission_count = count.count;

    // Mission Upload Inbound
    if (_last_download.lock()) {
        _incoming_mission_callbacks.queue(
            MissionRawServer::Result::Busy,
            MissionRawServer::MissionPlan{},
            [this](const auto& func) { _server_component_impl->call_user_callback(func); });
        return;
    }

    _last_download = _server_component_impl->mission_transfer_server().receive_incoming_items_async(
        count.mission_type,
        _mission_count,
        _target_system_id,
        _target_component_id,
        [this](
            MavlinkMissionTransferServer::Result result,
            uint8_t type,
            std::vector<MavlinkMissionTransferServer::ItemInt> items) {
            get_mission_items(type) = items;
            auto converted_result = convert_result(result);
            auto converted_items = convert_items(items);
            _incoming_mission_callbacks.queue(
                converted_result, {converted_items}, [this](const auto& func) {
                    _server_component_impl->call_user_callback(func);
                });

            // Reset mission state after receiving because the previous mission is now inactive.
            if (type == MAV_MISSION_TYPE_MISSION) {
                _mission_completed = false;
                set_current_seq(0);
            }
        });
}

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;
    }

    auto& requested_mission_items = get_mission_items(mission_type.mission_type);

    _last_upload = _server_component_impl->mission_transfer_server().send_outgoing_items_async(
        mission_type.mission_type,
        requested_mission_items,
        _target_system_id,
        _target_component_id,
        [this, requested_mission_items](MavlinkMissionTransferServer::Result result) {
            auto converted_result = convert_result(result);
            auto converted_items = convert_items(requested_mission_items);
            _outgoing_mission_callbacks.queue(
                converted_result, {converted_items}, [this](const auto& func) {
                    _server_component_impl->call_user_callback(func);
                });
        });
}

void MissionRawServerImpl::process_mission_set_current(const mavlink_message_t& message)
{
    LogDebug() << "Receive Mission Set Current";

    mavlink_mission_set_current_t set_current;
    mavlink_msg_mission_set_current_decode(&message, &set_current);

    if (_current_mission.size() == 0) {
        const char text[50] = "No Mission Loaded";
        _server_component_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {
            mavlink_message_t response_message;
            mavlink_msg_statustext_pack_chan(
                mavlink_address.system_id,
                mavlink_address.component_id,
                channel,
                &response_message,
                MAV_SEVERITY_ERROR,
                text,
                0,
                0);
            return response_message;
        });
    } else if (_current_mission.size() <= set_current.seq) {
        const char text[50] = "Unknown Mission seq id";
        _server_component_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {
            mavlink_message_t response_message;
            mavlink_msg_statustext_pack_chan(
                mavlink_address.system_id,
                mavlink_address.component_id,
                channel,
                &response_message,
                MAV_SEVERITY_ERROR,
                text,
                0,
                0);
            return response_message;
        });
    } else {
        set_current_seq(set_current.seq);
    }
}

void MissionRawServerImpl::process_mission_clear(const mavlink_message_t message)
{
    mavlink_mission_clear_all_t clear_all;
    mavlink_msg_mission_clear_all_decode(&message, &clear_all);

    if (clear_all.mission_type == MAV_MISSION_TYPE_ALL ||
        clear_all.mission_type == MAV_MISSION_TYPE_MISSION) {
        _current_mission.clear();
        _current_seq = 0;
        _clear_all_callbacks.queue(clear_all.mission_type, [this](const auto& func) {
            _server_component_impl->call_user_callback(func);
        });
    }

    if (clear_all.mission_type == MAV_MISSION_TYPE_ALL ||
        clear_all.mission_type == MAV_MISSION_TYPE_FENCE) {
        _current_fence.clear();
        _clear_all_callbacks.queue(clear_all.mission_type, [this](const auto& func) {
            _server_component_impl->call_user_callback(func);
        });
    }

    if (clear_all.mission_type == MAV_MISSION_TYPE_ALL ||
        clear_all.mission_type == MAV_MISSION_TYPE_RALLY) {
        _current_rally.clear();
        _clear_all_callbacks.queue(clear_all.mission_type, [this](const auto& func) {
            _server_component_impl->call_user_callback(func);
        });
    }

    // Send the MISSION_ACK
    _server_component_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {
        mavlink_message_t response_message;
        mavlink_msg_mission_ack_pack_chan(
            mavlink_address.system_id,
            mavlink_address.component_id,
            channel,
            &response_message,
            message.sysid,
            message.compid,
            MAV_MISSION_RESULT::MAV_MISSION_ACCEPTED,
            clear_all.mission_type,
            0);
        return response_message;
    });
}

std::vector<MavlinkMissionTransferServer::ItemInt>&
MissionRawServerImpl::get_mission_items(const uint8_t mission_type)
{
    switch (mission_type) {
        case MAV_MISSION_TYPE_MISSION:
            return _current_mission;
        case MAV_MISSION_TYPE_FENCE:
            return _current_fence;
        case MAV_MISSION_TYPE_RALLY:
            return _current_rally;
        default:
            return _empty_mission_items;
    }
}

MissionRawServer::IncomingMissionHandle MissionRawServerImpl::subscribe_incoming_mission(
    const MissionRawServer::IncomingMissionCallback& callback)
{
    return _incoming_mission_callbacks.subscribe(callback);
}

void MissionRawServerImpl::unsubscribe_incoming_mission(
    MissionRawServer::IncomingMissionHandle handle)
{
    _incoming_mission_callbacks.unsubscribe(handle);
}

MissionRawServer::CurrentItemChangedHandle MissionRawServerImpl::subscribe_current_item_changed(
    const MissionRawServer::CurrentItemChangedCallback& callback)
{
    return _current_item_changed_callbacks.subscribe(callback);
}

void MissionRawServerImpl::unsubscribe_current_item_changed(
    MissionRawServer::CurrentItemChangedHandle handle)
{
    _current_item_changed_callbacks.unsubscribe(handle);
}

MissionRawServer::ClearAllHandle
MissionRawServerImpl::subscribe_clear_all(const MissionRawServer::ClearAllCallback& callback)
{
    return _clear_all_callbacks.subscribe(callback);
}

void MissionRawServerImpl::unsubscribe_clear_all(MissionRawServer::ClearAllHandle handle)
{
    _clear_all_callbacks.unsubscribe(handle);
}

void MissionRawServerImpl::set_current_item_complete()
{
    if (_current_seq + 1 > _current_mission.size()) {
        return;
    }

    _server_component_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {
        mavlink_message_t message;
        mavlink_msg_mission_item_reached_pack_chan(
            mavlink_address.system_id,
            mavlink_address.component_id,
            channel,
            &message,
            static_cast<uint16_t>(_current_seq));
        return message;
    });

    if (_current_seq + 1 == _current_mission.size()) {
        _mission_completed = true;
    }
    // This will publish 0 - N mission current
    // mission_current mission.size() is end of mission!
    set_current_seq(_current_seq + 1);
}

void MissionRawServerImpl::set_current_seq(std::size_t seq)
{
    if (_current_mission.size() < static_cast<size_t>(seq) || _current_mission.empty()) {
        return;
    }

    _current_seq = seq;

    // If mission is over, just set item to last one again
    auto item = seq == _current_mission.size() ? _current_mission.back() :
                                                 _current_mission.at(_current_seq);
    auto converted_item = convert_item(item);
    _current_item_changed_callbacks.queue(converted_item, [this](const auto& func) {
        _server_component_impl->call_user_callback(func);
    });

    _server_component_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {
        mavlink_message_t message;
        mavlink_msg_mission_current_pack_chan(
            mavlink_address.system_id,
            mavlink_address.component_id,
            channel,
            &message,
            static_cast<uint16_t>(_current_seq),
            0,
            0,
            0,
            0,
            0,
            0);
        return message;
    });
}

} // namespace mavsdk

1	#include "mission_raw_server_impl.h"
2	#include "callback_list.tpp"
3	#include "mavlink_address.h"
4
5	namespace mavsdk {
6
7	template class CallbackList<MissionRawServer::Result, MissionRawServer::MissionPlan>;
8	template class CallbackList<MissionRawServer::MissionItem>;
9	template class CallbackList<uint32_t>;
10
11	MissionRawServerImpl::MissionRawServerImpl(std::shared_ptr<ServerComponent> server_component) :	1✔
12	ServerPluginImplBase(server_component)	1✔
13	{
14	_server_component_impl->register_plugin(this);	1✔
15	}	1✔
16
17	MissionRawServerImpl::~MissionRawServerImpl()	2✔
18	{
19	_server_component_impl->unregister_plugin(this);	1✔
20	}	2✔
21
22	MavlinkMissionTransferServer::ItemInt
23	convert_mission_raw(const MissionRawServer::MissionItem transfer_mission_raw)	×
24	{
25	MavlinkMissionTransferServer::ItemInt new_item_int{};	×
26
27	new_item_int.seq = transfer_mission_raw.seq;	×
28	new_item_int.frame = transfer_mission_raw.frame;	×
29	new_item_int.command = transfer_mission_raw.command;	×
30	new_item_int.current = transfer_mission_raw.current;	×
31	new_item_int.autocontinue = transfer_mission_raw.autocontinue;	×
32	new_item_int.param1 = transfer_mission_raw.param1;	×
33	new_item_int.param2 = transfer_mission_raw.param2;	×
34	new_item_int.param3 = transfer_mission_raw.param3;	×
35	new_item_int.param4 = transfer_mission_raw.param4;	×
36	new_item_int.x = transfer_mission_raw.x;	×
37	new_item_int.y = transfer_mission_raw.y;	×
38	new_item_int.z = transfer_mission_raw.z;	×
39	new_item_int.mission_type = transfer_mission_raw.mission_type;	×
40
41	return new_item_int;	×
42	}
43
44	std::vector<MavlinkMissionTransferServer::ItemInt>
45	convert_to_int_items(const std::vector<MissionRawServer::MissionItem>& mission_raw)	×
46	{
47	std::vector<MavlinkMissionTransferServer::ItemInt> int_items;	×
48
49	for (const auto& item : mission_raw) {	×
50	int_items.push_back(convert_mission_raw(item));	×
51	}
52
53	return int_items;	×
54	}
55
56	MissionRawServer::MissionItem
57	convert_item(const MavlinkMissionTransferServer::ItemInt& transfer_item)	7✔
58	{
59	MissionRawServer::MissionItem new_item;	7✔
60
61	new_item.seq = transfer_item.seq;	7✔
62	new_item.frame = transfer_item.frame;	7✔
63	new_item.command = transfer_item.command;	7✔
64	new_item.current = transfer_item.current;	7✔
65	new_item.autocontinue = transfer_item.autocontinue;	7✔
66	new_item.param1 = transfer_item.param1;	7✔
67	new_item.param2 = transfer_item.param2;	7✔
68	new_item.param3 = transfer_item.param3;	7✔
69	new_item.param4 = transfer_item.param4;	7✔
70	new_item.x = transfer_item.x;	7✔
71	new_item.y = transfer_item.y;	7✔
72	new_item.z = transfer_item.z;	7✔
73	new_item.mission_type = transfer_item.mission_type;	7✔
74
75	return new_item;	7✔
76	}
77
78	std::vector<MissionRawServer::MissionItem>
79	convert_items(const std::vector<MavlinkMissionTransferServer::ItemInt>& transfer_items)	1✔
80	{
81	std::vector<MissionRawServer::MissionItem> new_items;	1✔
82	new_items.reserve(transfer_items.size());	1✔
83
84	for (const auto& transfer_item : transfer_items) {	7✔
85	new_items.push_back(convert_item(transfer_item));	6✔
86	}
87
88	return new_items;	1✔
89	}
90
91	MissionRawServer::Result convert_result(MavlinkMissionTransferServer::Result result)	1✔
92	{
93	switch (result) {	1✔
94	case MavlinkMissionTransferServer::Result::Success:	1✔
95	return MissionRawServer::Result::Success;	1✔
96	case MavlinkMissionTransferServer::Result::ConnectionError:	×
97	return MissionRawServer::Result::Error; // FIXME	×
98	case MavlinkMissionTransferServer::Result::Denied:	×
99	return MissionRawServer::Result::Error; // FIXME	×
100	case MavlinkMissionTransferServer::Result::TooManyMissionItems:	×
101	return MissionRawServer::Result::TooManyMissionItems;	×
102	case MavlinkMissionTransferServer::Result::Timeout:	×
103	return MissionRawServer::Result::Timeout;	×
104	case MavlinkMissionTransferServer::Result::Unsupported:	×
105	return MissionRawServer::Result::Unsupported;	×
106	case MavlinkMissionTransferServer::Result::UnsupportedFrame:	×
107	return MissionRawServer::Result::Unsupported;	×
108	case MavlinkMissionTransferServer::Result::NoMissionAvailable:	×
109	return MissionRawServer::Result::NoMissionAvailable;	×
110	case MavlinkMissionTransferServer::Result::Cancelled:	×
111	return MissionRawServer::Result::TransferCancelled;	×
112	case MavlinkMissionTransferServer::Result::MissionTypeNotConsistent:	×
113	return MissionRawServer::Result::InvalidArgument; // FIXME	×
114	case MavlinkMissionTransferServer::Result::InvalidSequence:	×
115	return MissionRawServer::Result::InvalidArgument; // FIXME	×
116	case MavlinkMissionTransferServer::Result::CurrentInvalid:	×
117	return MissionRawServer::Result::InvalidArgument; // FIXME	×
118	case MavlinkMissionTransferServer::Result::ProtocolError:	×
119	return MissionRawServer::Result::Error; // FIXME	×
120	case MavlinkMissionTransferServer::Result::InvalidParam:	×
121	return MissionRawServer::Result::InvalidArgument; // FIXME	×
122	case MavlinkMissionTransferServer::Result::IntMessagesNotSupported:	×
123	return MissionRawServer::Result::Unsupported; // FIXME	×
124	default:	×
125	return MissionRawServer::Result::Unknown;	×
126	}
127	}
128
129	void MissionRawServerImpl::init()	1✔
130	{
131	_server_component_impl->add_capabilities(	1✔
132	MAV_PROTOCOL_CAPABILITY_MISSION_INT \| MAV_PROTOCOL_CAPABILITY_MISSION_FENCE \|
133	MAV_PROTOCOL_CAPABILITY_MISSION_RALLY);
134
135	// Handle Initiate Upload
136	_server_component_impl->register_mavlink_message_handler(	1✔
137	MAVLINK_MSG_ID_MISSION_COUNT,
138	[this](const mavlink_message_t& message) { process_mission_count(message); },	1✔
139	this);
140
141	// Handle Initiate Download
142	_server_component_impl->register_mavlink_message_handler(	1✔
143	MAVLINK_MSG_ID_MISSION_REQUEST_LIST,
144	[this](const mavlink_message_t& message) { process_mission_request_list(message); },	×
145	this);
146
147	// Handle Set Current from GCS
148	_server_component_impl->register_mavlink_message_handler(	1✔
149	MAVLINK_MSG_ID_MISSION_SET_CURRENT,
150	[this](const mavlink_message_t& message) { process_mission_set_current(message); },	×
151	this);
152
153	// Handle Clears
154	_server_component_impl->register_mavlink_message_handler(	1✔
155	MAVLINK_MSG_ID_MISSION_CLEAR_ALL,
156	[this](const mavlink_message_t& message) { process_mission_clear(message); },	×
157	this);
158	}	1✔
159
160	void MissionRawServerImpl::deinit()	1✔
161	{
162	_server_component_impl->unregister_all_mavlink_message_handlers(this);	1✔
163	}	1✔
164
165	void MissionRawServerImpl::process_mission_count(const mavlink_message_t& message)	1✔
166	{
167	UNUSED(message);	1✔
168
169	// Decode the count
170	_target_system_id = message.sysid;	1✔
171	_target_component_id = message.compid;	1✔
172
173	mavlink_mission_count_t count;
174	mavlink_msg_mission_count_decode(&message, &count);	1✔
175	_mission_count = count.count;	1✔
176
177	// Mission Upload Inbound
178	if (_last_download.lock()) {	1✔
179	_incoming_mission_callbacks.queue(	×
180	MissionRawServer::Result::Busy,
181	MissionRawServer::MissionPlan{},	×
182	[this](const auto& func) { _server_component_impl->call_user_callback(func); });	×
183	return;	×
184	}
185
186	_last_download = _server_component_impl->mission_transfer_server().receive_incoming_items_async(	2✔
187	count.mission_type,	1✔
188	_mission_count,
189	_target_system_id,
190	_target_component_id,
191	[this](	1✔
192	MavlinkMissionTransferServer::Result result,
193	uint8_t type,
194	std::vector<MavlinkMissionTransferServer::ItemInt> items) {	4✔
195	get_mission_items(type) = items;	1✔
196	auto converted_result = convert_result(result);	1✔
197	auto converted_items = convert_items(items);	1✔
198	_incoming_mission_callbacks.queue(	1✔
199	converted_result, {converted_items}, [this](const auto& func) {	×
200	_server_component_impl->call_user_callback(func);	×
201	});	×
202
203	// Reset mission state after receiving because the previous mission is now inactive.
204	if (type == MAV_MISSION_TYPE_MISSION) {	1✔
205	_mission_completed = false;	1✔
206	set_current_seq(0);	1✔
207	}
208	});	2✔
209	}
210
211	void MissionRawServerImpl::process_mission_request_list(const mavlink_message_t& message)	×
212	{
213	UNUSED(message);	×
214
215	_target_system_id = message.sysid;	×
216	_target_component_id = message.compid;	×
217
218	mavlink_mission_request_list_t mission_type;
219	mavlink_msg_mission_request_list_decode(&message, &mission_type);	×
220
221	// Mission Download Outbound
222	if (_last_upload.lock()) {	×
223	_outgoing_mission_callbacks.queue(	×
224	MissionRawServer::Result::Busy,
225	MissionRawServer::MissionPlan{},	×
226	[this](const auto& func) { _server_component_impl->call_user_callback(func); });	×
227	return;	×
228	}
229
230	auto& requested_mission_items = get_mission_items(mission_type.mission_type);	×
231
232	_last_upload = _server_component_impl->mission_transfer_server().send_outgoing_items_async(	×
233	mission_type.mission_type,	×
234	requested_mission_items,
235	_target_system_id,
236	_target_component_id,
NEW 237	[this, requested_mission_items](MavlinkMissionTransferServer::Result result) {	×
238	auto converted_result = convert_result(result);	×
239	auto converted_items = convert_items(requested_mission_items);	×
240	_outgoing_mission_callbacks.queue(	×
241	converted_result, {converted_items}, [this](const auto& func) {	×
242	_server_component_impl->call_user_callback(func);	×
243	});	×
244	});	×
245	}
246
247	void MissionRawServerImpl::process_mission_set_current(const mavlink_message_t& message)	×
248	{
249	LogDebug() << "Receive Mission Set Current";	×
250
251	mavlink_mission_set_current_t set_current;
252	mavlink_msg_mission_set_current_decode(&message, &set_current);	×
253
254	if (_current_mission.size() == 0) {	×
255	const char text[50] = "No Mission Loaded";	×
256	_server_component_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	×
257	mavlink_message_t response_message;
258	mavlink_msg_statustext_pack_chan(	×
259	mavlink_address.system_id,	×
260	mavlink_address.component_id,	×
261	channel,
262	&response_message,
263	MAV_SEVERITY_ERROR,
264	text,	×
265	0,
266	0);
267	return response_message;	×
268	});
269	} else if (_current_mission.size() <= set_current.seq) {	×
270	const char text[50] = "Unknown Mission seq id";	×
271	_server_component_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	×
272	mavlink_message_t response_message;
273	mavlink_msg_statustext_pack_chan(	×
274	mavlink_address.system_id,	×
275	mavlink_address.component_id,	×
276	channel,
277	&response_message,
278	MAV_SEVERITY_ERROR,
279	text,	×
280	0,
281	0);
282	return response_message;	×
283	});
284	} else {
285	set_current_seq(set_current.seq);	×
286	}
287	}	×
288
289	void MissionRawServerImpl::process_mission_clear(const mavlink_message_t message)	×
290	{
291	mavlink_mission_clear_all_t clear_all;
292	mavlink_msg_mission_clear_all_decode(&message, &clear_all);	×
293
294	if (clear_all.mission_type == MAV_MISSION_TYPE_ALL \|\|	×
295	clear_all.mission_type == MAV_MISSION_TYPE_MISSION) {	×
296	_current_mission.clear();	×
297	_current_seq = 0;	×
298	_clear_all_callbacks.queue(clear_all.mission_type, [this](const auto& func) {	×
299	_server_component_impl->call_user_callback(func);	×
300	});	×
301	}
302
303	if (clear_all.mission_type == MAV_MISSION_TYPE_ALL \|\|	×
304	clear_all.mission_type == MAV_MISSION_TYPE_FENCE) {	×
305	_current_fence.clear();	×
306	_clear_all_callbacks.queue(clear_all.mission_type, [this](const auto& func) {	×
307	_server_component_impl->call_user_callback(func);	×
308	});	×
309	}
310
311	if (clear_all.mission_type == MAV_MISSION_TYPE_ALL \|\|	×
312	clear_all.mission_type == MAV_MISSION_TYPE_RALLY) {	×
313	_current_rally.clear();	×
314	_clear_all_callbacks.queue(clear_all.mission_type, [this](const auto& func) {	×
315	_server_component_impl->call_user_callback(func);	×
316	});	×
317	}
318
319	// Send the MISSION_ACK
320	_server_component_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	×
321	mavlink_message_t response_message;
322	mavlink_msg_mission_ack_pack_chan(	×
323	mavlink_address.system_id,	×
324	mavlink_address.component_id,	×
325	channel,
326	&response_message,
327	message.sysid,	×
328	message.compid,	×
329	MAV_MISSION_RESULT::MAV_MISSION_ACCEPTED,
330	clear_all.mission_type,	×
331	0);
332	return response_message;	×
333	});
334	}	×
335
336	std::vector<MavlinkMissionTransferServer::ItemInt>&
337	MissionRawServerImpl::get_mission_items(const uint8_t mission_type)	1✔
338	{
339	switch (mission_type) {	1✔
340	case MAV_MISSION_TYPE_MISSION:	1✔
341	return _current_mission;	1✔
342	case MAV_MISSION_TYPE_FENCE:	×
343	return _current_fence;	×
344	case MAV_MISSION_TYPE_RALLY:	×
345	return _current_rally;	×
346	default:	×
347	return _empty_mission_items;	×
348	}
349	}
350
351	MissionRawServer::IncomingMissionHandle MissionRawServerImpl::subscribe_incoming_mission(	×
352	const MissionRawServer::IncomingMissionCallback& callback)
353	{
354	return _incoming_mission_callbacks.subscribe(callback);	×
355	}
356
357	void MissionRawServerImpl::unsubscribe_incoming_mission(	×
358	MissionRawServer::IncomingMissionHandle handle)
359	{
360	_incoming_mission_callbacks.unsubscribe(handle);	×
361	}	×
362
363	MissionRawServer::CurrentItemChangedHandle MissionRawServerImpl::subscribe_current_item_changed(	×
364	const MissionRawServer::CurrentItemChangedCallback& callback)
365	{
366	return _current_item_changed_callbacks.subscribe(callback);	×
367	}
368
369	void MissionRawServerImpl::unsubscribe_current_item_changed(	×
370	MissionRawServer::CurrentItemChangedHandle handle)
371	{
372	_current_item_changed_callbacks.unsubscribe(handle);	×
373	}	×
374
375	MissionRawServer::ClearAllHandle
376	MissionRawServerImpl::subscribe_clear_all(const MissionRawServer::ClearAllCallback& callback)	×
377	{
378	return _clear_all_callbacks.subscribe(callback);	×
379	}
380
381	void MissionRawServerImpl::unsubscribe_clear_all(MissionRawServer::ClearAllHandle handle)	×
382	{
383	_clear_all_callbacks.unsubscribe(handle);	×
384	}	×
385
386	void MissionRawServerImpl::set_current_item_complete()	×
387	{
388	if (_current_seq + 1 > _current_mission.size()) {	×
389	return;	×
390	}
391
392	_server_component_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	×
393	mavlink_message_t message;
394	mavlink_msg_mission_item_reached_pack_chan(	×
395	mavlink_address.system_id,	×
396	mavlink_address.component_id,	×
397	channel,
398	&message,
399	static_cast<uint16_t>(_current_seq));	×
400	return message;	×
401	});
402
403	if (_current_seq + 1 == _current_mission.size()) {	×
404	_mission_completed = true;	×
405	}
406	// This will publish 0 - N mission current
407	// mission_current mission.size() is end of mission!
408	set_current_seq(_current_seq + 1);	×
409	}
410
411	void MissionRawServerImpl::set_current_seq(std::size_t seq)	1✔
412	{
413	if (_current_mission.size() < static_cast<size_t>(seq) \|\| _current_mission.empty()) {	1✔
414	return;	×
415	}
416
417	_current_seq = seq;	1✔
418
419	// If mission is over, just set item to last one again
420	auto item = seq == _current_mission.size() ? _current_mission.back() :	1✔
421	_current_mission.at(_current_seq);	1✔
422	auto converted_item = convert_item(item);	1✔
423	_current_item_changed_callbacks.queue(converted_item, [this](const auto& func) {	1✔
424	_server_component_impl->call_user_callback(func);	×
425	});	×
426
427	_server_component_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	1✔
428	mavlink_message_t message;
429	mavlink_msg_mission_current_pack_chan(	1✔
430	mavlink_address.system_id,	1✔
431	mavlink_address.component_id,	1✔
432	channel,
433	&message,
434	static_cast<uint16_t>(_current_seq),	1✔
435	0,
436	0,
437	0,
438	0,
439	0,
440	0);
441	return message;	1✔
442	});
443	}
444
445	} // namespace mavsdk

mavlink / MAVSDK / 16665088909

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