6752913415

Committed 04 Nov 2023 05:06AM UTC coverage: 36.901% (-0.05%) from 36.952%

Build # 6752913415

Build Type

push

github

Committed by

web-flow

Commit Message

Merge pull request #2160 from mavlink/pr-split-mission-transfer

Split mission transfer into client and server

Run Details

488 of 683 new or added lines in 14 files covered. (71.45%)

24 existing lines in 7 files now uncovered.

9925 of 26896 relevant lines covered (36.9%)

118.33 hits per line

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

35.07

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

    // 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 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(
        MAV_MISSION_TYPE_MISSION,
        _mission_count,
        _target_system_id,
        _target_component_id,
        [this](
            MavlinkMissionTransferServer::Result result,
            std::vector<MavlinkMissionTransferServer::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);
        });
}

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

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

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	MavlinkMissionTransferServer::ItemInt
23	convert_mission_raw(const MissionRawServer::MissionItem transfer_mission_raw)	×
24	{
NEW 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	{
NEW 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✔
NEW 96	case MavlinkMissionTransferServer::Result::ConnectionError:	×
97	return MissionRawServer::Result::Error; // FIXME	×
NEW 98	case MavlinkMissionTransferServer::Result::Denied:	×
99	return MissionRawServer::Result::Error; // FIXME	×
NEW 100	case MavlinkMissionTransferServer::Result::TooManyMissionItems:	×
101	return MissionRawServer::Result::TooManyMissionItems;	×
NEW 102	case MavlinkMissionTransferServer::Result::Timeout:	×
103	return MissionRawServer::Result::Timeout;	×
NEW 104	case MavlinkMissionTransferServer::Result::Unsupported:	×
105	return MissionRawServer::Result::Unsupported;	×
NEW 106	case MavlinkMissionTransferServer::Result::UnsupportedFrame:	×
107	return MissionRawServer::Result::Unsupported;	×
NEW 108	case MavlinkMissionTransferServer::Result::NoMissionAvailable:	×
109	return MissionRawServer::Result::NoMissionAvailable;	×
NEW 110	case MavlinkMissionTransferServer::Result::Cancelled:	×
111	return MissionRawServer::Result::TransferCancelled;	×
NEW 112	case MavlinkMissionTransferServer::Result::MissionTypeNotConsistent:	×
113	return MissionRawServer::Result::InvalidArgument; // FIXME	×
NEW 114	case MavlinkMissionTransferServer::Result::InvalidSequence:	×
115	return MissionRawServer::Result::InvalidArgument; // FIXME	×
NEW 116	case MavlinkMissionTransferServer::Result::CurrentInvalid:	×
117	return MissionRawServer::Result::InvalidArgument; // FIXME	×
NEW 118	case MavlinkMissionTransferServer::Result::ProtocolError:	×
119	return MissionRawServer::Result::Error; // FIXME	×
NEW 120	case MavlinkMissionTransferServer::Result::InvalidParam:	×
121	return MissionRawServer::Result::InvalidArgument; // FIXME	×
NEW 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(MAV_PROTOCOL_CAPABILITY_MISSION_INT);	1✔
132
133	// Handle Initiate Upload
134	_server_component_impl->register_mavlink_message_handler(	1✔
135	MAVLINK_MSG_ID_MISSION_COUNT,
136	[this](const mavlink_message_t& message) { process_mission_count(message); },	1✔
137	this);
138
139	// Handle Set Current from GCS
140	_server_component_impl->register_mavlink_message_handler(	1✔
141	MAVLINK_MSG_ID_MISSION_SET_CURRENT,
NEW 142	[this](const mavlink_message_t& message) { process_mission_set_current(message); },	×
143	this);
144
145	// Handle Clears
146	_server_component_impl->register_mavlink_message_handler(	1✔
147	MAVLINK_MSG_ID_MISSION_CLEAR_ALL,
NEW 148	[this](const mavlink_message_t& message) { process_mission_clear(message); },	×
149	this);
150	}	1✔
151
152	void MissionRawServerImpl::deinit()	1✔
153	{
154	_server_component_impl->unregister_all_mavlink_message_handlers(this);	1✔
155	}	1✔
156
157	void MissionRawServerImpl::process_mission_count(const mavlink_message_t& message)	1✔
158	{
159	UNUSED(message);	1✔
160
161	// Decode the count
162	_target_system_id = message.sysid;	1✔
163	_target_component_id = message.compid;	1✔
164
165	mavlink_mission_count_t count;	1✔
166	mavlink_msg_mission_count_decode(&message, &count);	1✔
167	_mission_count = count.count;	1✔
168
169	// Mission Upload Inbound
170	if (_last_download.lock()) {	1✔
NEW 171	_incoming_mission_callbacks.queue(	×
172	MissionRawServer::Result::Busy,
NEW 173	MissionRawServer::MissionPlan{},	×
NEW 174	[this](const auto& func) { _server_component_impl->call_user_callback(func); });	×
NEW 175	return;	×
176	}
177
178	_last_download = _server_component_impl->mission_transfer_server().receive_incoming_items_async(	3✔
179	MAV_MISSION_TYPE_MISSION,
180	_mission_count,
181	_target_system_id,
182	_target_component_id,
183	[this](	1✔
184	MavlinkMissionTransferServer::Result result,
185	std::vector<MavlinkMissionTransferServer::ItemInt> items) {	5✔
186	_current_mission = items;	1✔
187	auto converted_result = convert_result(result);	1✔
188	auto converted_items = convert_items(items);	2✔
189	_incoming_mission_callbacks.queue(	1✔
NEW 190	converted_result, {converted_items}, [this](const auto& func) {	×
NEW 191	_server_component_impl->call_user_callback(func);	×
NEW 192	});	×
193	_mission_completed = false;	1✔
194	set_current_seq(0);	1✔
195	});	2✔
196	}
197
NEW 198	void MissionRawServerImpl::process_mission_set_current(const mavlink_message_t& message)	×
199	{
NEW 200	LogDebug() << "Receive Mission Set Current";	×
201
NEW 202	mavlink_mission_set_current_t set_current;	×
NEW 203	mavlink_msg_mission_set_current_decode(&message, &set_current);	×
204
NEW 205	if (_current_mission.size() == 0) {	×
NEW 206	const char text[50] = "No Mission Loaded";	×
NEW 207	_server_component_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	×
208	mavlink_message_t response_message;
NEW 209	mavlink_msg_statustext_pack_chan(	×
NEW 210	mavlink_address.system_id,	×
NEW 211	mavlink_address.component_id,	×
212	channel,
213	&response_message,
214	MAV_SEVERITY_ERROR,
NEW 215	text,	×
216	0,
217	0);
NEW 218	return response_message;	×
219	});
NEW 220	} else if (_current_mission.size() <= set_current.seq) {	×
NEW 221	const char text[50] = "Unknown Mission seq id";	×
NEW 222	_server_component_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	×
223	mavlink_message_t response_message;
NEW 224	mavlink_msg_statustext_pack_chan(	×
NEW 225	mavlink_address.system_id,	×
NEW 226	mavlink_address.component_id,	×
227	channel,
228	&response_message,
229	MAV_SEVERITY_ERROR,
NEW 230	text,	×
231	0,
232	0);
NEW 233	return response_message;	×
234	});
235	} else {
NEW 236	set_current_seq(set_current.seq);	×
237	}
NEW 238	}	×
239
NEW 240	void MissionRawServerImpl::process_mission_clear(const mavlink_message_t message)	×
241	{
NEW 242	mavlink_mission_clear_all_t clear_all;	×
NEW 243	mavlink_msg_mission_clear_all_decode(&message, &clear_all);	×
NEW 244	if (clear_all.mission_type == MAV_MISSION_TYPE_ALL \|\|	×
NEW 245	clear_all.mission_type == MAV_MISSION_TYPE_MISSION) {	×
NEW 246	_current_mission.clear();	×
NEW 247	_current_seq = 0;	×
NEW 248	_clear_all_callbacks.queue(clear_all.mission_type, [this](const auto& func) {	×
NEW 249	_server_component_impl->call_user_callback(func);	×
NEW 250	});	×
251	}
252
253	// Send the MISSION_ACK
NEW 254	_server_component_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	×
255	mavlink_message_t response_message;
NEW 256	mavlink_msg_mission_ack_pack_chan(	×
NEW 257	mavlink_address.system_id,	×
NEW 258	mavlink_address.component_id,	×
259	channel,
260	&response_message,
NEW 261	message.sysid,	×
NEW 262	message.compid,	×
263	MAV_MISSION_RESULT::MAV_MISSION_ACCEPTED,
NEW 264	clear_all.mission_type);	×
NEW 265	return response_message;	×
266	});
UNCOV 267	}	×
268
269	MissionRawServer::IncomingMissionHandle MissionRawServerImpl::subscribe_incoming_mission(	×
270	const MissionRawServer::IncomingMissionCallback& callback)
271	{
272	return _incoming_mission_callbacks.subscribe(callback);	×
273	}
274
275	void MissionRawServerImpl::unsubscribe_incoming_mission(	×
276	MissionRawServer::IncomingMissionHandle handle)
277	{
278	_incoming_mission_callbacks.unsubscribe(handle);	×
279	}	×
280
281	MissionRawServer::MissionPlan MissionRawServerImpl::incoming_mission() const	×
282	{
283	// FIXME: this doesn't look right.
284	return {};	×
285	}
286
287	MissionRawServer::CurrentItemChangedHandle MissionRawServerImpl::subscribe_current_item_changed(	×
288	const MissionRawServer::CurrentItemChangedCallback& callback)
289	{
290	return _current_item_changed_callbacks.subscribe(callback);	×
291	}
292
293	void MissionRawServerImpl::unsubscribe_current_item_changed(	×
294	MissionRawServer::CurrentItemChangedHandle handle)
295	{
296	_current_item_changed_callbacks.unsubscribe(handle);	×
297	}	×
298
299	MissionRawServer::ClearAllHandle
300	MissionRawServerImpl::subscribe_clear_all(const MissionRawServer::ClearAllCallback& callback)	×
301	{
302	return _clear_all_callbacks.subscribe(callback);	×
303	}
304
305	void MissionRawServerImpl::unsubscribe_clear_all(MissionRawServer::ClearAllHandle handle)	×
306	{
307	_clear_all_callbacks.unsubscribe(handle);	×
308	}	×
309
310	uint32_t MissionRawServerImpl::clear_all() const	×
311	{
312	// TO-DO
313	return {};	×
314	}
315
316	MissionRawServer::MissionItem MissionRawServerImpl::current_item_changed() const	×
317	{
318	// TO-DO
319	return {};	×
320	}
321
322	void MissionRawServerImpl::set_current_item_complete()	×
323	{
324	if (_current_seq + 1 > _current_mission.size()) {	×
325	return;	×
326	}
327
328	_server_component_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	×
329	mavlink_message_t message;
330	mavlink_msg_mission_item_reached_pack_chan(	×
331	mavlink_address.system_id,	×
332	mavlink_address.component_id,	×
333	channel,
334	&message,
335	static_cast<uint16_t>(_current_seq));	×
336	return message;	×
337	});
338
339	if (_current_seq + 1 == _current_mission.size()) {	×
340	_mission_completed = true;	×
341	}
342	// This will publish 0 - N mission current
343	// mission_current mission.size() is end of mission!
344	set_current_seq(_current_seq + 1);	×
345	}
346
347	void MissionRawServerImpl::set_current_seq(std::size_t seq)	1✔
348	{
349	if (_current_mission.size() < static_cast<size_t>(seq) \|\| _current_mission.empty()) {	1✔
350	return;	×
351	}
352
353	_current_seq = seq;	1✔
354
355	// If mission is over, just set item to last one again
356	auto item = seq == _current_mission.size() ? _current_mission.back() :	1✔
357	_current_mission.at(_current_seq);	1✔
358	auto converted_item = convert_item(item);	1✔
359	_current_item_changed_callbacks.queue(converted_item, [this](const auto& func) {	1✔
360	_server_component_impl->call_user_callback(func);	×
361	});	×
362
363	_server_component_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	1✔
364	mavlink_message_t message;
365	mavlink_msg_mission_current_pack_chan(	1✔
366	mavlink_address.system_id,	1✔
367	mavlink_address.component_id,	1✔
368	channel,
369	&message,
370	static_cast<uint16_t>(_current_seq),	1✔
371	0,
372	0,
373	0);
374	return message;	1✔
375	});
376	}
377
378	} // namespace mavsdk

mavlink / MAVSDK / 6752913415

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