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

51.4

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