diff core/ClockSync.cpp @ 151:e9c9404e3d1f ClockSync

Pff partially working. No PID. When setting the audio clock on the bbb to 44098 the master and slave clock keep diverging instead of converging ...

--- a/core/ClockSync.cpp	Tue Sep 22 04:09:13 2015 +0100
+++ b/core/ClockSync.cpp	Tue Sep 22 04:10:07 2015 +0100
@@ -265,48 +265,80 @@
 	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
+	if(calls<30){
+		 // wait for the VirtualClock to stabilize and produce meaningful outputs
+	} else if(calls<50) { //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);
+	} else if (calls==50){ //then compensate for initial offset
+//		printf("compensating for offset: %f\n", movingAverage.getAverage());
 		virtualClock->addOffset(movingAverage.getAverage());
 		movingAverage.reset();
-	} else if (calls>=10){ //use IIR filter from now on
+	} else if (calls>50){ //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 IirFilter iir(1);
+		static bool init = false;
+		if(init == false){
+//			double coeffs[5]={0.00554271721028068, 0.0110854344205614, 0.00554271721028068,
+//							     -1.77863177782459, 0.800802646665708};
+			double coeffs[5]={0.0200833655642113, 0.0401667311284225, 0.0200833655642113,
+						-1.56101807580072, 0.641351538057563};
+			iir.setCoefficients(coeffs);
+			init = true;
+		}
+		float expectedOffsetDifferenceMean = 0.2432;
+		float expectedOffsetDifferenceStd = 6.487;
+		float nlOffset;
+		static float pastOffset = 0;
+		float offsetIncrement = offset-pastOffset;
+		if ( fabsf(offsetIncrement) > 1*expectedOffsetDifferenceStd && calls>51){
+			// non-linear filtering: remove outliers
+			nlOffset = pastOffset;
+//			printf("%f %f                                             0,0,0,0,0,0\n", offset,pastOffset);
+		} else {
+			nlOffset=offset;
+		}
+		pastOffset=nlOffset;
+		float iirOffset = iir.process(nlOffset);
 		static float maxOffset=0;
-		if(calls > 0 ) {
-			maxOffset=fabsf(offset) > fabsf(maxOffset) ? offset : maxOffset;
-			pid.setError(offset);
-			float correction=pid.getOutput();
+		static float pastIirOffset=0;
+		if( calls > 0 ) {
+			maxOffset=fabsf(iirOffset) > fabsf(maxOffset) ? iirOffset : maxOffset;
+			pid.setError(iirOffset);
+			float correction=pid.getOutput()*0.0001;
 			static float oldSamplingRate=44100;
-			printf("%10.3f, %10.3f, %10.3f, %10.3f, %10.3f, %10.3f\n", in, offset, offset-pastOut[2], maxOffset, correction, oldSamplingRate); //unfiltered, filtered
-	//		if(fabsf(offset)>3 && calls>30){
-				//TODO: correct for offset
-	//			float targetSamplingRate=offset>0 ? 44095 : 44105;
-			float targetSamplingRate = oldSamplingRate - correction;
-	#ifndef USE_JUCE
-//			if(oldSamplingRate != targetSamplingRate)
-//				gAudioCodec->setAudioSamplingRate(targetSamplingRate);
-	#endif
+			printf("%10.3f, %10.3f, %10.3f, %10.3f, %10.3f, %10.3f\n",
+					offset, nlOffset, iirOffset, iirOffset - pastIirOffset, correction, oldSamplingRate); //unfiltered, filtered
+			pastIirOffset = iirOffset;
+			float targetSamplingRate;
+			static int direction=1;
+			float thresholdL = -1;
+//			targetSamplingRate = oldSamplingRate - correction;
+			float thresholdH = 3;
+
+			targetSamplingRate = 44100;
+			if(
+				(iirOffset > thresholdH && direction == 1) ||
+				(iirOffset > thresholdL && direction == -1)
+			){
+				targetSamplingRate = 44097;
+				direction = -1;
+			}
+			else if (
+				(iirOffset < -thresholdH && direction == -1) ||
+				(iirOffset < -thresholdL && direction == 1)
+			)
+			{
+				iirOffset = 44102;
+				direction = 1;
+			}
+#ifndef USE_JUCE
+			if(oldSamplingRate != targetSamplingRate){
+				gAudioCodec->setAudioSamplingRate(targetSamplingRate);
+			}
+#endif
 			oldSamplingRate=targetSamplingRate;
-	//			pastOut[1]=pastOut[2]=pastIn[1]=pastIn[2]=offset;
-	//			printf("------setAudioSmplingRate to %f\n", targetSamplingRate);
-	//		}
 		}
 	}
 	calls++;
@@ -330,7 +362,7 @@
 	if(isSlave()==true){
 		 //printf("Waiting for a sync clockSync\n");
 	} else { //if this is master
-		usleep(100000); //this times (roughly) how often sync clockSyncs are being sent.
+		usleep(30000); //this times (roughly) how often sync clockSyncs are being sent.
 		int ret=masterSendSync();
 		if(ret<=0)
 			return -1;