17421280055

Committed 03 Sep 2025 02:29AM UTC coverage: 47.241% (-0.02%) from 47.262%

Build # 17421280055

Build Type

push

github

Committed by

web-flow

Commit Message

Merge pull request #2646 from mavlink/pr-null-not-nan

mavlink_direct: handle NaN correctly

Run Details

126 of 232 new or added lines in 3 files covered. (54.31%)

41 existing lines in 8 files now uncovered.

16781 of 35522 relevant lines covered (47.24%)

442.39 hits per line

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

44.44

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

#include "mavlink_direct_impl.h"
#include <mav/Message.h>
#include <mav/MessageSet.h>
#include <variant>
#include <json/json.h>
#include <cmath>
#include <limits>
#include "log.h"
#include "connection.h"
#include "callback_list.tpp"

namespace mavsdk {

template class CallbackList<MavlinkDirect::MavlinkMessage>;

MavlinkDirectImpl::MavlinkDirectImpl(System& system) : PluginImplBase(system)
{
    if (const char* env_p = std::getenv("MAVSDK_MAVLINK_DIRECT_DEBUGGING")) {
        if (std::string(env_p) == "1") {
            _debugging = true;
        }
    }
    _system_impl->register_plugin(this);
}

MavlinkDirectImpl::MavlinkDirectImpl(std::shared_ptr<System> system) :
    PluginImplBase(std::move(system))
{
    if (const char* env_p = std::getenv("MAVSDK_MAVLINK_DIRECT_DEBUGGING")) {
        if (std::string(env_p) == "1") {
            _debugging = true;
        }
    }
    _system_impl->register_plugin(this);
}

MavlinkDirectImpl::~MavlinkDirectImpl()
{
    _system_impl->unregister_plugin(this);
}

void MavlinkDirectImpl::init()
{
    // No need to initialize MessageSet here - LibmavReceiver handles that
    // No need to register for mavlink messages anymore - we'll use libmav subscription system
}

void MavlinkDirectImpl::deinit()
{
    // Clean up all libmav message handlers registered by this plugin
    _system_impl->unregister_all_libmav_message_handlers(this);

    // Clear internal state
    _handle_to_message_name.clear();
    _message_subscriptions.clear();
}

void MavlinkDirectImpl::enable() {}

void MavlinkDirectImpl::disable() {}

MavlinkDirect::Result MavlinkDirectImpl::send_message(MavlinkDirect::MavlinkMessage message)
{
    // Get access to the MessageSet through the system
    auto& message_set = _system_impl->get_message_set();

    // Create libmav message from the message name
    auto libmav_message_opt = message_set.create(message.message_name);
    if (!libmav_message_opt) {
        LogErr() << "Failed to create message: " << message.message_name;
        return MavlinkDirect::Result::InvalidMessage; // Message type not found
    }

    if (_debugging) {
        LogDebug() << "Created message " << message.message_name
                   << " with ID: " << libmav_message_opt.value().id();
    }

    auto libmav_message = libmav_message_opt.value();

    // Convert JSON fields to libmav message fields
    if (!json_to_libmav_message(message.fields_json, libmav_message)) {
        LogErr() << "Failed to convert JSON fields to libmav message for " << message.message_name;
        return MavlinkDirect::Result::InvalidField; // JSON conversion failed
    }

    if (_debugging) {
        LogDebug() << "Successfully populated fields for " << message.message_name;
    }

    // Set target system/component if specified
    if (message.target_system_id != 0) {
        // For messages that have target_system field, set it
        libmav_message.set("target_system", static_cast<uint8_t>(message.target_system_id));
    }
    if (message.target_component_id != 0) {
        // For messages that have target_component field, set it
        libmav_message.set(
            "target_component_id", static_cast<uint8_t>(message.target_component_id));
    }

    if (_debugging) {
        LogDebug() << "Sending " << message.message_name << " via unified libmav API";
    }

    _system_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {
        mavlink_message_t mavlink_message;

        // Use clean libmav helper methods to get payload data
        auto payload_view = libmav_message.getPayloadView();
        const uint8_t payload_length = libmav_message.getPayloadLength();

        // Set up the mavlink_message_t structure
        mavlink_message.msgid = libmav_message.id();
        mavlink_message.len = payload_length;
        memcpy(mavlink_message.payload64, payload_view.first, payload_length);

        mavlink_finalize_message_chan(
            &mavlink_message,
            mavlink_address.system_id,
            mavlink_address.component_id,
            channel,
            payload_length,
            libmav_message.type().maxPayloadSize(),
            libmav_message.type().crcExtra());

        return mavlink_message;
    });

    if (_debugging) {
        LogDebug() << "Successfully sent " << message.message_name << " as raw data";
    }

    return MavlinkDirect::Result::Success;
}

