4505821829

Build Type

push

github

Committed by GitHub

Commit Message

Merge pull request #2006 from mavlink/pr-wait-for-info

Run Details

10 of 10 new or added lines in 1 file covered. (100.0%)

7419 of 24203 relevant lines covered (30.65%)

22.02 hits per line

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

0.0

/src/mavsdk/plugins/info/info_impl.cpp

#include <functional>
#include <cstring>
#include <numeric>
#include "info_impl.h"
#include "system.h"

namespace mavsdk {

InfoImpl::InfoImpl(System& system) : PluginImplBase(system)
{
    _parent->register_plugin(this);
}

InfoImpl::InfoImpl(std::shared_ptr<System> system) : PluginImplBase(std::move(system))
{
    _parent->register_plugin(this);
}

InfoImpl::~InfoImpl()
{
    _parent->unregister_plugin(this);
}

void InfoImpl::init()
{
    _parent->register_mavlink_message_handler(
        MAVLINK_MSG_ID_AUTOPILOT_VERSION,
        [this](const mavlink_message_t& message) { process_autopilot_version(message); },
        this);

    _parent->register_mavlink_message_handler(
        MAVLINK_MSG_ID_FLIGHT_INFORMATION,
        [this](const mavlink_message_t& message) { process_flight_information(message); },
        this);

    _parent->register_mavlink_message_handler(
        MAVLINK_MSG_ID_ATTITUDE,
        [this](const mavlink_message_t& message) { process_attitude(message); },
        this);
}

void InfoImpl::deinit()
{
    _parent->unregister_all_mavlink_message_handlers(this);
}

void InfoImpl::enable()
{
    // We can't rely on System to request the autopilot_version,
    // so we do it here, anyway.
    _parent->send_autopilot_version_request();
    _parent->send_flight_information_request();

    // We're going to retry until we have the version.
    _parent->add_call_every([this]() { request_version_again(); }, 1.0f, &_call_every_cookie);

    // We're going to periodically ask for the flight information
    _parent->add_call_every(
        [this]() { request_flight_information(); }, 1.0f, &_flight_info_call_every_cookie);
}

void InfoImpl::disable()
{
    _parent->remove_call_every(_call_every_cookie);
    _parent->remove_call_every(_flight_info_call_every_cookie);

    {
        std::lock_guard<std::mutex> lock(_mutex);
        _information_received = false;
        _flight_information_received = false;
    }
}

void InfoImpl::request_version_again()
{
    {
        std::lock_guard<std::mutex> lock(_mutex);
        if (_information_received) {
            _parent->remove_call_every(_call_every_cookie);
            return;
        }
    }

    _parent->send_autopilot_version_request();
}

void InfoImpl::request_flight_information()
{
    // We will request new flight information from the autopilot only if
    // we go from an armed to disarmed state or if we haven't received any
    // information yet
    if ((_was_armed && !_parent->is_armed()) || !_flight_information_received) {
        _parent->send_flight_information_request();
    }

    _was_armed = _parent->is_armed();
}

void InfoImpl::process_autopilot_version(const mavlink_message_t& message)
{
    std::lock_guard<std::mutex> lock(_mutex);

    mavlink_autopilot_version_t autopilot_version;
    mavlink_msg_autopilot_version_decode(&message, &autopilot_version);

    _version.flight_sw_major = (autopilot_version.flight_sw_version >> (8 * 3)) & 0xFF;
    _version.flight_sw_minor = (autopilot_version.flight_sw_version >> (8 * 2)) & 0xFF;
    _version.flight_sw_patch = (autopilot_version.flight_sw_version >> (8 * 1)) & 0xFF;
    _version.flight_sw_version_type =
        get_flight_software_version_type(static_cast<FIRMWARE_VERSION_TYPE>(
            (autopilot_version.flight_sw_version >> (8 * 0)) & 0xFF));

    // first three bytes of flight_custom_version (little endian) describe vendor version
    _version.flight_sw_git_hash = swap_and_translate_binary_to_str(
        autopilot_version.flight_custom_version + 3,
        sizeof(autopilot_version.flight_custom_version) - 3);

    _version.flight_sw_vendor_major = autopilot_version.flight_custom_version[2];
    _version.flight_sw_vendor_minor = autopilot_version.flight_custom_version[1];
    _version.flight_sw_vendor_patch = autopilot_version.flight_custom_version[0];

    _version.os_sw_major = (autopilot_version.os_sw_version >> (8 * 3)) & 0xFF;
    _version.os_sw_minor = (autopilot_version.os_sw_version >> (8 * 2)) & 0xFF;
    _version.os_sw_patch = (autopilot_version.os_sw_version >> (8 * 1)) & 0xFF;

    // Debug() << "flight version: "
    //     << _version.flight_sw_major
    //     << "."
    //     << _version.flight_sw_minor
    //     << "."
    //     << _version.flight_sw_patch;

    // Debug() << "os version: "
    //     << _version.os_sw_major
    //     << "."
    //     << _version.os_sw_minor
    //     << "."
    //     << _version.os_sw_patch;

    _version.os_sw_git_hash = swap_and_translate_binary_to_str(
        autopilot_version.os_custom_version, sizeof(autopilot_version.os_custom_version));

    _product.vendor_id = autopilot_version.vendor_id;
    _product.vendor_name = vendor_id_str(autopilot_version.vendor_id);

    _product.product_id = autopilot_version.product_id;
    _product.product_name = product_id_str(autopilot_version.product_id);

    _identification.hardware_uid =
        translate_binary_to_str(autopilot_version.uid2, sizeof(autopilot_version.uid2));

    _identification.legacy_uid = autopilot_version.uid;

    _information_received = true;
}

Info::Version::FlightSoftwareVersionType
InfoImpl::get_flight_software_version_type(FIRMWARE_VERSION_TYPE firmwareVersionType)
{
    switch (firmwareVersionType) {
        case FIRMWARE_VERSION_TYPE_DEV:
            return Info::Version::FlightSoftwareVersionType::Dev;

        case FIRMWARE_VERSION_TYPE_ALPHA:
            return Info::Version::FlightSoftwareVersionType::Alpha;

        case FIRMWARE_VERSION_TYPE_BETA:
            return Info::Version::FlightSoftwareVersionType::Beta;

        case FIRMWARE_VERSION_TYPE_RC:
            return Info::Version::FlightSoftwareVersionType::Rc;

        case FIRMWARE_VERSION_TYPE_OFFICIAL:
            return Info::Version::FlightSoftwareVersionType::Release;

        default:
            return Info::Version::FlightSoftwareVersionType::Unknown;
    }
}

void InfoImpl::process_flight_information(const mavlink_message_t& message)
{
    std::lock_guard<std::mutex> lock(_mutex);

    mavlink_flight_information_t flight_information;
    mavlink_msg_flight_information_decode(&message, &flight_information);

    _flight_info.time_boot_ms = flight_information.time_boot_ms;
    _flight_info.flight_uid = flight_information.flight_uuid;

    _flight_information_received = true;
}

std::string InfoImpl::swap_and_translate_binary_to_str(uint8_t* binary, unsigned binary_len)
{
    std::string str(binary_len * 2, '0');

    for (unsigned i = 0; i < binary_len; ++i) {
        // One hex number occupies 2 chars.
        // The binary is in little endian, therefore we need to swap the bytes for us to read.
        snprintf(&str[i * 2], str.length() - i * 2, "%02x", binary[binary_len - 1 - i]);
    }

    return str;
}

std::string InfoImpl::translate_binary_to_str(uint8_t* binary, unsigned binary_len)
{
    std::string str(binary_len * 2 + 1, '0');

    for (unsigned i = 0; i < binary_len; ++i) {
        // One hex number occupies 2 chars.
        snprintf(&str[i * 2], str.length() - i * 2, "%02x", binary[i]);
    }

    return str;
}

std::pair<Info::Result, Info::Identification> InfoImpl::get_identification() const
{
    wait_for_information();

    std::lock_guard<std::mutex> lock(_mutex);
    return std::make_pair<>(
        (_information_received ? Info::Result::Success : Info::Result::InformationNotReceivedYet),
        _identification);
}

std::pair<Info::Result, Info::Version> InfoImpl::get_version() const
{
    wait_for_information();

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

    return std::make_pair<>(
        (_information_received ? Info::Result::Success : Info::Result::InformationNotReceivedYet),
        _version);
}

std::pair<Info::Result, Info::Product> InfoImpl::get_product() const
{
    wait_for_information();
    std::lock_guard<std::mutex> lock(_mutex);

    return std::make_pair<>(
        (_information_received ? Info::Result::Success : Info::Result::InformationNotReceivedYet),
        _product);
}

std::pair<Info::Result, Info::FlightInfo> InfoImpl::get_flight_information() const
{
    wait_for_information();
    std::lock_guard<std::mutex> lock(_mutex);

    return std::make_pair<>(
        (_flight_information_received ? Info::Result::Success :
                                        Info::Result::InformationNotReceivedYet),
        _flight_info);
}

const std::string InfoImpl::vendor_id_str(uint16_t vendor_id)
{
    switch (vendor_id) {
        case 0x26ac:
            return "3D Robotics Inc.";
        default:
            return "undefined";
    }
}

const std::string InfoImpl::product_id_str(uint16_t product_id)
{
    switch (product_id) {
        case 0x0010:
            return "H520";
        default:
            return "undefined";
    }
}

void InfoImpl::process_attitude(const mavlink_message_t& message)
{
    // We use the attitude message to estimate the lockstep speed factor
    // because it's common to be sent, arrives at high rate, and contains
    // the timestamp field.
    mavlink_attitude_t attitude;
    mavlink_msg_attitude_decode(&message, &attitude);

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

    if (_last_time_boot_ms != 0) {
        _speed_factor_measurements.push(SpeedFactorMeasurement{
            static_cast<double>(attitude.time_boot_ms - _last_time_boot_ms) * 1e-3,
            _time.elapsed_since_s(_last_time_attitude_arrived)});
    }

    _last_time_boot_ms = attitude.time_boot_ms;
    _last_time_attitude_arrived = _time.steady_time();
}

std::pair<Info::Result, double> InfoImpl::get_speed_factor() const
{
    std::lock_guard<std::mutex> lock(_mutex);

    if (_speed_factor_measurements.size() == 0) {
        return std::make_pair<>(Info::Result::InformationNotReceivedYet, NAN);
    }

    const auto sum = std::accumulate(
        _speed_factor_measurements.begin(),
        _speed_factor_measurements.end(),
        SpeedFactorMeasurement{},
        [](SpeedFactorMeasurement a, SpeedFactorMeasurement b) { return a + b; });

    const double speed_factor = sum.simulated_duration_s / sum.real_time_s;

    return std::make_pair<>(Info::Result::Success, speed_factor);
}

void InfoImpl::wait_for_information() const
{
    // Wait 1.5 seconds max
    for (unsigned i = 0; i < 150; ++i) {
        if (_information_received) {
            break;
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
}

} // namespace mavsdk

1	#include <functional>
2	#include <cstring>
3	#include <numeric>
4	#include "info_impl.h"
5	#include "system.h"
6
7	namespace mavsdk {
8
9	InfoImpl::InfoImpl(System& system) : PluginImplBase(system)	×
10	{
11	_parent->register_plugin(this);	×
12	}	×
13
14	InfoImpl::InfoImpl(std::shared_ptr<System> system) : PluginImplBase(std::move(system))	×
15	{
16	_parent->register_plugin(this);	×
17	}	×
18
19	InfoImpl::~InfoImpl()	×
20	{
21	_parent->unregister_plugin(this);	×
22	}	×
23
24	void InfoImpl::init()	×
25	{
26	_parent->register_mavlink_message_handler(	×
27	MAVLINK_MSG_ID_AUTOPILOT_VERSION,
28	[this](const mavlink_message_t& message) { process_autopilot_version(message); },	×
29	this);
30
31	_parent->register_mavlink_message_handler(	×
32	MAVLINK_MSG_ID_FLIGHT_INFORMATION,
33	[this](const mavlink_message_t& message) { process_flight_information(message); },	×
34	this);
35
36	_parent->register_mavlink_message_handler(	×
37	MAVLINK_MSG_ID_ATTITUDE,
38	[this](const mavlink_message_t& message) { process_attitude(message); },	×
39	this);
40	}	×
41
42	void InfoImpl::deinit()	×
43	{
44	_parent->unregister_all_mavlink_message_handlers(this);	×
45	}	×
46
47	void InfoImpl::enable()	×
48	{
49	// We can't rely on System to request the autopilot_version,
50	// so we do it here, anyway.
51	_parent->send_autopilot_version_request();	×
52	_parent->send_flight_information_request();	×
53
54	// We're going to retry until we have the version.
55	_parent->add_call_every([this]() { request_version_again(); }, 1.0f, &_call_every_cookie);	×
56
57	// We're going to periodically ask for the flight information
58	_parent->add_call_every(	×
59	[this]() { request_flight_information(); }, 1.0f, &_flight_info_call_every_cookie);	×
60	}	×
61
62	void InfoImpl::disable()	×
63	{
64	_parent->remove_call_every(_call_every_cookie);	×
65	_parent->remove_call_every(_flight_info_call_every_cookie);	×
66
67	{
68	std::lock_guard<std::mutex> lock(_mutex);	×
69	_information_received = false;	×
70	_flight_information_received = false;	×
71	}
72	}	×
73
74	void InfoImpl::request_version_again()	×
75	{
76	{
77	std::lock_guard<std::mutex> lock(_mutex);	×
78	if (_information_received) {	×
79	_parent->remove_call_every(_call_every_cookie);	×
80	return;	×
81	}
82	}
83
84	_parent->send_autopilot_version_request();	×
85	}
86
87	void InfoImpl::request_flight_information()	×
88	{
89	// We will request new flight information from the autopilot only if
90	// we go from an armed to disarmed state or if we haven't received any
91	// information yet
92	if ((_was_armed && !_parent->is_armed()) \|\| !_flight_information_received) {	×
93	_parent->send_flight_information_request();	×
94	}
95
96	_was_armed = _parent->is_armed();	×
97	}	×
98
99	void InfoImpl::process_autopilot_version(const mavlink_message_t& message)	×
100	{
101	std::lock_guard<std::mutex> lock(_mutex);	×
102
103	mavlink_autopilot_version_t autopilot_version;	×
104	mavlink_msg_autopilot_version_decode(&message, &autopilot_version);	×
105
106	_version.flight_sw_major = (autopilot_version.flight_sw_version >> (8 * 3)) & 0xFF;	×
107	_version.flight_sw_minor = (autopilot_version.flight_sw_version >> (8 * 2)) & 0xFF;	×
108	_version.flight_sw_patch = (autopilot_version.flight_sw_version >> (8 * 1)) & 0xFF;	×
109	_version.flight_sw_version_type =	×
110	get_flight_software_version_type(static_cast<FIRMWARE_VERSION_TYPE>(	×
111	(autopilot_version.flight_sw_version >> (8 * 0)) & 0xFF));	×
112
113	// first three bytes of flight_custom_version (little endian) describe vendor version
114	_version.flight_sw_git_hash = swap_and_translate_binary_to_str(	×
115	autopilot_version.flight_custom_version + 3,
116	sizeof(autopilot_version.flight_custom_version) - 3);	×
117
118	_version.flight_sw_vendor_major = autopilot_version.flight_custom_version[2];	×
119	_version.flight_sw_vendor_minor = autopilot_version.flight_custom_version[1];	×
120	_version.flight_sw_vendor_patch = autopilot_version.flight_custom_version[0];	×
121
122	_version.os_sw_major = (autopilot_version.os_sw_version >> (8 * 3)) & 0xFF;	×
123	_version.os_sw_minor = (autopilot_version.os_sw_version >> (8 * 2)) & 0xFF;	×
124	_version.os_sw_patch = (autopilot_version.os_sw_version >> (8 * 1)) & 0xFF;	×
125
126	// Debug() << "flight version: "
127	// << _version.flight_sw_major
128	// << "."
129	// << _version.flight_sw_minor
130	// << "."
131	// << _version.flight_sw_patch;
132
133	// Debug() << "os version: "
134	// << _version.os_sw_major
135	// << "."
136	// << _version.os_sw_minor
137	// << "."
138	// << _version.os_sw_patch;
139
140	_version.os_sw_git_hash = swap_and_translate_binary_to_str(	×
141	autopilot_version.os_custom_version, sizeof(autopilot_version.os_custom_version));	×
142
143	_product.vendor_id = autopilot_version.vendor_id;	×
144	_product.vendor_name = vendor_id_str(autopilot_version.vendor_id);	×
145
146	_product.product_id = autopilot_version.product_id;	×
147	_product.product_name = product_id_str(autopilot_version.product_id);	×
148
149	_identification.hardware_uid =	×
150	translate_binary_to_str(autopilot_version.uid2, sizeof(autopilot_version.uid2));	×
151
152	_identification.legacy_uid = autopilot_version.uid;	×
153
154	_information_received = true;	×
155	}	×
156
157	Info::Version::FlightSoftwareVersionType
158	InfoImpl::get_flight_software_version_type(FIRMWARE_VERSION_TYPE firmwareVersionType)	×
159	{
160	switch (firmwareVersionType) {	×
161	case FIRMWARE_VERSION_TYPE_DEV:	×
162	return Info::Version::FlightSoftwareVersionType::Dev;	×
163
164	case FIRMWARE_VERSION_TYPE_ALPHA:	×
165	return Info::Version::FlightSoftwareVersionType::Alpha;	×
166
167	case FIRMWARE_VERSION_TYPE_BETA:	×
168	return Info::Version::FlightSoftwareVersionType::Beta;	×
169
170	case FIRMWARE_VERSION_TYPE_RC:	×
171	return Info::Version::FlightSoftwareVersionType::Rc;	×
172
173	case FIRMWARE_VERSION_TYPE_OFFICIAL:	×
174	return Info::Version::FlightSoftwareVersionType::Release;	×
175
176	default:	×
177	return Info::Version::FlightSoftwareVersionType::Unknown;	×
178	}
179	}
180
181	void InfoImpl::process_flight_information(const mavlink_message_t& message)	×
182	{
183	std::lock_guard<std::mutex> lock(_mutex);	×
184
185	mavlink_flight_information_t flight_information;	×
186	mavlink_msg_flight_information_decode(&message, &flight_information);	×
187
188	_flight_info.time_boot_ms = flight_information.time_boot_ms;	×
189	_flight_info.flight_uid = flight_information.flight_uuid;	×
190
191	_flight_information_received = true;	×
192	}	×
193
194	std::string InfoImpl::swap_and_translate_binary_to_str(uint8_t* binary, unsigned binary_len)	×
195	{
196	std::string str(binary_len * 2, '0');	×
197
198	for (unsigned i = 0; i < binary_len; ++i) {	×
199	// One hex number occupies 2 chars.
200	// The binary is in little endian, therefore we need to swap the bytes for us to read.
201	snprintf(&str[i * 2], str.length() - i * 2, "%02x", binary[binary_len - 1 - i]);	×
202	}
203
204	return str;	×
205	}
206
207	std::string InfoImpl::translate_binary_to_str(uint8_t* binary, unsigned binary_len)	×
208	{
209	std::string str(binary_len * 2 + 1, '0');	×
210
211	for (unsigned i = 0; i < binary_len; ++i) {	×
212	// One hex number occupies 2 chars.
213	snprintf(&str[i * 2], str.length() - i * 2, "%02x", binary[i]);	×
214	}
215
216	return str;	×
217	}
218
219	std::pair<Info::Result, Info::Identification> InfoImpl::get_identification() const	×
220	{
221	wait_for_information();	×
222
223	std::lock_guard<std::mutex> lock(_mutex);	×
224	return std::make_pair<>(
225	(_information_received ? Info::Result::Success : Info::Result::InformationNotReceivedYet),	×
226	_identification);	×
227	}
228
229	std::pair<Info::Result, Info::Version> InfoImpl::get_version() const	×
230	{
231	wait_for_information();	×
232
233	std::lock_guard<std::mutex> lock(_mutex);	×
234
235	return std::make_pair<>(
236	(_information_received ? Info::Result::Success : Info::Result::InformationNotReceivedYet),	×
237	_version);	×
238	}
239
240	std::pair<Info::Result, Info::Product> InfoImpl::get_product() const	×
241	{
242	wait_for_information();	×
243	std::lock_guard<std::mutex> lock(_mutex);	×
244
245	return std::make_pair<>(
246	(_information_received ? Info::Result::Success : Info::Result::InformationNotReceivedYet),	×
247	_product);	×
248	}
249
250	std::pair<Info::Result, Info::FlightInfo> InfoImpl::get_flight_information() const	×
251	{
252	wait_for_information();	×
253	std::lock_guard<std::mutex> lock(_mutex);	×
254
255	return std::make_pair<>(
256	(_flight_information_received ? Info::Result::Success :	×
257	Info::Result::InformationNotReceivedYet),
258	_flight_info);	×
259	}
260
261	const std::string InfoImpl::vendor_id_str(uint16_t vendor_id)	×
262	{
263	switch (vendor_id) {	×
264	case 0x26ac:	×
265	return "3D Robotics Inc.";	×
266	default:	×
267	return "undefined";	×
268	}
269	}
270
271	const std::string InfoImpl::product_id_str(uint16_t product_id)	×
272	{
273	switch (product_id) {	×
274	case 0x0010:	×
275	return "H520";	×
276	default:	×
277	return "undefined";	×
278	}
279	}
280
281	void InfoImpl::process_attitude(const mavlink_message_t& message)	×
282	{
283	// We use the attitude message to estimate the lockstep speed factor
284	// because it's common to be sent, arrives at high rate, and contains
285	// the timestamp field.
286	mavlink_attitude_t attitude;	×
287	mavlink_msg_attitude_decode(&message, &attitude);	×
288
289	std::lock_guard<std::mutex> lock(_mutex);	×
290
291	if (_last_time_boot_ms != 0) {	×
292	_speed_factor_measurements.push(SpeedFactorMeasurement{	×
293	static_cast<double>(attitude.time_boot_ms - _last_time_boot_ms) * 1e-3,	×
294	_time.elapsed_since_s(_last_time_attitude_arrived)});	×
295	}
296
297	_last_time_boot_ms = attitude.time_boot_ms;	×
298	_last_time_attitude_arrived = _time.steady_time();	×
299	}	×
300
301	std::pair<Info::Result, double> InfoImpl::get_speed_factor() const	×
302	{
303	std::lock_guard<std::mutex> lock(_mutex);	×
304
305	if (_speed_factor_measurements.size() == 0) {	×
306	return std::make_pair<>(Info::Result::InformationNotReceivedYet, NAN);	×
307	}
308
309	const auto sum = std::accumulate(
310	_speed_factor_measurements.begin(),
311	_speed_factor_measurements.end(),
312	SpeedFactorMeasurement{},
313	[](SpeedFactorMeasurement a, SpeedFactorMeasurement b) { return a + b; });	×
314
315	const double speed_factor = sum.simulated_duration_s / sum.real_time_s;	×
316
317	return std::make_pair<>(Info::Result::Success, speed_factor);	×
318	}
319
320	void InfoImpl::wait_for_information() const	×
321	{
322	// Wait 1.5 seconds max
323	for (unsigned i = 0; i < 150; ++i) {	×
324	if (_information_received) {	×
325	break;	×
326	}
327	std::this_thread::sleep_for(std::chrono::milliseconds(10));	×
328	}
329	}	×
330
331	} // namespace mavsdk

mavlink / MAVSDK / 4505821829

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