23876929365

Committed 02 Apr 2026 12:04AM UTC coverage: 50.496%. First build

Build # 23876929365

Build Type

Pull #2833

github

Committed by

web-flow

Commit Message

Merge 44c9fa960 into 721efdc45

Pull Request Pull Request #2833: Backport recent main fixes and features to v3

Coverage Stats

632 of 815 new or added lines in 29 files covered. (77.55%)

19259 of 38140 relevant lines covered (50.5%)

670.81 hits per line

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

62.62

/src/mavsdk/core/mavlink_command_receiver.cpp

#include "mavlink_command_receiver.h"
#include "mavsdk_impl.h"
#include "log.h"
#include "server_component_impl.h"
#include <cmath>
#include <future>
#include <memory>

namespace mavsdk {

MavlinkCommandReceiver::MavlinkCommandReceiver(ServerComponentImpl& server_component_impl) :
    _server_component_impl(server_component_impl)
{
    _server_component_impl.register_mavlink_message_handler(
        MAVLINK_MSG_ID_COMMAND_LONG,
        [this](const mavlink_message_t& message) { receive_command_long(message); },
        this);

    _server_component_impl.register_mavlink_message_handler(
        MAVLINK_MSG_ID_COMMAND_INT,
        [this](const mavlink_message_t& message) { receive_command_int(message); },
        this);

    if (const char* env_p = std::getenv("MAVSDK_COMMAND_DEBUGGING")) {
        if (std::string(env_p) == "1") {
            LogDebug() << "Command debugging is on.";
            _debugging = true;
        }
    }
}

MavlinkCommandReceiver::~MavlinkCommandReceiver()
{
    _server_component_impl.unregister_all_mavlink_command_handlers(this);
    _server_component_impl.unregister_all_mavlink_message_handlers(this);
}

void MavlinkCommandReceiver::receive_command_int(const mavlink_message_t& message)
{
    MavlinkCommandReceiver::CommandInt cmd(message);

    if (_debugging) {
        LogDebug() << "Received command int " << std::to_string(cmd.command);
    }

    if (cmd.target_component_id != _server_component_impl.get_own_component_id() &&
        cmd.target_component_id != MAV_COMP_ID_ALL) {
        if (_debugging) {
            LogDebug() << "Ignored command int to component "
                       << std::to_string(cmd.target_component_id) << " instead of "
                       << std::to_string(_server_component_impl.get_own_component_id());
        }
        return;
    }

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

    for (auto& handler : _mavlink_command_int_handler_table) {
        if (handler.cmd_id == cmd.command) {
            if (_debugging) {
                LogDebug() << "Handling command int " << std::to_string(cmd.command);
            }

            // The client side can pack a COMMAND_ACK as a response to receiving the command.
            auto maybe_command_ack = handler.callback(cmd);
            if (maybe_command_ack) {
                _server_component_impl.queue_message(
                    [ack = maybe_command_ack.value()](
                        MavlinkAddress mavlink_address, uint8_t channel) {
                        mavlink_message_t response_message;
                        mavlink_msg_command_ack_encode_chan(
                            mavlink_address.system_id,
                            mavlink_address.component_id,
                            channel,
                            &response_message,
                            &ack);
                        return response_message;
                    });

                if (_debugging) {
                    LogDebug() << "Acked command int " << std::to_string(cmd.command) << " with "
                               << std::to_string(maybe_command_ack.value().result);
                }
            }
        }
    }
}

void MavlinkCommandReceiver::receive_command_long(const mavlink_message_t& message)
{
    MavlinkCommandReceiver::CommandLong cmd(message);

    if (_debugging) {
        LogDebug() << "Received command long " << std::to_string(cmd.command);
    }

    if (cmd.target_component_id != _server_component_impl.get_own_component_id() &&
        cmd.target_component_id != MAV_COMP_ID_ALL) {
        if (_debugging) {
            LogDebug() << "Ignored command long to component "
                       << std::to_string(cmd.target_component_id) << " instead of "
                       << std::to_string(_server_component_impl.get_own_component_id());
        }
        return;
    }

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

    for (auto& handler : _mavlink_command_long_handler_table) {
        if (handler.cmd_id == cmd.command) {
            if (_debugging) {
                LogDebug() << "Handling command long " << std::to_string(cmd.command);
            }

            // The client side can pack a COMMAND_ACK as a response to receiving the command.
            auto maybe_command_ack = handler.callback(cmd);
            if (maybe_command_ack) {
                _server_component_impl.queue_message(
                    [ack = maybe_command_ack.value()](
                        MavlinkAddress mavlink_address, uint8_t channel) {
                        mavlink_message_t response_message;
                        mavlink_msg_command_ack_encode_chan(
                            mavlink_address.system_id,
                            mavlink_address.component_id,
                            channel,
                            &response_message,
                            &ack);
                        return response_message;
                    });
                if (_debugging) {
                    LogDebug() << "Acked command long " << std::to_string(cmd.command) << " with "
                               << std::to_string(maybe_command_ack.value().result);
                }
            }
        }
    }
}

void MavlinkCommandReceiver::register_mavlink_command_handler(
    uint16_t cmd_id, const MavlinkCommandIntHandler& callback, const void* cookie)
{
    std::lock_guard<std::mutex> lock(_mavlink_command_handler_table_mutex);

    MAVLinkCommandIntHandlerTableEntry entry = {cmd_id, callback, cookie};
    _mavlink_command_int_handler_table.push_back(entry);
}

void MavlinkCommandReceiver::register_mavlink_command_handler(
    uint16_t cmd_id, const MavlinkCommandLongHandler& callback, const void* cookie)
{
    std::lock_guard<std::mutex> lock(_mavlink_command_handler_table_mutex);

    MAVLinkCommandLongHandlerTableEntry entry = {cmd_id, callback, cookie};
    _mavlink_command_long_handler_table.push_back(entry);
}

void MavlinkCommandReceiver::unregister_mavlink_command_handler(uint16_t cmd_id, const void* cookie)
{
    std::lock_guard<std::mutex> lock(_mavlink_command_handler_table_mutex);

    // COMMAND_INT
    for (auto it = _mavlink_command_int_handler_table.begin();
         it != _mavlink_command_int_handler_table.end();
         /* no ++it */) {
        if (it->cmd_id == cmd_id && it->cookie == cookie) {
            it = _mavlink_command_int_handler_table.erase(it);
        } else {
            ++it;
        }
    }

    // COMMAND_LONG
    for (auto it = _mavlink_command_long_handler_table.begin();
         it != _mavlink_command_long_handler_table.end();
         /* no ++it */) {
        if (it->cmd_id == cmd_id && it->cookie == cookie) {
            it = _mavlink_command_long_handler_table.erase(it);
        } else {
            ++it;
        }
    }
}

void MavlinkCommandReceiver::unregister_all_mavlink_command_handlers(const void* cookie)
{
    std::lock_guard<std::mutex> lock(_mavlink_command_handler_table_mutex);

    // COMMAND_INT
    for (auto it = _mavlink_command_int_handler_table.begin();
         it != _mavlink_command_int_handler_table.end();
         /* no ++it */) {
        if (it->cookie == cookie) {
            it = _mavlink_command_int_handler_table.erase(it);
        } else {
            ++it;
        }
    }

    // COMMAND_LONG
    for (auto it = _mavlink_command_long_handler_table.begin();
         it != _mavlink_command_long_handler_table.end();
         /* no ++it */) {
        if (it->cookie == cookie) {
            it = _mavlink_command_long_handler_table.erase(it);
        } else {
            ++it;
        }
    }
}

} // namespace mavsdk

