diff options
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 11 |
1 files changed, 5 insertions, 6 deletions
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d84b70e46..97108270c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -969,7 +969,7 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) mavlink_timesync_t tsync; mavlink_msg_timesync_decode(msg, &tsync); - uint64_t now_ns = hrt_absolute_time() * 1000 ; + uint64_t now_ns = hrt_absolute_time() * 1000LL ; if (tsync.tc1 == 0) { @@ -984,13 +984,12 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) } else if (tsync.tc1 > 0) { - int64_t offset_ns = (9*_time_offset + (tsync.ts1 + now_ns - tsync.tc1*2)/2 )/10; // average offset + int64_t offset_ns = (tsync.ts1 + now_ns - tsync.tc1*2)/2 ; int64_t dt = _time_offset - offset_ns; if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew - _time_offset = (tsync.ts1 + now_ns - tsync.tc1*2)/2; - warnx("[timesync] Resetting offset sync."); - + _time_offset = offset_ns; + warnx("[timesync] Hard setting offset."); } else { smooth_time_offset(offset_ns); } @@ -1473,7 +1472,7 @@ uint64_t MavlinkReceiver::to_hrt(uint64_t usec) void MavlinkReceiver::smooth_time_offset(uint64_t offset_ns) { - /* alpha = 0.75 fixed for now. The closer alpha is to 1.0, + /* alpha = 0.6 fixed for now. The closer alpha is to 1.0, * the faster the moving average updates in response to * new offset samples. */ |