21724008972

Committed 05 Feb 2026 06:42PM UTC coverage: 49.029% (+0.07%) from 48.959%

Build # 21724008972

Build Type

push

github

Committed by

web-flow

Commit Message

core: fix MAVLink message sequence (#2762)

* core: fix MAVLink message sequence

The MAVLink headers use an inline function to get the mavlink channel,
so we end up having separate sequence numbers in different translation
units. By defining this function ourselves and linking it later, we make
sure to use the same sequence everywhere.

This should fix seq numbering that's invalid (per component).

We also add a system test to actually test this.

* system_tests: fixup thread sanitizer issues

* system_tests: use TCP for seq to avoid drops

Run Details

51 of 56 new or added lines in 6 files covered. (91.07%)

5 existing lines in 4 files now uncovered.

18362 of 37451 relevant lines covered (49.03%)

671.17 hits per line

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

62.7

/src/mavsdk/core/server_component_impl.cpp

#include "server_component_impl.h"
#include "server_plugin_impl_base.h"
#include "mavsdk_impl.h"

namespace mavsdk {

ServerComponentImpl::ServerComponentImpl(
    MavsdkImpl& mavsdk_impl, uint8_t component_id, uint8_t mav_type) :
    _mavsdk_impl(mavsdk_impl),
    _own_component_id(component_id),
    _own_mav_type(mav_type),
    _our_sender(*this),
    _mavlink_command_receiver(*this),
    _mission_transfer_server(
        _our_sender,
        mavsdk_impl.mavlink_message_handler,
        mavsdk_impl.timeout_handler,
        [this]() { return _mavsdk_impl.timeout_s(); }),
    _mavlink_parameter_server(_our_sender, mavsdk_impl.mavlink_message_handler),
    _mavlink_request_message_handler(mavsdk_impl, *this, _mavlink_command_receiver),
    _mavlink_ftp_server(*this)
{
    _autopilot_version.capabilities |= MAV_PROTOCOL_CAPABILITY_MAVLINK2;

    if (!MavlinkChannels::Instance().checkout_free_channel(_channel)) {
        // We use a default of channel 0 which will still work but not track
        // seq correctly.
        _channel = 0;
        LogErr() << "Could not get a MAVLink channel, using default 0";
    }

    register_mavlink_command_handler(
        MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES,
        [this](const MavlinkCommandReceiver::CommandLong& command) {
            send_autopilot_version();
            return make_command_ack_message(command, MAV_RESULT_ACCEPTED);
        },
        this);

    register_mavlink_command_handler(
        MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES,
        [this](const MavlinkCommandReceiver::CommandInt& command) {
            send_autopilot_version();
            return make_command_ack_message(command, MAV_RESULT_ACCEPTED);
        },
        this);

    _mavlink_request_message_handler.register_handler(
        MAVLINK_MSG_ID_PROTOCOL_VERSION,
        [this](uint8_t, uint8_t, const MavlinkRequestMessageHandler::Params&) {
            send_protocol_version();
            return MAV_RESULT_ACCEPTED;
        },
        this);

    _mavlink_request_message_handler.register_handler(
        MAVLINK_MSG_ID_AUTOPILOT_VERSION,
        [this](uint8_t, uint8_t, const MavlinkRequestMessageHandler::Params&) {
            send_autopilot_version();
            return MAV_RESULT_ACCEPTED;
        },
        this);
}

ServerComponentImpl::~ServerComponentImpl()
{
    unregister_all_mavlink_command_handlers(this);
    _mavlink_request_message_handler.unregister_all_handlers(this);

    MavlinkChannels::Instance().checkin_used_channel(_channel);
}

void ServerComponentImpl::register_plugin(ServerPluginImplBase* server_plugin_impl)
{
    // This is a bit pointless now but just mirrors what is done for the client plugins.
    server_plugin_impl->init();
}

void ServerComponentImpl::unregister_plugin(ServerPluginImplBase* server_plugin_impl)
{
    // This is a bit pointless now but just mirrors what is done for the client plugins.
    server_plugin_impl->deinit();
}

void ServerComponentImpl::register_mavlink_command_handler(
    uint16_t cmd_id,
    const MavlinkCommandReceiver::MavlinkCommandIntHandler& callback,
    const void* cookie)
{
    _mavlink_command_receiver.register_mavlink_command_handler(cmd_id, callback, cookie);
}

void ServerComponentImpl::register_mavlink_command_handler(
    uint16_t cmd_id,
    const MavlinkCommandReceiver::MavlinkCommandLongHandler& callback,
    const void* cookie)
{
    _mavlink_command_receiver.register_mavlink_command_handler(cmd_id, callback, cookie);
}

void ServerComponentImpl::unregister_mavlink_command_handler(uint16_t cmd_id, const void* cookie)
{
    _mavlink_command_receiver.unregister_mavlink_command_handler(cmd_id, cookie);
}

void ServerComponentImpl::unregister_all_mavlink_command_handlers(const void* cookie)
{
    _mavlink_command_receiver.unregister_all_mavlink_command_handlers(cookie);
}

void ServerComponentImpl::register_mavlink_message_handler(
    uint16_t msg_id, const MavlinkMessageHandler& callback, const void* cookie)
{
    _mavsdk_impl.mavlink_message_handler.register_one(msg_id, callback, cookie);
}

void ServerComponentImpl::unregister_mavlink_message_handler(uint16_t msg_id, const void* cookie)
{
    _mavsdk_impl.mavlink_message_handler.unregister_one(msg_id, cookie);
}

void ServerComponentImpl::unregister_all_mavlink_message_handlers(const void* cookie)
{
    _mavsdk_impl.mavlink_message_handler.unregister_all(cookie);
}

void ServerComponentImpl::unregister_all_mavlink_message_handlers_blocking(const void* cookie)
{
    _mavsdk_impl.mavlink_message_handler.unregister_all_blocking(cookie);
}

void ServerComponentImpl::do_work()
{
    _mavlink_parameter_server.do_work();
    _mission_transfer_server.do_work();
}

Sender& ServerComponentImpl::sender()
{
    return _our_sender;
}

uint8_t ServerComponentImpl::get_own_system_id() const
{
    return _mavsdk_impl.get_own_system_id();
}

void ServerComponentImpl::set_own_component_id(uint8_t own_component_id)
{
    _own_component_id = own_component_id;
}
uint8_t ServerComponentImpl::get_own_component_id() const
{
    return _own_component_id;
}

Time& ServerComponentImpl::get_time()
{
    return _mavsdk_impl.time;
}

bool ServerComponentImpl::send_message(mavlink_message_t& message)
{
    return _mavsdk_impl.send_message(message);
}

bool ServerComponentImpl::send_command_ack(mavlink_command_ack_t& command_ack)
{
    return queue_message([&, this](MavlinkAddress mavlink_address, uint8_t channel) {
        mavlink_message_t message;
        mavlink_msg_command_ack_encode_chan(
            mavlink_address.system_id,
            mavlink_address.component_id,
            channel,
            &message,
            &command_ack);
        return message;
    });
}

bool ServerComponentImpl::queue_message(
    std::function<mavlink_message_t(MavlinkAddress mavlink_address, uint8_t channel)> fun)
{
    std::lock_guard<std::mutex> lock(_mavlink_pack_mutex);
    MavlinkAddress mavlink_address{get_own_system_id(), get_own_component_id()};
    mavlink_message_t message = fun(mavlink_address, _channel);
    return _mavsdk_impl.send_message(message);
}

CallEveryHandler::Cookie
ServerComponentImpl::add_call_every(std::function<void()> callback, float interval_s)
{
    return _mavsdk_impl.call_every_handler.add(
        std::move(callback), static_cast<double>(interval_s));
}

void ServerComponentImpl::change_call_every(float interval_s, CallEveryHandler::Cookie cookie)
{
    _mavsdk_impl.call_every_handler.change(static_cast<double>(interval_s), cookie);
}

void ServerComponentImpl::reset_call_every(CallEveryHandler::Cookie cookie)
{
    _mavsdk_impl.call_every_handler.reset(cookie);
}

void ServerComponentImpl::remove_call_every(CallEveryHandler::Cookie cookie)
{
    _mavsdk_impl.call_every_handler.remove(cookie);
}

mavlink_command_ack_t ServerComponentImpl::make_command_ack_message(
    const MavlinkCommandReceiver::CommandLong& command, MAV_RESULT result)
{
    mavlink_command_ack_t command_ack{};
    command_ack.command = command.command;
    command_ack.result = result;
    command_ack.progress = std::numeric_limits<uint8_t>::max();
    command_ack.result_param2 = 0;
    command_ack.target_system = command.origin_system_id;
    command_ack.target_component = command.origin_component_id;

    return command_ack;
}

mavlink_command_ack_t ServerComponentImpl::make_command_ack_message(
    const MavlinkCommandReceiver::CommandInt& command, MAV_RESULT result)
{
    mavlink_command_ack_t command_ack{};
    command_ack.command = command.command;
    command_ack.result = result;
    command_ack.progress = std::numeric_limits<uint8_t>::max();
    command_ack.result_param2 = 0;
    command_ack.target_system = command.origin_system_id;
    command_ack.target_component = command.origin_component_id;

    return command_ack;
}

void ServerComponentImpl::send_heartbeat()
{
    queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {
        mavlink_message_t message;
        mavlink_msg_heartbeat_pack_chan(
            get_own_system_id(),
            mavlink_address.component_id,
            channel,
            &message,
            _own_mav_type,
            mavlink_address.component_id == MAV_COMP_ID_AUTOPILOT1 ?
                _mavsdk_impl.get_mav_autopilot() :
                static_cast<uint8_t>(MAV_AUTOPILOT_INVALID),
            mavlink_address.component_id == MAV_COMP_ID_AUTOPILOT1 ? _base_mode.load() : 0,
            mavlink_address.component_id == MAV_COMP_ID_AUTOPILOT1 ? _custom_mode.load() : 0,
            get_system_status());
        return message;
    });
}

void ServerComponentImpl::set_system_status(uint8_t system_status)
{
    _system_status = static_cast<MAV_STATE>(system_status);
}

uint8_t ServerComponentImpl::get_system_status() const
{
    return _system_status;
}

void ServerComponentImpl::set_base_mode(uint8_t base_mode)
{
    _base_mode = base_mode;
}

uint8_t ServerComponentImpl::get_base_mode() const
{
    return _base_mode;
}

void ServerComponentImpl::set_custom_mode(uint32_t custom_mode)
{
    _custom_mode = custom_mode;
}

uint32_t ServerComponentImpl::get_custom_mode() const
{
    return _custom_mode;
}

void ServerComponentImpl::call_user_callback_located(
    const std::string& filename, const int linenumber, const std::function<void()>& func)
{
    _mavsdk_impl.call_user_callback_located(filename, linenumber, func);
}

TimeoutHandler::Cookie ServerComponentImpl::register_timeout_handler(
    const std::function<void()>& callback, double duration_s)
{
    return _mavsdk_impl.timeout_handler.add(callback, duration_s);
}

void ServerComponentImpl::refresh_timeout_handler(TimeoutHandler::Cookie cookie)
{
    _mavsdk_impl.timeout_handler.refresh(cookie);
}

void ServerComponentImpl::unregister_timeout_handler(TimeoutHandler::Cookie cookie)
{
    _mavsdk_impl.timeout_handler.remove(cookie);
}

void ServerComponentImpl::add_capabilities(uint64_t add_capabilities)
{
    {
        std::lock_guard<std::mutex> lock(_autopilot_version_mutex);
        _autopilot_version.capabilities |= add_capabilities;
    }

    // We need to resend capabilities.
    send_autopilot_version();
}

void ServerComponentImpl::set_flight_sw_version(uint32_t flight_sw_version)
{
    std::lock_guard<std::mutex> lock(_autopilot_version_mutex);
    _autopilot_version.flight_sw_version = flight_sw_version;
}

void ServerComponentImpl::set_middleware_sw_version(uint32_t middleware_sw_version)
{
    std::lock_guard<std::mutex> lock(_autopilot_version_mutex);
    _autopilot_version.middleware_sw_version = middleware_sw_version;
}

void ServerComponentImpl::set_os_sw_version(uint32_t os_sw_version)
{
    std::lock_guard<std::mutex> lock(_autopilot_version_mutex);
    _autopilot_version.os_sw_version = os_sw_version;
}

void ServerComponentImpl::set_board_version(uint32_t board_version)
{
    std::lock_guard<std::mutex> lock(_autopilot_version_mutex);
    _autopilot_version.board_version = board_version;
}

void ServerComponentImpl::set_vendor_id(uint16_t vendor_id)
{
    std::lock_guard<std::mutex> lock(_autopilot_version_mutex);
    _autopilot_version.vendor_id = vendor_id;
}

void ServerComponentImpl::set_product_id(uint16_t product_id)
{
    std::lock_guard<std::mutex> lock(_autopilot_version_mutex);
    _autopilot_version.product_id = product_id;
}

bool ServerComponentImpl::set_uid2(std::string uid2)
{
    std::lock_guard<std::mutex> lock(_autopilot_version_mutex);
    if (uid2.size() > _autopilot_version.uid2.size()) {
        return false;
    }
    _autopilot_version.uid2 = {0};
    std::copy(uid2.begin(), uid2.end(), _autopilot_version.uid2.data());
    return true;
}

void ServerComponentImpl::send_autopilot_version()
{
    std::lock_guard<std::mutex> lock(_autopilot_version_mutex);
    const uint8_t custom_values[8] = {0}; // TO-DO: maybe?

    queue_message([&, this](MavlinkAddress mavlink_address, uint8_t channel) {
        mavlink_message_t message;
        mavlink_msg_autopilot_version_pack_chan(
            mavlink_address.system_id,
            mavlink_address.component_id,
            channel,
            &message,
            _autopilot_version.capabilities,
            _autopilot_version.flight_sw_version,
            _autopilot_version.middleware_sw_version,
            _autopilot_version.os_sw_version,
            _autopilot_version.board_version,
            custom_values,
            custom_values,
            custom_values,
            _autopilot_version.vendor_id,
            _autopilot_version.product_id,
            0,
            _autopilot_version.uid2.data());
        return message;
    });
}

void ServerComponentImpl::send_protocol_version()
{
    queue_message([&, this](MavlinkAddress mavlink_address, uint8_t channel) {
        mavlink_message_t message;
        mavlink_msg_protocol_version_pack_chan(
            mavlink_address.system_id,
            mavlink_address.component_id,
            channel,
            &message,
            MAVLINK_VERSION_INFO.version,
            MAVLINK_VERSION_INFO.min_version,
            MAVLINK_VERSION_INFO.max_version,
            MAVLINK_VERSION_INFO.spec_version_hash,
            MAVLINK_VERSION_INFO.library_version_hash);
        return message;
    });
}

ServerComponentImpl::OurSender::OurSender(ServerComponentImpl& server_component_impl) :
    _server_component_impl(server_component_impl)
{}

bool ServerComponentImpl::OurSender::send_message(mavlink_message_t& message)
{
    return _server_component_impl.send_message(message);
}

bool ServerComponentImpl::OurSender::queue_message(
    std::function<mavlink_message_t(MavlinkAddress, uint8_t)> fun)
{
    return _server_component_impl.queue_message(fun);
}

uint8_t ServerComponentImpl::OurSender::get_own_system_id() const
{
    return _server_component_impl.get_own_system_id();
}

uint8_t ServerComponentImpl::OurSender::get_own_component_id() const
{
    return _server_component_impl.get_own_component_id();
}

CompatibilityMode ServerComponentImpl::OurSender::compatibility_mode() const
{
    return _server_component_impl._mavsdk_impl.get_compatibility_mode();
}

} // namespace mavsdk

