16247473021

Committed 13 Jul 2025 09:04AM UTC coverage: 46.353% (+1.1%) from 45.212%

Build # 16247473021

Build Type

Pull #2610

github

Committed by

web-flow

Commit Message

Merge 679f9a2e4 into 6c112e71f

Pull Request Pull Request #2610: Integrate parts of libmav into MAVSDK and add MavlinkDirect plugin

Run Details

797 of 1029 new or added lines in 15 files covered. (77.45%)

26 existing lines in 5 files now uncovered.

16286 of 35135 relevant lines covered (46.35%)

147452.99 hits per line

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

56.36

/src/mavsdk/plugins/mavlink_direct/mavlink_direct_impl.cpp

1	#include "mavlink_direct_impl.h"
2	#include <mav/Message.h>
3	#include <variant>
4	#include <json/json.h>
5	#include "log.h"
6	#include "connection.h"
7
8	namespace mavsdk {
9
NEW 10	MavlinkDirectImpl::MavlinkDirectImpl(System& system) : PluginImplBase(system)	×
11	{
NEW 12	if (const char* env_p = std::getenv("MAVSDK_MAVLINK_DIRECT_DEBUGGING")) {	×
NEW 13	if (std::string(env_p) == "1") {	×
NEW 14	_debugging = true;	×
15	}
16	}
NEW 17	_system_impl->register_plugin(this);	×
NEW 18	}	×
19
20	MavlinkDirectImpl::MavlinkDirectImpl(std::shared_ptr<System> system) :	12✔
21	PluginImplBase(std::move(system))	12✔
22	{
23	if (const char* env_p = std::getenv("MAVSDK_MAVLINK_DIRECT_DEBUGGING")) {	12✔
NEW 24	if (std::string(env_p) == "1") {	×
NEW 25	_debugging = true;	×
26	}
27	}
28	_system_impl->register_plugin(this);	12✔
29	}	12✔
30
31	MavlinkDirectImpl::~MavlinkDirectImpl()	24✔
32	{
33	_system_impl->unregister_plugin(this);	12✔
34	}	24✔
35
36	void MavlinkDirectImpl::init()	12✔
37	{
38	// No need to initialize MessageSet here - LibmavReceiver handles that
39	// No need to register for mavlink messages anymore - we'll use libmav subscription system
40	}	12✔
41
42	void MavlinkDirectImpl::deinit() {}	12✔
43
44	void MavlinkDirectImpl::enable() {}	12✔
45
46	void MavlinkDirectImpl::disable() {}	12✔
47
48	MavlinkDirect::Result MavlinkDirectImpl::send_message(MavlinkDirect::MavlinkMessage message)	8✔
49	{
50	// Get access to the LibmavReceiver's MessageSet through the system
51	// For now, we'll access it through the connection's libmav receiver
52	auto connections = _system_impl->get_connections();	8✔
53	if (connections.empty()) {	8✔
NEW 54	return MavlinkDirect::Result::ConnectionError;	×
55	}
56
57	// Use the first connection to get the MessageSet
58	auto connection = connections[0];	8✔
59	auto libmav_receiver = connection->get_libmav_receiver();	8✔
60	if (!libmav_receiver) {	8✔
NEW 61	return MavlinkDirect::Result::ConnectionError;	×
62	}
63
64	// Create libmav message from the message name
65	auto libmav_message_opt = libmav_receiver->create_message(message.message_name);	8✔
66	if (!libmav_message_opt) {	8✔
NEW 67	LogErr() << "Failed to create message: " << message.message_name;	×
NEW 68	return MavlinkDirect::Result::InvalidMessage; // Message type not found	×
69	}
70
71	if (_debugging) {	8✔
NEW 72	LogDebug() << "Created message " << message.message_name	×
NEW 73	<< " with ID: " << libmav_message_opt.value().id();	×
74	}
75
76	auto libmav_message = libmav_message_opt.value();	8✔
77
78	// Convert JSON fields to libmav message fields
79	if (!json_to_libmav_message(message.fields_json, libmav_message)) {	8✔
NEW 80	LogErr() << "Failed to convert JSON fields to libmav message for " << message.message_name;	×
NEW 81	return MavlinkDirect::Result::InvalidField; // JSON conversion failed	×
82	}
83
84	if (_debugging) {	8✔
NEW 85	LogDebug() << "Successfully populated fields for " << message.message_name;	×
86	}
87
88	// Set target system/component if specified
89	if (message.target_system != 0) {	8✔
90	// For messages that have target_system field, set it
NEW 91	libmav_message.set("target_system", static_cast<uint8_t>(message.target_system));	×
92	}
93	if (message.target_component != 0) {	8✔
94	// For messages that have target_component field, set it
NEW 95	libmav_message.set("target_component", static_cast<uint8_t>(message.target_component));	×
96	}
97
98	if (_debugging) {	8✔
NEW 99	LogDebug() << "Sending " << message.message_name << " via unified libmav API";	×
100	}
101
102	_system_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	8✔
103	mavlink_message_t mavlink_message;
104
105	// Use clean libmav helper methods to get payload data
106	auto payload_view = libmav_message.getPayloadView();	8✔
107	const uint8_t payload_length = libmav_message.getPayloadLength();	8✔
108
109	// Set up the mavlink_message_t structure
110	mavlink_message.msgid = libmav_message.id();	8✔
111	mavlink_message.len = payload_length;	8✔
112	memcpy(mavlink_message.payload64, payload_view.first, payload_length);	8✔
113
114	mavlink_finalize_message_chan(	16✔
115	&mavlink_message,
116	mavlink_address.system_id,	8✔
117	mavlink_address.component_id,	8✔
118	channel,
119	payload_length,
120	libmav_message.type().maxPayloadSize(),	8✔
121	libmav_message.type().crcExtra());	8✔
122
123	return mavlink_message;	8✔
124	});
125
126	if (_debugging) {	8✔
NEW 127	LogDebug() << "Successfully sent " << message.message_name << " as raw data";	×
128	}
129
130	return MavlinkDirect::Result::Success;	8✔
131	}	8✔
132
133	MavlinkDirect::MessageHandle MavlinkDirectImpl::subscribe_message(	6✔
134	std::string message_name, const MavlinkDirect::MessageCallback& callback)
135	{
136	std::lock_guard<std::mutex> lock(_message_callbacks_mutex);	6✔
137
138	// Create a proper handle
139	auto handle = _message_handle_factory.create();	6✔
140
141	// Store the callback and message name mapped to the handle
142	_message_callbacks[handle] = callback;	6✔
143	_message_handle_to_name[handle] = message_name;	6✔
144
145	// Register with SystemImpl
146	_system_impl->register_libmav_message_handler(	6✔
147	message_name, // Empty string means all messages, specific name means filtered
148	[this, handle](const LibmavMessage& libmav_msg) {	32✔
149	std::lock_guard<std::mutex> callback_lock(_message_callbacks_mutex);	8✔
150	auto it = _message_callbacks.find(handle);	8✔
151	if (it != _message_callbacks.end()) {	8✔
152	// Convert LibmavMessage to MavlinkDirect::MavlinkMessage
153	MavlinkDirect::MavlinkMessage message;	8✔
154	message.message_name = libmav_msg.message_name;	8✔
155	message.system_id = libmav_msg.system_id;	8✔
156	message.component_id = libmav_msg.component_id;	8✔
157	message.target_system = libmav_msg.target_system;	8✔
158	message.target_component = libmav_msg.target_component;	8✔
159	message.fields_json = libmav_msg.fields_json;	8✔
160
161	it->second(message);	8✔
162	}	8✔
163	},	8✔
164	&handle // Use handle address as cookie for specific unregistration
165	);
166
167	return handle;	12✔
168	}	6✔
169
170	void MavlinkDirectImpl::unsubscribe_message(MavlinkDirect::MessageHandle handle)	6✔
171	{
172	std::lock_guard<std::mutex> lock(_message_callbacks_mutex);	6✔
173
174	// Find the message name for this handle
175	auto name_it = _message_handle_to_name.find(handle);	6✔
176	if (name_it != _message_handle_to_name.end()) {	6✔
177	const std::string& message_name = name_it->second;	6✔
178
179	// Unregister from SystemImpl using the handle address as cookie
180	_system_impl->unregister_libmav_message_handler(message_name, &handle);	6✔
181
182	// Remove from our callback maps
183	_message_callbacks.erase(handle);	6✔
184	_message_handle_to_name.erase(handle);	6✔
185	}
186	}	6✔
187
NEW 188	std::optional<uint32_t> MavlinkDirectImpl::message_name_to_id(const std::string& name) const	×
189	{
190	// Get connections to access LibmavReceiver
NEW 191	auto connections = _system_impl->get_connections();	×
NEW 192	if (connections.empty()) {	×
NEW 193	return std::nullopt;	×
194	}
195
NEW 196	auto connection = connections[0];	×
NEW 197	auto libmav_receiver = connection->get_libmav_receiver();	×
NEW 198	if (!libmav_receiver) {	×
NEW 199	return std::nullopt;	×
200	}
201
202	// Use LibmavReceiver's message name to ID conversion
NEW 203	auto id_opt = libmav_receiver->message_name_to_id(name);	×
NEW 204	if (id_opt.has_value()) {	×
NEW 205	return static_cast<uint32_t>(id_opt.value());	×
206	}
207
NEW 208	return std::nullopt;	×
NEW 209	}	×
210
NEW 211	std::optional<std::string> MavlinkDirectImpl::message_id_to_name(uint32_t id) const	×
212	{
213	// Get connections to access LibmavReceiver
NEW 214	auto connections = _system_impl->get_connections();	×
NEW 215	if (connections.empty()) {	×
NEW 216	return std::nullopt;	×
217	}
218
NEW 219	auto connection = connections[0];	×
NEW 220	auto libmav_receiver = connection->get_libmav_receiver();	×
NEW 221	if (!libmav_receiver) {	×
NEW 222	return std::nullopt;	×
223	}
224
225	// Use LibmavReceiver's message ID to name conversion
NEW 226	return libmav_receiver->message_id_to_name(id);	×
NEW 227	}	×
228
NEW 229	Json::Value MavlinkDirectImpl::libmav_to_json(const mav::Message& msg) const	×
230	{
231	(void)msg;
232	// TODO: Implement field iteration once libmav noexcept API is stable
NEW 233	return Json::Value();	×
234	}
235
NEW 236	bool MavlinkDirectImpl::json_to_libmav(const Json::Value& json, mav::Message& msg) const	×
237	{
238	(void)json;
239	(void)msg;
240	// TODO: Implement once libmav noexcept API set methods are stable
NEW 241	return false;	×
242	}
243
244	bool MavlinkDirectImpl::json_to_libmav_message(	8✔
245	const std::string& json_string, mav::Message& msg) const
246	{
247	Json::Value json;	8✔
248	Json::Reader reader;	8✔
249
250	if (!reader.parse(json_string, json)) {	8✔
NEW 251	LogErr() << "Failed to parse JSON: " << json_string;	×
NEW 252	return false;	×
253	}
254
255	// Iterate through all JSON fields and set them in the libmav message
256	for (const auto& field_name : json.getMemberNames()) {	72✔
257	const Json::Value& field_value = json[field_name];	64✔
258
259	// Convert JSON values to appropriate types and set in message
260	if (field_value.isInt()) {	64✔
261	auto result = msg.set(field_name, static_cast<int32_t>(field_value.asInt()));	54✔
262	if (result != mav::MessageResult::Success) {	54✔
263	// Try as other integer types
NEW 264	if (msg.set(field_name, static_cast<uint32_t>(field_value.asUInt())) !=	×
NEW 265	mav::MessageResult::Success &&	×
NEW 266	msg.set(field_name, static_cast<int16_t>(field_value.asInt())) !=	×
NEW 267	mav::MessageResult::Success &&	×
NEW 268	msg.set(field_name, static_cast<uint16_t>(field_value.asUInt())) !=	×
NEW 269	mav::MessageResult::Success &&	×
NEW 270	msg.set(field_name, static_cast<int8_t>(field_value.asInt())) !=	×
NEW 271	mav::MessageResult::Success &&	×
NEW 272	msg.set(field_name, static_cast<uint8_t>(field_value.asUInt())) !=	×
273	mav::MessageResult::Success) {
NEW 274	LogWarn() << "Failed to set integer field " << field_name << " = "	×
NEW 275	<< field_value.asInt();	×
276	}
277	}
278	} else if (field_value.isUInt()) {	10✔
NEW 279	auto result = msg.set(field_name, static_cast<uint32_t>(field_value.asUInt()));	×
NEW 280	if (result != mav::MessageResult::Success) {	×
281	// Try as other unsigned integer types
NEW 282	if (msg.set(field_name, static_cast<uint64_t>(field_value.asUInt64())) !=	×
NEW 283	mav::MessageResult::Success &&	×
NEW 284	msg.set(field_name, static_cast<uint16_t>(field_value.asUInt())) !=	×
NEW 285	mav::MessageResult::Success &&	×
NEW 286	msg.set(field_name, static_cast<uint8_t>(field_value.asUInt())) !=	×
287	mav::MessageResult::Success) {
NEW 288	LogWarn() << "Failed to set unsigned integer field " << field_name << " = "	×
NEW 289	<< field_value.asUInt();	×
290	}
291	}
292	} else if (field_value.isDouble()) {	10✔
NEW 293	auto result = msg.set(field_name, static_cast<float>(field_value.asFloat()));	×
NEW 294	if (result != mav::MessageResult::Success) {	×
295	// Try as double
NEW 296	if (msg.set(field_name, field_value.asDouble()) != mav::MessageResult::Success) {	×
NEW 297	LogWarn() << "Failed to set float/double field " << field_name << " = "	×
NEW 298	<< field_value.asDouble();	×
299	}
300	}
301	} else if (field_value.isString()) {	10✔
NEW 302	auto result = msg.setString(field_name, field_value.asString());	×
NEW 303	if (result != mav::MessageResult::Success) {	×
NEW 304	LogWarn() << "Failed to set string field " << field_name << " = "	×
NEW 305	<< field_value.asString();	×
306	}
307	} else if (field_value.isArray()) {	10✔
308	// Handle array fields
309	auto array_size = field_value.size();	10✔
310
311	// Try different vector types based on typical MAVLink array field types
312	std::vector<uint8_t> uint8_vec;	10✔
313	std::vector<uint16_t> uint16_vec;	10✔
314	std::vector<uint32_t> uint32_vec;	10✔
315	std::vector<int8_t> int8_vec;	10✔
316	std::vector<int16_t> int16_vec;	10✔
317	std::vector<int32_t> int32_vec;	10✔
318	std::vector<float> float_vec;	10✔
319	std::vector<double> double_vec;	10✔
320
321	// Convert JSON array to vectors of different types
322	uint8_vec.reserve(array_size);	10✔
323	uint16_vec.reserve(array_size);	10✔
324	uint32_vec.reserve(array_size);	10✔
325	int8_vec.reserve(array_size);	10✔
326	int16_vec.reserve(array_size);	10✔
327	int32_vec.reserve(array_size);	10✔
328	float_vec.reserve(array_size);	10✔
329	double_vec.reserve(array_size);	10✔
330
331	for (Json::ArrayIndex i = 0; i < array_size; ++i) {	210✔
332	const auto& elem = field_value[i];	200✔
333	if (elem.isNumeric()) {	200✔
334	uint8_vec.push_back(static_cast<uint8_t>(elem.asUInt()));	200✔
335	uint16_vec.push_back(static_cast<uint16_t>(elem.asUInt()));	200✔
336	uint32_vec.push_back(static_cast<uint32_t>(elem.asUInt()));	200✔
337	int8_vec.push_back(static_cast<int8_t>(elem.asInt()));	200✔
338	int16_vec.push_back(static_cast<int16_t>(elem.asInt()));	200✔
339	int32_vec.push_back(static_cast<int32_t>(elem.asInt()));	200✔
340	float_vec.push_back(static_cast<float>(elem.asFloat()));	200✔
341	double_vec.push_back(elem.asDouble());	200✔
342	} else {
343	// Default to 0 for non-numeric values
NEW 344	uint8_vec.push_back(0);	×
NEW 345	uint16_vec.push_back(0);	×
NEW 346	uint32_vec.push_back(0);	×
NEW 347	int8_vec.push_back(0);	×
NEW 348	int16_vec.push_back(0);	×
NEW 349	int32_vec.push_back(0);	×
NEW 350	float_vec.push_back(0.0f);	×
NEW 351	double_vec.push_back(0.0);	×
352	}
353	}
354
355	// Try to set the array field with different vector types
356	if (msg.set(field_name, uint8_vec) == mav::MessageResult::Success \|\|	10✔
NEW 357	msg.set(field_name, uint16_vec) == mav::MessageResult::Success \|\|	×
NEW 358	msg.set(field_name, uint32_vec) == mav::MessageResult::Success \|\|	×
NEW 359	msg.set(field_name, int8_vec) == mav::MessageResult::Success \|\|	×
NEW 360	msg.set(field_name, int16_vec) == mav::MessageResult::Success \|\|	×
NEW 361	msg.set(field_name, int32_vec) == mav::MessageResult::Success \|\|	×
362	msg.set(field_name, float_vec) == mav::MessageResult::Success \|\|	10✔
NEW 363	msg.set(field_name, double_vec) == mav::MessageResult::Success) {	×
364	// Successfully set the array field
365	} else {
NEW 366	LogWarn() << "Failed to set array field " << field_name;	×
367	}
368	} else {	10✔
NEW 369	LogWarn() << "Unsupported JSON field type for " << field_name;	×
370	}
371	}	8✔
372
373	return true;	8✔
374	}	8✔
375
376	MavlinkDirect::Result MavlinkDirectImpl::load_custom_xml(const std::string& xml_content)	2✔
377	{
378	// Get access to the LibmavReceiver through the system connections
379	auto connections = _system_impl->get_connections();	2✔
380	if (connections.empty()) {	2✔
NEW 381	LogErr() << "No connections available for loading custom XML";	×
NEW 382	return MavlinkDirect::Result::ConnectionError;	×
383	}
384
385	// Use the first connection to get the MessageSet
386	auto connection = connections[0];	2✔
387	auto libmav_receiver = connection->get_libmav_receiver();	2✔
388	if (!libmav_receiver) {	2✔
NEW 389	LogErr() << "LibmavReceiver not available";	×
NEW 390	return MavlinkDirect::Result::ConnectionError;	×
391	}
392
393	if (_debugging) {	2✔
NEW 394	LogDebug() << "Loading custom XML definitions...";	×
395	}
396
397	// Load the custom XML into the MessageSet
398	bool success = libmav_receiver->load_custom_xml(xml_content);	2✔
399
400	if (success) {	2✔
401	if (_debugging) {	2✔
NEW 402	LogDebug() << "Successfully loaded custom XML definitions";	×
403	}
404	return MavlinkDirect::Result::Success;	2✔
405	} else {
NEW 406	LogErr() << "Failed to load custom XML definitions";	×
NEW 407	return MavlinkDirect::Result::Error;	×
408	}
409	}	2✔
410
411	} // namespace mavsdk

mavlink / MAVSDK / 16247473021

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