MavlinkDirect::MessageHandle MavlinkDirectImpl::subscribe_message(
    std::string message_name, const MavlinkDirect::MessageCallback& callback)
{
    // Subscribe using CallbackList pattern - this handles thread safety internally
    auto handle = _message_subscriptions.subscribe(callback);

    // Store message name for this handle (CallbackList protects this too)
    _handle_to_message_name[handle] = message_name;

    // Register with SystemImpl - no lock-order-inversion because CallbackList handles
    // synchronization
    _system_impl->register_libmav_message_handler(
        message_name,
        [this](const Mavsdk::MavlinkMessage& message) {
            // Use CallbackList::queue to safely call all subscribed callbacks
            // Convert Mavsdk::MavlinkMessage to MavlinkDirect::MavlinkMessage (they're identical)
            MavlinkDirect::MavlinkMessage direct_message;
            direct_message.message_name = message.message_name;
            direct_message.system_id = message.system_id;
            direct_message.component_id = message.component_id;
            direct_message.target_system_id = message.target_system_id;
            direct_message.target_component_id = message.target_component_id;
            direct_message.fields_json = message.fields_json;

            _message_subscriptions.queue(direct_message, [this](const auto& func) {
                _system_impl->call_user_callback(func);
            });
        },
        &handle // Use handle address as cookie for specific unregistration
    );

    return handle;
}

void MavlinkDirectImpl::unsubscribe_message(MavlinkDirect::MessageHandle handle)
{
    // Get the message name for unregistration
    auto name_it = _handle_to_message_name.find(handle);
    if (name_it == _handle_to_message_name.end()) {
        return; // Handle not found, nothing to unsubscribe
    }
    std::string message_name = name_it->second;

    // Unregister from SystemImpl - no lock-order-inversion with CallbackList pattern
    _system_impl->unregister_libmav_message_handler(message_name, &handle);

    // Remove from CallbackList and message name map - CallbackList handles thread safety
    _message_subscriptions.unsubscribe(handle);
    _handle_to_message_name.erase(handle);
}

std::optional<uint32_t> MavlinkDirectImpl::message_name_to_id(const std::string& name) const
{
    // Get MessageSet to access message definitions
    auto& message_set = _system_impl->get_message_set();

    // Use MessageSet's message name to ID conversion
    auto id_opt = message_set.idForMessage(name);
    if (id_opt.has_value()) {
        return static_cast<uint32_t>(id_opt.value());
    }

    return std::nullopt;
}

std::optional<std::string> MavlinkDirectImpl::message_id_to_name(uint32_t id) const
{
    // Get MessageSet to access message definitions
    auto& message_set = _system_impl->get_message_set();

    // Use MessageSet's message ID to name conversion
    auto message_def = message_set.getMessageDefinition(static_cast<int>(id));
    if (message_def) {
        return message_def.get().name();
    }
    return std::nullopt;
}

Json::Value MavlinkDirectImpl::libmav_to_json(const mav::Message& msg) const
{
    (void)msg;
    // TODO: Implement field iteration once libmav noexcept API is stable
    return Json::Value();
}

bool MavlinkDirectImpl::json_to_libmav(const Json::Value& json, mav::Message& msg) const
{
    (void)json;
    (void)msg;
    // TODO: Implement once libmav noexcept API set methods are stable
    return false;
}

