fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-13 00:50: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);
src/modules/mavlink/mavlink_receiver.h
-6
View File
@@ -137,11 +137,6 @@ private:
*/
uint64_t to_hrt(uint64_t nsec);
/**
* Exponential moving average filter to smooth time offset
*/
void average_time_offset(uint64_t offset_ns);
mavlink_status_t status;
struct vehicle_local_position_s hil_local_pos;
struct vehicle_control_mode_s _control_mode;
@@ -176,7 +171,6 @@ private:
bool _hil_local_proj_inited;
float _hil_local_alt0;
struct map_projection_reference_s _hil_local_proj_ref;
double _time_offset_avg_alpha;
uint64_t _time_offset;
/* do not allow copying this class */