diff options
| author | M.H.Kabir <mhkabir98@gmail.com> | 2015-01-04 22:38:47 +0530 |
|---|---|---|
| committer | M.H.Kabir <mhkabir98@gmail.com> | 2015-01-04 22:38:47 +0530 |
| commit | 3df73cde3e3bd092cdf28990c0736001681a9289 (patch) | |
| tree | ce328f24f60950250cf2f132883f1fa516629c23 /src | |
| parent | c47e8c22688ad3ac358c71337ef8cb0feeeba409 (diff) | |
| download | px4-firmware-3df73cde3e3bd092cdf28990c0736001681a9289.tar.gz px4-firmware-3df73cde3e3bd092cdf28990c0736001681a9289.tar.bz2 px4-firmware-3df73cde3e3bd092cdf28990c0736001681a9289.zip | |
Fix offset calculation.
Diffstat (limited to 'src')
| -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. */ |