removed exponential filter. simple weighted average works fine.

2 files changed, 4 insertions, 21 deletions

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 88d12e775..cd385ffac 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -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);
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 71273786d..7c79f661b 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -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 */