7317183253

Committed 24 Dec 2023 10:25PM UTC coverage: 36.26% (-0.6%) from 36.89%

Build # 7317183253

Build Type

push

github

Committed by

web-flow

Commit Message

Merge pull request #2088 from tbago/add_more_camera_function

More camera server functionality

Run Details

28 of 718 new or added lines in 12 files covered. (3.9%)

20 existing lines in 6 files now uncovered.

10035 of 27675 relevant lines covered (36.26%)

127.93 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,
            0);
        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,
            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	{
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(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,
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,
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✔
171	_incoming_mission_callbacks.queue(	×
172	MissionRawServer::Result::Busy,
173	MissionRawServer::MissionPlan{},	×
174	[this](const auto& func) { _server_component_impl->call_user_callback(func); });	×
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✔
190	converted_result, {converted_items}, [this](const auto& func) {	×
191	_server_component_impl->call_user_callback(func);	×
192	});	×
193	_mission_completed = false;	1✔
194	set_current_seq(0);	1✔
195	});	2✔
196	}
197
198	void MissionRawServerImpl::process_mission_set_current(const mavlink_message_t& message)	×
199	{
200	LogDebug() << "Receive Mission Set Current";	×
201
202	mavlink_mission_set_current_t set_current;	×
203	mavlink_msg_mission_set_current_decode(&message, &set_current);	×
204
205	if (_current_mission.size() == 0) {	×
206	const char text[50] = "No Mission Loaded";	×
207	_server_component_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	×
208	mavlink_message_t response_message;
209	mavlink_msg_statustext_pack_chan(	×
210	mavlink_address.system_id,	×
211	mavlink_address.component_id,	×
212	channel,
213	&response_message,
214	MAV_SEVERITY_ERROR,
215	text,	×
216	0,
217	0);
218	return response_message;	×
219	});
220	} else if (_current_mission.size() <= set_current.seq) {	×
221	const char text[50] = "Unknown Mission seq id";	×
222	_server_component_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	×
223	mavlink_message_t response_message;
224	mavlink_msg_statustext_pack_chan(	×
225	mavlink_address.system_id,	×
226	mavlink_address.component_id,	×
227	channel,
228	&response_message,
229	MAV_SEVERITY_ERROR,
230	text,	×
231	0,
232	0);
233	return response_message;	×
234	});
235	} else {
236	set_current_seq(set_current.seq);	×
237	}
238	}	×
239
240	void MissionRawServerImpl::process_mission_clear(const mavlink_message_t message)	×
241	{
242	mavlink_mission_clear_all_t clear_all;	×
243	mavlink_msg_mission_clear_all_decode(&message, &clear_all);	×
244	if (clear_all.mission_type == MAV_MISSION_TYPE_ALL \|\|	×
245	clear_all.mission_type == MAV_MISSION_TYPE_MISSION) {	×
246	_current_mission.clear();	×
247	_current_seq = 0;	×
248	_clear_all_callbacks.queue(clear_all.mission_type, [this](const auto& func) {	×
249	_server_component_impl->call_user_callback(func);	×
250	});	×
251	}
252
253	// Send the MISSION_ACK
254	_server_component_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	×
255	mavlink_message_t response_message;
256	mavlink_msg_mission_ack_pack_chan(	×
257	mavlink_address.system_id,	×
258	mavlink_address.component_id,	×
259	channel,
260	&response_message,
261	message.sysid,	×
262	message.compid,	×
263	MAV_MISSION_RESULT::MAV_MISSION_ACCEPTED,
NEW 264	clear_all.mission_type,	×
265	0);
UNCOV 266	return response_message;	×
267	});
268	}	×
269
270	MissionRawServer::IncomingMissionHandle MissionRawServerImpl::subscribe_incoming_mission(	×
271	const MissionRawServer::IncomingMissionCallback& callback)
272	{
273	return _incoming_mission_callbacks.subscribe(callback);	×
274	}
275
276	void MissionRawServerImpl::unsubscribe_incoming_mission(	×
277	MissionRawServer::IncomingMissionHandle handle)
278	{
279	_incoming_mission_callbacks.unsubscribe(handle);	×
280	}	×
281
282	MissionRawServer::MissionPlan MissionRawServerImpl::incoming_mission() const	×
283	{
284	// FIXME: this doesn't look right.
285	return {};	×
286	}
287
288	MissionRawServer::CurrentItemChangedHandle MissionRawServerImpl::subscribe_current_item_changed(	×
289	const MissionRawServer::CurrentItemChangedCallback& callback)
290	{
291	return _current_item_changed_callbacks.subscribe(callback);	×
292	}
293
294	void MissionRawServerImpl::unsubscribe_current_item_changed(	×
295	MissionRawServer::CurrentItemChangedHandle handle)
296	{
297	_current_item_changed_callbacks.unsubscribe(handle);	×
298	}	×
299
300	MissionRawServer::ClearAllHandle
301	MissionRawServerImpl::subscribe_clear_all(const MissionRawServer::ClearAllCallback& callback)	×
302	{
303	return _clear_all_callbacks.subscribe(callback);	×
304	}
305
306	void MissionRawServerImpl::unsubscribe_clear_all(MissionRawServer::ClearAllHandle handle)	×
307	{
308	_clear_all_callbacks.unsubscribe(handle);	×
309	}	×
310
311	uint32_t MissionRawServerImpl::clear_all() const	×
312	{
313	// TO-DO
314	return {};	×
315	}
316
317	MissionRawServer::MissionItem MissionRawServerImpl::current_item_changed() const	×
318	{
319	// TO-DO
320	return {};	×
321	}
322
323	void MissionRawServerImpl::set_current_item_complete()	×
324	{
325	if (_current_seq + 1 > _current_mission.size()) {	×
326	return;	×
327	}
328
329	_server_component_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	×
330	mavlink_message_t message;
331	mavlink_msg_mission_item_reached_pack_chan(	×
332	mavlink_address.system_id,	×
333	mavlink_address.component_id,	×
334	channel,
335	&message,
336	static_cast<uint16_t>(_current_seq));	×
337	return message;	×
338	});
339
340	if (_current_seq + 1 == _current_mission.size()) {	×
341	_mission_completed = true;	×
342	}
343	// This will publish 0 - N mission current
344	// mission_current mission.size() is end of mission!
345	set_current_seq(_current_seq + 1);	×
346	}
347
348	void MissionRawServerImpl::set_current_seq(std::size_t seq)	1✔
349	{
350	if (_current_mission.size() < static_cast<size_t>(seq) \|\| _current_mission.empty()) {	1✔
351	return;	×
352	}
353
354	_current_seq = seq;	1✔
355
356	// If mission is over, just set item to last one again
357	auto item = seq == _current_mission.size() ? _current_mission.back() :	1✔
358	_current_mission.at(_current_seq);	1✔
359	auto converted_item = convert_item(item);	1✔
360	_current_item_changed_callbacks.queue(converted_item, [this](const auto& func) {	1✔
361	_server_component_impl->call_user_callback(func);	×
362	});	×
363
364	_server_component_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	1✔
365	mavlink_message_t message;
366	mavlink_msg_mission_current_pack_chan(	1✔
367	mavlink_address.system_id,	1✔
368	mavlink_address.component_id,	1✔
369	channel,
370	&message,
371	static_cast<uint16_t>(_current_seq),	1✔
372	0,
373	0,
374	0,
375	0,
376	0,
377	0);
378	return message;	1✔
379	});
380	}
381
382	} // namespace mavsdk

mavlink / MAVSDK / 7317183253

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