bool MavlinkDirectImpl::json_to_libmav_message(
    const std::string& json_string, mav::Message& msg) const
{
    Json::Value json;
    Json::Reader reader;

    if (!reader.parse(json_string, json)) {
        LogErr() << "Failed to parse JSON: " << json_string;
        return false;
    }

    // Iterate through all JSON fields and set them in the libmav message
    for (const auto& field_name : json.getMemberNames()) {
        const Json::Value& field_value = json[field_name];

        // Convert JSON values to appropriate types and set in message
        if (field_value.isInt()) {
            auto result = msg.set(field_name, static_cast<int32_t>(field_value.asInt()));
            if (result != ::mav::MessageResult::Success) {
                // Try as other integer types
                if (msg.set(field_name, static_cast<uint32_t>(field_value.asUInt())) !=
                        ::mav::MessageResult::Success &&
                    msg.set(field_name, static_cast<int16_t>(field_value.asInt())) !=
                        ::mav::MessageResult::Success &&
                    msg.set(field_name, static_cast<uint16_t>(field_value.asUInt())) !=
                        ::mav::MessageResult::Success &&
                    msg.set(field_name, static_cast<int8_t>(field_value.asInt())) !=
                        ::mav::MessageResult::Success &&
                    msg.set(field_name, static_cast<uint8_t>(field_value.asUInt())) !=
                        ::mav::MessageResult::Success) {
                    LogWarn() << "Failed to set integer field " << field_name << " = "
                              << field_value.asInt();
                }
            }
        } else if (field_value.isUInt()) {
            auto result = msg.set(field_name, static_cast<uint32_t>(field_value.asUInt()));
            if (result != ::mav::MessageResult::Success) {
                // Try as other unsigned integer types
                if (msg.set(field_name, static_cast<uint64_t>(field_value.asUInt64())) !=
                        ::mav::MessageResult::Success &&
                    msg.set(field_name, static_cast<uint16_t>(field_value.asUInt())) !=
                        ::mav::MessageResult::Success &&
                    msg.set(field_name, static_cast<uint8_t>(field_value.asUInt())) !=
                        ::mav::MessageResult::Success) {
                    LogWarn() << "Failed to set unsigned integer field " << field_name << " = "
                              << field_value.asUInt();
                }
            }
        } else if (field_value.isNull() || field_value.isDouble()) {
            // Handle float/double values (including null -> NaN)
            auto field_opt = msg.type().getField(field_name);
            if (!field_opt) {
                LogWarn() << "Field " << field_name << " not found in message definition";
                continue;
            }

            auto field = field_opt.value();
            ::mav::MessageResult result = ::mav::MessageResult::FieldNotFound;

            if (field.type.base_type == ::mav::FieldType::BaseType::FLOAT) {
                if (field_value.isNull()) {
                    result = msg.set(field_name, std::numeric_limits<float>::quiet_NaN());
                } else {
                    result = msg.set(field_name, static_cast<float>(field_value.asFloat()));
                }
            } else if (field.type.base_type == ::mav::FieldType::BaseType::DOUBLE) {
                if (field_value.isNull()) {
                    result = msg.set(field_name, std::numeric_limits<double>::quiet_NaN());
                } else {
                    result = msg.set(field_name, field_value.asDouble());
                }
            } else {
                LogWarn() << "Field " << field_name << " is not a float or double field";
                continue;
            }

            if (result != ::mav::MessageResult::Success) {
                LogWarn() << "Failed to set float/double field " << field_name << " = "
                          << (field_value.isNull() ? "null" :
                                                     std::to_string(field_value.asDouble()));
            }
        } else if (field_value.isString()) {
            auto result = msg.setString(field_name, field_value.asString());
            if (result != ::mav::MessageResult::Success) {
                LogWarn() << "Failed to set string field " << field_name << " = "
                          << field_value.asString();
            }
        } else if (field_value.isArray()) {
            // Handle array fields with proper type detection
            auto array_size = field_value.size();

            // Get field definition to determine correct type
            auto field_opt = msg.type().getField(field_name);
            if (!field_opt) {
                LogWarn() << "Field " << field_name << " not found in message definition";
                continue;
            }

            auto field = field_opt.value();
            if (field.type.size <= 1) {
                LogWarn() << "Field " << field_name << " is not an array field";
                continue;
            }

            // Create and populate only the correct vector type
            ::mav::MessageResult result = ::mav::MessageResult::FieldNotFound;
            switch (field.type.base_type) {
                case ::mav::FieldType::BaseType::UINT8: {
                    std::vector<uint8_t> vec;
                    vec.reserve(array_size);
                    for (Json::ArrayIndex i = 0; i < array_size; ++i) {
                        const auto& elem = field_value[i];
                        vec.push_back(elem.isNumeric() ? static_cast<uint8_t>(elem.asUInt()) : 0);
                    }
                    result = msg.set(field_name, vec);
                    break;
                }
                case ::mav::FieldType::BaseType::UINT16: {
                    std::vector<uint16_t> vec;
                    vec.reserve(array_size);
                    for (Json::ArrayIndex i = 0; i < array_size; ++i) {
                        const auto& elem = field_value[i];
                        vec.push_back(elem.isNumeric() ? static_cast<uint16_t>(elem.asUInt()) : 0);
                    }
                    result = msg.set(field_name, vec);
                    break;
                }
                case ::mav::FieldType::BaseType::UINT32: {
                    std::vector<uint32_t> vec;
                    vec.reserve(array_size);
                    for (Json::ArrayIndex i = 0; i < array_size; ++i) {
                        const auto& elem = field_value[i];
                        vec.push_back(elem.isNumeric() ? static_cast<uint32_t>(elem.asUInt()) : 0);
                    }
                    result = msg.set(field_name, vec);
                    break;
                }
                case ::mav::FieldType::BaseType::UINT64: {
                    std::vector<uint64_t> vec;
                    vec.reserve(array_size);
                    for (Json::ArrayIndex i = 0; i < array_size; ++i) {
                        const auto& elem = field_value[i];
                        vec.push_back(
                            elem.isNumeric() ? static_cast<uint64_t>(elem.asUInt64()) : 0);
                    }
                    result = msg.set(field_name, vec);
                    break;
                }
                case ::mav::FieldType::BaseType::INT8: {
                    std::vector<int8_t> vec;
                    vec.reserve(array_size);
                    for (Json::ArrayIndex i = 0; i < array_size; ++i) {
                        const auto& elem = field_value[i];
                        vec.push_back(elem.isNumeric() ? static_cast<int8_t>(elem.asInt()) : 0);
                    }
                    result = msg.set(field_name, vec);
                    break;
                }
                case ::mav::FieldType::BaseType::INT16: {
                    std::vector<int16_t> vec;
                    vec.reserve(array_size);
                    for (Json::ArrayIndex i = 0; i < array_size; ++i) {
                        const auto& elem = field_value[i];
                        vec.push_back(elem.isNumeric() ? static_cast<int16_t>(elem.asInt()) : 0);
                    }
                    result = msg.set(field_name, vec);
                    break;
                }
                case ::mav::FieldType::BaseType::INT32: {
                    std::vector<int32_t> vec;
                    vec.reserve(array_size);
                    for (Json::ArrayIndex i = 0; i < array_size; ++i) {
                        const auto& elem = field_value[i];
                        vec.push_back(elem.isNumeric() ? static_cast<int32_t>(elem.asInt()) : 0);
                    }
                    result = msg.set(field_name, vec);
                    break;
                }
                case ::mav::FieldType::BaseType::INT64: {
                    std::vector<int64_t> vec;
                    vec.reserve(array_size);
                    for (Json::ArrayIndex i = 0; i < array_size; ++i) {
                        const auto& elem = field_value[i];
                        vec.push_back(elem.isNumeric() ? static_cast<int64_t>(elem.asInt64()) : 0);
                    }
                    result = msg.set(field_name, vec);
                    break;
                }
                case ::mav::FieldType::BaseType::FLOAT: {
                    std::vector<float> vec;
                    vec.reserve(array_size);
                    for (Json::ArrayIndex i = 0; i < array_size; ++i) {
                        const auto& elem = field_value[i];
                        if (elem.isNumeric()) {
                            vec.push_back(static_cast<float>(elem.asFloat()));
                        } else {
                            // For non-numeric values (including null), use NaN
                            vec.push_back(std::numeric_limits<float>::quiet_NaN());
                        }
                    }
                    result = msg.set(field_name, vec);
                    break;
                }
                case ::mav::FieldType::BaseType::DOUBLE: {
                    std::vector<double> vec;
                    vec.reserve(array_size);
                    for (Json::ArrayIndex i = 0; i < array_size; ++i) {
                        const auto& elem = field_value[i];
                        if (elem.isNumeric()) {
                            vec.push_back(elem.asDouble());
                        } else {
                            // For non-numeric values (including null), use NaN
                            vec.push_back(std::numeric_limits<double>::quiet_NaN());
                        }
                    }
                    result = msg.set(field_name, vec);
                    break;
                }
                case ::mav::FieldType::BaseType::CHAR: {
                    std::vector<char> vec;
                    vec.reserve(array_size);
                    for (Json::ArrayIndex i = 0; i < array_size; ++i) {
                        const auto& elem = field_value[i];
                        vec.push_back(elem.isNumeric() ? static_cast<char>(elem.asInt()) : 0);
                    }
                    result = msg.set(field_name, vec);
                    break;
                }
                default:
                    LogWarn() << "Unsupported array field type for " << field_name;
                    break;
            }

            if (result != ::mav::MessageResult::Success) {
                LogWarn() << "Failed to set array field " << field_name;
            }
        } else {
            LogWarn() << "Unsupported JSON field type for " << field_name;
        }
    }

    return true;
}

