giuliomoro@135: #include "VirtualClock.h"
giuliomoro@149: void VirtualClock::init(float initialValueUs){
giuliomoro@135: 	firstRun=true;
giuliomoro@141: 	movingAverage.setLength(101); //TODO: a better filtering algorithm ( Did you say Kalman?)
giuliomoro@149: 	blockPeriod=-1;
giuliomoro@141: 	elapsedPeriods=0;
giuliomoro@141: 	startTime=0;
giuliomoro@149: 	elapsedPeriodsOffset = 0;
giuliomoro@149: 	lastSyncEstimatedTime = 0;
giuliomoro@149: 	double coefficients[IIR_FILTER_STAGE_COEFFICIENTS] = {
giuliomoro@149: 			0.000241359049041961, 0.000482718098083923, 0.000241359049041961, -1.95557824031504, 0.956543676511203
giuliomoro@149: 	};
giuliomoro@150: 	double states[4];
giuliomoro@150: 	for(int n = 0; n < 4; n++){
giuliomoro@150: 		states[n] = initialValueUs;
giuliomoro@150: 	}
giuliomoro@150: 	iir.setNumberOfStages(2);
giuliomoro@149: 	iir.setCoefficients(coefficients);
giuliomoro@150: 	iir.setStates(states);
giuliomoro@150: //	printf("kalmanInit=%f\n",initialValueUs);
giuliomoro@150: //	kalman.init(0.01, 158575.715009816 /*measured var() */,	initialValueUs);
giuliomoro@150: 	kalman.init(10, 1588.715009816 /*measured var() */,	initialValueUs);
giuliomoro@135: }
giuliomoro@135: 	
giuliomoro@149: VirtualClock::VirtualClock(){}
giuliomoro@135: void VirtualClock::sync(){
giuliomoro@135: 	sync(1);
giuliomoro@135: }
giuliomoro@150: 
giuliomoro@141: void VirtualClock::sync(double numPeriods){
giuliomoro@149: 	myClock_t currentTime = Clock::getTimeUs();
giuliomoro@149: 	elapsedPeriods += numPeriods;
giuliomoro@149: 	static int calls = 0;
giuliomoro@149: 	if(calls == 0){
giuliomoro@149: 		startTime = currentTime;
giuliomoro@149: 		lastSyncEstimatedTime = 0;
giuliomoro@149: 	} else {
giuliomoro@149: 		double newBlockPeriod = (currentTime - lastSyncTime);
giuliomoro@150: #ifdef USE_JUCE
giuliomoro@149: 		blockPeriod = iir.process(kalman.process(newBlockPeriod));
giuliomoro@150: #else
giuliomoro@150: 		// this leverages the fact that on the Beaglebone the audio clock
giuliomoro@150: 		// is synced to the xenomai clock.
giuliomoro@150: 		// CAVEAT: we are actually drifting by fractions of a sample everytime
giuliomoro@150: 		// there is a clock adjustment because we do not know exactly at what point
giuliomoro@150: 		// time the clock change actually takes place
giuliomoro@150: 		// Avoiding this would actually require a redesign and would anyhow produce overall
giuliomoro@150: 		// less accurate results.
giuliomoro@150: 		blockPeriod = numPeriods/gAudioCodec->getAudioSamplingRate()*1e6;
giuliomoro@150: #endif /* USE_JUCE */
giuliomoro@150: 		//		printf("%f %f\n", blockPeriod, newBlockPeriod);
giuliomoro@149: 		period = blockPeriod / numPeriods;
giuliomoro@149: 		lastSyncEstimatedTime += (period * numPeriods);
giuliomoro@141: 	}
giuliomoro@149: 	lastSyncTime = currentTime;
giuliomoro@149: //	printf("%lld\n", lastSyncTime);
giuliomoro@141: 	calls++;
giuliomoro@135: }
giuliomoro@135: 
giuliomoro@135: double VirtualClock::getNow(){
giuliomoro@139: 	myClock_t currentSystemTime=Clock::getTimeUs();
giuliomoro@149: 	if(blockPeriod<=0){
giuliomoro@141: 		return currentSystemTime; // TODO: this is not very meaningful.
giuliomoro@135: 	}
giuliomoro@149: 	double elapsed = (currentSystemTime - startTime) - lastSyncEstimatedTime;
giuliomoro@149: 	double now = elapsedPeriods + elapsed / (double)period;
giuliomoro@149: //	if(now>currentSystemTime+10*1e6)
giuliomoro@149: //		now=0;
giuliomoro@149: //	static long long int pastSy=0;
giuliomoro@149: //	printf("%lld\n", currentSystemTime-pastSy);
giuliomoro@149: //	pastSy=currentSystemTime;
giuliomoro@149: 	static int count=0;
giuliomoro@149: 	count++;
giuliomoro@149: //	if(count&1)
giuliomoro@149: #ifdef USE_JUCE
giuliomoro@149: #else
giuliomoro@149: //	printf("%f %f %f\n", (currentSystemTime - startTime)/1e6*44100.0, blockPeriod, now);
giuliomoro@149: #endif
giuliomoro@139: 	return now;
giuliomoro@135: }
giuliomoro@135: 
giuliomoro@141: void VirtualClock::addOffset(double periodOffset){
giuliomoro@141: 	elapsedPeriods-=periodOffset;
giuliomoro@141: 	if(elapsedPeriods<0){
giuliomoro@141: 		printf("ERROR: periodOffset adjustment of %f resulted in elapsedPeriods=%f\n", periodOffset, elapsedPeriods);
giuliomoro@141: 		exit(1);
giuliomoro@141: 	}
giuliomoro@141: 	movingAverage.reset();
giuliomoro@141: }
giuliomoro@135: double VirtualClock::getPeriod(){
giuliomoro@149: 	return blockPeriod;
giuliomoro@135: }