11044081504

Committed 26 Sep 2024 01:46AM UTC coverage: 37.368%. First build

Build # 11044081504

Build Type

push

github

Committed by

web-flow

Commit Message

Merge pull request #2407 from mavlink/pr-v2.12-system-debugging

[v2.12 BACKPORT] Add some system debugging

Run Details

2 of 14 new or added lines in 2 files covered. (14.29%)

11105 of 29718 relevant lines covered (37.37%)

255.14 hits per line

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

52.38

/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 " << (int)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 " << (int)cmd.target_component_id
                       << " instead of " << (int)_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 " << (int)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(
                    [&, this](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,
                            &maybe_command_ack.value());
                        return response_message;
                    });

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

mavlink / MAVSDK / 11044081504

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