6568658631

Committed 19 Oct 2023 01:31AM UTC coverage: 31.23% (+0.02%) from 31.215%

Build # 6568658631

Build Type

push

github

Committed by

web-flow

Commit Message

Merge pull request #2155 from mavlink/pr-static-fixes

Threading fixes, MAVLink sequence number cleanup

Run Details

1386 of 1386 new or added lines in 46 files covered. (100.0%)

7906 of 25315 relevant lines covered (31.23%)

23.54 hits per line

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

39.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);
}

MavlinkMissionTransfer::ItemInt
convert_mission_raw(const MissionRawServer::MissionItem transfer_mission_raw)
{
    MavlinkMissionTransfer::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<MavlinkMissionTransfer::ItemInt>
convert_to_int_items(const std::vector<MissionRawServer::MissionItem>& mission_raw)
{
    std::vector<MavlinkMissionTransfer::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 MavlinkMissionTransfer::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<MavlinkMissionTransfer::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(MavlinkMissionTransfer::Result result)
{
    switch (result) {
        case MavlinkMissionTransfer::Result::Success:
            return MissionRawServer::Result::Success;
        case MavlinkMissionTransfer::Result::ConnectionError:
            return MissionRawServer::Result::Error; // FIXME
        case MavlinkMissionTransfer::Result::Denied:
            return MissionRawServer::Result::Error; // FIXME
        case MavlinkMissionTransfer::Result::TooManyMissionItems:
            return MissionRawServer::Result::TooManyMissionItems;
        case MavlinkMissionTransfer::Result::Timeout:
            return MissionRawServer::Result::Timeout;
        case MavlinkMissionTransfer::Result::Unsupported:
            return MissionRawServer::Result::Unsupported;
        case MavlinkMissionTransfer::Result::UnsupportedFrame:
            return MissionRawServer::Result::Unsupported;
        case MavlinkMissionTransfer::Result::NoMissionAvailable:
            return MissionRawServer::Result::NoMissionAvailable;
        case MavlinkMissionTransfer::Result::Cancelled:
            return MissionRawServer::Result::TransferCancelled;
        case MavlinkMissionTransfer::Result::MissionTypeNotConsistent:
            return MissionRawServer::Result::InvalidArgument; // FIXME
        case MavlinkMissionTransfer::Result::InvalidSequence:
            return MissionRawServer::Result::InvalidArgument; // FIXME
        case MavlinkMissionTransfer::Result::CurrentInvalid:
            return MissionRawServer::Result::InvalidArgument; // FIXME
        case MavlinkMissionTransfer::Result::ProtocolError:
            return MissionRawServer::Result::Error; // FIXME
        case MavlinkMissionTransfer::Result::InvalidParam:
            return MissionRawServer::Result::InvalidArgument; // FIXME
        case MavlinkMissionTransfer::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);

    _thread_mission = std::thread([this] {
        while (!_stop_work_thread) {
            std::unique_lock<std::mutex> lock(_work_mutex);
            if (!_work_queue.empty()) {
                auto task = _work_queue.front();
                _work_queue.pop();
                lock.unlock();
                task();
            } else {
                _wait_for_new_task.wait(lock);
            }
        }
    });

    // Handle Initiate Upload
    _server_component_impl->register_mavlink_message_handler(
        MAVLINK_MSG_ID_MISSION_COUNT,
        [this](const mavlink_message_t& message) {
            LogDebug() << "Receive Mission Count in Server";

            // Decode the count
            _target_system_id = message.sysid;
            _target_component_id = message.compid;
            mavlink_mission_count_t count;
            mavlink_msg_mission_count_decode(&message, &count);
            _mission_count = count.count;

            // We need to queue this on a different thread or it will deadlock
            add_task([this, target_system_id = message.sysid]() {
                // Mission Upload Inbound
                if (_last_download.lock()) {
                    _incoming_mission_callbacks.queue(
                        MissionRawServer::Result::Busy,
                        MissionRawServer::MissionPlan{},
                        [this](const auto& func) {
                            _server_component_impl->call_user_callback(func);
                        });
                    return;
                }

                _server_component_impl->set_our_current_target_system_id(target_system_id);

                _last_download =
                    _server_component_impl->mission_transfer().receive_incoming_items_async(
                        MAV_MISSION_TYPE_MISSION,
                        _mission_count,
                        _target_system_id,
                        _target_component_id,
                        [this](
                            MavlinkMissionTransfer::Result result,
                            std::vector<MavlinkMissionTransfer::ItemInt> items) {
                            _current_mission = items;
                            auto converted_result = convert_result(result);
                            auto converted_items = convert_items(items);
                            _incoming_mission_callbacks.queue(
                                converted_result, {converted_items}, [this](const auto& func) {
                                    _server_component_impl->call_user_callback(func);
                                });
                            _mission_completed = false;
                            set_current_seq(0);
                        });
            });
        },
        this);

    // Handle Set Current from GCS
    _server_component_impl->register_mavlink_message_handler(
        MAVLINK_MSG_ID_MISSION_SET_CURRENT,
        [this](const mavlink_message_t& message) {
            LogDebug() << "Receive Mission Set Current";

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

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

    // Handle Clears
    _server_component_impl->register_mavlink_message_handler(
        MAVLINK_MSG_ID_MISSION_CLEAR_ALL,
        [this](const mavlink_message_t& message) {
            mavlink_mission_clear_all_t clear_all;
            mavlink_msg_mission_clear_all_decode(&message, &clear_all);
            if (clear_all.mission_type == MAV_MISSION_TYPE_ALL ||
                clear_all.mission_type == MAV_MISSION_TYPE_MISSION) {
                _current_mission.clear();
                _current_seq = 0;
                _clear_all_callbacks.queue(clear_all.mission_type, [this](const auto& func) {
                    _server_component_impl->call_user_callback(func);
                });
            }

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

void MissionRawServerImpl::deinit()
{
    _server_component_impl->unregister_all_mavlink_message_handlers(this);
    _stop_work_thread = true;
    _wait_for_new_task.notify_all();
    _thread_mission.join();
}

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::MissionPlan MissionRawServerImpl::incoming_mission() const
{
    // FIXME: this doesn't look right.
    return {};
}

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

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()) {
        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);
        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()	1✔
18	{
19	_server_component_impl->unregister_plugin(this);	1✔
20	}	1✔
21
22	MavlinkMissionTransfer::ItemInt
23	convert_mission_raw(const MissionRawServer::MissionItem transfer_mission_raw)	×
24	{
25	MavlinkMissionTransfer::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<MavlinkMissionTransfer::ItemInt>
45	convert_to_int_items(const std::vector<MissionRawServer::MissionItem>& mission_raw)	×
46	{
47	std::vector<MavlinkMissionTransfer::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 convert_item(const MavlinkMissionTransfer::ItemInt& transfer_item)	7✔
57	{
58	MissionRawServer::MissionItem new_item;	7✔
59
60	new_item.seq = transfer_item.seq;	7✔
61	new_item.frame = transfer_item.frame;	7✔
62	new_item.command = transfer_item.command;	7✔
63	new_item.current = transfer_item.current;	7✔
64	new_item.autocontinue = transfer_item.autocontinue;	7✔
65	new_item.param1 = transfer_item.param1;	7✔
66	new_item.param2 = transfer_item.param2;	7✔
67	new_item.param3 = transfer_item.param3;	7✔
68	new_item.param4 = transfer_item.param4;	7✔
69	new_item.x = transfer_item.x;	7✔
70	new_item.y = transfer_item.y;	7✔
71	new_item.z = transfer_item.z;	7✔
72	new_item.mission_type = transfer_item.mission_type;	7✔
73
74	return new_item;	7✔
75	}
76
77	std::vector<MissionRawServer::MissionItem>
78	convert_items(const std::vector<MavlinkMissionTransfer::ItemInt>& transfer_items)	1✔
79	{
80	std::vector<MissionRawServer::MissionItem> new_items;	1✔
81	new_items.reserve(transfer_items.size());	1✔
82
83	for (const auto& transfer_item : transfer_items) {	7✔
84	new_items.push_back(convert_item(transfer_item));	6✔
85	}
86
87	return new_items;	1✔
88	}
89
90	MissionRawServer::Result convert_result(MavlinkMissionTransfer::Result result)	1✔
91	{
92	switch (result) {	1✔
93	case MavlinkMissionTransfer::Result::Success:	1✔
94	return MissionRawServer::Result::Success;	1✔
95	case MavlinkMissionTransfer::Result::ConnectionError:	×
96	return MissionRawServer::Result::Error; // FIXME	×
97	case MavlinkMissionTransfer::Result::Denied:	×
98	return MissionRawServer::Result::Error; // FIXME	×
99	case MavlinkMissionTransfer::Result::TooManyMissionItems:	×
100	return MissionRawServer::Result::TooManyMissionItems;	×
101	case MavlinkMissionTransfer::Result::Timeout:	×
102	return MissionRawServer::Result::Timeout;	×
103	case MavlinkMissionTransfer::Result::Unsupported:	×
104	return MissionRawServer::Result::Unsupported;	×
105	case MavlinkMissionTransfer::Result::UnsupportedFrame:	×
106	return MissionRawServer::Result::Unsupported;	×
107	case MavlinkMissionTransfer::Result::NoMissionAvailable:	×
108	return MissionRawServer::Result::NoMissionAvailable;	×
109	case MavlinkMissionTransfer::Result::Cancelled:	×
110	return MissionRawServer::Result::TransferCancelled;	×
111	case MavlinkMissionTransfer::Result::MissionTypeNotConsistent:	×
112	return MissionRawServer::Result::InvalidArgument; // FIXME	×
113	case MavlinkMissionTransfer::Result::InvalidSequence:	×
114	return MissionRawServer::Result::InvalidArgument; // FIXME	×
115	case MavlinkMissionTransfer::Result::CurrentInvalid:	×
116	return MissionRawServer::Result::InvalidArgument; // FIXME	×
117	case MavlinkMissionTransfer::Result::ProtocolError:	×
118	return MissionRawServer::Result::Error; // FIXME	×
119	case MavlinkMissionTransfer::Result::InvalidParam:	×
120	return MissionRawServer::Result::InvalidArgument; // FIXME	×
121	case MavlinkMissionTransfer::Result::IntMessagesNotSupported:	×
122	return MissionRawServer::Result::Unsupported; // FIXME	×
123	default:	×
124	return MissionRawServer::Result::Unknown;	×
125	}
126	}
127
128	void MissionRawServerImpl::init()	1✔
129	{
130	_server_component_impl->add_capabilities(MAV_PROTOCOL_CAPABILITY_MISSION_INT);	1✔
131
132	_thread_mission = std::thread([this] {	2✔
133	while (!_stop_work_thread) {	4✔
134	std::unique_lock<std::mutex> lock(_work_mutex);	6✔
135	if (!_work_queue.empty()) {	3✔
136	auto task = _work_queue.front();	2✔
137	_work_queue.pop();	1✔
138	lock.unlock();	1✔
139	task();	1✔
140	} else {
141	_wait_for_new_task.wait(lock);	2✔
142	}
143	}
144	});	1✔
145
146	// Handle Initiate Upload
147	_server_component_impl->register_mavlink_message_handler(	1✔
148	MAVLINK_MSG_ID_MISSION_COUNT,
149	[this](const mavlink_message_t& message) {	6✔
150	LogDebug() << "Receive Mission Count in Server";	1✔
151
152	// Decode the count
153	_target_system_id = message.sysid;	1✔
154	_target_component_id = message.compid;	1✔
155	mavlink_mission_count_t count;	1✔
156	mavlink_msg_mission_count_decode(&message, &count);	1✔
157	_mission_count = count.count;	1✔
158
159	// We need to queue this on a different thread or it will deadlock
160	add_task([this, target_system_id = message.sysid]() {	1✔
161	// Mission Upload Inbound
162	if (_last_download.lock()) {	1✔
163	_incoming_mission_callbacks.queue(	×
164	MissionRawServer::Result::Busy,
165	MissionRawServer::MissionPlan{},	×
166	[this](const auto& func) {	×
167	_server_component_impl->call_user_callback(func);	×
168	});	×
169	return;	×
170	}
171
172	_server_component_impl->set_our_current_target_system_id(target_system_id);	1✔
173
174	_last_download =	1✔
175	_server_component_impl->mission_transfer().receive_incoming_items_async(	3✔
176	MAV_MISSION_TYPE_MISSION,
177	_mission_count,
178	_target_system_id,
179	_target_component_id,
180	[this](	1✔
181	MavlinkMissionTransfer::Result result,
182	std::vector<MavlinkMissionTransfer::ItemInt> items) {	5✔
183	_current_mission = items;	1✔
184	auto converted_result = convert_result(result);	1✔
185	auto converted_items = convert_items(items);	2✔
186	_incoming_mission_callbacks.queue(	1✔
187	converted_result, {converted_items}, [this](const auto& func) {	×
188	_server_component_impl->call_user_callback(func);	×
189	});	×
190	_mission_completed = false;	1✔
191	set_current_seq(0);	1✔
192	});	2✔
193	});
194	},	1✔
195	this);
196
197	// Handle Set Current from GCS
198	_server_component_impl->register_mavlink_message_handler(	1✔
199	MAVLINK_MSG_ID_MISSION_SET_CURRENT,
200	[this](const mavlink_message_t& message) {	×
201	LogDebug() << "Receive Mission Set Current";	×
202
203	mavlink_mission_set_current_t set_current;	×
204	mavlink_msg_mission_set_current_decode(&message, &set_current);	×
205
206	if (_current_mission.size() == 0) {	×
207	const char text[50] = "No Mission Loaded";	×
208	_server_component_impl->queue_message(	×
209	[&](MavlinkAddress mavlink_address, uint8_t channel) {	×
210	mavlink_message_t response_message;
211	mavlink_msg_statustext_pack_chan(	×
212	mavlink_address.system_id,	×
213	mavlink_address.component_id,	×
214	channel,
215	&response_message,
216	MAV_SEVERITY_ERROR,
217	text,	×
218	0,
219	0);
220	return response_message;	×
221	});
222	} else if (_current_mission.size() <= set_current.seq) {	×
223	const char text[50] = "Unknown Mission seq id";	×
224	_server_component_impl->queue_message(	×
225	[&](MavlinkAddress mavlink_address, uint8_t channel) {	×
226	mavlink_message_t response_message;
227	mavlink_msg_statustext_pack_chan(	×
228	mavlink_address.system_id,	×
229	mavlink_address.component_id,	×
230	channel,
231	&response_message,
232	MAV_SEVERITY_ERROR,
233	text,	×
234	0,
235	0);
236	return response_message;	×
237	});
238	} else {
239	set_current_seq(set_current.seq);	×
240	}
241	},	×
242	this);
243
244	// Handle Clears
245	_server_component_impl->register_mavlink_message_handler(	1✔
246	MAVLINK_MSG_ID_MISSION_CLEAR_ALL,
247	[this](const mavlink_message_t& message) {	×
248	mavlink_mission_clear_all_t clear_all;	×
249	mavlink_msg_mission_clear_all_decode(&message, &clear_all);	×
250	if (clear_all.mission_type == MAV_MISSION_TYPE_ALL \|\|	×
251	clear_all.mission_type == MAV_MISSION_TYPE_MISSION) {	×
252	_current_mission.clear();	×
253	_current_seq = 0;	×
254	_clear_all_callbacks.queue(clear_all.mission_type, [this](const auto& func) {	×
255	_server_component_impl->call_user_callback(func);	×
256	});	×
257	}
258
259	// Send the MISSION_ACK
260	_server_component_impl->queue_message(	×
261	[&](MavlinkAddress mavlink_address, uint8_t channel) {	×
262	mavlink_message_t response_message;
263	mavlink_msg_mission_ack_pack_chan(	×
264	mavlink_address.system_id,	×
265	mavlink_address.component_id,	×
266	channel,
267	&response_message,
268	message.sysid,	×
269	message.compid,	×
270	MAV_MISSION_RESULT::MAV_MISSION_ACCEPTED,
271	clear_all.mission_type);	×
272	return response_message;	×
273	});
274	},	×
275	this);
276	}	1✔
277
278	void MissionRawServerImpl::deinit()	1✔
279	{
280	_server_component_impl->unregister_all_mavlink_message_handlers(this);	1✔
281	_stop_work_thread = true;	1✔
282	_wait_for_new_task.notify_all();	1✔
283	_thread_mission.join();	1✔
284	}	1✔
285
286	MissionRawServer::IncomingMissionHandle MissionRawServerImpl::subscribe_incoming_mission(	×
287	const MissionRawServer::IncomingMissionCallback& callback)
288	{
289	return _incoming_mission_callbacks.subscribe(callback);	×
290	}
291
292	void MissionRawServerImpl::unsubscribe_incoming_mission(	×
293	MissionRawServer::IncomingMissionHandle handle)
294	{
295	_incoming_mission_callbacks.unsubscribe(handle);	×
296	}	×
297
298	MissionRawServer::MissionPlan MissionRawServerImpl::incoming_mission() const	×
299	{
300	// FIXME: this doesn't look right.
301	return {};	×
302	}
303
304	MissionRawServer::CurrentItemChangedHandle MissionRawServerImpl::subscribe_current_item_changed(	×
305	const MissionRawServer::CurrentItemChangedCallback& callback)
306	{
307	return _current_item_changed_callbacks.subscribe(callback);	×
308	}
309
310	void MissionRawServerImpl::unsubscribe_current_item_changed(	×
311	MissionRawServer::CurrentItemChangedHandle handle)
312	{
313	_current_item_changed_callbacks.unsubscribe(handle);	×
314	}	×
315
316	MissionRawServer::ClearAllHandle
317	MissionRawServerImpl::subscribe_clear_all(const MissionRawServer::ClearAllCallback& callback)	×
318	{
319	return _clear_all_callbacks.subscribe(callback);	×
320	}
321
322	void MissionRawServerImpl::unsubscribe_clear_all(MissionRawServer::ClearAllHandle handle)	×
323	{
324	_clear_all_callbacks.unsubscribe(handle);	×
325	}	×
326
327	uint32_t MissionRawServerImpl::clear_all() const	×
328	{
329	// TO-DO
330	return {};	×
331	}
332
333	MissionRawServer::MissionItem MissionRawServerImpl::current_item_changed() const	×
334	{
335	// TO-DO
336	return {};	×
337	}
338
339	void MissionRawServerImpl::set_current_item_complete()	×
340	{
341	if (_current_seq + 1 > _current_mission.size()) {	×
342	return;	×
343	}
344
345	_server_component_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	×
346	mavlink_message_t message;
347	mavlink_msg_mission_item_reached_pack_chan(	×
348	mavlink_address.system_id,	×
349	mavlink_address.component_id,	×
350	channel,
351	&message,
352	static_cast<uint16_t>(_current_seq));	×
353	return message;	×
354	});
355
356	if (_current_seq + 1 == _current_mission.size()) {	×
357	_mission_completed = true;	×
358	}
359	// This will publish 0 - N mission current
360	// mission_current mission.size() is end of mission!
361	set_current_seq(_current_seq + 1);	×
362	}
363
364	void MissionRawServerImpl::set_current_seq(std::size_t seq)	1✔
365	{
366	if (_current_mission.size() < static_cast<size_t>(seq) \|\| _current_mission.empty()) {	1✔
367	return;	×
368	}
369
370	_current_seq = seq;	1✔
371
372	// If mission is over, just set item to last one again
373	auto item = seq == _current_mission.size() ? _current_mission.back() :	1✔
374	_current_mission.at(_current_seq);	1✔
375	auto converted_item = convert_item(item);	1✔
376	_current_item_changed_callbacks.queue(converted_item, [this](const auto& func) {	1✔
377	_server_component_impl->call_user_callback(func);	×
378	});	×
379
380	_server_component_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	1✔
381	mavlink_message_t message;
382	mavlink_msg_mission_current_pack_chan(	1✔
383	mavlink_address.system_id,	1✔
384	mavlink_address.component_id,	1✔
385	channel,
386	&message,
387	static_cast<uint16_t>(_current_seq),	1✔
388	0,
389	0,
390	0);
391	return message;	1✔
392	});
393	}
394
395	} // namespace mavsdk

mavlink / MAVSDK / 6568658631

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