1	#include "mavlink_command_receiver.h"
2	#include "mavsdk_impl.h"
3	#include "log.h"
4	#include "server_component_impl.h"
5	#include <cmath>
6	#include <future>
7	#include <memory>
8
9	namespace mavsdk {
10
11	MavlinkCommandReceiver::MavlinkCommandReceiver(ServerComponentImpl& server_component_impl) :	169✔
12	_server_component_impl(server_component_impl)	169✔
13	{
14	_server_component_impl.register_mavlink_message_handler(	169✔
15	MAVLINK_MSG_ID_COMMAND_LONG,
16	[this](const mavlink_message_t& message) { receive_command_long(message); },	286✔
17	this);
18
19	_server_component_impl.register_mavlink_message_handler(	169✔
20	MAVLINK_MSG_ID_COMMAND_INT,
21	[this](const mavlink_message_t& message) { receive_command_int(message); },	×
22	this);
23
24	if (const char* env_p = std::getenv("MAVSDK_COMMAND_DEBUGGING")) {	169✔
25	if (std::string(env_p) == "1") {	×
26	LogDebug() << "Command debugging is on.";	×
27	_debugging = true;	×
28	}
29	}
30	}	169✔
31
32	MavlinkCommandReceiver::~MavlinkCommandReceiver()	169✔
33	{
34	_server_component_impl.unregister_all_mavlink_command_handlers(this);	169✔
35	_server_component_impl.unregister_all_mavlink_message_handlers(this);	169✔
36	}	169✔
37
38	void MavlinkCommandReceiver::receive_command_int(const mavlink_message_t& message)	×
39	{
40	MavlinkCommandReceiver::CommandInt cmd(message);	×
41
42	if (_debugging) {	×
43	LogDebug() << "Received command int " << std::to_string(cmd.command);	×
44	}
45
46	if (cmd.target_component_id != _server_component_impl.get_own_component_id() &&	×
47	cmd.target_component_id != MAV_COMP_ID_ALL) {	×
48	if (_debugging) {	×
49	LogDebug() << "Ignored command int to component "	×
50	<< std::to_string(cmd.target_component_id) << " instead of "	×
51	<< std::to_string(_server_component_impl.get_own_component_id());	×
52	}
53	return;	×
54	}
55
56	std::lock_guard<std::mutex> lock(_mavlink_command_handler_table_mutex);	×
57
58	for (auto& handler : _mavlink_command_int_handler_table) {	×
59	if (handler.cmd_id == cmd.command) {	×
60	if (_debugging) {	×
61	LogDebug() << "Handling command int " << std::to_string(cmd.command);	×
62	}
63
64	// The client side can pack a COMMAND_ACK as a response to receiving the command.
65	auto maybe_command_ack = handler.callback(cmd);	×
66	if (maybe_command_ack) {	×
67	_server_component_impl.queue_message(	×
NEW 68	[ack = maybe_command_ack.value()](	×
69	MavlinkAddress mavlink_address, uint8_t channel) {
70	mavlink_message_t response_message;
71	mavlink_msg_command_ack_encode_chan(	×
72	mavlink_address.system_id,	×
73	mavlink_address.component_id,	×
74	channel,
75	&response_message,
76	&ack);
77	return response_message;	×
78	});
79
80	if (_debugging) {	×
81	LogDebug() << "Acked command int " << std::to_string(cmd.command) << " with "	×
82	<< std::to_string(maybe_command_ack.value().result);	×
83	}
84	}
85	}
86	}
87	}	×
88
89	void MavlinkCommandReceiver::receive_command_long(const mavlink_message_t& message)	286✔
90	{
91	MavlinkCommandReceiver::CommandLong cmd(message);	286✔
92
93	if (_debugging) {	286✔
94	LogDebug() << "Received command long " << std::to_string(cmd.command);	×
95	}
96
97	if (cmd.target_component_id != _server_component_impl.get_own_component_id() &&	291✔
98	cmd.target_component_id != MAV_COMP_ID_ALL) {	5✔
99	if (_debugging) {	5✔
100	LogDebug() << "Ignored command long to component "	×
101	<< std::to_string(cmd.target_component_id) << " instead of "	×
102	<< std::to_string(_server_component_impl.get_own_component_id());	×
103	}
104	return;	5✔
105	}
106
107	std::lock_guard<std::mutex> lock(_mavlink_command_handler_table_mutex);	281✔
108
109	for (auto& handler : _mavlink_command_long_handler_table) {	5,229✔
110	if (handler.cmd_id == cmd.command) {	4,948✔
111	if (_debugging) {	402✔
112	LogDebug() << "Handling command long " << std::to_string(cmd.command);	×
113	}
114
115	// The client side can pack a COMMAND_ACK as a response to receiving the command.
116	auto maybe_command_ack = handler.callback(cmd);	402✔
117	if (maybe_command_ack) {	402✔
118	_server_component_impl.queue_message(	238✔
119	[ack = maybe_command_ack.value()](	238✔
120	MavlinkAddress mavlink_address, uint8_t channel) {
121	mavlink_message_t response_message;
122	mavlink_msg_command_ack_encode_chan(	238✔
123	mavlink_address.system_id,	238✔
124	mavlink_address.component_id,	238✔
125	channel,
126	&response_message,
127	&ack);
128	return response_message;	238✔
129	});
130	if (_debugging) {	238✔
131	LogDebug() << "Acked command long " << std::to_string(cmd.command) << " with "	×
132	<< std::to_string(maybe_command_ack.value().result);	×
133	}
134	}
135	}
136	}
137	}	281✔
138
139	void MavlinkCommandReceiver::register_mavlink_command_handler(	169✔
140	uint16_t cmd_id, const MavlinkCommandIntHandler& callback, const void* cookie)
141	{
142	std::lock_guard<std::mutex> lock(_mavlink_command_handler_table_mutex);	169✔
143
144	MAVLinkCommandIntHandlerTableEntry entry = {cmd_id, callback, cookie};	169✔
145	_mavlink_command_int_handler_table.push_back(entry);	169✔
146	}	169✔
147
148	void MavlinkCommandReceiver::register_mavlink_command_handler(	617✔
149	uint16_t cmd_id, const MavlinkCommandLongHandler& callback, const void* cookie)
150	{
151	std::lock_guard<std::mutex> lock(_mavlink_command_handler_table_mutex);	617✔
152
153	MAVLinkCommandLongHandlerTableEntry entry = {cmd_id, callback, cookie};	617✔
154	_mavlink_command_long_handler_table.push_back(entry);	617✔
155	}	617✔
156
157	void MavlinkCommandReceiver::unregister_mavlink_command_handler(uint16_t cmd_id, const void* cookie)	12✔
158	{
159	std::lock_guard<std::mutex> lock(_mavlink_command_handler_table_mutex);	12✔
160
161	// COMMAND_INT
162	for (auto it = _mavlink_command_int_handler_table.begin();	12✔
163	it != _mavlink_command_int_handler_table.end();	24✔
164	/* no ++it */) {
165	if (it->cmd_id == cmd_id && it->cookie == cookie) {	12✔
166	it = _mavlink_command_int_handler_table.erase(it);	×
167	} else {
168	++it;	12✔
169	}
170	}
171
172	// COMMAND_LONG
173	for (auto it = _mavlink_command_long_handler_table.begin();	12✔
174	it != _mavlink_command_long_handler_table.end();	54✔
175	/* no ++it */) {
176	if (it->cmd_id == cmd_id && it->cookie == cookie) {	42✔
177	it = _mavlink_command_long_handler_table.erase(it);	12✔
178	} else {
179	++it;	30✔
180	}
181	}
182	}	12✔
183
184	void MavlinkCommandReceiver::unregister_all_mavlink_command_handlers(const void* cookie)	351✔
185	{
186	std::lock_guard<std::mutex> lock(_mavlink_command_handler_table_mutex);	351✔
187
188	// COMMAND_INT
189	for (auto it = _mavlink_command_int_handler_table.begin();	351✔
190	it != _mavlink_command_int_handler_table.end();	533✔
191	/* no ++it */) {
192	if (it->cookie == cookie) {	182✔
193	it = _mavlink_command_int_handler_table.erase(it);	169✔
194	} else {
195	++it;	13✔
196	}
197	}
198
199	// COMMAND_LONG
200	for (auto it = _mavlink_command_long_handler_table.begin();	351✔
201	it != _mavlink_command_long_handler_table.end();	1,151✔
202	/* no ++it */) {
203	if (it->cookie == cookie) {	800✔
204	it = _mavlink_command_long_handler_table.erase(it);	436✔
205	} else {
206	++it;	364✔
207	}
208	}
209	}	351✔
210
211	} // namespace mavsdk

mavlink / MAVSDK / 23876929365

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