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

64.58

/src/mavsdk/core/mavlink_message_handler.cpp

#include <algorithm>
#include <mutex>
#include "mavlink_message_handler.h"
#include "log.h"

namespace mavsdk {

MavlinkMessageHandler::MavlinkMessageHandler()
{
    if (const char* env_p = std::getenv("MAVSDK_MESSAGE_HANDLER_DEBUGGING")) {
        if (std::string(env_p) == "1") {
            LogDebug() << "Mavlink message handler debugging is on.";
            _debugging = true;
        }
    }
}

void MavlinkMessageHandler::register_one(
    uint16_t msg_id, const Callback& callback, const void* cookie)
{
    register_one_impl(msg_id, {}, callback, cookie);
}

void MavlinkMessageHandler::register_one_with_component_id(
    uint16_t msg_id, uint8_t component_id, const Callback& callback, const void* cookie)
{
    register_one_impl(msg_id, {component_id}, callback, cookie);
}

void MavlinkMessageHandler::register_one_impl(
    uint16_t msg_id,
    std::optional<uint8_t> maybe_component_id,
    const Callback& callback,
    const void* cookie)
{
    Entry entry = {msg_id, maybe_component_id, callback, cookie};

    std::unique_lock<std::mutex> lock(_mutex, std::try_to_lock);
    if (lock.owns_lock()) {
        _table.push_back(entry);
    } else {
        std::lock_guard<std::mutex> register_later_lock(_register_later_mutex);
        _register_later_table.push_back(entry);
    }
}

void MavlinkMessageHandler::unregister_one(uint16_t msg_id, const void* cookie)
{
    unregister_impl({msg_id}, cookie);
}

void MavlinkMessageHandler::unregister_all(const void* cookie)
{
    unregister_impl({}, cookie);
}

void MavlinkMessageHandler::unregister_impl(
    std::optional<uint16_t> maybe_msg_id, const void* cookie)
{
    std::unique_lock<std::mutex> lock(_mutex, std::try_to_lock);
    if (lock.owns_lock()) {
        _table.erase(
            std::remove_if(
                _table.begin(),
                _table.end(),
                [&](auto& entry) {
                    if (maybe_msg_id) {
                        return (entry.msg_id == maybe_msg_id.value() && entry.cookie == cookie);
                    } else {
                        return (entry.cookie == cookie);
                    }
                }),
            _table.end());
    } else {
        std::lock_guard<std::mutex> unregister_later_lock(_unregister_later_mutex);
        _unregister_later_table.push_back(UnregisterEntry{maybe_msg_id, cookie});
    }
}

void MavlinkMessageHandler::check_register_later()
{
    std::lock_guard<std::mutex> _register_later_lock(_register_later_mutex);

    // We could probably just grab the lock here, but it's safer not to
    // acquire both locks to avoid deadlocks.
    std::unique_lock<std::mutex> lock(_mutex, std::try_to_lock);
    if (!lock.owns_lock()) {
        // Try again later.
        return;
    }

    for (const auto& entry : _register_later_table) {
        _table.push_back(entry);
    }

    _register_later_table.clear();
}

void MavlinkMessageHandler::check_unregister_later()
{
    std::lock_guard<std::mutex> _unregister_later_lock(_unregister_later_mutex);

    // We could probably just grab the lock here, but it's safer not to
    // acquire both locks to avoid deadlocks.
    std::unique_lock<std::mutex> lock(_mutex, std::try_to_lock);
    if (!lock.owns_lock()) {
        // Try again later.
        return;
    }

    for (const auto& unregister_entry : _unregister_later_table) {
        _table.erase(
            std::remove_if(
                _table.begin(),
                _table.end(),
                [&](auto& entry) {
                    if (unregister_entry.maybe_msg_id) {
                        return (
                            entry.msg_id == unregister_entry.maybe_msg_id.value() &&
                            entry.cookie == unregister_entry.cookie);
                    } else {
                        return (entry.cookie == unregister_entry.cookie);
                    }
                }),
            _table.end());
    }

    _unregister_later_table.clear();
}

void MavlinkMessageHandler::process_message(const mavlink_message_t& message)
{
    check_register_later();
    check_unregister_later();

    std::lock_guard<std::mutex> lock(_mutex);

    bool forwarded = false;

    if (_debugging) {
        LogDebug() << "Table entries: ";
    }

    for (auto& entry : _table) {
        if (_debugging) {
            LogDebug() << "Msg id: " << entry.msg_id << ", component id: "
                       << (entry.component_id.has_value() ?
                               std::to_string(entry.component_id.value()) :
                               "none");
        }

        if (entry.msg_id == message.msgid &&
            (!entry.component_id.has_value() || entry.component_id.value() == message.compid)) {
            if (_debugging) {
                LogDebug() << "Using msg " << int(message.msgid) << " to " << size_t(entry.cookie);
            }

            forwarded = true;
            entry.callback(message);
        }
    }

    if (_debugging && !forwarded) {
        LogDebug() << "Ignoring msg " << int(message.msgid);
    }
}

void MavlinkMessageHandler::update_component_id(
    uint16_t msg_id, uint8_t component_id, const void* cookie)
{
    check_register_later();
    check_unregister_later();

    std::lock_guard<std::mutex> lock(_mutex);

    for (auto& entry : _table) {
        if (entry.msg_id == msg_id && entry.cookie == cookie) {
            entry.component_id = component_id;
        }
    }
}

} // namespace mavsdk

