fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-13 21:11:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

removed exponential filter. simple weighted average works fine.

Browse Source
This commit is contained in:
M.H.Kabir
2014-12-16 20:39:28 +05:30
parent 5c6155c94d
commit 0e41624f79
2 changed files with 4 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+4 -15
View File
@@ -122,7 +122,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_hil_local_proj_inited(0),
_hil_local_alt0(0.0f),
_hil_local_proj_ref{},
_time_offset_avg_alpha(0.75),
_time_offset(0)
{
@@ -971,15 +970,15 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg)
} else if (tsync.tc1 > 0) {
int64_t offset_ns = ((tsync.ts1 + now_ns) - (tsync.tc1 * 2)) / 2;
int64_t offset_ns = (9*_time_offset + (tsync.ts1 + now_ns - tsync.tc1*2)/2 )/10; // average offset
int64_t dt = _time_offset - offset_ns;
if (dt > 10000000 || dt < -1000000) { // 10 millisecond skew
_time_offset = offset_ns;
warnx("[timesync] Timesync offset is off. Hard-setting offset");
_time_offset = (tsync.ts1 + now_ns - tsync.tc1*2)/2;
warnx("[timesync] Companion clock offset is skewed. Hard-setting offset");
} else {
average_time_offset(offset_ns);
_time_offset = offset_ns;
}
}
@@ -1457,16 +1456,6 @@ uint64_t MavlinkReceiver::to_hrt(uint64_t usec)
return usec - (_time_offset / 1000) ;
}
void MavlinkReceiver::average_time_offset(uint64_t offset_ns)
{
/* alpha = 0.75 fixed for now. The closer alpha is to 1.0,
* the faster the moving average updates in response to
* new offset samples.
*/
_time_offset = (_time_offset_avg_alpha * offset_ns) + (1.0 - _time_offset_avg_alpha) * _time_offset;
}
void *MavlinkReceiver::start_helper(void *context)
{
MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink *)context);