# HG changeset patch
# User Giulio Moro <giuliomoro@yahoo.it>
# Date 1442241731 -3600
# Node ID 44d07fa9bd03bbc975087b70297fdffc27a86cca
# Parent  5edc6d0713ef26271897cc3bc387683ea353f37a
Ultra-basic feedback for clock sync works^CIssues: response time of the IIR filter is too slow, requires PID and better filtering algorithm.

diff -r 5edc6d0713ef -r 44d07fa9bd03 core/ClockSync.cpp
--- a/core/ClockSync.cpp	Mon Sep 14 14:57:54 2015 +0100
+++ b/core/ClockSync.cpp	Mon Sep 14 15:42:11 2015 +0100
@@ -10,14 +10,20 @@
 	//  isSlave() ? client.setServer("127.0.0.1") : client.setServer("127.0.0.1");
 	isSlave() ? client.setServer("192.168.7.1") : client.setServer("192.168.7.2");
 	bufferLength=kSyncMessageLength;
+	resetTs();
+	receiveLoopSleepUs=100;
+	receiveLoopTimeout=1e5;
+	movingAverage.setLength(31);
+	expectedClockSyncType=isSlave() ? kSync : kNone;
+}
+void ClockSync::resetTs(){
 	T1=-1;
 	T1p=-1;
 	T2=-1;
 	T2p=-1;
-	receiveLoopSleepUs=100;
-	receiveLoopTimeout=1e5;
-	movingAverage.setLength(31);
-	expectedClockSyncType=isSlave() ? kSync : kNone;
+}
+bool ClockSync::areTsValid(){
+	return T1>0 && T1p>0 && T2>0 && T2p>0;
 }
 ClockSync::ClockSync(bool thisIsSlave, int aPort, VirtualClock &aVirtualClock){
 	init(thisIsSlave, aPort, aVirtualClock);
@@ -190,6 +196,7 @@
 int ClockSync::slaveHandleMessage(){
 	switch(clockSyncType){
 		case kSync: //the clockSync timestamp is meaningless, the localTimestamp is when kSync was received
+			resetTs();
 			T1p=localTimestamp;
 			expectedClockSyncType=kFollowUp;
 			break;
@@ -209,8 +216,42 @@
 			T2p=clockSyncTimestamp;
 			//TODO: evaluate things
 			double offset=(T1p-T1-T2p+T2)/2.0d;
-			printf("-----------OFFSET IS : %04.4f seconds, average: %04.4f seconds\n",
-					offset/44100.f, movingAverage.add(offset)/44100.f);
+			if(areTsValid()){
+				processOffset(offset);
+
+				/*
+				static int calls=0;
+				static double referenceOffset=0;
+
+				if(calls<100){ // start by averaging everything
+					movingAverage.add(offset);
+				} else { //once we get an estimate, start discarding outliers
+					float maxOffsetDeviation=20;
+					float deviation=fabsf(movingAverage.getAverage()-offset);
+					if(deviation<maxOffsetDeviation){
+						movingAverage.add(offset);
+						printf("w(end+1)=%4.1f;\n", movingAverage.getAverage());
+					} else {
+//						printf("Out of range: %f \n", deviation);
+					}
+				}
+				printf("offset(end+1)=%f;\n", offset);
+				if (calls==100){
+//					printf("Update reference\n");
+					referenceOffset=movingAverage.getAverage();
+				} else 	if (calls==200){
+					calls=99;
+				}
+				calls++;
+*/
+
+//				printf("%lld, %lld, %lld, %lld \n", T1, T1p, T2, T2p);
+//				if(T2-T1p<0){
+//					printf("Negative: %lld, %lld, %lld, %lld \n", T1, T1p, T2, T2p);
+//				}
+			} else {
+				printf("InvalidTs:\n  %lld, %lld, %lld, %lld \n", T1, T1p, T2, T2p);
+			}
 			expectedClockSyncType=kSync; //end of the cycle, wait for next sync.
 			break;
 		}
@@ -220,17 +261,61 @@
 	}
 	return 1;
 }
