Mercurial > hg > beaglert

changeset 149:134bff10e561 ClockSync

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Added simple one-variable one-measurement Kalman filter, Pid controller(which output is not used). Virtual clock is now much more precise and reactive for period. Still it is lagging behind a bit on the overall offset.
author Giulio Moro <giuliomoro@yahoo.it>
date Mon, 21 Sep 2015 03:12:21 +0100
parents 6cd38e261027
children ebbfb154351a
files .cproject core/ClockSync.cpp core/ClockSyncThread.cpp core/I2c_Codec.cpp core/Kalman.cpp core/Pid.cpp core/UdpServer.cpp core/VirtualClock.cpp include/ClockSync.h include/Kalman.h include/Pid.h include/VirtualClock.h projects/scope/render.cpp
diffstat 13 files changed, 244 insertions(+), 73 deletions(-) [+]
line wrap: on
line diff
--- a/.cproject	Mon Sep 21 03:11:32 2015 +0100
+++ b/.cproject	Mon Sep 21 03:12:21 2015 +0100
@@ -14,7 +14,7 @@
 				</extensions>
 			</storageModule>
 			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
-				<configuration artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.debug,org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe" cleanCommand="rm -rf" description="" id="cdt.managedbuild.config.gnu.exe.debug.528876549" name="Debug" parent="cdt.managedbuild.config.gnu.exe.debug" postannouncebuildStep="Kills the process running on the BeagleBone (if any) and copies the new binary to the BeagleBone in beaglert/" postbuildStep="ssh root@192.168.7.2 &quot;kill -s 2 \`pidof ${BuildArtifactFileName}\` 2&gt;/dev/null; sleep 0.5; scp host:${PWD}/${BuildArtifactFileName} ~/beaglert/ &amp;&amp; echo 'done copying\n' | wall || echo 'error'|wall&quot;">
+				<configuration artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.debug,org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe" cleanCommand="rm -rf" description="" id="cdt.managedbuild.config.gnu.exe.debug.528876549" name="Debug" parent="cdt.managedbuild.config.gnu.exe.debug" postannouncebuildStep="Kills the process running on the BeagleBone (if any) and copies the new binary to the BeagleBone in beaglert/" postbuildStep="ssh root@192.168.7.2 &quot;kill -s 2 \`pidof ${BuildArtifactFileName}\` 2&gt;/dev/null; sleep 0.5;&quot;; scp ${PWD}/${BuildArtifactFileName} bbb:beaglert/ &amp;&amp; echo 'done copying\n' | wall || echo 'error'|wall">
 					<folderInfo id="cdt.managedbuild.config.gnu.exe.debug.528876549." name="/" resourcePath="">
 						<toolChain id="cdt.managedbuild.toolchain.gnu.exe.debug.681872250" name="Linux GCC" superClass="cdt.managedbuild.toolchain.gnu.exe.debug">
 							<targetPlatform id="cdt.managedbuild.target.gnu.platform.exe.debug.295375065" name="Debug Platform" superClass="cdt.managedbuild.target.gnu.platform.exe.debug"/>
@@ -25,7 +25,7 @@
 								</outputEntries>
 							</builder>
 							<tool command="arm-linux-gnueabihf-g++&#10;" id="cdt.managedbuild.tool.gnu.archiver.base.1542380883" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.base"/>
-							<tool command="arm-linux-gnueabihf-g++&#10;" id="cdt.managedbuild.tool.gnu.cpp.compiler.exe.debug.2030825480" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.exe.debug">
+							<tool command="arm-linux-gnueabihf-g++" id="cdt.managedbuild.tool.gnu.cpp.compiler.exe.debug.2030825480" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.exe.debug">
 								<option id="gnu.cpp.compiler.exe.debug.option.optimization.level.2006448218" name="Optimization Level" superClass="gnu.cpp.compiler.exe.debug.option.optimization.level" value="gnu.cpp.compiler.optimization.level.none" valueType="enumerated"/>
 								<option id="gnu.cpp.compiler.exe.debug.option.debugging.level.1267706246" name="Debug Level" superClass="gnu.cpp.compiler.exe.debug.option.debugging.level" value="gnu.cpp.compiler.debugging.level.max" valueType="enumerated"/>
 								<option id="gnu.cpp.compiler.option.include.paths.2031219124" name="Include paths (-I)" superClass="gnu.cpp.compiler.option.include.paths" valueType="includePath">
@@ -59,11 +59,9 @@
 							<tool command="arm-linux-gnueabihf-g++&#10;" id="cdt.managedbuild.tool.gnu.c.linker.exe.debug.214461086" name="GCC C Linker" superClass="cdt.managedbuild.tool.gnu.c.linker.exe.debug"/>
 							<tool command="arm-linux-gnueabihf-g++&#10;" commandLinePattern="${COMMAND} ${FLAGS} ${OUTPUT_FLAG} ${OUTPUT_PREFIX}${OUTPUT} ${INPUTS}" id="cdt.managedbuild.tool.gnu.cpp.linker.exe.debug.1669966018" name="GCC C++ Linker" superClass="cdt.managedbuild.tool.gnu.cpp.linker.exe.debug">
 								<option id="gnu.cpp.link.option.paths.462980690" name="Library search path (-L)" superClass="gnu.cpp.link.option.paths" valueType="libPaths">
-									<listOptionValue builtIn="false" value="/usr/xenomai/lib"/>
 									<listOptionValue builtIn="false" value="/usr/local/linaro/arm-linux-gnueabihf/include/xenomai/lib"/>
-									<listOptionValue builtIn="false" value="/import/teaching/ECS732/arm-gcc/arm-linux-gnueabihf/lib"/>
 									<listOptionValue builtIn="false" value="/usr/lib/arm-linux-gnueabihf"/>
-									<listOptionValue builtIn="false" value="/import/teaching/ECS732/arm-gcc/arm-linux-gnueabihf/lib/xenomai"/>
+									<listOptionValue builtIn="false" value="/usr/arm-linux-gnueabihf/include/xenomai/lib"/>
 								</option>
 								<option id="gnu.cpp.link.option.libs.139390951" name="Libraries (-l)" superClass="gnu.cpp.link.option.libs" valueType="libs">
 									<listOptionValue builtIn="false" value="rt"/>
@@ -95,6 +93,7 @@
 					<sourceEntries>
 						<entry excluding="default_main.cpp|audio_routines_old.S" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="core"/>
 						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="include"/>
+						<entry flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="projects/scope"/>
 						<entry flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="source"/>
 					</sourceEntries>
 				</configuration>
@@ -183,7 +182,7 @@
 					<sourceEntries>
 						<entry excluding="default_main.cpp|audio_routines_old.S" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="core"/>
 						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="include"/>
-						<entry flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="projects/basic"/>
+						<entry flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="projects/scope"/>
 						<entry flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="source"/>
 					</sourceEntries>
 				</configuration>
--- a/core/ClockSync.cpp	Mon Sep 21 03:11:32 2015 +0100
+++ b/core/ClockSync.cpp	Mon Sep 21 03:12:21 2015 +0100
@@ -70,7 +70,7 @@
 myClock_t ClockSync::send(){
 	//  print();
 	int ret;
-	ret=client.waitUntilReady(false, 0);
+	ret=client.waitUntilReady(false, isSlave() ? 110 : 5);
 	if(ret<=0){ //don't retry
 		return -1;
 	}
@@ -270,7 +270,7 @@
 //		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);
+		printf("compensating for offset: %f\n", offset);
 		virtualClock->addOffset(movingAverage.getAverage());
 		movingAverage.reset();
 	} else if (calls>=10){ //use IIR filter from now on
@@ -289,17 +289,24 @@
 		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;
-#ifndef USE_JUCE
-			gAudioCodec->setAudioSamplingRate(targetSamplingRate);
-#endif
-//			pastOut[1]=pastOut[2]=pastIn[1]=pastIn[2]=offset;
-			printf("------setAudioSmplingRate to %f\n", targetSamplingRate);
+		if(calls > 0 ) {
+			maxOffset=fabsf(offset) > fabsf(maxOffset) ? offset : maxOffset;
+			pid.setError(offset);
+			float correction=pid.getOutput();
+			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
+			oldSamplingRate=targetSamplingRate;
+	//			pastOut[1]=pastOut[2]=pastIn[1]=pastIn[2]=offset;
+	//			printf("------setAudioSmplingRate to %f\n", targetSamplingRate);
+	//		}
 		}
 	}
 	calls++;
--- a/core/ClockSyncThread.cpp	Mon Sep 21 03:11:32 2015 +0100
+++ b/core/ClockSyncThread.cpp	Mon Sep 21 03:12:21 2015 +0100
@@ -52,11 +52,21 @@
 void ClockSyncThread::setVirtualClock(VirtualClock &aVirtualClock){
 	virtualClock=&aVirtualClock;
 };
-
+#ifndef USE_JUCE
+extern I2c_Codec* gAudioCodec;
+#endif
 void ClockSyncThread::run(){
-	printf("var=[");
+	printf("variable=[");
 	while(!threadShouldExit()){
+		static int count = 0;
 		clockSync.sendReceiveLoop();
+#ifndef USE_JUCE
+//		if (count == 300){
+//			printf("0 0 0\n");
+//			gAudioCodec->setAudioSamplingRate(44101);
+//		}
+#endif
+		count++;
 //		double now=virtualClock->getNow();
 //		printf("th(end+1)=%f;\n", now);
 //		printf("act(end+1)=%lld;\n", Clock::getTimeUs());
--- a/core/I2c_Codec.cpp	Mon Sep 21 03:11:32 2015 +0100
+++ b/core/I2c_Codec.cpp	Mon Sep 21 03:12:21 2015 +0100
@@ -47,8 +47,6 @@
 	// The master clock PLLCLK_IN is 12MHz
 	// K can be varied in intervals of resolution of 0.0001 up to 63.9999
 	// using P=8 and R=1 gives a resolution of 0.0732421875Hz ( 0.000166% at 44.1kHz)
-	// for whatever reason, P=8 does not work (i.e.: setting the sampling rate works fine,
-	// but when changing K by a small-ish value (0.0040), the clock does not seem to change).
 	// to obtain Fs=44100 we need to have K=60.2112
 
 	if(setPllP(7))
@@ -220,7 +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);
-//	printf("P: %d, R: %d, J: %d, D: %d\n", pllP, pllR, pllJ, pllD);
+	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);
 	return ret;
 }
 
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/core/Kalman.cpp	Mon Sep 21 03:12:21 2015 +0100
@@ -0,0 +1,24 @@
+/*
+ * Kalman.cpp
+ *
+ *  Created on: 20 Sep 2015
+ *      Author: giulio
+ */
+#include "Kalman.h"
+void KalmanOne::init(double newQ, double newR, double newX){
+	A = 1;
+	H = 1;
+	Q = newQ;  // covariance of the error on the prediction
+	R = newR; // covariance of the measurement error
+	x = newX; // 5805.09230769231; % predicted x
+	P = 6; // predicted covariance
+	return;
+}
+double KalmanOne:: process(double z){
+	double xp = A*x; // I. Prediction of the estimate
+	double Pp = A*P*A + Q; // Prediction of the error covariance
+	double K = Pp*H/(H*Pp*H + R); // II. Computation of Kalman gain
+	x = xp + K*(z - H*xp); // III. Computation of the estimate
+	P = Pp - K*H*Pp; // IV. Computation of the error covariance
+	return x;
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/core/Pid.cpp	Mon Sep 21 03:12:21 2015 +0100
@@ -0,0 +1,43 @@
+/*
+ * Pid.cpp
+ *
+ *  Created on: 14 Sep 2015
+ *      Author: giulio
+ */
+
+#include "Pid.h"
+
+Pid::Pid(){
+	integralError = 0;
+	pastError = 0;
+	error = 0;
+	differentialError = 0;
+	kp = 5;
+	ki = 2;
+	kd = 0.8;
+	idleTimeout = 0;
+	timeoutCount = 0;
+	output = 0;
+//	printf("----------%f %f %f %f %f %f\n", kp, error, ki, integralError, kd, differentialError);
+}
+void Pid::updateIntegralError(){
+	integralError += (error + pastError) * 0.5 * ts;
+}
+void Pid::updateDifferentialError(){
+	differentialError = (error - pastError) / ts;
+}
+float Pid::setError(float anError){
+	pastError = error;
+	error = anError;
+	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;
+//	printf("%f %f %f %f %f %f %f\n", output, kp, error, ki, integralError, kd, differentialError);
+	return output;
+}
+float Pid::getOutput(){
+	return output;
+}
+
+
--- a/core/UdpServer.cpp	Mon Sep 21 03:11:32 2015 +0100
+++ b/core/UdpServer.cpp	Mon Sep 21 03:12:21 2015 +0100
@@ -63,15 +63,15 @@
 		return select(inSocket+1, &stReadFDS, NULL, NULL, NULL); //calling this with a NULL timeout will block indefinitely
 	FD_ZERO(&stReadFDS);
 	FD_SET(inSocket, &stReadFDS);
-	if(timeoutMsecs>=1000){
-		float timeOutSecs=timeoutMsecs*0.001;
-		stTimeOut.tv_sec=(long int)timeOutSecs;
-		timeOutSecs-=(int)timeOutSecs;
-		stTimeOut.tv_usec=(long int)(timeOutSecs*1000000);
-	} else //faster!
-		stTimeOut.tv_usec=(long int)timeoutMsecs*1000;
+	float timeOutSecs=timeoutMsecs*0.001;
+	stTimeOut.tv_sec=(long int)timeOutSecs;
+	timeOutSecs-=(int)timeOutSecs;
+	long int timeOutUsecs=timeOutSecs*1000000;
+	stTimeOut.tv_usec=timeOutUsecs;
 	int descriptorReady= select(inSocket+1, &stReadFDS, NULL, NULL, &stTimeOut);
-	return descriptorReady>0? 1 : descriptorReady;
+//	printf("stTimeOut.tv_sec=%ld, stTimeOut.tv_usec=%ld, descriptorReady: \n",stTimeOut.tv_sec,stTimeOut.tv_usec, descriptorReady);
+//	return descriptorReady>0 ? (timeOutUsecs-stTimeOut.tv_usec) : descriptorReady;
+	return descriptorReady>0 ? 1 : descriptorReady;
 }
 
 int UdpServer::read(//Returns the number of bytes read, or -1 if there was an error.
--- a/core/VirtualClock.cpp	Mon Sep 21 03:11:32 2015 +0100
+++ b/core/VirtualClock.cpp	Mon Sep 21 03:12:21 2015 +0100
@@ -1,58 +1,75 @@
 #include "VirtualClock.h"
-void VirtualClock::init(){
+void VirtualClock::init(float initialValueUs){
 	firstRun=true;
 	movingAverage.setLength(101); //TODO: a better filtering algorithm ( Did you say Kalman?)
-	period=-1;
+	blockPeriod=-1;
 	elapsedPeriods=0;
 	startTime=0;
-	startTimeOffset=0;
-	elapsedPeriodsOffset=0;
+	elapsedPeriodsOffset = 0;
+	lastSyncEstimatedTime = 0;
+	double coefficients[IIR_FILTER_STAGE_COEFFICIENTS] = {
+			0.000241359049041961, 0.000482718098083923, 0.000241359049041961, -1.95557824031504, 0.956543676511203
+	};
+	iir.setNumberOfStages(4);
+	iir.setCoefficients(coefficients);
+	printf("kalmanInit=%f\n",initialValueUs);
+	kalman.init(0.01, 158575.715009816 /*measured var() */,
+			initialValueUs
+//#ifdef USE_JUCE
+//			5804.98866213152 /*blockSize/Fs*1e6*/
+//#else
+//			5804.98866213152 /*blockSize/Fs*1e6*/
+//#endif
+			);
 }
 	
-VirtualClock::VirtualClock(){
-	init();
-}
+VirtualClock::VirtualClock(){}
 void VirtualClock::sync(){
 	sync(1);
 }
 void VirtualClock::sync(double numPeriods){
-	myClock_t currentTime=Clock::getTimeUs();
-	static int calls=0;
-	elapsedPeriods+=numPeriods;
-	if(calls==50){ //TODO: this is dangerous as the clock might jump suddenly if currentTime is not precise
-		calls=0;
-		startTimeOffset=startTime;
-		startTime=currentTime;
-		elapsedPeriodsOffset=elapsedPeriods;
+	myClock_t currentTime = Clock::getTimeUs();
+	elapsedPeriods += numPeriods;
+	static int calls = 0;
+	if(calls == 0){
+		startTime = currentTime;
+//AA		lastSyncEstimatedTime = startTime;
+		lastSyncEstimatedTime = 0;
+	} else {
+		double newBlockPeriod = (currentTime - lastSyncTime);
+//		period = iir.process(movingAverage.add(newPeriod)); //TODO: replace with Kalman filter
+		blockPeriod = iir.process(kalman.process(newBlockPeriod));
+//		period = 22.6760958210091;
+		period = blockPeriod / numPeriods;
+		lastSyncEstimatedTime += (period * numPeriods);
+//		if(calls == 800)
+//			lastSyncEstimatedTime = lastSyncTime;
+//		printf("%lld, %lld\n", lastSyncTime, lastSyncEstimatedTime);
 	}
+	lastSyncTime = currentTime;
+//	printf("%lld\n", lastSyncTime);
 	calls++;
-	if(firstRun==true){
-		firstRun=false;
-		startTime=currentTime;
-	} else {
-		double newPeriod=(currentTime-lastSync)/numPeriods;
-		double expectedPeriod=22.67;
-		double maxPeriodDeviation=10;
-		if(fabs(newPeriod-expectedPeriod)<maxPeriodDeviation){ // filtering outliers
-			period=movingAverage.add(newPeriod); //TODO: replace with Kalman filter
-		} else {
-			printf("period out of range: %f\n", newPeriod);
-		}
-	}
-	lastSync=currentTime;
-//	printf("lastSync: %lld\n",lastSync-startTime);
 }
 
 double VirtualClock::getNow(){
 	myClock_t currentSystemTime=Clock::getTimeUs();
-	if(period<=0){
+	if(blockPeriod<=0){
 		return currentSystemTime; // TODO: this is not very meaningful.
 	}
-	//  double beginningOfPeriod=lastSync; // TODO: if sync() does not get called every time (but e.g. only every so often),
-													 // then this line (and the class) needs editing
-	myClock_t elapsed=(currentSystemTime-startTime);
-	double now=elapsedPeriodsOffset + elapsed/(double)period;
-//	printf("elapsed=%lld;  sincelastSync=%lld; period=%f; now=%f\n", elapsed, currentSystemTime-lastSync, period, now);
+	double elapsed = (currentSystemTime - startTime) - lastSyncEstimatedTime;
+	double now = elapsedPeriods + elapsed / (double)period;
+//	if(now>currentSystemTime+10*1e6)
+//		now=0;
+//	static long long int pastSy=0;
+//	printf("%lld\n", currentSystemTime-pastSy);
+//	pastSy=currentSystemTime;
+	static int count=0;
+	count++;
+//	if(count&1)
+#ifdef USE_JUCE
+#else
+//	printf("%f %f %f\n", (currentSystemTime - startTime)/1e6*44100.0, blockPeriod, now);
+#endif
 	return now;
 }
 
@@ -62,8 +79,9 @@
 		printf("ERROR: periodOffset adjustment of %f resulted in elapsedPeriods=%f\n", periodOffset, elapsedPeriods);
 		exit(1);
 	}
+	printf("this is not called\n");
 	movingAverage.reset();
 }
 double VirtualClock::getPeriod(){
-	return period;
+	return blockPeriod;
 }
--- a/include/ClockSync.h	Mon Sep 21 03:11:32 2015 +0100
+++ b/include/ClockSync.h	Mon Sep 21 03:12:21 2015 +0100
@@ -5,6 +5,7 @@
 #include "UdpClient.h"
 #include "Clock.h"
 #include "VirtualClock.h"
+#include "Pid.h"
 #ifdef USE_JUCE
 #else
 #include <I2c_Codec.h>
@@ -51,6 +52,7 @@
 	void resetTs();
 	bool areTsValid();
 	void processOffset(double offset);
+	Pid pid;
 public:
 	ClockSync(){};
 	ClockSync(bool thisIsSlave, int aPort, VirtualClock &aVirtualClock);
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Kalman.h	Mon Sep 21 03:12:21 2015 +0100
@@ -0,0 +1,25 @@
+/*
+ * Kalman.h
+ *
+ *  Created on: 20 Sep 2015
+ *      Author: giulio
+ */
+
+#ifndef KALMAN_H_
+#define KALMAN_H_
+
+class KalmanOne{
+public:
+	double A;
+	double H;
+	double Q;
+	double R;
+	double x;
+	double P;
+	void init(double newQ, double newR, double newX);
+	double process(double z);
+};
+
+
+
+#endif /* KALMAN_H_ */
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Pid.h	Mon Sep 21 03:12:21 2015 +0100
@@ -0,0 +1,36 @@
+/*
+ * Pid.h
+ *
+ *  Created on: 14 Sep 2015
+ *      Author: giulio
+ */
+
+#ifndef PID_H_
+#define PID_H_
+#include <stdio.h>
+
+class Pid{
+private:
+	float integralError;
+	float pastError;
+	float error;
+	float differentialError;
+	float output;
+	float kp;
+	float ki;
+	float kd;
+	float ts;
+	int idleTimeout;
+	int timeoutCount;
+	void updateIntegralError();
+	void updateDifferentialError();
+public:
+	Pid();
+	float setError(float anError);
+	void setIdleTimeout(int aIdleTimeout);
+	float getOutput();
+};
+
+
+
+#endif /* PID_H_ */
--- a/include/VirtualClock.h	Mon Sep 21 03:11:32 2015 +0100
+++ b/include/VirtualClock.h	Mon Sep 21 03:12:21 2015 +0100
@@ -4,6 +4,8 @@
 #include "math.h"
 #include "stats.hpp"
 #include "Clock.h"
+#include "IirFilter.h"
+#include "Kalman.h"
 #ifdef USE_JUCE
 #else
 #include <BeagleRT.h>
@@ -13,14 +15,19 @@
 private:
 	myClock_t startTime;
 	myClock_t startTimeOffset;
-	myClock_t lastSync;
+	myClock_t lastSyncTime;
+//	myClock_t lastSyncEstimatedTime;
+	double lastSyncEstimatedTime;
 	bool firstRun;
 	double elapsedPeriods;
 	double elapsedPeriodsOffset;
+	double blockPeriod;
 	double period;
 	MovingAverage<double> movingAverage;
+	IirFilter iir;
+	KalmanOne kalman;
 public:
-	void init();
+	void init(float initialValueUs);
 	VirtualClock();
 /**
 	Call this method at regular intervals to sync che virtual clock
--- a/projects/scope/render.cpp	Mon Sep 21 03:11:32 2015 +0100
+++ b/projects/scope/render.cpp	Mon Sep 21 03:12:21 2015 +0100
@@ -49,7 +49,7 @@
 //	scope.setPort(1, 10000);
 //	networkSend.setup(context->audioSampleRate, context->audioFrames, 0, 9999, "192.168.7.1");
 //	clockSynchronizer.setup();
-	virtualClock.init();
+	virtualClock.init(context->audioFrames / context->audioSampleRate * 1e6);
 	clockSyncThread.init(true, 5000, virtualClock); //start as slave
 	gInverseSampleRate = 1.0/context->audioSampleRate;
 	
@@ -84,7 +84,7 @@
 		phase+=200.0/44100.0*2*M_PI;
 		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];
 	}
 	count++;
 	/*