29465682626

Committed 16 Jul 2026 02:04AM UTC coverage: 50.47%. First build

Build # 29465682626

Build Type

Pull #2936

github

Committed by

web-flow

Commit Message

Merge 2f4a4488b into 23fd28534

Pull Request Pull Request #2936: Backport various bugfixes to v3

Coverage Stats

161 of 256 new or added lines in 23 files covered. (62.89%)

19329 of 38298 relevant lines covered (50.47%)

673.1 hits per line

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

64.22

/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_all_blocking(const void* cookie)
{
    // Blocking version for use in destructors - waits for any in-flight callbacks to complete.
    // WARNING: Do NOT call this from within a message handler callback - it will deadlock.

    // Also purge any deferred registrations for this cookie from _register_later_table.
    // If register_one() was called while process_message() held _mutex, the entries land in
    // _register_later_table instead of _table.  Without this step, check_register_later()
    // would later promote those stale entries into _table and fire callbacks on a
    // destroyed object (heap-use-after-free).
    {
        std::lock_guard<std::mutex> register_later_lock(_register_later_mutex);
        _register_later_table.erase(
            std::remove_if(
                _register_later_table.begin(),
                _register_later_table.end(),
                [&](const auto& entry) { return entry.cookie == cookie; }),
            _register_later_table.end());
    }

    std::lock_guard<std::mutex> lock(_mutex);
    _table.erase(
        std::remove_if(
            _table.begin(), _table.end(), [&](auto& entry) { return entry.cookie == cookie; }),
        _table.end());
}

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()	470✔
9	{
10	if (const char* env_p = std::getenv("MAVSDK_MESSAGE_HANDLER_DEBUGGING")) {	470✔
11	if (std::string(env_p) == "1") {	×
12	LogDebug() << "Mavlink message handler debugging is on.";	×
13	_debugging = true;	×
14	}
15	}
16	}	470✔
17
18	void MavlinkMessageHandler::register_one(	3,545✔
19	uint16_t msg_id, const Callback& callback, const void* cookie)
20	{
21	register_one_impl(msg_id, {}, callback, cookie);	3,545✔
22	}	3,545✔
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(	3,545✔
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};	3,545✔
37
38	std::unique_lock<std::mutex> lock(_mutex, std::try_to_lock);	3,545✔
39	if (lock.owns_lock()) {	3,545✔
40	_table.push_back(entry);	3,445✔
41	} else {
42	std::lock_guard<std::mutex> register_later_lock(_register_later_mutex);	100✔
43	_register_later_table.push_back(entry);	100✔
44	}	100✔
45	}	3,545✔
46
47	void MavlinkMessageHandler::unregister_one(uint16_t msg_id, const void* cookie)	×
48	{
49	unregister_impl({msg_id}, cookie);	×
50	}	×
51
52	void MavlinkMessageHandler::unregister_all(const void* cookie)	1,378✔
53	{
54	unregister_impl({}, cookie);	1,378✔
55	}	1,378✔
56
57	void MavlinkMessageHandler::unregister_all_blocking(const void* cookie)	538✔
58	{
59	// Blocking version for use in destructors - waits for any in-flight callbacks to complete.
60	// WARNING: Do NOT call this from within a message handler callback - it will deadlock.
61
62	// Also purge any deferred registrations for this cookie from _register_later_table.
63	// If register_one() was called while process_message() held _mutex, the entries land in
64	// _register_later_table instead of _table. Without this step, check_register_later()
65	// would later promote those stale entries into _table and fire callbacks on a
66	// destroyed object (heap-use-after-free).
67	{
68	std::lock_guard<std::mutex> register_later_lock(_register_later_mutex);	538✔
69	_register_later_table.erase(	1,076✔
70	std::remove_if(	538✔
71	_register_later_table.begin(),
72	_register_later_table.end(),
NEW 73	[&](const auto& entry) { return entry.cookie == cookie; }),	×
74	_register_later_table.end());	538✔
75	}	538✔
76
77	std::lock_guard<std::mutex> lock(_mutex);	538✔
78	_table.erase(	1,076✔
79	std::remove_if(	538✔
80	_table.begin(), _table.end(), [&](auto& entry) { return entry.cookie == cookie; }),	3,142✔
81	_table.end());	538✔
82	}	538✔
83
84	void MavlinkMessageHandler::unregister_impl(	1,378✔
85	std::optional<uint16_t> maybe_msg_id, const void* cookie)
86	{
87	std::unique_lock<std::mutex> lock(_mutex, std::try_to_lock);	1,378✔
88	if (lock.owns_lock()) {	1,378✔
89	_table.erase(	2,756✔
90	std::remove_if(	1,378✔
91	_table.begin(),
92	_table.end(),
93	[&](auto& entry) {	6,100✔
94	if (maybe_msg_id) {	6,100✔
95	return (entry.msg_id == maybe_msg_id.value() && entry.cookie == cookie);	×
96	} else {
97	return (entry.cookie == cookie);	6,100✔
98	}
99	}),
100	_table.end());	2,756✔
101	} else {
102	std::lock_guard<std::mutex> unregister_later_lock(_unregister_later_mutex);	×
103	_unregister_later_table.push_back(UnregisterEntry{maybe_msg_id, cookie});	×
104	}	×
105	}	1,378✔
106
107	void MavlinkMessageHandler::check_register_later()	18,407✔
108	{
109	std::lock_guard<std::mutex> _register_later_lock(_register_later_mutex);	18,407✔
110
111	// We could probably just grab the lock here, but it's safer not to
112	// acquire both locks to avoid deadlocks.
113	std::unique_lock<std::mutex> lock(_mutex, std::try_to_lock);	18,411✔
114	if (!lock.owns_lock()) {	18,415✔
115	// Try again later.
116	return;	×
117	}
118
119	for (const auto& entry : _register_later_table) {	18,511✔
120	_table.push_back(entry);	100✔
121	}
122
123	_register_later_table.clear();	18,408✔
124	}	18,412✔
125
126	void MavlinkMessageHandler::check_unregister_later()	18,410✔
127	{
128	std::lock_guard<std::mutex> _unregister_later_lock(_unregister_later_mutex);	18,410✔
129
130	// We could probably just grab the lock here, but it's safer not to
131	// acquire both locks to avoid deadlocks.
132	std::unique_lock<std::mutex> lock(_mutex, std::try_to_lock);	18,412✔
133	if (!lock.owns_lock()) {	18,413✔
134	// Try again later.
135	return;	×
136	}
137
138	for (const auto& unregister_entry : _unregister_later_table) {	18,413✔
139	_table.erase(	×
140	std::remove_if(	×
141	_table.begin(),
142	_table.end(),
143	[&](auto& entry) {	×
144	if (unregister_entry.maybe_msg_id) {	×
145	return (
146	entry.msg_id == unregister_entry.maybe_msg_id.value() &&	×
147	entry.cookie == unregister_entry.cookie);	×
148	} else {
149	return (entry.cookie == unregister_entry.cookie);	×
150	}
151	}),
152	_table.end());	×
153	}
154
155	_unregister_later_table.clear();	18,410✔
156	}	18,408✔
157
158	void MavlinkMessageHandler::process_message(const mavlink_message_t& message)	18,409✔
159	{
160	check_register_later();	18,409✔
161	check_unregister_later();	18,411✔
162
163	std::lock_guard<std::mutex> lock(_mutex);	18,410✔
164
165	bool forwarded = false;	18,410✔
166
167	if (_debugging) {	18,410✔
168	LogDebug() << "Table entries: ";	×
169	}
170
171	for (auto& entry : _table) {	198,264✔
172	if (_debugging) {	179,835✔
173	LogDebug() << "Msg id: " << entry.msg_id << ", component id: "	×
174	<< (entry.component_id.has_value() ?	×
175	std::to_string(entry.component_id.value()) :	×
176	"none");	×
177	}
178
179	if (entry.msg_id == message.msgid &&	190,563✔
180	(!entry.component_id.has_value() \|\| entry.component_id.value() == message.compid)) {	10,708✔
181	if (_debugging) {	10,708✔
182	LogDebug() << "Using msg " << int(message.msgid) << " to " << size_t(entry.cookie);	×
183	}
184
185	forwarded = true;	10,708✔
186	entry.callback(message);	10,708✔
187	}
188	}
189
190	if (_debugging && !forwarded) {	18,397✔
191	LogDebug() << "Ignoring msg " << int(message.msgid);	×
192	}
193	}	18,397✔
194
195	void MavlinkMessageHandler::update_component_id(	×
196	uint16_t msg_id, uint8_t component_id, const void* cookie)
197	{
198	check_register_later();	×
199	check_unregister_later();	×
200
201	std::lock_guard<std::mutex> lock(_mutex);	×
202
203	for (auto& entry : _table) {	×
204	if (entry.msg_id == msg_id && entry.cookie == cookie) {	×
205	entry.component_id = component_id;	×
206	}
207	}
208	}	×
209
210	} // namespace mavsdk

mavlink / MAVSDK / 29465682626

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