view core/VirtualClock.cpp @ 149:134bff10e561 ClockSync

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.

#include "VirtualClock.h"
void VirtualClock::init(float initialValueUs){
	firstRun=true;
	movingAverage.setLength(101); //TODO: a better filtering algorithm ( Did you say Kalman?)
	blockPeriod=-1;
	elapsedPeriods=0;
	startTime=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(){}
void VirtualClock::sync(){
	sync(1);
}
void VirtualClock::sync(double numPeriods){
	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++;
}

double VirtualClock::getNow(){
	myClock_t currentSystemTime=Clock::getTimeUs();
	if(blockPeriod<=0){
		return currentSystemTime; // TODO: this is not very meaningful.
	}
	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;
}

void VirtualClock::addOffset(double periodOffset){
	elapsedPeriods-=periodOffset;
	if(elapsedPeriods<0){
		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 blockPeriod;
}