diff core/ClockSync.cpp @ 152:8f98b32d0e23 ClockSync

Last commit on this branch for a while. Overall not very succesful

--- a/core/ClockSync.cpp	Tue Sep 22 04:10:07 2015 +0100
+++ b/core/ClockSync.cpp	Mon Oct 05 13:06:14 2015 +0100
@@ -3,6 +3,7 @@
 void ClockSync::setVirtualClock(VirtualClock &aVirtualClock){
 	virtualClock=&aVirtualClock;
 }
+Pid* gClockSyncPid;
 void ClockSync::init(bool thisIsSlave, int aPort, VirtualClock &aVirtualClock){
 	setVirtualClock(aVirtualClock);
 	slave=thisIsSlave;
@@ -15,6 +16,7 @@
 	receiveLoopTimeout=1e5;
 	movingAverage.setLength(31);
 	expectedClockSyncType=isSlave() ? kSync : kNone;
+	gClockSyncPid = &pid;
 }
 void ClockSync::resetTs(){
 	T1=-1;
@@ -215,40 +217,9 @@
 		case kDelayResp: {//the clockSyncTimestamp is the instant when the master received the kDelayResp clockSync, the localTimestamp is meaningless
 			T2p=clockSyncTimestamp;
 			//TODO: evaluate things
-			double offset=(T1p-T1-T2p+T2)/2.0d;
 			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);
-//				}
+				offset = (T1p-T1-T2p+T2)/2.0d;
+				processOffset();
 			} else {
 				printf("InvalidTs:\n  %lld, %lld, %lld, %lld \n", T1, T1p, T2, T2p);
 			}
@@ -261,7 +232,7 @@
 	}
 	return 1;
 }
-void ClockSync::processOffset(double offset){
+void ClockSync::processOffset(){
 	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)
@@ -280,43 +251,63 @@
 		static IirFilter iir(1);
 		static bool init = false;
 		if(init == false){
+			//cutoff 0.05
 //			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};
+			//cutoff 0.015
+			double coeffs[5]={0.000537169774812052, 0.0010743395496241, 0.000537169774812052,
+					-1.93338022587993, 0.935528904979178};
+//			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){
+		float expectedOffsetDifference = 3.5;
+		static float pastNlOffset = 0;
+		static float pastIncrement = 0;
+		float offsetIncrement = offset-pastNlOffset;
+		static int filteredOut=0;
+		if (calls>51 && (fabsf(offsetIncrement) > expectedOffsetDifference && filteredOut < 8)){
 			// non-linear filtering: remove outliers
-			nlOffset = pastOffset;
+			nlOffset = pastNlOffset;
+//			printf("this actually happened\n");
+//nlOffset=offset; // BYPASS non linear filter
 //			printf("%f %f                                             0,0,0,0,0,0\n", offset,pastOffset);
+			filteredOut++;
 		} else {
 			nlOffset=offset;
+			filteredOut = 0;
 		}
-		pastOffset=nlOffset;
-		float iirOffset = iir.process(nlOffset);
+		pastIncrement = nlOffset - pastNlOffset;
+		pastNlOffset = nlOffset;
+		iirOffset = iir.process(nlOffset);
 		static float maxOffset=0;
 		static float pastIirOffset=0;
 		if( calls > 0 ) {
 			maxOffset=fabsf(iirOffset) > fabsf(maxOffset) ? iirOffset : maxOffset;
 			pid.setError(iirOffset);
-			float correction=pid.getOutput()*0.0001;
+			float correction=pid.getOutput();
 			static float oldSamplingRate=44100;
-			printf("%10.3f, %10.3f, %10.3f, %10.3f, %10.3f, %10.3f\n",
-					offset, nlOffset, iirOffset, iirOffset - pastIirOffset, correction, oldSamplingRate); //unfiltered, filtered
+			if( (calls&0) == 0){
+				printf("%10.3f, %10.3f, %10.3f, %10.3f, %10.3f, %10.3f, %10.3f, %10.3f, %10.3f, %10.3f, %10.3f\n",
+						offset, nlOffset, iirOffset, iirOffset - pastIirOffset,
+						pid.getProportionalGain(), pid.getIntegralGain(), pid.getDerivativeGain(), pid.getGlobalGain(),
+						correction, pid.getIntegralError(), oldSamplingRate); //unfiltered, filtered
+			}
 			pastIirOffset = iirOffset;
 			float targetSamplingRate;
+			// Applit PID
+			targetSamplingRate = oldSamplingRate - correction;
+			if(targetSamplingRate> 44110) //clip the Pid !
+				targetSamplingRate = 44110;
+			if (targetSamplingRate < 44090)
+				targetSamplingRate = 44090;
+
+			// or use a naive hysteresis comparator
 			static int direction=1;
 			float thresholdL = -1;
-//			targetSamplingRate = oldSamplingRate - correction;
 			float thresholdH = 3;
-
 			targetSamplingRate = 44100;
 			if(
 				(iirOffset > thresholdH && direction == 1) ||
@@ -330,7 +321,7 @@
 				(iirOffset < -thresholdL && direction == 1)
 			)
 			{
-				iirOffset = 44102;
+				iirOffset = 44103;
 				direction = 1;
 			}
 #ifndef USE_JUCE
@@ -362,7 +353,7 @@
 	if(isSlave()==true){
 		 //printf("Waiting for a sync clockSync\n");
 	} else { //if this is master
-		usleep(30000); //this times (roughly) how often sync clockSyncs are being sent.
+		usleep(10000); //this times (roughly) how often sync clockSyncs are being sent.
 		int ret=masterSendSync();
 		if(ret<=0)
 			return -1;
@@ -373,4 +364,9 @@
 	return 1;
 }
 
-
+float ClockSync::getOffset(){
+	return offset;
+}
+float ClockSync::getIirOffset(){
+	return iirOffset;
+}