-
+#include <I2c_Codec.h>
+extern I2c_Codec* gAudioCodec;
+void ClockSync::processOffset(double offset){
+	static int calls=0;
+	// TODO: change the flow control below so that it can happen multiple times
+	//(base it upon the length of movingAverage rather than the number of calls)
+	if(calls<10) { //get an initial guess
+		movingAverage.add(offset);
+//		printf("-----------OFFSET IS : %04.4f samples, average: %04.4f samples\n",
+//				offset, movingAverage.getAverage());
+	} else if (calls==10){ //then compensate for initial offset
+//		printf("compensating for offset: %f\n", offset);
+		virtualClock->addOffset(movingAverage.getAverage());
+		movingAverage.reset();
+	} else if (calls>=10){ //use IIR filter from now on
+		//filter coefficients obtained from Matlab : [B,A]=butter(2,0.005);
+//		static float B[3]={6.10061787580662e-05, 0.000122012357516132, 6.10061787580662e-05};
+//		static float A[3]={1, -1.97778648377676, 0.978030508491796};
+		static float B[3]={6.10061787580662e-05, 0.000122012357516132, 6.10061787580662e-05};
+		static float A[3]={1, -1.97778648377676, 0.978030508491796};
+		static float pastOut[3]={0,0,0};
+		static float pastIn[3]={0,0,0};
+		float in=offset;
+		float out= -pastOut[1]*A[1] -pastOut[2]*A[2] +in*B[0] +pastIn[1]*B[1] +pastIn[2]*B[2];
+		pastOut[2]=pastOut[1];
+		pastOut[1]=out;
+		pastIn[2]=pastIn[1];
+		pastIn[1]=in;
+		offset=out;
+		static float maxOffset=0;
+		maxOffset=fabsf(offset) > fabsf(maxOffset) ? offset : maxOffset;
+		printf("%10.3f, %10.3f, %10.3f, %10.3f\n", in, offset, offset-pastOut[2], maxOffset); //unfiltered, filtered
+		if(fabsf(offset)>10 && calls>30){
+			calls=11;
+			//TODO: correct for offset
+			float targetSamplingRate=offset>0 ? 44097 : 44103;
+			gAudioCodec->setAudioSamplingRate(targetSamplingRate);
+//			pastOut[1]=pastOut[2]=pastIn[1]=pastIn[2]=offset;
+			printf("------setAudioSmplingRate to %f\n", targetSamplingRate);
+		}
+	}
+	calls++;
+}
 int ClockSync::masterHandleMessage(){
 	switch(clockSyncType){
 		case kDelayReq:
-			//TODO: do something with it
 			//send kDelayResp
 			setType(kDelayResp);
 			setTimestamp(localTimestamp);
 			send();
 			expectedClockSyncType=kNone;
+			return 1;
 		break;
+		default:
+			return -1;
 	}
 }
 
diff -r 5edc6d0713ef -r 44d07fa9bd03 core/ClockSyncThread.cpp
--- a/core/ClockSyncThread.cpp	Mon Sep 14 14:57:54 2015 +0100
+++ b/core/ClockSyncThread.cpp	Mon Sep 14 15:42:11 2015 +0100
@@ -39,9 +39,7 @@
 #ifdef USE_JUCE
 #else
 void ClockSyncThread::startThread(){
-	printf("starting\n");
 	BeagleRT_scheduleAuxiliaryTask(clockSyncTask);
-	printf("started\n");
 }
 void ClockSyncThread::stopThread(){
 	threadIsExiting=true;
@@ -56,11 +54,13 @@
 };
 
 void ClockSyncThread::run(){
+	printf("var=[");
 	while(!threadShouldExit()){
 		clockSync.sendReceiveLoop();
 //		double now=virtualClock->getNow();
 //		printf("th(end+1)=%f;\n", now);
 //		printf("act(end+1)=%lld;\n", Clock::getTimeUs());
 	}
-	printf("Thread is not running \n");
+	printf("];\n");
+//	printf("Thread is not running \n");
 }
diff -r 5edc6d0713ef -r 44d07fa9bd03 core/VirtualClock.cpp
--- 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;
 }
diff -r 5edc6d0713ef -r 44d07fa9bd03 include/ClockSync.h
--- a/include/ClockSync.h	Mon Sep 14 14:57:54 2015 +0100
+++ b/include/ClockSync.h	Mon Sep 14 15:42:11 2015 +0100
@@ -43,6 +43,9 @@
 	int receiveLoopTimeout;
 	char buffer[kSyncMessageLength];
 	VirtualClock *virtualClock;
+	void resetTs();
+	bool areTsValid();
+	void processOffset(double offset);
 public:
 	ClockSync(){};
 	ClockSync(bool thisIsSlave, int aPort, VirtualClock &aVirtualClock);
