4578731844

Build Type

push

github

Committed by GitHub

Commit Message

Merge pull request #2012 from mavlink/rename-parent

Run Details

885 of 885 new or added lines in 31 files covered. (100.0%)

7416 of 24250 relevant lines covered (30.58%)

21.54 hits per line

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

13.21

/src/mavsdk/core/timesync.cpp

#include "timesync.h"
#include "log.h"
#include "system_impl.h"

// Partially based on: https://github.com/mavlink/mavros/blob/master/mavros/src/plugins/sys_time.cpp

namespace mavsdk {

Timesync::Timesync(SystemImpl& parent) : _system_impl(parent) {}

Timesync::~Timesync()
{
    _system_impl.unregister_all_mavlink_message_handlers(this);
}

void Timesync::enable()
{
    _is_enabled = true;
    _system_impl.register_mavlink_message_handler(
        MAVLINK_MSG_ID_TIMESYNC,
        [this](const mavlink_message_t& message) { process_timesync(message); },
        this);
}

void Timesync::do_work()
{
    if (!_is_enabled) {
        return;
    }

    if (_system_impl.get_time().elapsed_since_s(_last_time) >= TIMESYNC_SEND_INTERVAL_S) {
        if (_system_impl.is_connected()) {
            uint64_t now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
                                  _system_impl.get_autopilot_time().now().time_since_epoch())
                                  .count();
            send_timesync(0, now_ns);
        } else {
            _autopilot_timesync_acquired = false;
        }
        _last_time = _system_impl.get_time().steady_time();
    }
}

void Timesync::process_timesync(const mavlink_message_t& message)
{
    mavlink_timesync_t timesync{};

    mavlink_msg_timesync_decode(&message, &timesync);

    int64_t now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
                         _system_impl.get_autopilot_time().now().time_since_epoch())
                         .count();

    if (timesync.tc1 == 0 && _autopilot_timesync_acquired) {
        // Send synced time to remote system
        send_timesync(now_ns, timesync.ts1);
    } else if (timesync.tc1 > 0) {
        // Time offset between this system and the remote system is calculated assuming RTT for
        // the timesync packet is roughly equal both ways.
        set_timesync_offset((timesync.tc1 * 2 - (timesync.ts1 + now_ns)) / 2, timesync.ts1);
    }
}

void Timesync::send_timesync(uint64_t tc1, uint64_t ts1)
{
    mavlink_message_t message;

    mavlink_msg_timesync_pack(
        _system_impl.get_own_system_id(),
        _system_impl.get_own_component_id(),
        &message,
        static_cast<int64_t>(tc1),
        static_cast<int64_t>(ts1),
        0,
        0);
    _system_impl.send_message(message);
}

void Timesync::set_timesync_offset(int64_t offset_ns, uint64_t start_transfer_local_time_ns)
{
    uint64_t now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
                          _system_impl.get_autopilot_time().now().time_since_epoch())
                          .count();

    // Calculate the round trip time (RTT) it took the timesync packet to bounce back to us from
    // remote system
    uint64_t rtt_ns = now_ns - start_transfer_local_time_ns;

    if (rtt_ns < MAX_RTT_SAMPLE_MS * 1000000ULL) { // Only use samples with low RTT

        // Save time offset for other components to use
        _system_impl.get_autopilot_time().shift_time_by(std::chrono::nanoseconds(offset_ns));
        _autopilot_timesync_acquired = true;

        // Reset high RTT count after filter update
        _high_rtt_count = 0;
    } else {
        // Increment counter if round trip time is too high for accurate timesync
        _high_rtt_count++;

        if (_high_rtt_count > MAX_CONS_HIGH_RTT) {
            // Issue a warning to the user if the RTT is constantly high
            LogWarn() << "RTT too high for timesync: " << static_cast<double>(rtt_ns) / 1000000.0
                      << " ms.";

            // Reset counter
            _high_rtt_count = 0;
        }
    }
}

} // namespace mavsdk

