16485382075

Committed 24 Jul 2025 01:13AM UTC coverage: 46.329% (+1.2%) from 45.096%

Build # 16485382075

Build Type

push

github

Committed by

web-flow

Commit Message

Merge pull request #2610 from mavlink/pr-add-libmavlike

Integrate parts of libmav into MAVSDK and add MavlinkDirect plugin

Run Details

764 of 987 new or added lines in 14 files covered. (77.41%)

10 existing lines in 1 file now uncovered.

16272 of 35123 relevant lines covered (46.33%)

360.36 hits per line

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

59.05

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

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

mavlink / MAVSDK / 16485382075

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