giuliomoro@132: /*
giuliomoro@132:  * ClockSynchronizer.cpp
giuliomoro@132:  *
giuliomoro@132:  *  Created on: 26 Aug 2015
giuliomoro@132:  *      Author: giulio
giuliomoro@132:  */
giuliomoro@132: 
giuliomoro@132: #include "ClockSynchronizer.h"
giuliomoro@132: 
giuliomoro@132: // declare static members
giuliomoro@132: float ClockSynchronizer::targetSamplingRate;
giuliomoro@132: float ClockSynchronizer::currentSamplingRate;
giuliomoro@132: bool ClockSynchronizer::threadRunning;
giuliomoro@132: int ClockSynchronizer::threadWasRunning;
giuliomoro@132: bool ClockSynchronizer::staticConstructed=false;
giuliomoro@132: AuxiliaryTask ClockSynchronizer::setClockTask;
giuliomoro@132: 
giuliomoro@132: void ClockSynchronizer::staticConstructor(){
giuliomoro@132: 	int priority=90;
giuliomoro@132: 	setClockTask=BeagleRT_createAuxiliaryTask(&ClockSynchronizer::setClock, priority, "setClockTask");
giuliomoro@132: 	threadRunning=false;
giuliomoro@132: }
giuliomoro@132: 
giuliomoro@132: void ClockSynchronizer::setClock(){
giuliomoro@133: //	rt_printf("Setting clock to %f\n",targetSamplingRate);
giuliomoro@132: 	threadRunning=true;
giuliomoro@132: //I2C magic
giuliomoro@132: 	gAudioCodec->setAudioSamplingRate(targetSamplingRate);
giuliomoro@132: 	threadRunning=false;
giuliomoro@133: 	threadWasRunning=1;
giuliomoro@133: //	rt_printf("Exiting thread\n");
giuliomoro@132: };
giuliomoro@132: 
giuliomoro@132: ClockSynchronizer::ClockSynchronizer() {
giuliomoro@132: 	reset();
giuliomoro@132: }
giuliomoro@132: 
giuliomoro@132: ClockSynchronizer::~ClockSynchronizer(){};
giuliomoro@132: 
giuliomoro@132: void ClockSynchronizer::setup(){
giuliomoro@132: 	if(staticConstructed==false) //This should be called in the constructor, but because of the current limitations of
giuliomoro@132: 		// BeagleRT, it is here and setup() needs to be called in BeagleRT setup();
giuliomoro@132: 		staticConstructor();
giuliomoro@132: }
giuliomoro@132: void ClockSynchronizer::reset(){
giuliomoro@132: 	localCounter=0;
giuliomoro@132: 	remoteCounter=0;
giuliomoro@132: 	lastTime=0;
giuliomoro@132: 	localOffset=-1;
giuliomoro@132: 	remoteOffset=-1;
giuliomoro@132: 	timeOffset=-1;
giuliomoro@132: 	currentSamplingRate=44100;
giuliomoro@132: }
giuliomoro@132: 
giuliomoro@132: void ClockSynchronizer::update(int aLocalCounter, int aRemoteCounter, RTIME aLastTime){
giuliomoro@132: 	if(threadRunning==true){ //do nothing if clock is being adjusted
giuliomoro@132: 		rt_printf("do nothing if clock is being adjusted\n");
giuliomoro@132: 		return;
giuliomoro@132: 	}
giuliomoro@132: 	if(threadWasRunning > 0){ //reset variables after clock has been adjusted
giuliomoro@132: 		threadWasRunning--;  // wait a few calls to make sure the clock stabilizes
giuliomoro@132: 		rt_printf("wait a few calls to make sure the clock stabilizes\n");
giuliomoro@132: 		return;
giuliomoro@132: 	}/*
giuliomoro@132: 	if(threadWasRunning==1){
giuliomoro@132: 		threadWasRunning=0;
giuliomoro@132: 		// reset offsets after a correction
giuliomoro@132: 		localOffset=aLocalCounter;
giuliomoro@132: 		remoteOffset=aRemoteCounter;
giuliomoro@132: 		timeOffset=aLastTime;
giuliomoro@132: 		rt_printf("reset variables after clock has been adjusted\n");
giuliomoro@132: 		return;
giuliomoro@132: 	}*/
giuliomoro@132: 	if (localOffset<=0 || remoteOffset<=0){ // probably this is the first run
giuliomoro@132: 		localOffset=aLocalCounter;
giuliomoro@132: 		remoteOffset=aRemoteCounter;
giuliomoro@132: 		timeOffset=aLastTime;
giuliomoro@132: 		rt_printf("First run of update(), localOffset: %d, remoteOffset: %d, timeOffset: %llu\n",
giuliomoro@132: 				localOffset, remoteOffset, timeOffset);
giuliomoro@132: 		return;
giuliomoro@132: 	}
giuliomoro@132: 	localCounter=aLocalCounter-localOffset;
giuliomoro@132: 	remoteCounter=aRemoteCounter-remoteOffset;
giuliomoro@132: 	lastTime=aLastTime-timeOffset;
giuliomoro@132: 	if (localCounter<=0 || remoteCounter<=0 || timeOffset<=0) {// did anything wrong happened?
giuliomoro@132: 		rt_printf("fourth\n");
giuliomoro@132: 		return;
giuliomoro@132: 	}
giuliomoro@132: 	// TODO: make sure we do not get actually adjust the clock too often (e.g.: limits on the timestamp)
giuliomoro@132: 	// Should keep track of last time a change has been made, so that when a large compensation is needed
giuliomoro@132: 	// (e.g.: as the program has just started), adjustClock() is called more frequently
giuliomoro@132: 	// but gets called less often later.
giuliomoro@132: 	// Should also try to avoid drastic correction after a while that the program
giuliomoro@132: 	// has started, so to avoid glitches. But this can maybe be handled by the thread itself.
giuliomoro@132: 	// TODO: should keep a log and maybe compensate for (and prevent) overshoot according to previous changes
giuliomoro@133: 	static RTIME lastlastTime=0;
giuliomoro@133: //	rt_printf("interval: %f, \n",(double)(remoteCounter*300.0)/lastTime*1000000000.0);
giuliomoro@133: 	lastlastTime=lastTime;
giuliomoro@132: 	currentSamplingRate=gAudioCodec->getAudioSamplingRate();
giuliomoro@132: 	float T=1/currentSamplingRate;
giuliomoro@132: 	int elapsedSamples=remoteCounter*(NetworkBuffer::bufferLength-NetworkBuffer::headerLength);
giuliomoro@132: 	double expectedTimeUs=T*elapsedSamples*1000000;
giuliomoro@133: 	double actualTimeS=lastTime/1000000000.0;
giuliomoro@132: 	double actualTimeUs=lastTime/1000.0;
giuliomoro@133: 	static float averageBuffer[101]={0};
giuliomoro@133: 	static int averageBufferPointer=0;
giuliomoro@133: 	static float average=0;
giuliomoro@133: 	static int bufferFull=0;
giuliomoro@133: 	average-=averageBuffer[averageBufferPointer];
giuliomoro@133: 	averageBuffer[averageBufferPointer]=elapsedSamples/actualTimeS;
giuliomoro@133: 	average+=averageBuffer[averageBufferPointer];
giuliomoro@133: 	averageBufferPointer++;
giuliomoro@133: 	if(averageBufferPointer==101){
giuliomoro@133: 		averageBufferPointer=0;
giuliomoro@133: 		bufferFull++;
giuliomoro@133: 	}
giuliomoro@133: 
giuliomoro@132: //	rt_printf("Fs: %f, expectedTimeUs: %f, lastTime: %ul\n", expectedTimeUs, lastTime);
giuliomoro@133: //	rt_printf("Fs: %.1f, actualTimeS: %4.6f, targetFs_ %.3f\n",
giuliomoro@133: 	if((averageBufferPointer&3)==0){
giuliomoro@133: 		static float oldTargetSamplingRate=0;
giuliomoro@133: 		targetSamplingRate=average/101;
giuliomoro@133: 		if(bufferFull>=3 && fabsf(targetSamplingRate-oldTargetSamplingRate) < 1){
giuliomoro@133: 			rt_printf("%.1f, %4.6f, %.3f;\n",
giuliomoro@133: 				gAudioCodec->getAudioSamplingRate(),  actualTimeS, targetSamplingRate, targetSamplingRate-oldTargetSamplingRate);
giuliomoro@133: 			adjustClock();
giuliomoro@133: 		}
giuliomoro@133: 		oldTargetSamplingRate=targetSamplingRate;
giuliomoro@133: 	}
giuliomoro@132: }
giuliomoro@132: 
giuliomoro@132: void ClockSynchronizer::adjustClock(){
giuliomoro@133: 	if(fabsf(currentSamplingRate-targetSamplingRate)>0.7){ //TODO: actually check that the difference is less than the quantization error in the PLL
giuliomoro@132: 		BeagleRT_scheduleAuxiliaryTask(setClockTask);
giuliomoro@132: 	}
giuliomoro@132: }