1	#include <algorithm>
2	#include <mutex>
3	#include "mavlink_message_handler.h"
4	#include "log.h"
5
6	namespace mavsdk {
7
8	MavlinkMessageHandler::MavlinkMessageHandler()	367✔
9	{
10	if (const char* env_p = std::getenv("MAVSDK_MESSAGE_HANDLER_DEBUGGING")) {	367✔
11	if (std::string(env_p) == "1") {	×
12	LogDebug() << "Mavlink message handler debugging is on.";	×
13	_debugging = true;	×
14	}
15	}
16	}	367✔
17
18	void MavlinkMessageHandler::register_one(	2,462✔
19	uint16_t msg_id, const Callback& callback, const void* cookie)
20	{
21	register_one_impl(msg_id, {}, callback, cookie);	2,462✔
22	}	2,462✔
23
24	void MavlinkMessageHandler::register_one_with_component_id(	×
25	uint16_t msg_id, uint8_t component_id, const Callback& callback, const void* cookie)
26	{
27	register_one_impl(msg_id, {component_id}, callback, cookie);	×
28	}	×
29
30	void MavlinkMessageHandler::register_one_impl(	2,462✔
31	uint16_t msg_id,
32	std::optional<uint8_t> maybe_component_id,
33	const Callback& callback,
34	const void* cookie)
35	{
36	Entry entry = {msg_id, maybe_component_id, callback, cookie};	2,462✔
37
38	std::unique_lock<std::mutex> lock(_mutex, std::try_to_lock);	2,462✔
39	if (lock.owns_lock()) {	2,462✔
40	_table.push_back(entry);	2,402✔
41	} else {
42	std::lock_guard<std::mutex> register_later_lock(_register_later_mutex);	60✔
43	_register_later_table.push_back(entry);	60✔
44	}	60✔
45	}	2,462✔
46
47	void MavlinkMessageHandler::unregister_one(uint16_t msg_id, const void* cookie)	18✔
48	{
49	unregister_impl({msg_id}, cookie);	18✔
50	}	18✔
51
52	void MavlinkMessageHandler::unregister_all(const void* cookie)	1,236✔
53	{
54	unregister_impl({}, cookie);	1,236✔
55	}	1,236✔
56
57	void MavlinkMessageHandler::unregister_impl(	1,254✔
58	std::optional<uint16_t> maybe_msg_id, const void* cookie)
59	{
60	std::unique_lock<std::mutex> lock(_mutex, std::try_to_lock);	1,254✔
61	if (lock.owns_lock()) {	1,254✔
62	_table.erase(	2,508✔
63	std::remove_if(	1,254✔
64	_table.begin(),
65	_table.end(),
66	[&](auto& entry) {	6,349✔
67	if (maybe_msg_id) {	6,349✔
68	return (entry.msg_id == maybe_msg_id.value() && entry.cookie == cookie);	346✔
69	} else {
70	return (entry.cookie == cookie);	6,003✔
71	}
72	}),
73	_table.end());	2,508✔
74	} else {
UNCOV 75	std::lock_guard<std::mutex> unregister_later_lock(_unregister_later_mutex);	×
UNCOV 76	_unregister_later_table.push_back(UnregisterEntry{maybe_msg_id, cookie});	×
UNCOV 77	}	×
78	}	1,254✔
79
80	void MavlinkMessageHandler::check_register_later()	4,633✔
81	{
82	std::lock_guard<std::mutex> _register_later_lock(_register_later_mutex);	4,633✔
83
84	// We could probably just grab the lock here, but it's safer not to
85	// acquire both locks to avoid deadlocks.
86	std::unique_lock<std::mutex> lock(_mutex, std::try_to_lock);	4,641✔
87	if (!lock.owns_lock()) {	4,643✔
88	// Try again later.
89	return;	×
90	}
91
92	for (const auto& entry : _register_later_table) {	4,699✔
93	_table.push_back(entry);	60✔
94	}
95
96	_register_later_table.clear();	4,638✔
97	}	4,641✔
98
99	void MavlinkMessageHandler::check_unregister_later()	4,635✔
100	{
101	std::lock_guard<std::mutex> _unregister_later_lock(_unregister_later_mutex);	4,635✔
102
103	// We could probably just grab the lock here, but it's safer not to
104	// acquire both locks to avoid deadlocks.
105	std::unique_lock<std::mutex> lock(_mutex, std::try_to_lock);	4,641✔
106	if (!lock.owns_lock()) {	4,640✔
107	// Try again later.
108	return;	×
109	}
110
111	for (const auto& unregister_entry : _unregister_later_table) {	4,640✔
UNCOV 112	_table.erase(	×
UNCOV 113	std::remove_if(	×
114	_table.begin(),
115	_table.end(),
UNCOV 116	[&](auto& entry) {	×
UNCOV 117	if (unregister_entry.maybe_msg_id) {	×
118	return (
UNCOV 119	entry.msg_id == unregister_entry.maybe_msg_id.value() &&	×
UNCOV 120	entry.cookie == unregister_entry.cookie);	×
121	} else {
122	return (entry.cookie == unregister_entry.cookie);	×
123	}
124	}),
UNCOV 125	_table.end());	×
126	}
127
128	_unregister_later_table.clear();	4,642✔
129	}	4,639✔
130
131	void MavlinkMessageHandler::process_message(const mavlink_message_t& message)	4,637✔
132	{
133	check_register_later();	4,637✔
134	check_unregister_later();	4,641✔
135
136	std::lock_guard<std::mutex> lock(_mutex);	4,641✔
137
138	bool forwarded = false;	4,640✔
139
140	if (_debugging) {	4,640✔
141	LogDebug() << "Table entries: ";	×
142	}
143
144	for (auto& entry : _table) {	43,334✔
145	if (_debugging) {	38,684✔
146	LogDebug() << "Msg id: " << entry.msg_id << ", component id: "	×
147	<< (entry.component_id.has_value() ?	×
148	std::to_string(entry.component_id.value()) :	×
149	"none");	×
150	}
151
152	if (entry.msg_id == message.msgid &&	42,457✔
153	(!entry.component_id.has_value() \|\| entry.component_id.value() == message.compid)) {	3,762✔
154	if (_debugging) {	3,764✔
155	LogDebug() << "Using msg " << int(message.msgid) << " to " << size_t(entry.cookie);	×
156	}
157
158	forwarded = true;	3,764✔
159	entry.callback(message);	3,764✔
160	}
161	}
162
163	if (_debugging && !forwarded) {	4,631✔
164	LogDebug() << "Ignoring msg " << int(message.msgid);	×
165	}
166	}	4,631✔
167
168	void MavlinkMessageHandler::update_component_id(	×
169	uint16_t msg_id, uint8_t component_id, const void* cookie)
170	{
171	check_register_later();	×
172	check_unregister_later();	×
173
174	std::lock_guard<std::mutex> lock(_mutex);	×
175
176	for (auto& entry : _table) {	×
177	if (entry.msg_id == msg_id && entry.cookie == cookie) {	×
178	entry.component_id = component_id;	×
179	}
180	}
181	}	×
182
183	} // 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