diff core/VirtualClock.cpp @ 149:134bff10e561 ClockSync

Added simple one-variable one-measurement Kalman filter, Pid controller(which output is not used). Virtual clock is now much more precise and reactive for period. Still it is lagging behind a bit on the overall offset.

--- a/core/VirtualClock.cpp	Mon Sep 21 03:11:32 2015 +0100
+++ b/core/VirtualClock.cpp	Mon Sep 21 03:12:21 2015 +0100
@@ -1,58 +1,75 @@
 #include "VirtualClock.h"
-void VirtualClock::init(){
+void VirtualClock::init(float initialValueUs){
 	firstRun=true;
 	movingAverage.setLength(101); //TODO: a better filtering algorithm ( Did you say Kalman?)
-	period=-1;
+	blockPeriod=-1;
 	elapsedPeriods=0;
 	startTime=0;
-	startTimeOffset=0;
-	elapsedPeriodsOffset=0;
+	elapsedPeriodsOffset = 0;
+	lastSyncEstimatedTime = 0;
+	double coefficients[IIR_FILTER_STAGE_COEFFICIENTS] = {
+			0.000241359049041961, 0.000482718098083923, 0.000241359049041961, -1.95557824031504, 0.956543676511203
+	};
+	iir.setNumberOfStages(4);
+	iir.setCoefficients(coefficients);
+	printf("kalmanInit=%f\n",initialValueUs);
+	kalman.init(0.01, 158575.715009816 /*measured var() */,
+			initialValueUs
+//#ifdef USE_JUCE
+//			5804.98866213152 /*blockSize/Fs*1e6*/
+//#else
+//			5804.98866213152 /*blockSize/Fs*1e6*/
+//#endif
+			);
 }
 	
-VirtualClock::VirtualClock(){
-	init();
-}
+VirtualClock::VirtualClock(){}
 void VirtualClock::sync(){
 	sync(1);
 }
 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;
+	myClock_t currentTime = Clock::getTimeUs();
+	elapsedPeriods += numPeriods;
+	static int calls = 0;
+	if(calls == 0){
+		startTime = currentTime;
+//AA		lastSyncEstimatedTime = startTime;
+		lastSyncEstimatedTime = 0;
+	} else {
+		double newBlockPeriod = (currentTime - lastSyncTime);
+//		period = iir.process(movingAverage.add(newPeriod)); //TODO: replace with Kalman filter
+		blockPeriod = iir.process(kalman.process(newBlockPeriod));
+//		period = 22.6760958210091;
+		period = blockPeriod / numPeriods;
+		lastSyncEstimatedTime += (period * numPeriods);
+//		if(calls == 800)
+//			lastSyncEstimatedTime = lastSyncTime;
+//		printf("%lld, %lld\n", lastSyncTime, lastSyncEstimatedTime);
 	}
+	lastSyncTime = currentTime;
+//	printf("%lld\n", lastSyncTime);
 	calls++;
-	if(firstRun==true){
-		firstRun=false;
-		startTime=currentTime;
-	} else {
-		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);
 }
 
 double VirtualClock::getNow(){
 	myClock_t currentSystemTime=Clock::getTimeUs();
-	if(period<=0){
+	if(blockPeriod<=0){
 		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=elapsedPeriodsOffset + elapsed/(double)period;
-//	printf("elapsed=%lld;  sincelastSync=%lld; period=%f; now=%f\n", elapsed, currentSystemTime-lastSync, period, now);
+	double elapsed = (currentSystemTime - startTime) - lastSyncEstimatedTime;
+	double now = elapsedPeriods + elapsed / (double)period;
+//	if(now>currentSystemTime+10*1e6)
+//		now=0;
+//	static long long int pastSy=0;
+//	printf("%lld\n", currentSystemTime-pastSy);
+//	pastSy=currentSystemTime;
+	static int count=0;
+	count++;
+//	if(count&1)
+#ifdef USE_JUCE
+#else
+//	printf("%f %f %f\n", (currentSystemTime - startTime)/1e6*44100.0, blockPeriod, now);
+#endif
 	return now;
 }
 
@@ -62,8 +79,9 @@
 		printf("ERROR: periodOffset adjustment of %f resulted in elapsedPeriods=%f\n", periodOffset, elapsedPeriods);
 		exit(1);
 	}
+	printf("this is not called\n");
 	movingAverage.reset();
 }
 double VirtualClock::getPeriod(){
-	return period;
+	return blockPeriod;
 }