11316831941

Committed 13 Oct 2024 06:41PM UTC coverage: 37.871% (-0.08%) from 37.947%

Build # 11316831941

Build Type

push

github

Committed by

web-flow

Commit Message

Merge pull request #2423 from mavlink/pr-autopilot-server-fixes

Autopilot server and command server fixes

Run Details

1 of 24 new or added lines in 2 files covered. (4.17%)

21 existing lines in 4 files now uncovered.

11435 of 30195 relevant lines covered (37.87%)

262.93 hits per line

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

62.77

/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()	200✔
9	{
10	if (const char* env_p = std::getenv("MAVSDK_MESSAGE_HANDLER_DEBUGGING")) {	200✔
11	if (std::string(env_p) == "1") {	×
12	LogDebug() << "Mavlink message handler debugging is on.";	×
13	_debugging = true;	×
14	}
15	}
16	}	200✔
17
18	void MavlinkMessageHandler::register_one(	1,289✔
19	uint16_t msg_id, const Callback& callback, const void* cookie)
20	{
21	register_one_impl(msg_id, {}, callback, cookie);	1,289✔
22	}	1,289✔
23
24	void MavlinkMessageHandler::register_one_with_component_id(	7✔
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);	7✔
28	}	7✔
29
30	void MavlinkMessageHandler::register_one_impl(	1,296✔
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,592✔
37
38	std::unique_lock<std::mutex> lock(_mutex, std::try_to_lock);	2,592✔
39	if (lock.owns_lock()) {	1,296✔
40	_table.push_back(entry);	1,263✔
41	} else {
42	std::lock_guard<std::mutex> register_later_lock(_register_later_mutex);	66✔
43	_register_later_table.push_back(entry);	33✔
44	}
45	}	1,296✔
46
UNCOV 47	void MavlinkMessageHandler::unregister_one(uint16_t msg_id, const void* cookie)	×
48	{
UNCOV 49	unregister_impl({msg_id}, cookie);	×
UNCOV 50	}	×
51
52	void MavlinkMessageHandler::unregister_all(const void* cookie)	708✔
53	{
54	unregister_impl({}, cookie);	708✔
55	}	708✔
56
57	void MavlinkMessageHandler::unregister_impl(	708✔
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,416✔
61	if (lock.owns_lock()) {	708✔
62	_table.erase(	708✔
63	std::remove_if(	708✔
64	_table.begin(),
65	_table.end(),
66	[&](auto& entry) {	3,382✔
67	if (maybe_msg_id) {	3,382✔
UNCOV 68	return (entry.msg_id == maybe_msg_id.value() && entry.cookie == cookie);	×
69	} else {
70	return (entry.cookie == cookie);	3,382✔
71	}
72	}),	708✔
73	_table.end());	2,832✔
74	} else {
75	std::lock_guard<std::mutex> unregister_later_lock(_unregister_later_mutex);	×
76	_unregister_later_table.push_back(UnregisterEntry{maybe_msg_id, cookie});	×
77	}
78	}	708✔
79
80	void MavlinkMessageHandler::check_register_later()	3,102✔
81	{
82	std::lock_guard<std::mutex> _register_later_lock(_register_later_mutex);	3,102✔
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);	3,103✔
87	if (!lock.owns_lock()) {	3,103✔
88	// Try again later.
89	return;	×
90	}
91
92	for (const auto& entry : _register_later_table) {	3,135✔
93	_table.push_back(entry);	33✔
94	}
95
96	_register_later_table.clear();	3,103✔
97	}
98
99	void MavlinkMessageHandler::check_unregister_later()	3,103✔
100	{
101	std::lock_guard<std::mutex> _unregister_later_lock(_unregister_later_mutex);	3,103✔
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);	3,103✔
106	if (!lock.owns_lock()) {	3,103✔
107	// Try again later.
108	return;	×
109	}
110
111	for (const auto& unregister_entry : _unregister_later_table) {	3,103✔
112	_table.erase(	×
113	std::remove_if(	×
114	_table.begin(),
115	_table.end(),
116	[&](auto& entry) {	×
117	if (unregister_entry.maybe_msg_id) {	×
118	return (
119	entry.msg_id == unregister_entry.maybe_msg_id.value() &&	×
120	entry.cookie == unregister_entry.cookie);	×
121	} else {
122	return (entry.cookie == unregister_entry.cookie);	×
123	}
124	}),	×
125	_table.end());	×
126	}
127
128	_unregister_later_table.clear();	3,102✔
129	}
130
131	void MavlinkMessageHandler::process_message(const mavlink_message_t& message)	3,102✔
132	{
133	check_register_later();	3,102✔
134	check_unregister_later();	3,102✔
135
136	std::lock_guard<std::mutex> lock(_mutex);	6,205✔
137
138	bool forwarded = false;	3,103✔
139
140	if (_debugging) {	3,103✔
141	LogDebug() << "Table entries: ";	×
142	}
143
144	for (auto& entry : _table) {	27,245✔
145	if (_debugging) {	24,139✔
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 &&	26,945✔
153	(!entry.component_id.has_value() \|\| entry.component_id.value() == message.compid)) {	2,803✔
154	if (_debugging) {	2,803✔
155	LogDebug() << "Using msg " << int(message.msgid) << " to " << size_t(entry.cookie);	×
156	}
157
158	forwarded = true;	2,803✔
159	entry.callback(message);	2,803✔
160	}
161	}
162
163	if (_debugging && !forwarded) {	3,103✔
164	LogDebug() << "Ignoring msg " << int(message.msgid);	×
165	}
166	}	3,103✔
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 / 11316831941

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