diff core/VirtualClock.cpp @ 141:44d07fa9bd03 ClockSync

Ultra-basic feedback for clock sync works^CIssues: response time of the IIR filter is too slow, requires PID and better filtering algorithm.

--- a/core/VirtualClock.cpp	Mon Sep 14 14:57:54 2015 +0100
+++ b/core/VirtualClock.cpp	Mon Sep 14 15:42:11 2015 +0100
@@ -1,8 +1,12 @@
 #include "VirtualClock.h"
 void VirtualClock::init(){
 	firstRun=true;
-	movingAverage.setLength(31); //TODO: a better filtering algorithm ( Did you say Kalman?)
+	movingAverage.setLength(101); //TODO: a better filtering algorithm ( Did you say Kalman?)
 	period=-1;
+	elapsedPeriods=0;
+	startTime=0;
+	startTimeOffset=0;
+	elapsedPeriodsOffset=0;
 }
 	
 VirtualClock::VirtualClock(){
@@ -11,13 +15,29 @@
 void VirtualClock::sync(){
 	sync(1);
 }
-void VirtualClock::sync(double count){
+void VirtualClock::sync(double numPeriods){
 	myClock_t currentTime=Clock::getTimeUs();
+	static int calls=0;
+	elapsedPeriods+=numPeriods;
+	if(calls==50){ //TODO: this is dangerous as the clock might jump suddenly if currentTime is not precise
+		calls=0;
+		startTimeOffset=startTime;
+		startTime=currentTime;
+		elapsedPeriodsOffset=elapsedPeriods;
+	}
+	calls++;
 	if(firstRun==true){
 		firstRun=false;
 		startTime=currentTime;
 	} else {
-		period=movingAverage.add((currentTime-lastSync)/count); //TODO: replace with Kalman filter
+		double newPeriod=(currentTime-lastSync)/numPeriods;
+		double expectedPeriod=22.67;
+		double maxPeriodDeviation=10;
+		if(fabs(newPeriod-expectedPeriod)<maxPeriodDeviation){ // filtering outliers
+			period=movingAverage.add(newPeriod); //TODO: replace with Kalman filter
+		} else {
+			printf("period out of range: %f\n", newPeriod);
+		}
 	}
 	lastSync=currentTime;
 //	printf("lastSync: %lld\n",lastSync-startTime);
@@ -26,16 +46,24 @@
 double VirtualClock::getNow(){
 	myClock_t currentSystemTime=Clock::getTimeUs();
 	if(period<=0){
-		return currentSystemTime;
+		return currentSystemTime; // TODO: this is not very meaningful.
 	}
 	//  double beginningOfPeriod=lastSync; // TODO: if sync() does not get called every time (but e.g. only every so often),
 													 // then this line (and the class) needs editing
 	myClock_t elapsed=(currentSystemTime-startTime);
-	double now=elapsed/(double)period;
+	double now=elapsedPeriodsOffset + elapsed/(double)period;
 //	printf("elapsed=%lld;  sincelastSync=%lld; period=%f; now=%f\n", elapsed, currentSystemTime-lastSync, period, now);
 	return now;
 }
 
+void VirtualClock::addOffset(double periodOffset){
+	elapsedPeriods-=periodOffset;
+	if(elapsedPeriods<0){
+		printf("ERROR: periodOffset adjustment of %f resulted in elapsedPeriods=%f\n", periodOffset, elapsedPeriods);
+		exit(1);
+	}
+	movingAverage.reset();
+}
 double VirtualClock::getPeriod(){
 	return period;
 }