yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fix offset calculation.

Browse Source
This commit is contained in:
M.H.Kabir
2015-01-04 22:38:47 +05:30
parent c47e8c2268
commit 3df73cde3e
1 changed files with 5 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
src/modules/mavlink/mavlink_receiver.cpp
View File

@@ -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.
*/