MavlinkDirect::Result MavlinkDirectImpl::load_custom_xml(const std::string& xml_content)
{
    if (_debugging) {
        LogDebug() << "Loading custom XML definitions...";
    }

    // Load the custom XML into the MessageSet with thread safety
    // This uses the thread-safe method that protects against concurrent read operations
    bool success = _system_impl->load_custom_xml_to_message_set(xml_content);

    if (success) {
        if (_debugging) {
            LogDebug() << "Successfully loaded custom XML definitions";
        }
        return MavlinkDirect::Result::Success;
    } else {
        LogErr() << "Failed to load custom XML definitions";
        return MavlinkDirect::Result::Error;
    }
}

} // namespace mavsdk

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 <cmath>
7	#include <limits>
8	#include "log.h"
9	#include "connection.h"
10	#include "callback_list.tpp"
11
12	namespace mavsdk {
13
14	template class CallbackList<MavlinkDirect::MavlinkMessage>;
15
16	MavlinkDirectImpl::MavlinkDirectImpl(System& system) : PluginImplBase(system)	×
17	{
18	if (const char* env_p = std::getenv("MAVSDK_MAVLINK_DIRECT_DEBUGGING")) {	×
19	if (std::string(env_p) == "1") {	×
20	_debugging = true;	×
21	}
22	}
23	_system_impl->register_plugin(this);	×
24	}	×
25
26	MavlinkDirectImpl::MavlinkDirectImpl(std::shared_ptr<System> system) :	18✔
27	PluginImplBase(std::move(system))	18✔
28	{
29	if (const char* env_p = std::getenv("MAVSDK_MAVLINK_DIRECT_DEBUGGING")) {	18✔
30	if (std::string(env_p) == "1") {	×
31	_debugging = true;	×
32	}
33	}
34	_system_impl->register_plugin(this);	18✔
35	}	18✔
36
37	MavlinkDirectImpl::~MavlinkDirectImpl()	36✔
38	{
39	_system_impl->unregister_plugin(this);	18✔
40	}	36✔
41
42	void MavlinkDirectImpl::init()	18✔
43	{
44	// No need to initialize MessageSet here - LibmavReceiver handles that
45	// No need to register for mavlink messages anymore - we'll use libmav subscription system
46	}	18✔
47
48	void MavlinkDirectImpl::deinit()	18✔
49	{
50	// Clean up all libmav message handlers registered by this plugin
51	_system_impl->unregister_all_libmav_message_handlers(this);	18✔
52
53	// Clear internal state
54	_handle_to_message_name.clear();	18✔
55	_message_subscriptions.clear();	18✔
56	}	18✔
57
58	void MavlinkDirectImpl::enable() {}	18✔
59
60	void MavlinkDirectImpl::disable() {}	18✔
61
62	MavlinkDirect::Result MavlinkDirectImpl::send_message(MavlinkDirect::MavlinkMessage message)	11✔
63	{
64	// Get access to the MessageSet through the system
65	auto& message_set = _system_impl->get_message_set();	11✔
66
67	// Create libmav message from the message name
68	auto libmav_message_opt = message_set.create(message.message_name);	11✔
69	if (!libmav_message_opt) {	11✔
70	LogErr() << "Failed to create message: " << message.message_name;	×
71	return MavlinkDirect::Result::InvalidMessage; // Message type not found	×
72	}
73
74	if (_debugging) {	11✔
75	LogDebug() << "Created message " << message.message_name	×
76	<< " with ID: " << libmav_message_opt.value().id();	×
77	}
78
79	auto libmav_message = libmav_message_opt.value();	11✔
80
81	// Convert JSON fields to libmav message fields
82	if (!json_to_libmav_message(message.fields_json, libmav_message)) {	11✔
83	LogErr() << "Failed to convert JSON fields to libmav message for " << message.message_name;	×
84	return MavlinkDirect::Result::InvalidField; // JSON conversion failed	×
85	}
86
87	if (_debugging) {	11✔
88	LogDebug() << "Successfully populated fields for " << message.message_name;	×
89	}
90
91	// Set target system/component if specified
92	if (message.target_system_id != 0) {	11✔
93	// For messages that have target_system field, set it
94	libmav_message.set("target_system", static_cast<uint8_t>(message.target_system_id));	×
95	}
96	if (message.target_component_id != 0) {	11✔
97	// For messages that have target_component field, set it
98	libmav_message.set(	×
99	"target_component_id", static_cast<uint8_t>(message.target_component_id));	×
100	}
101
102	if (_debugging) {	11✔
103	LogDebug() << "Sending " << message.message_name << " via unified libmav API";	×
104	}
105
106	_system_impl->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	11✔
107	mavlink_message_t mavlink_message;
108
109	// Use clean libmav helper methods to get payload data
110	auto payload_view = libmav_message.getPayloadView();	11✔
111	const uint8_t payload_length = libmav_message.getPayloadLength();	11✔
112
113	// Set up the mavlink_message_t structure
114	mavlink_message.msgid = libmav_message.id();	11✔
115	mavlink_message.len = payload_length;	11✔
116	memcpy(mavlink_message.payload64, payload_view.first, payload_length);	11✔
117
118	mavlink_finalize_message_chan(	22✔
119	&mavlink_message,
120	mavlink_address.system_id,	11✔
121	mavlink_address.component_id,	11✔
122	channel,
123	payload_length,
124	libmav_message.type().maxPayloadSize(),	11✔
125	libmav_message.type().crcExtra());	11✔
126
127	return mavlink_message;	11✔
128	});
129
130	if (_debugging) {	11✔
131	LogDebug() << "Successfully sent " << message.message_name << " as raw data";	×
132	}
133
134	return MavlinkDirect::Result::Success;	11✔
135	}
136
137	MavlinkDirect::MessageHandle MavlinkDirectImpl::subscribe_message(	9✔
138	std::string message_name, const MavlinkDirect::MessageCallback& callback)
139	{
140	// Subscribe using CallbackList pattern - this handles thread safety internally
141	auto handle = _message_subscriptions.subscribe(callback);	9✔
142
143	// Store message name for this handle (CallbackList protects this too)
144	_handle_to_message_name[handle] = message_name;	9✔
145
146	// Register with SystemImpl - no lock-order-inversion because CallbackList handles
147	// synchronization
148	_system_impl->register_libmav_message_handler(	9✔
149	message_name,
150	[this](const Mavsdk::MavlinkMessage& message) {	22✔
151	// Use CallbackList::queue to safely call all subscribed callbacks
152	// Convert Mavsdk::MavlinkMessage to MavlinkDirect::MavlinkMessage (they're identical)
153	MavlinkDirect::MavlinkMessage direct_message;	11✔
154	direct_message.message_name = message.message_name;	11✔
155	direct_message.system_id = message.system_id;	11✔
156	direct_message.component_id = message.component_id;	11✔
157	direct_message.target_system_id = message.target_system_id;	11✔
158	direct_message.target_component_id = message.target_component_id;	11✔
159	direct_message.fields_json = message.fields_json;	11✔
160
161	_message_subscriptions.queue(direct_message, [this](const auto& func) {	11✔
162	_system_impl->call_user_callback(func);	11✔
163	});	11✔
164	},	11✔
165	&handle // Use handle address as cookie for specific unregistration
166	);
167
168	return handle;	9✔
169	}
170
171	void MavlinkDirectImpl::unsubscribe_message(MavlinkDirect::MessageHandle handle)	9✔
172	{
173	// Get the message name for unregistration
174	auto name_it = _handle_to_message_name.find(handle);	9✔
175	if (name_it == _handle_to_message_name.end()) {	9✔
176	return; // Handle not found, nothing to unsubscribe	×
177	}
178	std::string message_name = name_it->second;	9✔
179
180	// Unregister from SystemImpl - no lock-order-inversion with CallbackList pattern
181	_system_impl->unregister_libmav_message_handler(message_name, &handle);	9✔
182
183	// Remove from CallbackList and message name map - CallbackList handles thread safety
184	_message_subscriptions.unsubscribe(handle);	9✔
185	_handle_to_message_name.erase(handle);	9✔
186	}	9✔
187
188	std::optional<uint32_t> MavlinkDirectImpl::message_name_to_id(const std::string& name) const	×
189	{
190	// Get MessageSet to access message definitions
191	auto& message_set = _system_impl->get_message_set();	×
192
193	// Use MessageSet's message name to ID conversion
194	auto id_opt = message_set.idForMessage(name);	×
195	if (id_opt.has_value()) {	×
196	return static_cast<uint32_t>(id_opt.value());	×
197	}
198
199	return std::nullopt;	×
200	}
201
202	std::optional<std::string> MavlinkDirectImpl::message_id_to_name(uint32_t id) const	×
203	{
204	// Get MessageSet to access message definitions
205	auto& message_set = _system_impl->get_message_set();	×
206
207	// Use MessageSet's message ID to name conversion
208	auto message_def = message_set.getMessageDefinition(static_cast<int>(id));	×
209	if (message_def) {	×
210	return message_def.get().name();	×
211	}
212	return std::nullopt;	×
213	}
214
215	Json::Value MavlinkDirectImpl::libmav_to_json(const mav::Message& msg) const	×
216	{
217	(void)msg;
218	// TODO: Implement field iteration once libmav noexcept API is stable
219	return Json::Value();	×
220	}
221
222	bool MavlinkDirectImpl::json_to_libmav(const Json::Value& json, mav::Message& msg) const	×
223	{
224	(void)json;
225	(void)msg;
226	// TODO: Implement once libmav noexcept API set methods are stable
227	return false;	×
228	}
229
230	bool MavlinkDirectImpl::json_to_libmav_message(	11✔
231	const std::string& json_string, mav::Message& msg) const
232	{
233	Json::Value json;	11✔
234	Json::Reader reader;	11✔
235
236	if (!reader.parse(json_string, json)) {	11✔
237	LogErr() << "Failed to parse JSON: " << json_string;	×
238	return false;	×
239	}
240
241	// Iterate through all JSON fields and set them in the libmav message
242	for (const auto& field_name : json.getMemberNames()) {	95✔
243	const Json::Value& field_value = json[field_name];	84✔
244
245	// Convert JSON values to appropriate types and set in message
246	if (field_value.isInt()) {	84✔
247	auto result = msg.set(field_name, static_cast<int32_t>(field_value.asInt()));	66✔
248	if (result != ::mav::MessageResult::Success) {	66✔
249	// Try as other integer types
250	if (msg.set(field_name, static_cast<uint32_t>(field_value.asUInt())) !=	×
251	::mav::MessageResult::Success &&	×
252	msg.set(field_name, static_cast<int16_t>(field_value.asInt())) !=	×
253	::mav::MessageResult::Success &&	×
254	msg.set(field_name, static_cast<uint16_t>(field_value.asUInt())) !=	×
255	::mav::MessageResult::Success &&	×
256	msg.set(field_name, static_cast<int8_t>(field_value.asInt())) !=	×
257	::mav::MessageResult::Success &&	×
258	msg.set(field_name, static_cast<uint8_t>(field_value.asUInt())) !=	×
259	::mav::MessageResult::Success) {
260	LogWarn() << "Failed to set integer field " << field_name << " = "	×
261	<< field_value.asInt();	×
262	}
263	}
264	} else if (field_value.isUInt()) {	18✔
265	auto result = msg.set(field_name, static_cast<uint32_t>(field_value.asUInt()));	×
266	if (result != ::mav::MessageResult::Success) {	×
267	// Try as other unsigned integer types
268	if (msg.set(field_name, static_cast<uint64_t>(field_value.asUInt64())) !=	×
269	::mav::MessageResult::Success &&	×
270	msg.set(field_name, static_cast<uint16_t>(field_value.asUInt())) !=	×
271	::mav::MessageResult::Success &&	×
272	msg.set(field_name, static_cast<uint8_t>(field_value.asUInt())) !=	×
273	::mav::MessageResult::Success) {
274	LogWarn() << "Failed to set unsigned integer field " << field_name << " = "	×
275	<< field_value.asUInt();	×
276	}
277	}
278	} else if (field_value.isNull() \|\| field_value.isDouble()) {	18✔
279	// Handle float/double values (including null -> NaN)
280	auto field_opt = msg.type().getField(field_name);	6✔
281	if (!field_opt) {	6✔
NEW 282	LogWarn() << "Field " << field_name << " not found in message definition";	×
NEW 283	continue;	×
284	}
285
286	auto field = field_opt.value();	6✔
287	::mav::MessageResult result = ::mav::MessageResult::FieldNotFound;	6✔
288
289	if (field.type.base_type == ::mav::FieldType::BaseType::FLOAT) {	6✔
290	if (field_value.isNull()) {	4✔
291	result = msg.set(field_name, std::numeric_limits<float>::quiet_NaN());	3✔
292	} else {
293	result = msg.set(field_name, static_cast<float>(field_value.asFloat()));	1✔
294	}
295	} else if (field.type.base_type == ::mav::FieldType::BaseType::DOUBLE) {	2✔
296	if (field_value.isNull()) {	2✔
297	result = msg.set(field_name, std::numeric_limits<double>::quiet_NaN());	1✔
298	} else {
299	result = msg.set(field_name, field_value.asDouble());	1✔
300	}
301	} else {
NEW 302	LogWarn() << "Field " << field_name << " is not a float or double field";	×
NEW 303	continue;	×
304	}
305
306	if (result != ::mav::MessageResult::Success) {	6✔
NEW 307	LogWarn() << "Failed to set float/double field " << field_name << " = "	×
NEW 308	<< (field_value.isNull() ? "null" :	×
NEW 309	std::to_string(field_value.asDouble()));	×
310	}
311	} else if (field_value.isString()) {	12✔
312	auto result = msg.setString(field_name, field_value.asString());	1✔
313	if (result != ::mav::MessageResult::Success) {	1✔
314	LogWarn() << "Failed to set string field " << field_name << " = "	×
315	<< field_value.asString();	×
316	}
317	} else if (field_value.isArray()) {	11✔
318	// Handle array fields with proper type detection
319	auto array_size = field_value.size();	11✔
320
321	// Get field definition to determine correct type
322	auto field_opt = msg.type().getField(field_name);	11✔
323	if (!field_opt) {	11✔
NEW 324	LogWarn() << "Field " << field_name << " not found in message definition";	×
NEW 325	continue;	×
326	}
327
328	auto field = field_opt.value();	11✔
329	if (field.type.size <= 1) {	11✔
NEW 330	LogWarn() << "Field " << field_name << " is not an array field";	×
NEW 331	continue;	×
332	}
333
334	// Create and populate only the correct vector type
335	::mav::MessageResult result = ::mav::MessageResult::FieldNotFound;	11✔
336	switch (field.type.base_type) {	11✔
337	case ::mav::FieldType::BaseType::UINT8: {	10✔
338	std::vector<uint8_t> vec;	10✔
339	vec.reserve(array_size);	10✔
340	for (Json::ArrayIndex i = 0; i < array_size; ++i) {	210✔
341	const auto& elem = field_value[i];	200✔
342	vec.push_back(elem.isNumeric() ? static_cast<uint8_t>(elem.asUInt()) : 0);	200✔
343	}
344	result = msg.set(field_name, vec);	10✔
345	break;	10✔
346	}	10✔
NEW 347	case ::mav::FieldType::BaseType::UINT16: {	×
NEW 348	std::vector<uint16_t> vec;	×
NEW 349	vec.reserve(array_size);	×
NEW 350	for (Json::ArrayIndex i = 0; i < array_size; ++i) {	×
NEW 351	const auto& elem = field_value[i];	×
NEW 352	vec.push_back(elem.isNumeric() ? static_cast<uint16_t>(elem.asUInt()) : 0);	×
353	}
NEW 354	result = msg.set(field_name, vec);	×
NEW 355	break;	×
NEW 356	}	×
NEW 357	case ::mav::FieldType::BaseType::UINT32: {	×
NEW 358	std::vector<uint32_t> vec;	×
NEW 359	vec.reserve(array_size);	×
NEW 360	for (Json::ArrayIndex i = 0; i < array_size; ++i) {	×
NEW 361	const auto& elem = field_value[i];	×
NEW 362	vec.push_back(elem.isNumeric() ? static_cast<uint32_t>(elem.asUInt()) : 0);	×
363	}
NEW 364	result = msg.set(field_name, vec);	×
NEW 365	break;	×
NEW 366	}	×
NEW 367	case ::mav::FieldType::BaseType::UINT64: {	×
NEW 368	std::vector<uint64_t> vec;	×
NEW 369	vec.reserve(array_size);	×
NEW 370	for (Json::ArrayIndex i = 0; i < array_size; ++i) {	×
NEW 371	const auto& elem = field_value[i];	×
NEW 372	vec.push_back(	×
NEW 373	elem.isNumeric() ? static_cast<uint64_t>(elem.asUInt64()) : 0);	×
374	}
NEW 375	result = msg.set(field_name, vec);	×
NEW 376	break;	×
NEW 377	}	×
NEW 378	case ::mav::FieldType::BaseType::INT8: {	×
NEW 379	std::vector<int8_t> vec;	×
NEW 380	vec.reserve(array_size);	×
NEW 381	for (Json::ArrayIndex i = 0; i < array_size; ++i) {	×
NEW 382	const auto& elem = field_value[i];	×
NEW 383	vec.push_back(elem.isNumeric() ? static_cast<int8_t>(elem.asInt()) : 0);	×
384	}
NEW 385	result = msg.set(field_name, vec);	×
NEW 386	break;	×
NEW 387	}	×
NEW 388	case ::mav::FieldType::BaseType::INT16: {	×
NEW 389	std::vector<int16_t> vec;	×
NEW 390	vec.reserve(array_size);	×
NEW 391	for (Json::ArrayIndex i = 0; i < array_size; ++i) {	×
NEW 392	const auto& elem = field_value[i];	×
NEW 393	vec.push_back(elem.isNumeric() ? static_cast<int16_t>(elem.asInt()) : 0);	×
394	}
NEW 395	result = msg.set(field_name, vec);	×
NEW 396	break;	×
UNCOV 397	}	×
NEW 398	case ::mav::FieldType::BaseType::INT32: {	×
NEW 399	std::vector<int32_t> vec;	×
NEW 400	vec.reserve(array_size);	×
NEW 401	for (Json::ArrayIndex i = 0; i < array_size; ++i) {	×
NEW 402	const auto& elem = field_value[i];	×
NEW 403	vec.push_back(elem.isNumeric() ? static_cast<int32_t>(elem.asInt()) : 0);	×
404	}
NEW 405	result = msg.set(field_name, vec);	×
NEW 406	break;	×
NEW 407	}	×
NEW 408	case ::mav::FieldType::BaseType::INT64: {	×
NEW 409	std::vector<int64_t> vec;	×
NEW 410	vec.reserve(array_size);	×
NEW 411	for (Json::ArrayIndex i = 0; i < array_size; ++i) {	×
NEW 412	const auto& elem = field_value[i];	×
NEW 413	vec.push_back(elem.isNumeric() ? static_cast<int64_t>(elem.asInt64()) : 0);	×
414	}
NEW 415	result = msg.set(field_name, vec);	×
NEW 416	break;	×
NEW 417	}	×
418	case ::mav::FieldType::BaseType::FLOAT: {	1✔
419	std::vector<float> vec;	1✔
420	vec.reserve(array_size);	1✔
421	for (Json::ArrayIndex i = 0; i < array_size; ++i) {	5✔
422	const auto& elem = field_value[i];	4✔
423	if (elem.isNumeric()) {	4✔
424	vec.push_back(static_cast<float>(elem.asFloat()));	2✔
425	} else {
426	// For non-numeric values (including null), use NaN
427	vec.push_back(std::numeric_limits<float>::quiet_NaN());	2✔
428	}
429	}
430	result = msg.set(field_name, vec);	1✔
431	break;	1✔
432	}	1✔
NEW 433	case ::mav::FieldType::BaseType::DOUBLE: {	×
NEW 434	std::vector<double> vec;	×
NEW 435	vec.reserve(array_size);	×
NEW 436	for (Json::ArrayIndex i = 0; i < array_size; ++i) {	×
NEW 437	const auto& elem = field_value[i];	×
NEW 438	if (elem.isNumeric()) {	×
NEW 439	vec.push_back(elem.asDouble());	×
440	} else {
441	// For non-numeric values (including null), use NaN
NEW 442	vec.push_back(std::numeric_limits<double>::quiet_NaN());	×
443	}
444	}
NEW 445	result = msg.set(field_name, vec);	×
NEW 446	break;	×
NEW 447	}	×
NEW 448	case ::mav::FieldType::BaseType::CHAR: {	×
NEW 449	std::vector<char> vec;	×
NEW 450	vec.reserve(array_size);	×
NEW 451	for (Json::ArrayIndex i = 0; i < array_size; ++i) {	×
NEW 452	const auto& elem = field_value[i];	×
NEW 453	vec.push_back(elem.isNumeric() ? static_cast<char>(elem.asInt()) : 0);	×
454	}
NEW 455	result = msg.set(field_name, vec);	×
NEW 456	break;	×
NEW 457	}	×
NEW 458	default:	×
NEW 459	LogWarn() << "Unsupported array field type for " << field_name;	×
NEW 460	break;	×
461	}
462
463	if (result != ::mav::MessageResult::Success) {	11✔
UNCOV 464	LogWarn() << "Failed to set array field " << field_name;	×
465	}
466	} else {
467	LogWarn() << "Unsupported JSON field type for " << field_name;	×
468	}
469	}	11✔
470
471	return true;	11✔
472	}	11✔
473
474	MavlinkDirect::Result MavlinkDirectImpl::load_custom_xml(const std::string& xml_content)	6✔
475	{
476	if (_debugging) {	6✔
477	LogDebug() << "Loading custom XML definitions...";	×
478	}
479
480	// Load the custom XML into the MessageSet with thread safety
481	// This uses the thread-safe method that protects against concurrent read operations
482	bool success = _system_impl->load_custom_xml_to_message_set(xml_content);	6✔
483
484	if (success) {	6✔
485	if (_debugging) {	6✔
486	LogDebug() << "Successfully loaded custom XML definitions";	×
487	}
488	return MavlinkDirect::Result::Success;	6✔
489	} else {
490	LogErr() << "Failed to load custom XML definitions";	×
491	return MavlinkDirect::Result::Error;	×
492	}
493	}
494
495	} // namespace mavsdk

mavlink / MAVSDK / 17421280055

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