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

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

 #include "ClockSync.h"
 
 void ClockSync::setVirtualClock(VirtualClock &aVirtualClock){
 	virtualClock=&aVirtualClock;
 }
+Pid* gClockSyncPid;
 void ClockSync::init(bool thisIsSlave, int aPort, VirtualClock &aVirtualClock){
 	setVirtualClock(aVirtualClock);
 	slave=thisIsSlave;
 	setPort(aPort);
 	//  isSlave() ? client.setServer("127.0.0.1") : client.setServer("127.0.0.1");

 	resetTs();
 	receiveLoopSleepUs=100;
 	receiveLoopTimeout=1e5;
 	movingAverage.setLength(31);
 	expectedClockSyncType=isSlave() ? kSync : kNone;
+	gClockSyncPid = &pid;
 }
 void ClockSync::resetTs(){
 	T1=-1;
 	T1p=-1;
 	T2=-1;

 			expectedClockSyncType=kDelayResp;
 			break;
 		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);
 			}
 			expectedClockSyncType=kSync; //end of the cycle, wait for next sync.
 			break;

 			//printf("Unexpected message type\n"); // we should never get here
 			return -1;
 	}
 	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)
 	if(calls<30){
 		 // wait for the VirtualClock to stabilize and produce meaningful outputs

 	} else if (calls>50){ //use IIR filter from now on
 		//filter coefficients obtained from Matlab : [B,A]=butter(2,0.005);
 		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;
-		}
-		pastOffset=nlOffset;
-		float iirOffset = iir.process(nlOffset);
+			filteredOut = 0;
+		}
+		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) ||
 				(iirOffset > thresholdL && direction == -1)
 			){

 			else if (
 				(iirOffset < -thresholdH && direction == -1) ||
 				(iirOffset < -thresholdL && direction == 1)
 			)
 			{
-				iirOffset = 44102;
+				iirOffset = 44103;
 				direction = 1;
 			}
 #ifndef USE_JUCE
 			if(oldSamplingRate != targetSamplingRate){
 				gAudioCodec->setAudioSamplingRate(targetSamplingRate);

 
 int ClockSync::sendReceiveLoop(){
 	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;
 	}
 	int ret=receiveLoop();
 	if(ret<=0)
 		return -2;
 	return 1;
 }
 
-
+float ClockSync::getOffset(){
+	return offset;
+}
+float ClockSync::getIirOffset(){
+	return iirOffset;
+}