16233609062

Committed 12 Jul 2025 02:58AM UTC coverage: 45.799% (+0.6%) from 45.212%

Build # 16233609062

Build Type

Pull #2610

github

Committed by

web-flow

Commit Message

Merge 313266bea into 6c112e71f

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

Run Details

473 of 727 new or added lines in 14 files covered. (65.06%)

35 existing lines in 7 files now uncovered.

15955 of 34837 relevant lines covered (45.8%)

145116.67 hits per line

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

40.74

/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) :	6✔
21	PluginImplBase(std::move(system))	6✔
22	{
23	if (const char* env_p = std::getenv("MAVSDK_MAVLINK_DIRECT_DEBUGGING")) {	6✔
NEW 24	if (std::string(env_p) == "1") {	×
NEW 25	_debugging = true;	×
26	}
27	}
28	_system_impl->register_plugin(this);	6✔
29	}	6✔
30
31	MavlinkDirectImpl::~MavlinkDirectImpl()	12✔
32	{
33	_system_impl->unregister_plugin(this);	6✔
34	}	12✔
35
36	void MavlinkDirectImpl::init()	6✔
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	}	6✔
41
42	void MavlinkDirectImpl::deinit() {}	6✔
43
44	void MavlinkDirectImpl::enable() {}	6✔
45
46	void MavlinkDirectImpl::disable() {}	6✔
47
48	MavlinkDirect::Result MavlinkDirectImpl::send_message(MavlinkDirect::MavlinkMessage message)	4✔
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();	4✔
53	if (connections.empty()) {	4✔
NEW 54	return MavlinkDirect::Result::ConnectionError;	×
55	}
56
57	// Use the first connection to get the MessageSet
58	auto connection = connections[0];	4✔
59	auto libmav_receiver = connection->get_libmav_receiver();	4✔
60	if (!libmav_receiver) {	4✔
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);	4✔
66	if (!libmav_message_opt) {	4✔
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) {	4✔
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();	4✔
77
78	// Convert JSON fields to libmav message fields
79	if (!json_to_libmav_message(message.fields_json, libmav_message)) {	4✔
NEW 80	return MavlinkDirect::Result::InvalidField; // JSON conversion failed	×
81	}
82
83	// Set target system/component if specified
84	if (message.target_system != 0) {	4✔
85	// For messages that have target_system field, set it
NEW 86	libmav_message.set("target_system", static_cast<uint8_t>(message.target_system));	×
87	}
88	if (message.target_component != 0) {	4✔
89	// For messages that have target_component field, set it
NEW 90	libmav_message.set("target_component", static_cast<uint8_t>(message.target_component));	×
91	}
92
93	// Finalize the message (add header, CRC, etc.)
94	mav::Identifier sender{	4✔
95	static_cast<uint8_t>(message.system_id), static_cast<uint8_t>(message.component_id)};	4✔
96	auto message_size_opt = libmav_message.finalize(0, sender); // seq=0 for now	4✔
97	if (!message_size_opt) {	4✔
NEW 98	return MavlinkDirect::Result::Error; // Message finalization failed	×
99	}
100
101	// Convert libmav message back to mavlink_message_t for sending
102	mavlink_message_t mavlink_msg;	4✔
103	if (!libmav_receiver->libmav_to_mavlink_message(libmav_message, mavlink_msg)) {	4✔
NEW 104	return MavlinkDirect::Result::Error; // Conversion failed	×
105	}
106
107	// Send through existing connection interface
108	auto send_result = connection->send_message(mavlink_msg);	4✔
109	if (!send_result.first) {	4✔
NEW 110	return MavlinkDirect::Result::ConnectionError;	×
111	}
112
113	return MavlinkDirect::Result::Success;	4✔
114	}	4✔
115
116	MavlinkDirect::MessageHandle
NEW 117	MavlinkDirectImpl::subscribe_message(const MavlinkDirect::MessageCallback& callback)	×
118	{
NEW 119	std::lock_guard<std::mutex> lock(_message_callback_mutex);	×
120
121	// Create a proper handle
NEW 122	auto handle = _message_handle_factory.create();	×
123
NEW 124	_message_received_callback = callback;	×
NEW 125	_message_callback_handle = handle;	×
126
127	// Register for all libmav messages from the system
NEW 128	_system_impl->register_libmav_message_handler(	×
129	"", // Empty string means all messages
NEW 130	[this](const LibmavMessage& libmav_msg) {	×
NEW 131	std::lock_guard<std::mutex> callback_lock(_message_callback_mutex);	×
NEW 132	if (_message_received_callback) {	×
133	// Convert LibmavMessage to MavlinkDirect::MavlinkMessage
NEW 134	MavlinkDirect::MavlinkMessage message;	×
NEW 135	message.message_name = libmav_msg.message_name;	×
NEW 136	message.system_id = libmav_msg.system_id;	×
NEW 137	message.component_id = libmav_msg.component_id;	×
NEW 138	message.target_system = libmav_msg.target_system;	×
NEW 139	message.target_component = libmav_msg.target_component;	×
NEW 140	message.fields_json = libmav_msg.fields_json;	×
141
NEW 142	_message_received_callback(message);	×
NEW 143	}	×
NEW 144	},	×
145	this // Use 'this' as cookie for later unregistration
146	);
147
NEW 148	return handle;	×
NEW 149	}	×
150
NEW 151	void MavlinkDirectImpl::unsubscribe_message(MavlinkDirect::MessageHandle handle)	×
152	{
NEW 153	std::lock_guard<std::mutex> lock(_message_callback_mutex);	×
154
NEW 155	if (handle == _message_callback_handle) {	×
156	// Unregister from SystemImpl using our 'this' cookie
NEW 157	_system_impl->unregister_all_libmav_message_handlers(this);	×
158
159	// Clear the callback and reset handle
NEW 160	_message_received_callback = nullptr;	×
NEW 161	_message_callback_handle = MavlinkDirect::MessageHandle{};	×
162	}
NEW 163	}	×
164
165	MavlinkDirect::MessageTypeHandle MavlinkDirectImpl::subscribe_message_type(	3✔
166	std::string message_name, const MavlinkDirect::MessageTypeCallback& callback)
167	{
168	std::lock_guard<std::mutex> lock(_message_type_callbacks_mutex);	3✔
169
170	// Create a proper handle
171	auto handle = _message_type_handle_factory.create();	3✔
172
173	// Store the callback and message name mapped to the handle
174	_message_type_callbacks[handle] = callback;	3✔
175	_message_type_handle_to_name[handle] = message_name;	3✔
176
177	// Register with SystemImpl for this specific message type
178	_system_impl->register_libmav_message_handler(	3✔
179	message_name,
180	[this, handle](const LibmavMessage& libmav_msg) {	16✔
181	std::lock_guard<std::mutex> callback_lock(_message_type_callbacks_mutex);	4✔
182	auto it = _message_type_callbacks.find(handle);	4✔
183	if (it != _message_type_callbacks.end()) {	4✔
184	// Convert LibmavMessage to MavlinkDirect::MavlinkMessage
185	MavlinkDirect::MavlinkMessage message;	4✔
186	message.message_name = libmav_msg.message_name;	4✔
187	message.system_id = libmav_msg.system_id;	4✔
188	message.component_id = libmav_msg.component_id;	4✔
189	message.target_system = libmav_msg.target_system;	4✔
190	message.target_component = libmav_msg.target_component;	4✔
191	message.fields_json = libmav_msg.fields_json;	4✔
192
193	it->second(message);	4✔
194	}	4✔
195	},	4✔
196	&handle // Use handle address as cookie for specific unregistration
197	);
198
199	return handle;	6✔
200	}	3✔
201
NEW 202	void MavlinkDirectImpl::unsubscribe_message_type(MavlinkDirect::MessageTypeHandle handle)	×
203	{
NEW 204	std::lock_guard<std::mutex> lock(_message_type_callbacks_mutex);	×
205
206	// Find the message name for this handle
NEW 207	auto name_it = _message_type_handle_to_name.find(handle);	×
NEW 208	if (name_it != _message_type_handle_to_name.end()) {	×
NEW 209	const std::string& message_name = name_it->second;	×
210
211	// Unregister from SystemImpl using the handle address as cookie
NEW 212	_system_impl->unregister_libmav_message_handler(message_name, &handle);	×
213
214	// Remove from our callback maps
NEW 215	_message_type_callbacks.erase(handle);	×
NEW 216	_message_type_handle_to_name.erase(handle);	×
217	}
NEW 218	}	×
219
NEW 220	std::optional<uint32_t> MavlinkDirectImpl::message_name_to_id(const std::string& name) const	×
221	{
222	// Get connections to access LibmavReceiver
NEW 223	auto connections = _system_impl->get_connections();	×
NEW 224	if (connections.empty()) {	×
NEW 225	return std::nullopt;	×
226	}
227
NEW 228	auto connection = connections[0];	×
NEW 229	auto libmav_receiver = connection->get_libmav_receiver();	×
NEW 230	if (!libmav_receiver) {	×
NEW 231	return std::nullopt;	×
232	}
233
234	// Use LibmavReceiver's message name to ID conversion
NEW 235	auto id_opt = libmav_receiver->message_name_to_id(name);	×
NEW 236	if (id_opt.has_value()) {	×
NEW 237	return static_cast<uint32_t>(id_opt.value());	×
238	}
239
NEW 240	return std::nullopt;	×
NEW 241	}	×
242
NEW 243	std::optional<std::string> MavlinkDirectImpl::message_id_to_name(uint32_t id) const	×
244	{
245	// Get connections to access LibmavReceiver
NEW 246	auto connections = _system_impl->get_connections();	×
NEW 247	if (connections.empty()) {	×
NEW 248	return std::nullopt;	×
249	}
250
NEW 251	auto connection = connections[0];	×
NEW 252	auto libmav_receiver = connection->get_libmav_receiver();	×
NEW 253	if (!libmav_receiver) {	×
NEW 254	return std::nullopt;	×
255	}
256
257	// Use LibmavReceiver's message ID to name conversion
NEW 258	return libmav_receiver->message_id_to_name(id);	×
NEW 259	}	×
260
NEW 261	Json::Value MavlinkDirectImpl::libmav_to_json(const mav::Message& msg) const	×
262	{
263	(void)msg;
264	// TODO: Implement field iteration once libmav noexcept API is stable
NEW 265	return Json::Value();	×
266	}
267
NEW 268	bool MavlinkDirectImpl::json_to_libmav(const Json::Value& json, mav::Message& msg) const	×
269	{
270	(void)json;
271	(void)msg;
272	// TODO: Implement once libmav noexcept API set methods are stable
NEW 273	return false;	×
274	}
275
276	bool MavlinkDirectImpl::json_to_libmav_message(	4✔
277	const std::string& json_string, mav::Message& msg) const
278	{
279	Json::Value json;	4✔
280	Json::Reader reader;	4✔
281
282	if (!reader.parse(json_string, json)) {	4✔
NEW 283	LogErr() << "Failed to parse JSON: " << json_string;	×
NEW 284	return false;	×
285	}
286
287	// Iterate through all JSON fields and set them in the libmav message
288	for (const auto& field_name : json.getMemberNames()) {	29✔
289	const Json::Value& field_value = json[field_name];	25✔
290
291	// Convert JSON values to appropriate types and set in message
292	if (field_value.isInt()) {	25✔
293	auto result = msg.set(field_name, static_cast<int32_t>(field_value.asInt()));	22✔
294	if (result != mav::MessageResult::Success) {	22✔
295	// Try as other integer types
NEW 296	if (msg.set(field_name, static_cast<uint32_t>(field_value.asUInt())) !=	×
NEW 297	mav::MessageResult::Success &&	×
NEW 298	msg.set(field_name, static_cast<int16_t>(field_value.asInt())) !=	×
NEW 299	mav::MessageResult::Success &&	×
NEW 300	msg.set(field_name, static_cast<uint16_t>(field_value.asUInt())) !=	×
NEW 301	mav::MessageResult::Success &&	×
NEW 302	msg.set(field_name, static_cast<int8_t>(field_value.asInt())) !=	×
NEW 303	mav::MessageResult::Success &&	×
NEW 304	msg.set(field_name, static_cast<uint8_t>(field_value.asUInt())) !=	×
305	mav::MessageResult::Success) {
NEW 306	LogWarn() << "Failed to set integer field " << field_name << " = "	×
NEW 307	<< field_value.asInt();	×
308	}
309	}
310	} else if (field_value.isUInt()) {	3✔
NEW 311	auto result = msg.set(field_name, static_cast<uint32_t>(field_value.asUInt()));	×
NEW 312	if (result != mav::MessageResult::Success) {	×
313	// Try as other unsigned integer types
NEW 314	if (msg.set(field_name, static_cast<uint64_t>(field_value.asUInt64())) !=	×
NEW 315	mav::MessageResult::Success &&	×
NEW 316	msg.set(field_name, static_cast<uint16_t>(field_value.asUInt())) !=	×
NEW 317	mav::MessageResult::Success &&	×
NEW 318	msg.set(field_name, static_cast<uint8_t>(field_value.asUInt())) !=	×
319	mav::MessageResult::Success) {
NEW 320	LogWarn() << "Failed to set unsigned integer field " << field_name << " = "	×
NEW 321	<< field_value.asUInt();	×
322	}
323	}
324	} else if (field_value.isDouble()) {	3✔
NEW 325	auto result = msg.set(field_name, static_cast<float>(field_value.asFloat()));	×
NEW 326	if (result != mav::MessageResult::Success) {	×
327	// Try as double
NEW 328	if (msg.set(field_name, field_value.asDouble()) != mav::MessageResult::Success) {	×
NEW 329	LogWarn() << "Failed to set float/double field " << field_name << " = "	×
NEW 330	<< field_value.asDouble();	×
331	}
332	}
333	} else if (field_value.isString()) {	3✔
334	auto result = msg.setString(field_name, field_value.asString());	3✔
335	if (result != mav::MessageResult::Success) {	3✔
336	LogWarn() << "Failed to set string field " << field_name << " = "	2✔
337	<< field_value.asString();	2✔
338	}
339	} else {
NEW 340	LogWarn() << "Unsupported JSON field type for " << field_name;	×
341	}
342	}	4✔
343
344	return true;	4✔
345	}	4✔
346
347	} // namespace mavsdk

mavlink / MAVSDK / 16233609062

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