@@ -78,4 +81,4 @@
 	}
 };
 
-#endif /* CLOCK_SYNC_H_INCLUDED */
\ No newline at end of file
+#endif /* CLOCK_SYNC_H_INCLUDED */
diff -r 5edc6d0713ef -r 44d07fa9bd03 include/VirtualClock.h
--- a/include/VirtualClock.h	Mon Sep 14 14:57:54 2015 +0100
+++ b/include/VirtualClock.h	Mon Sep 14 15:42:11 2015 +0100
@@ -1,14 +1,22 @@
 #ifndef VIRTUAL_CLOCK_H_INCLUDED
 #define VIRTUAL_CLOCK_H_INCLUDED
 
+#include "math.h"
 #include "stats.hpp"
 #include "Clock.h"
+#ifdef USE_JUCE
+#else
+#include <BeagleRT.h>
+#endif /* USE_JUCE */
 
 class VirtualClock{
 private:
 	myClock_t startTime;
+	myClock_t startTimeOffset;
 	myClock_t lastSync;
 	bool firstRun;
+	double elapsedPeriods;
+	double elapsedPeriodsOffset;
 	double period;
 	MovingAverage<double> movingAverage;
 public:
@@ -21,7 +29,7 @@
 /**
 	Call this method asynchronously, passing a number of equally spaced events that have elapsed since the last call.
 */
-	void sync(double count);
+	void sync(double numPeriods);
 /**
 	Get the current time according to the VirtualClock.
 	
@@ -34,6 +42,12 @@
 	Get the length of the period (difference between calls to sync() after various filtering operations)
 */
 	double getPeriod();
+/**
+ * Add an offset to the number of elapsed periods.
+ *
+ * Add an offset to the number of elapsed periods. It also compensates for the corresponding time offset.
+ */
+	void addOffset(double periodOffset);
 };
 
 #endif /* VIRTUAL_CLOCK_H_INCLUDED */
diff -r 5edc6d0713ef -r 44d07fa9bd03 projects/scope/render.cpp
--- a/projects/scope/render.cpp	Mon Sep 14 14:57:54 2015 +0100
+++ b/projects/scope/render.cpp	Mon Sep 14 15:42:11 2015 +0100
@@ -26,6 +26,19 @@
 extern I2c_Codec* gAudioCodec;
 VirtualClock virtualClock;
 ClockSyncThread clockSyncThread;
+AuxiliaryTask testTime;
+void testTimeFunction(){
+	rt_printf("time=[");
+	while(!gShouldStop){
+		rt_task_sleep(50000*1e3);
+		rt_printf("%f, ", virtualClock.getNow());
+		rt_printf("%f, ", virtualClock.getPeriod());
+		rt_task_sleep(20000);
+		rt_printf("%f,", virtualClock.getNow());
+		rt_printf("%f\n", virtualClock.getPeriod());
+	}
+	rt_printf("];");
+}
 bool setup(BeagleRTContext *context, void *userData)
 {
 //	receiveAudio0.init(10000, context->audioFrames, 0);
@@ -35,7 +48,7 @@
 //	scope.setPort(0, 9999);
 //	scope.setPort(1, 10000);
 //	networkSend.setup(context->audioSampleRate, context->audioFrames, 0, 9999, "192.168.7.1");
-	clockSynchronizer.setup();
+//	clockSynchronizer.setup();
 	virtualClock.init();
 	clockSyncThread.init(true, 5000, virtualClock); //start as slave
 	gInverseSampleRate = 1.0/context->audioSampleRate;
@@ -46,6 +59,7 @@
 	gFrequency1 = 200.0;
 	gFrequency2 = 201.0;
 
+//	testTime=BeagleRT_createAuxiliaryTask(testTimeFunction, 80, "testTimeTask");
 	return true; 
 }
 
@@ -58,8 +72,10 @@
 {
 	virtualClock.sync(context->audioFrames);
 	static int count=0;
-	if(count==0)
-		clockSyncThread.startThread();
+	if(count==0){
+//		BeagleRT_scheduleAuxiliaryTask(testTime);
+		clockSyncThread.startThread(); //make sure you uncomment .init in setup()
+	}
 	static float phase=0;
 	float phaseInc=200.0/44100.0*2*M_PI;
 //	rt_printf("phaseInc: %f, phase: %f\n",phaseInc,phase);