comparison core/ClockSync.cpp @ 141:44d07fa9bd03 ClockSync

Ultra-basic feedback for clock sync works^CIssues: response time of the IIR filter is too slow, requires PID and better filtering algorithm.

 	slave=thisIsSlave;
 	setPort(aPort);
 	//  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);
 }
 void* ClockSync::getBuffer(){

 }
 
 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;
 		case kFollowUp: //the clockSyncTimestamp is the time when kSync was sent, the localTimestamp is meaningless
 			T1=clockSyncTimestamp;

 			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;
-			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;
 		}
 		default:
 			//printf("Unexpected message type\n"); // we should never get here
 			return -1;
 	}
 	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;
 	}
 }
 
 int ClockSync::sendReceiveLoop(){
 	if(isSlave()==true){