view core/VirtualClock.cpp @ 151:e9c9404e3d1f ClockSync

Pff partially working. No PID. When setting the audio clock on the bbb to 44098 the master and slave clock keep diverging instead of converging ...

#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
	};
	double states[4];
	for(int n = 0; n < 4; n++){
		states[n] = initialValueUs;
	}
	iir.setNumberOfStages(2);
	iir.setCoefficients(coefficients);
	iir.setStates(states);
//	printf("kalmanInit=%f\n",initialValueUs);
//	kalman.init(0.01, 158575.715009816 /*measured var() */,	initialValueUs);
	kalman.init(10, 1588.715009816 /*measured var() */,	initialValueUs);
}
	
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;
		lastSyncEstimatedTime = 0;
	} else {
		double newBlockPeriod = (currentTime - lastSyncTime);
#ifdef USE_JUCE
		blockPeriod = iir.process(kalman.process(newBlockPeriod));
#else
		// this leverages the fact that on the Beaglebone the audio clock
		// is synced to the xenomai clock.
		// CAVEAT: we are actually drifting by fractions of a sample everytime
		// there is a clock adjustment because we do not know exactly at what point
		// time the clock change actually takes place
		// Avoiding this would actually require a redesign and would anyhow produce overall
		// less accurate results.
		blockPeriod = numPeriods/gAudioCodec->getAudioSamplingRate()*1e6;
#endif /* USE_JUCE */
		//		printf("%f %f\n", blockPeriod, newBlockPeriod);
		period = blockPeriod / numPeriods;
		lastSyncEstimatedTime += (period * numPeriods);
	}
	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);
	}
	movingAverage.reset();
}
double VirtualClock::getPeriod(){
	return blockPeriod;
}