Mercurial > hg > beaglert

changeset 151:e9c9404e3d1f ClockSync

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
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 ...
author Giulio Moro <giuliomoro@yahoo.it>
date Tue, 22 Sep 2015 04:10:07 +0100
parents ebbfb154351a
children 8f98b32d0e23
files core/ClockSync.cpp core/I2c_Codec.cpp include/I2c_Codec.h projects/scope/render.cpp
diffstat 4 files changed, 93 insertions(+), 40 deletions(-) [+]
line wrap: on
line diff
--- 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;
--- a/core/I2c_Codec.cpp	Tue Sep 22 04:09:13 2015 +0100
+++ b/core/I2c_Codec.cpp	Tue Sep 22 04:10:07 2015 +0100
@@ -49,7 +49,7 @@
 	// using P=8 and R=1 gives a resolution of 0.0732421875Hz ( 0.000166% at 44.1kHz)
 	// to obtain Fs=44100 we need to have K=60.2112
 
-	if(setPllP(7))
+	if(setPllP(1))
 		return 1;
 	if(setPllR(1))
 		return 1;
@@ -218,9 +218,9 @@
 	//	f_{S(ref)} = (PLLCLK_IN × K × R)/(2048 × P)
 	float k = ((double)(newSamplingRate * pllP * 2048.0f/(float)pllR)) / PLLCLK_IN ;
 	int ret = setPllK(k);
-	float actualSamplingRate = PLLCLK_IN * (pllJ + pllD/10000.0) * pllR / (2048.0 * pllP);
-	printf("P: %d, R: %d, J: %d, D: %d, samplingRate= %f\n", pllP, pllR, pllJ, pllD, actualSamplingRate );
-//	printf("fs_ref=%f;\n",actualSamplingRate);
+//	printf("P: %d, R: %d, J: %d, D: %d, samplingRate= %f\n", pllP, pllR, pllJ, pllD, getAudioSamplingRate());
+//	printf("fs_ref=%f;\n",getAudioSamplingRate());
+//	printf("0 0 0 0 0 0\n");
 	return ret;
 }
 
@@ -243,7 +243,7 @@
 	return k;
 }
 
-float I2c_Codec::getAudioSamplingRate(){
+double I2c_Codec::getAudioSamplingRate(){
 	int pllP=1; //TODO: create get/set for pllP and pllR
 	int pllR=1;
 	long int PLLCLK_IN=12000000;
--- a/include/I2c_Codec.h	Tue Sep 22 04:09:13 2015 +0100
+++ b/include/I2c_Codec.h	Tue Sep 22 04:10:07 2015 +0100
@@ -38,12 +38,12 @@
 	int setPllR(unsigned int r);
 	int setPllK(float k);
 	int setAudioSamplingRate(float newSamplingRate);
+	double getAudioSamplingRate();
 	short unsigned int getPllJ();
 	unsigned int getPllD();
 	unsigned int getPllP();
 	unsigned int getPllR();
 	float getPllK();
-	float getAudioSamplingRate();
 	int setDACVolume(int halfDbSteps);
 	int writeDACVolumeRegisters(bool mute);
 	int setADCVolume(int halfDbSteps);
--- a/projects/scope/render.cpp	Tue Sep 22 04:09:13 2015 +0100
+++ b/projects/scope/render.cpp	Tue Sep 22 04:10:07 2015 +0100
@@ -76,6 +76,27 @@
 //		BeagleRT_scheduleAuxiliaryTask(testTime);
 		clockSyncThread.startThread(); //make sure you uncomment .init in setup()
 	}
+	/*
+	switch (count){
+	case 5000:
+			gAudioCodec->setAudioSamplingRate(44101);
+			printf("0 0 0\n");
+			break;
+	case 10000:
+			gAudioCodec->setAudioSamplingRate(44100);
+			printf("0 0 0\n");
+			break;
+	case 15000:
+			gAudioCodec->setAudioSamplingRate(44099);
+			printf("0 0 0\n");
+			break;
+	case 20000:
+			gAudioCodec->setAudioSamplingRate(44100);
+			printf("0 0 0\n");
+			count = 0;
+			break;
+	}
+	*/
 	static float phase=0;
 	float phaseInc=gFrequency1/44100.0*2*M_PI;
 //	rt_printf("phaseInc: %f, phase: %f\n",phaseInc,phase);