4899817366

Build Type

github

Committed by GitHub

Commit Message

Merge c0bb90862 into e4e9c71dd

Pull Request Pull Request #1772: Refactor MAVLinkParameters into client and server classes

Run Details

2192 of 2192 new or added lines in 34 files covered. (100.0%)

7708 of 24812 relevant lines covered (31.07%)

19.96 hits per line

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

39.73

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

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

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_component = 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_component,
                        [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";
                mavlink_message_t status_message;
                mavlink_msg_statustext_pack(
                    _server_component_impl->get_own_system_id(),
                    _server_component_impl->get_own_component_id(),
                    &status_message,
                    MAV_SEVERITY_ERROR,
                    text,
                    0,
                    0);
                _server_component_impl->send_message(status_message);
            } else if (_current_mission.size() <= set_current.seq) {
                const char text[50] = "Unknown Mission seq id";
                mavlink_message_t status_message;
                mavlink_msg_statustext_pack(
                    _server_component_impl->get_own_system_id(),
                    _server_component_impl->get_own_component_id(),
                    &status_message,
                    MAV_SEVERITY_ERROR,
                    text,
                    0,
                    0);
                _server_component_impl->send_message(status_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
            mavlink_message_t ack_message;
            mavlink_msg_mission_ack_pack(
                _server_component_impl->get_own_system_id(),
                _server_component_impl->get_own_component_id(),
                &ack_message,
                message.sysid,
                message.compid,
                MAV_MISSION_RESULT::MAV_MISSION_ACCEPTED,
                clear_all.mission_type);
            _server_component_impl->send_message(ack_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;
    }

    mavlink_message_t mission_reached;
    mavlink_msg_mission_item_reached_pack(
        _server_component_impl->get_own_system_id(),
        _server_component_impl->get_own_component_id(),
        &mission_reached,
        static_cast<uint16_t>(_current_seq));
    _server_component_impl->send_message(mission_reached);

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

    mavlink_message_t mission_current;
    mavlink_msg_mission_current_pack(
        _server_component_impl->get_own_system_id(),
        _server_component_impl->get_own_component_id(),
        &mission_current,
        static_cast<uint16_t>(_current_seq),
        0,
        0,
        0);
    _server_component_impl->send_message(mission_current);
}

} // namespace mavsdk

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

mavlink / MAVSDK / 4899817366

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