20111993644

Committed 10 Dec 2025 08:14PM UTC coverage: 47.958% (-0.08%) from 48.033%

Build # 20111993644

Build Type

push

github

Committed by

web-flow

Commit Message

Merge pull request #2732 from mavlink/pr-heartbeat-timeout

core: make timeout configurable

Run Details

4 of 10 new or added lines in 3 files covered. (40.0%)

28 existing lines in 7 files now uncovered.

17639 of 36780 relevant lines covered (47.96%)

461.39 hits per line

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

76.7

/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.
    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()	402✔
9	{
10	if (const char* env_p = std::getenv("MAVSDK_MESSAGE_HANDLER_DEBUGGING")) {	402✔
11	if (std::string(env_p) == "1") {	×
12	LogDebug() << "Mavlink message handler debugging is on.";	×
13	_debugging = true;	×
14	}
15	}
16	}	402✔
17
18	void MavlinkMessageHandler::register_one(	2,750✔
19	uint16_t msg_id, const Callback& callback, const void* cookie)
20	{
21	register_one_impl(msg_id, {}, callback, cookie);	2,750✔
22	}	2,750✔
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,750✔
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,750✔
37
38	std::unique_lock<std::mutex> lock(_mutex, std::try_to_lock);	2,750✔
39	if (lock.owns_lock()) {	2,750✔
40	_table.push_back(entry);	2,680✔
41	} else {
42	std::lock_guard<std::mutex> register_later_lock(_register_later_mutex);	70✔
43	_register_later_table.push_back(entry);	70✔
44	}	70✔
45	}	2,750✔
46
47	void MavlinkMessageHandler::unregister_one(uint16_t msg_id, const void* cookie)	26✔
48	{
49	unregister_impl({msg_id}, cookie);	26✔
50	}	26✔
51
52	void MavlinkMessageHandler::unregister_all(const void* cookie)	1,237✔
53	{
54	unregister_impl({}, cookie);	1,237✔
55	}	1,237✔
56
57	void MavlinkMessageHandler::unregister_all_blocking(const void* cookie)	156✔
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	std::lock_guard<std::mutex> lock(_mutex);	156✔
62	_table.erase(	312✔
63	std::remove_if(	156✔
64	_table.begin(), _table.end(), [&](auto& entry) { return entry.cookie == cookie; }),	1,311✔
65	_table.end());	156✔
66	}	156✔
67
68	void MavlinkMessageHandler::unregister_impl(	1,263✔
69	std::optional<uint16_t> maybe_msg_id, const void* cookie)
70	{
71	std::unique_lock<std::mutex> lock(_mutex, std::try_to_lock);	1,263✔
72	if (lock.owns_lock()) {	1,263✔
73	_table.erase(	2,522✔
74	std::remove_if(	1,261✔
75	_table.begin(),
76	_table.end(),
77	[&](auto& entry) {	5,937✔
78	if (maybe_msg_id) {	5,937✔
79	return (entry.msg_id == maybe_msg_id.value() && entry.cookie == cookie);	460✔
80	} else {
81	return (entry.cookie == cookie);	5,477✔
82	}
83	}),
84	_table.end());	2,522✔
85	} else {
86	std::lock_guard<std::mutex> unregister_later_lock(_unregister_later_mutex);	2✔
87	_unregister_later_table.push_back(UnregisterEntry{maybe_msg_id, cookie});	2✔
88	}	2✔
89	}	1,263✔
90
91	void MavlinkMessageHandler::check_register_later()	5,004✔
92	{
93	std::lock_guard<std::mutex> _register_later_lock(_register_later_mutex);	5,004✔
94
95	// We could probably just grab the lock here, but it's safer not to
96	// acquire both locks to avoid deadlocks.
97	std::unique_lock<std::mutex> lock(_mutex, std::try_to_lock);	5,006✔
98	if (!lock.owns_lock()) {	5,010✔
99	// Try again later.
UNCOV 100	return;	×
101	}
102
103	for (const auto& entry : _register_later_table) {	5,071✔
104	_table.push_back(entry);	70✔
105	}
106
107	_register_later_table.clear();	5,006✔
108	}	5,006✔
109
110	void MavlinkMessageHandler::check_unregister_later()	5,007✔
111	{
112	std::lock_guard<std::mutex> _unregister_later_lock(_unregister_later_mutex);	5,007✔
113
114	// We could probably just grab the lock here, but it's safer not to
115	// acquire both locks to avoid deadlocks.
116	std::unique_lock<std::mutex> lock(_mutex, std::try_to_lock);	5,014✔
117	if (!lock.owns_lock()) {	5,014✔
118	// Try again later.
UNCOV 119	return;	×
120	}
121
122	for (const auto& unregister_entry : _unregister_later_table) {	5,014✔
123	_table.erase(	4✔
124	std::remove_if(	2✔
125	_table.begin(),
126	_table.end(),
127	[&](auto& entry) {	16✔
128	if (unregister_entry.maybe_msg_id) {	16✔
129	return (
130	entry.msg_id == unregister_entry.maybe_msg_id.value() &&	22✔
131	entry.cookie == unregister_entry.cookie);	22✔
132	} else {
133	return (entry.cookie == unregister_entry.cookie);	×
134	}
135	}),
136	_table.end());	4✔
137	}
138
139	_unregister_later_table.clear();	5,004✔
140	}	5,005✔
141
142	void MavlinkMessageHandler::process_message(const mavlink_message_t& message)	5,009✔
143	{
144	check_register_later();	5,009✔
145	check_unregister_later();	5,008✔
146
147	std::lock_guard<std::mutex> lock(_mutex);	5,010✔
148
149	bool forwarded = false;	5,012✔
150
151	if (_debugging) {	5,012✔
152	LogDebug() << "Table entries: ";	×
153	}
154
155	for (auto& entry : _table) {	47,122✔
156	if (_debugging) {	42,109✔
157	LogDebug() << "Msg id: " << entry.msg_id << ", component id: "	×
158	<< (entry.component_id.has_value() ?	×
159	std::to_string(entry.component_id.value()) :	×
160	"none");	×
161	}
162
163	if (entry.msg_id == message.msgid &&	46,059✔
164	(!entry.component_id.has_value() \|\| entry.component_id.value() == message.compid)) {	3,951✔
165	if (_debugging) {	3,946✔
166	LogDebug() << "Using msg " << int(message.msgid) << " to " << size_t(entry.cookie);	×
167	}
168
169	forwarded = true;	3,946✔
170	entry.callback(message);	3,946✔
171	}
172	}
173
174	if (_debugging && !forwarded) {	4,995✔
175	LogDebug() << "Ignoring msg " << int(message.msgid);	×
176	}
177	}	4,995✔
178
179	void MavlinkMessageHandler::update_component_id(	×
180	uint16_t msg_id, uint8_t component_id, const void* cookie)
181	{
182	check_register_later();	×
183	check_unregister_later();	×
184
185	std::lock_guard<std::mutex> lock(_mutex);	×
186
187	for (auto& entry : _table) {	×
188	if (entry.msg_id == msg_id && entry.cookie == cookie) {	×
189	entry.component_id = component_id;	×
190	}
191	}
192	}	×
193
194	} // namespace mavsdk

mavlink / MAVSDK / 20111993644

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