Mercurial > hg > beaglert

changeset 152:8f98b32d0e23 ClockSync

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Last commit on this branch for a while. Overall not very succesful
author Giulio Moro <giuliomoro@yahoo.it>
date Mon, 05 Oct 2015 13:06:14 +0100
parents e9c9404e3d1f
children
files core/ClockSync.cpp core/ClockSyncThread.cpp core/Pid.cpp core/VirtualClock.cpp include/ClockSync.h include/Pid.h projects/scope/render.cpp
diffstat 7 files changed, 128 insertions(+), 63 deletions(-) [+]
line wrap: on
line diff
--- 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;
+}
--- a/core/ClockSyncThread.cpp	Tue Sep 22 04:10:07 2015 +0100
+++ b/core/ClockSyncThread.cpp	Mon Oct 05 13:06:14 2015 +0100
@@ -23,7 +23,10 @@
 	stopThread();
 #endif /* USE_JUCE */
 }
+
+ClockSync* gClockSync;
 void ClockSyncThread::init(bool isSlave, int aPort, VirtualClock &aVirtualClock){
+	gClockSync = &clockSync;
 	setVirtualClock(aVirtualClock);
 	listening=false;
 	clockSync.init(isSlave, aPort, *virtualClock);
--- a/core/Pid.cpp	Tue Sep 22 04:10:07 2015 +0100
+++ b/core/Pid.cpp	Mon Oct 05 13:06:14 2015 +0100
@@ -13,8 +13,9 @@
 	error = 0;
 	differentialError = 0;
 	kp = 5;
-	ki = 2;
-	kd = 0.8;
+	ki = 2;// 2;
+	kd = 0.8; //0.8;
+	gain = 1;
 	idleTimeout = 0;
 	timeoutCount = 0;
 	output = 0;
@@ -32,12 +33,37 @@
 	ts = 1; //TODO: this should be passed as a parameter, unless setError() gets always called at constant intervals
 	updateIntegralError();
 	updateDifferentialError();
-	output= kp * error + ki * integralError + kd * differentialError;
+	output = gain * (kp * error + ki * integralError + kd * differentialError);
 //	printf("%f %f %f %f %f %f %f\n", output, kp, error, ki, integralError, kd, differentialError);
 	return output;
 }
 float Pid::getOutput(){
 	return output;
 }