1	#include "timesync.h"
2	#include "log.h"
3	#include "system_impl.h"
4
5	// Partially based on: https://github.com/mavlink/mavros/blob/master/mavros/src/plugins/sys_time.cpp
6
7	namespace mavsdk {
8
9	Timesync::Timesync(SystemImpl& parent) : _system_impl(parent) {}	14✔
10
11	Timesync::~Timesync()	28✔
12	{
13	_system_impl.unregister_all_mavlink_message_handlers(this);	14✔
14	}	14✔
15
16	void Timesync::enable()	×
17	{
18	_is_enabled = true;	×
19	_system_impl.register_mavlink_message_handler(	×
20	MAVLINK_MSG_ID_TIMESYNC,
21	[this](const mavlink_message_t& message) { process_timesync(message); },	×
22	this);
23	}	×
24
25	void Timesync::do_work()	742✔
26	{
27	if (!_is_enabled) {	742✔
28	return;	742✔
29	}
30
31	if (_system_impl.get_time().elapsed_since_s(_last_time) >= TIMESYNC_SEND_INTERVAL_S) {	×
32	if (_system_impl.is_connected()) {	×
33	uint64_t now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(	×
34	_system_impl.get_autopilot_time().now().time_since_epoch())	×
35	.count();	×
36	send_timesync(0, now_ns);	×
37	} else {
38	_autopilot_timesync_acquired = false;	×
39	}
40	_last_time = _system_impl.get_time().steady_time();	×
41	}
42	}
43
44	void Timesync::process_timesync(const mavlink_message_t& message)	×
45	{
46	mavlink_timesync_t timesync{};	×
47
48	mavlink_msg_timesync_decode(&message, &timesync);	×
49
50	int64_t now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(	×
51	_system_impl.get_autopilot_time().now().time_since_epoch())	×
52	.count();	×
53
54	if (timesync.tc1 == 0 && _autopilot_timesync_acquired) {	×
55	// Send synced time to remote system
56	send_timesync(now_ns, timesync.ts1);	×
57	} else if (timesync.tc1 > 0) {	×
58	// Time offset between this system and the remote system is calculated assuming RTT for
59	// the timesync packet is roughly equal both ways.
60	set_timesync_offset((timesync.tc1 * 2 - (timesync.ts1 + now_ns)) / 2, timesync.ts1);	×
61	}
62	}	×
63
64	void Timesync::send_timesync(uint64_t tc1, uint64_t ts1)	×
65	{
66	mavlink_message_t message;	×
67
68	mavlink_msg_timesync_pack(	×
69	_system_impl.get_own_system_id(),	×
70	_system_impl.get_own_component_id(),	×
71	&message,
72	static_cast<int64_t>(tc1),
73	static_cast<int64_t>(ts1),
74	0,
75	0);
76	_system_impl.send_message(message);	×
77	}	×
78
79	void Timesync::set_timesync_offset(int64_t offset_ns, uint64_t start_transfer_local_time_ns)	×
80	{
81	uint64_t now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(	×
82	_system_impl.get_autopilot_time().now().time_since_epoch())	×
83	.count();	×
84
85	// Calculate the round trip time (RTT) it took the timesync packet to bounce back to us from
86	// remote system
87	uint64_t rtt_ns = now_ns - start_transfer_local_time_ns;	×
88
89	if (rtt_ns < MAX_RTT_SAMPLE_MS * 1000000ULL) { // Only use samples with low RTT	×
90
91	// Save time offset for other components to use
92	_system_impl.get_autopilot_time().shift_time_by(std::chrono::nanoseconds(offset_ns));	×
93	_autopilot_timesync_acquired = true;	×
94
95	// Reset high RTT count after filter update
96	_high_rtt_count = 0;	×
97	} else {
98	// Increment counter if round trip time is too high for accurate timesync
99	_high_rtt_count++;	×
100
101	if (_high_rtt_count > MAX_CONS_HIGH_RTT) {	×
102	// Issue a warning to the user if the RTT is constantly high
103	LogWarn() << "RTT too high for timesync: " << static_cast<double>(rtt_ns) / 1000000.0	×
104	<< " ms.";	×
105
106	// Reset counter
107	_high_rtt_count = 0;	×
108	}
109	}
110	}	×
111
112	} // namespace mavsdk

mavlink / MAVSDK / 4578731844

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