1	#include "server_component_impl.h"
2	#include "server_plugin_impl_base.h"
3	#include "mavsdk_impl.h"
4
5	namespace mavsdk {
6
7	ServerComponentImpl::ServerComponentImpl(	149✔
8	MavsdkImpl& mavsdk_impl, uint8_t component_id, uint8_t mav_type) :	149✔
9	_mavsdk_impl(mavsdk_impl),	149✔
10	_own_component_id(component_id),	149✔
11	_own_mav_type(mav_type),	149✔
12	_our_sender(*this),	149✔
13	_mavlink_command_receiver(*this),	149✔
14	_mission_transfer_server(	298✔
15	_our_sender,
16	mavsdk_impl.mavlink_message_handler,	149✔
17	mavsdk_impl.timeout_handler,	149✔
18	[this]() { return _mavsdk_impl.timeout_s(); }),	8✔
19	_mavlink_parameter_server(_our_sender, mavsdk_impl.mavlink_message_handler),	149✔
20	_mavlink_request_message_handler(mavsdk_impl, *this, _mavlink_command_receiver),	149✔
21	_mavlink_ftp_server(*this)	298✔
22	{
23	_autopilot_version.capabilities \|= MAV_PROTOCOL_CAPABILITY_MAVLINK2;	149✔
24
25	if (!MavlinkChannels::Instance().checkout_free_channel(_channel)) {	149✔
26	// We use a default of channel 0 which will still work but not track
27	// seq correctly.
28	_channel = 0;	×
29	LogErr() << "Could not get a MAVLink channel, using default 0";	×
30	}
31
32	register_mavlink_command_handler(	149✔
33	MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES,
34	[this](const MavlinkCommandReceiver::CommandLong& command) {	6✔
35	send_autopilot_version();	3✔
36	return make_command_ack_message(command, MAV_RESULT_ACCEPTED);	3✔
37	},
38	this);
39
40	register_mavlink_command_handler(	149✔
41	MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES,
42	[this](const MavlinkCommandReceiver::CommandInt& command) {	×
43	send_autopilot_version();	×
44	return make_command_ack_message(command, MAV_RESULT_ACCEPTED);	×
45	},
46	this);
47
48	_mavlink_request_message_handler.register_handler(	149✔
49	MAVLINK_MSG_ID_PROTOCOL_VERSION,
50	[this](uint8_t, uint8_t, const MavlinkRequestMessageHandler::Params&) {	×
51	send_protocol_version();	×
52	return MAV_RESULT_ACCEPTED;	×
53	},
54	this);
55
56	_mavlink_request_message_handler.register_handler(	149✔
57	MAVLINK_MSG_ID_AUTOPILOT_VERSION,
58	[this](uint8_t, uint8_t, const MavlinkRequestMessageHandler::Params&) {	69✔
59	send_autopilot_version();	69✔
60	return MAV_RESULT_ACCEPTED;	69✔
61	},
62	this);
63	}	149✔
64
65	ServerComponentImpl::~ServerComponentImpl()	149✔
66	{
67	unregister_all_mavlink_command_handlers(this);	149✔
68	_mavlink_request_message_handler.unregister_all_handlers(this);	149✔
69
70	MavlinkChannels::Instance().checkin_used_channel(_channel);	149✔
71	}	149✔
72
73	void ServerComponentImpl::register_plugin(ServerPluginImplBase* server_plugin_impl)	58✔
74	{
75	// This is a bit pointless now but just mirrors what is done for the client plugins.
76	server_plugin_impl->init();	58✔
77	}	58✔
78
79	void ServerComponentImpl::unregister_plugin(ServerPluginImplBase* server_plugin_impl)	58✔
80	{
81	// This is a bit pointless now but just mirrors what is done for the client plugins.
82	server_plugin_impl->deinit();	58✔
83	}	58✔
84
85	void ServerComponentImpl::register_mavlink_command_handler(	149✔
86	uint16_t cmd_id,
87	const MavlinkCommandReceiver::MavlinkCommandIntHandler& callback,
88	const void* cookie)
89	{
90	_mavlink_command_receiver.register_mavlink_command_handler(cmd_id, callback, cookie);	149✔
91	}	149✔
92
93	void ServerComponentImpl::register_mavlink_command_handler(	380✔
94	uint16_t cmd_id,
95	const MavlinkCommandReceiver::MavlinkCommandLongHandler& callback,
96	const void* cookie)
97	{
98	_mavlink_command_receiver.register_mavlink_command_handler(cmd_id, callback, cookie);	380✔
99	}	380✔
100
101	void ServerComponentImpl::unregister_mavlink_command_handler(uint16_t cmd_id, const void* cookie)	12✔
102	{
103	_mavlink_command_receiver.unregister_mavlink_command_handler(cmd_id, cookie);	12✔
104	}	12✔
105
106	void ServerComponentImpl::unregister_all_mavlink_command_handlers(const void* cookie)	309✔
107	{
108	_mavlink_command_receiver.unregister_all_mavlink_command_handlers(cookie);	309✔
109	}	309✔
110
111	void ServerComponentImpl::register_mavlink_message_handler(	468✔
112	uint16_t msg_id, const MavlinkMessageHandler& callback, const void* cookie)
113	{
114	_mavsdk_impl.mavlink_message_handler.register_one(msg_id, callback, cookie);	468✔
115	}	468✔
116
117	void ServerComponentImpl::unregister_mavlink_message_handler(uint16_t msg_id, const void* cookie)	×
118	{
119	_mavsdk_impl.mavlink_message_handler.unregister_one(msg_id, cookie);	×
120	}	×
121
122	void ServerComponentImpl::unregister_all_mavlink_message_handlers(const void* cookie)	298✔
123	{
124	_mavsdk_impl.mavlink_message_handler.unregister_all(cookie);	298✔
125	}	298✔
126
127	void ServerComponentImpl::unregister_all_mavlink_message_handlers_blocking(const void* cookie)	5✔
128	{
129	_mavsdk_impl.mavlink_message_handler.unregister_all_blocking(cookie);	5✔
130	}	5✔
131
132	void ServerComponentImpl::do_work()	41,300✔
133	{
134	_mavlink_parameter_server.do_work();	41,300✔
135	_mission_transfer_server.do_work();	40,758✔
136	}	40,901✔
137
138	Sender& ServerComponentImpl::sender()	164✔
139	{
140	return _our_sender;	164✔
141	}
142
143	uint8_t ServerComponentImpl::get_own_system_id() const	11,282✔
144	{
145	return _mavsdk_impl.get_own_system_id();	11,282✔
146	}
147
148	void ServerComponentImpl::set_own_component_id(uint8_t own_component_id)	×
149	{
150	_own_component_id = own_component_id;	×
151	}	×
152	uint8_t ServerComponentImpl::get_own_component_id() const	11,009✔
153	{
154	return _own_component_id;	11,009✔
155	}
156
157	Time& ServerComponentImpl::get_time()	35✔
158	{
159	return _mavsdk_impl.time;	35✔
160	}
161
UNCOV 162	bool ServerComponentImpl::send_message(mavlink_message_t& message)	×
163	{
UNCOV 164	return _mavsdk_impl.send_message(message);	×
165	}
166
167	bool ServerComponentImpl::send_command_ack(mavlink_command_ack_t& command_ack)	29✔
168	{
169	return queue_message([&, this](MavlinkAddress mavlink_address, uint8_t channel) {	58✔
170	mavlink_message_t message;
171	mavlink_msg_command_ack_encode_chan(	29✔
172	mavlink_address.system_id,	29✔
173	mavlink_address.component_id,	29✔
174	channel,
175	&message,
176	&command_ack);	29✔
177	return message;	29✔
178	});	29✔
179	}
180
181	bool ServerComponentImpl::queue_message(	9,463✔
182	std::function<mavlink_message_t(MavlinkAddress mavlink_address, uint8_t channel)> fun)
183	{
184	std::lock_guard<std::mutex> lock(_mavlink_pack_mutex);	9,463✔
185	MavlinkAddress mavlink_address{get_own_system_id(), get_own_component_id()};	9,465✔
186	mavlink_message_t message = fun(mavlink_address, _channel);	9,462✔
187	return _mavsdk_impl.send_message(message);	9,465✔
188	}	9,465✔
189
190	CallEveryHandler::Cookie
191	ServerComponentImpl::add_call_every(std::function<void()> callback, float interval_s)	2✔
192	{
193	return _mavsdk_impl.call_every_handler.add(	4✔
194	std::move(callback), static_cast<double>(interval_s));	4✔
195	}
196
197	void ServerComponentImpl::change_call_every(float interval_s, CallEveryHandler::Cookie cookie)	×
198	{
199	_mavsdk_impl.call_every_handler.change(static_cast<double>(interval_s), cookie);	×
200	}	×
201
202	void ServerComponentImpl::reset_call_every(CallEveryHandler::Cookie cookie)	×
203	{
204	_mavsdk_impl.call_every_handler.reset(cookie);	×
205	}	×
206
207	void ServerComponentImpl::remove_call_every(CallEveryHandler::Cookie cookie)	22✔
208	{
209	_mavsdk_impl.call_every_handler.remove(cookie);	22✔
210	}	22✔
211
212	mavlink_command_ack_t ServerComponentImpl::make_command_ack_message(	232✔
213	const MavlinkCommandReceiver::CommandLong& command, MAV_RESULT result)
214	{
215	mavlink_command_ack_t command_ack{};	232✔
216	command_ack.command = command.command;	232✔
217	command_ack.result = result;	232✔
218	command_ack.progress = std::numeric_limits<uint8_t>::max();	232✔
219	command_ack.result_param2 = 0;	232✔
220	command_ack.target_system = command.origin_system_id;	232✔
221	command_ack.target_component = command.origin_component_id;	232✔
222
223	return command_ack;	232✔
224	}
225
226	mavlink_command_ack_t ServerComponentImpl::make_command_ack_message(	×
227	const MavlinkCommandReceiver::CommandInt& command, MAV_RESULT result)
228	{
229	mavlink_command_ack_t command_ack{};	×
230	command_ack.command = command.command;	×
231	command_ack.result = result;	×
232	command_ack.progress = std::numeric_limits<uint8_t>::max();	×
233	command_ack.result_param2 = 0;	×
234	command_ack.target_system = command.origin_system_id;	×
235	command_ack.target_component = command.origin_component_id;	×
236
237	return command_ack;	×
238	}
239
240	void ServerComponentImpl::send_heartbeat()	494✔
241	{
242	queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {	494✔
243	mavlink_message_t message;
244	mavlink_msg_heartbeat_pack_chan(	1,442✔
245	get_own_system_id(),	1,776✔
246	mavlink_address.component_id,	493✔
247	channel,
248	&message,
249	_own_mav_type,	493✔
250	mavlink_address.component_id == MAV_COMP_ID_AUTOPILOT1 ?	493✔
251	_mavsdk_impl.get_mav_autopilot() :	265✔
252	static_cast<uint8_t>(MAV_AUTOPILOT_INVALID),
253	mavlink_address.component_id == MAV_COMP_ID_AUTOPILOT1 ? _base_mode.load() : 0,	758✔
254	mavlink_address.component_id == MAV_COMP_ID_AUTOPILOT1 ? _custom_mode.load() : 0,	752✔
255	get_system_status());	494✔
256	return message;	494✔
257	});
258	}	487✔
259
260	void ServerComponentImpl::set_system_status(uint8_t system_status)	×
261	{
262	_system_status = static_cast<MAV_STATE>(system_status);	×
263	}	×
264
265	uint8_t ServerComponentImpl::get_system_status() const	495✔
266	{
267	return _system_status;	495✔
268	}
269
270	void ServerComponentImpl::set_base_mode(uint8_t base_mode)	2✔
271	{
272	_base_mode = base_mode;	2✔
273	}	2✔
274
275	uint8_t ServerComponentImpl::get_base_mode() const	2✔
276	{
277	return _base_mode;	2✔
278	}
279
280	void ServerComponentImpl::set_custom_mode(uint32_t custom_mode)	×
281	{
282	_custom_mode = custom_mode;	×
283	}	×
284
285	uint32_t ServerComponentImpl::get_custom_mode() const	×
286	{
287	return _custom_mode;	×
288	}
289
290	void ServerComponentImpl::call_user_callback_located(	9✔
291	const std::string& filename, const int linenumber, const std::function<void()>& func)
292	{
293	_mavsdk_impl.call_user_callback_located(filename, linenumber, func);	9✔
294	}	9✔
295
296	TimeoutHandler::Cookie ServerComponentImpl::register_timeout_handler(	×
297	const std::function<void()>& callback, double duration_s)
298	{
299	return _mavsdk_impl.timeout_handler.add(callback, duration_s);	×
300	}
301
302	void ServerComponentImpl::refresh_timeout_handler(TimeoutHandler::Cookie cookie)	×
303	{
304	_mavsdk_impl.timeout_handler.refresh(cookie);	×
305	}	×
306
307	void ServerComponentImpl::unregister_timeout_handler(TimeoutHandler::Cookie cookie)	×
308	{
309	_mavsdk_impl.timeout_handler.remove(cookie);	×
310	}	×
311
312	void ServerComponentImpl::add_capabilities(uint64_t add_capabilities)	5✔
313	{
314	{
315	std::lock_guard<std::mutex> lock(_autopilot_version_mutex);	5✔
316	_autopilot_version.capabilities \|= add_capabilities;	5✔
317	}	5✔
318
319	// We need to resend capabilities.
320	send_autopilot_version();	5✔
321	}	5✔
322
323	void ServerComponentImpl::set_flight_sw_version(uint32_t flight_sw_version)	×
324	{
325	std::lock_guard<std::mutex> lock(_autopilot_version_mutex);	×
326	_autopilot_version.flight_sw_version = flight_sw_version;	×
327	}	×
328
329	void ServerComponentImpl::set_middleware_sw_version(uint32_t middleware_sw_version)	×
330	{
331	std::lock_guard<std::mutex> lock(_autopilot_version_mutex);	×
332	_autopilot_version.middleware_sw_version = middleware_sw_version;	×
333	}	×
334
335	void ServerComponentImpl::set_os_sw_version(uint32_t os_sw_version)	×
336	{
337	std::lock_guard<std::mutex> lock(_autopilot_version_mutex);	×
338	_autopilot_version.os_sw_version = os_sw_version;	×
339	}	×
340
341	void ServerComponentImpl::set_board_version(uint32_t board_version)	×
342	{
343	std::lock_guard<std::mutex> lock(_autopilot_version_mutex);	×
344	_autopilot_version.board_version = board_version;	×
345	}	×
346
347	void ServerComponentImpl::set_vendor_id(uint16_t vendor_id)	×
348	{
349	std::lock_guard<std::mutex> lock(_autopilot_version_mutex);	×
350	_autopilot_version.vendor_id = vendor_id;	×
351	}	×
352
353	void ServerComponentImpl::set_product_id(uint16_t product_id)	×
354	{
355	std::lock_guard<std::mutex> lock(_autopilot_version_mutex);	×
356	_autopilot_version.product_id = product_id;	×
357	}	×
358
359	bool ServerComponentImpl::set_uid2(std::string uid2)	×
360	{
361	std::lock_guard<std::mutex> lock(_autopilot_version_mutex);	×
362	if (uid2.size() > _autopilot_version.uid2.size()) {	×
363	return false;	×
364	}
365	_autopilot_version.uid2 = {0};	×
366	std::copy(uid2.begin(), uid2.end(), _autopilot_version.uid2.data());	×
367	return true;	×
368	}	×
369
370	void ServerComponentImpl::send_autopilot_version()	78✔
371	{
372	std::lock_guard<std::mutex> lock(_autopilot_version_mutex);	78✔
373	const uint8_t custom_values[8] = {0}; // TO-DO: maybe?	78✔
374
375	queue_message([&, this](MavlinkAddress mavlink_address, uint8_t channel) {	78✔
376	mavlink_message_t message;
377	mavlink_msg_autopilot_version_pack_chan(	78✔
378	mavlink_address.system_id,	78✔
379	mavlink_address.component_id,	78✔
380	channel,
381	&message,
382	_autopilot_version.capabilities,
383	_autopilot_version.flight_sw_version,
384	_autopilot_version.middleware_sw_version,
385	_autopilot_version.os_sw_version,
386	_autopilot_version.board_version,
387	custom_values,	78✔
388	custom_values,
389	custom_values,
390	_autopilot_version.vendor_id,	78✔
391	_autopilot_version.product_id,	78✔
392	0,
393	_autopilot_version.uid2.data());	78✔
394	return message;	78✔
395	});
396	}	78✔
397
398	void ServerComponentImpl::send_protocol_version()	×
399	{
400	queue_message([&, this](MavlinkAddress mavlink_address, uint8_t channel) {	×
401	mavlink_message_t message;
402	mavlink_msg_protocol_version_pack_chan(	×
403	mavlink_address.system_id,	×
404	mavlink_address.component_id,	×
405	channel,
406	&message,
407	MAVLINK_VERSION_INFO.version,	×
408	MAVLINK_VERSION_INFO.min_version,	×
409	MAVLINK_VERSION_INFO.max_version,	×
410	MAVLINK_VERSION_INFO.spec_version_hash,
411	MAVLINK_VERSION_INFO.library_version_hash);
412	return message;	×
413	});
414	}	×
415
416	ServerComponentImpl::OurSender::OurSender(ServerComponentImpl& server_component_impl) :	149✔
417	_server_component_impl(server_component_impl)	149✔
418	{}	149✔
419
420	bool ServerComponentImpl::OurSender::send_message(mavlink_message_t& message)	×
421	{
422	return _server_component_impl.send_message(message);	×
423	}
424
425	bool ServerComponentImpl::OurSender::queue_message(	6,980✔
426	std::function<mavlink_message_t(MavlinkAddress, uint8_t)> fun)
427	{
428	return _server_component_impl.queue_message(fun);	6,980✔
429	}
430
431	uint8_t ServerComponentImpl::OurSender::get_own_system_id() const	77✔
432	{
433	return _server_component_impl.get_own_system_id();	77✔
434	}
435
436	uint8_t ServerComponentImpl::OurSender::get_own_component_id() const	77✔
437	{
438	return _server_component_impl.get_own_component_id();	77✔
439	}
440
441	CompatibilityMode ServerComponentImpl::OurSender::compatibility_mode() const	40✔
442	{
443	return _server_component_impl._mavsdk_impl.get_compatibility_mode();	40✔
444	}
445
446	} // namespace mavsdk

mavlink / MAVSDK / 21724008972

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