-
-
+void Pid::setProportionalGain(float proportionalGain){
+	kp = proportionalGain;
+}
+void Pid::setDerivativeGain(float derivativeGain){
+	kd = derivativeGain;
+}
+void Pid::setIntegralGain(float integralGain){
+	ki = integralGain;
+}
+void Pid::setGlobalGain(float globalGain){
+	gain = globalGain;
+}
+float Pid::getProportionalGain(){
+	return kp;
+}
+float Pid::getDerivativeGain(){
+	return kd;
+}
+float Pid::getIntegralGain(){
+	return ki;
+}
+float Pid::getGlobalGain(){
+	return gain;
+}
+float Pid::getIntegralError(){
+	return integralError;
+}
--- a/core/VirtualClock.cpp	Tue Sep 22 04:10:07 2015 +0100
+++ b/core/VirtualClock.cpp	Mon Oct 05 13:06:14 2015 +0100
@@ -80,11 +80,13 @@
 }
 
 void VirtualClock::addOffset(double periodOffset){
+//	printf("Clock was : %f ", elapsedPeriods);
 	elapsedPeriods-=periodOffset;
 	if(elapsedPeriods<0){
 		printf("ERROR: periodOffset adjustment of %f resulted in elapsedPeriods=%f\n", periodOffset, elapsedPeriods);
 		exit(1);
 	}
+//	printf("and now is: %f after a correction of %f", elapsedPeriods, periodOffset);
 	movingAverage.reset();
 }
 double VirtualClock::getPeriod(){
--- a/include/ClockSync.h	Tue Sep 22 04:10:07 2015 +0100
+++ b/include/ClockSync.h	Mon Oct 05 13:06:14 2015 +0100
@@ -51,7 +51,10 @@
 	VirtualClock *virtualClock;
 	void resetTs();
 	bool areTsValid();
-	void processOffset(double offset);
+	void processOffset();
+	float offset;
+	float nlOffset;
+	float iirOffset;
 	Pid pid;
 public:
 	ClockSync(){};
@@ -67,6 +70,9 @@
 	void setType(int clockSyncType);
 	void setTimestamp(myClock_t timestamp);
 	void print();
+	float getOffset();
+	float getIirOffset();
+
 	/** 
 	 * sends a clockSync without blocking, checks results and returns the timestamp
 	 * immediately after the clockSync has been sent or -1 if there was an error or timeout expired.
--- a/include/Pid.h	Tue Sep 22 04:10:07 2015 +0100
+++ b/include/Pid.h	Mon Oct 05 13:06:14 2015 +0100
@@ -19,6 +19,7 @@
 	float kp;
 	float ki;
 	float kd;
+	float gain;
 	float ts;
 	int idleTimeout;
 	int timeoutCount;
@@ -29,6 +30,15 @@
 	float setError(float anError);
 	void setIdleTimeout(int aIdleTimeout);
 	float getOutput();
+	void setProportionalGain(float proportionalGain);
+	void setDerivativeGain(float derivativeGain);
+	void setIntegralGain(float integralGain);
+	void setGlobalGain(float globalGain);
+	float getProportionalGain();
+	float getDerivativeGain();
+	float getIntegralGain();
+	float getGlobalGain();
+	float getIntegralError();
 };
 
 
--- a/projects/scope/render.cpp	Tue Sep 22 04:10:07 2015 +0100
+++ b/projects/scope/render.cpp	Mon Oct 05 13:06:14 2015 +0100
@@ -4,6 +4,7 @@
 #include <ClockSynchronizer.h>
 #include <cmath>
 #include <ClockSyncThread.h>
+#include <Pid.h>
 
 float gPhase1, gPhase2;
 float gFrequency1, gFrequency2;
@@ -56,7 +57,7 @@
 	gPhase1 = 0.0;
 	gPhase2 = 0.0;
 	
-	gFrequency1 = 200.0;
+	gFrequency1 = 441.0;
 	gFrequency2 = 201.0;
 
 //	testTime=BeagleRT_createAuxiliaryTask(testTimeFunction, 80, "testTimeTask");
@@ -67,7 +68,8 @@
 // Input and output are given from the audio hardware and the other
 // ADCs and DACs (if available). If only audio is available, numMatrixFrames
 // will be 0.
-
+extern Pid* gClockSyncPid;
+extern ClockSync* gClockSync;
 void render(BeagleRTContext *context, void *userData)
 {
 	virtualClock.sync(context->audioFrames);
@@ -101,12 +103,32 @@
 	float phaseInc=gFrequency1/44100.0*2*M_PI;
 //	rt_printf("phaseInc: %f, phase: %f\n",phaseInc,phase);
 	for(unsigned int n=0; n<context->audioFrames; n++){
-		context->audioOut[n*2]=sinf(phase);//context->audioIn[n*2];
-		phase+=200.0/44100.0*2*M_PI;
+		context->audioOut[n*2]=sinf(phase) > 0 ? 1 : -1;
+		context->audioOut[n*2+1]=sinf(phase) > 0 ? 1 : -1;
+//		context->audioOut[n*2]= context->audioIn[n*2];
+//		context->audioOut[n*2+1]= context->audioIn[n*2+1];
+		phase+=phaseInc;
 		if(phase>=2*M_PI)
 			phase-=2*M_PI;
-		context->audioOut[n*2+1]=rand()/(float)RAND_MAX;//context->audioIn[n*2];
+//		context->audioOut[n*2+1]=rand()/(float)RAND_MAX;//context->audioIn[n*2];
 	}
+	gClockSyncPid->setProportionalGain(context->analogIn[0]*10);
+//	gClockSyncPid->setIntegralGain(context->analogIn[1]);
+	gClockSyncPid->setIntegralGain(0);
+//	gClockSyncPid->setDerivativeGain(context->analogIn[2]);
+	gClockSyncPid->setDerivativeGain(0);
+	gClockSyncPid->setGlobalGain(context->analogIn[3]*1/0.828);
+	for (int n = 0; n < context->analogFrames; n++){
+		context->analogOut[n*context->analogChannels + 0] = (gClockSync->getOffset() )/ 100 + 0.5;
+		context->analogOut[n*context->analogChannels + 1] = (gAudioCodec->getAudioSamplingRate()-44100)/20 + 0.5;
+	}
+	static bool pastSwitch = false;
+	bool thisSwitch = context->analogIn[4]>0.5;
+	if(thisSwitch && pastSwitch == false){
+		rt_printf("--------------reset offset\n");
+		virtualClock.addOffset(gClockSync->getIirOffset());
+	}
+	pastSwitch = thisSwitch;
 	count++;
 	/*
 //	if((count&262143)==0){