Restore EMA. Works better for low rates

2 files changed, 20 insertions, 2 deletions

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index cd385ffac..5aa41e5d8 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -122,6 +122,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_hil_local_proj_inited(0),
 	_hil_local_alt0(0.0f),
 	_hil_local_proj_ref{},
+	_time_offset_avg_alpha(0.6),
 	_time_offset(0)
 {
 
@@ -978,7 +979,7 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg)
 			warnx("[timesync] Companion clock offset is skewed. Hard-setting offset");
 
 		} else {
-			_time_offset = offset_ns;
+			smooth_time_offset(offset_ns);
 		}
 	}
 
@@ -1456,6 +1457,18 @@ uint64_t MavlinkReceiver::to_hrt(uint64_t usec)
 	return usec - (_time_offset / 1000) ;
 }
 
+
+void MavlinkReceiver::smooth_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 7c79f661b..a677e75cd 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -136,7 +136,11 @@ private:
 	* Convert remote nsec timestamp to local hrt time (usec)
 	*/
 	uint64_t to_hrt(uint64_t nsec);
-	
+	/**
+	* Exponential moving average filter to smooth time offset
+	*/
+	void smooth_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;
@@ -171,6 +175,7 @@ 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 */