view core/ClockSynchronizer.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 ...

/*
 * ClockSynchronizer.cpp
 *
 *  Created on: 26 Aug 2015
 *      Author: giulio
 */

#include "ClockSynchronizer.h"

// declare static members
float ClockSynchronizer::targetSamplingRate;
float ClockSynchronizer::currentSamplingRate;
bool ClockSynchronizer::threadRunning;
int ClockSynchronizer::threadWasRunning;
bool ClockSynchronizer::staticConstructed=false;
AuxiliaryTask ClockSynchronizer::setClockTask;

void ClockSynchronizer::staticConstructor(){
	int priority=90;
	setClockTask=BeagleRT_createAuxiliaryTask(&ClockSynchronizer::setClock, priority, "setClockTask");
	threadRunning=false;
}

void ClockSynchronizer::setClock(){
//	rt_printf("Setting clock to %f\n",targetSamplingRate);
	threadRunning=true;
//I2C magic
	gAudioCodec->setAudioSamplingRate(targetSamplingRate);
	threadRunning=false;
	threadWasRunning=1;
//	rt_printf("Exiting thread\n");
};

ClockSynchronizer::ClockSynchronizer() {
	reset();
}

ClockSynchronizer::~ClockSynchronizer(){};

void ClockSynchronizer::setup(){
	if(staticConstructed==false) //This should be called in the constructor, but because of the current limitations of
		// BeagleRT, it is here and setup() needs to be called in BeagleRT setup();
		staticConstructor();
}
void ClockSynchronizer::reset(){
	localCounter=0;
	remoteCounter=0;
	lastTime=0;
	localOffset=-1;
	remoteOffset=-1;
	timeOffset=-1;
	currentSamplingRate=44100;
}

void ClockSynchronizer::update(int aLocalCounter, int aRemoteCounter, RTIME aLastTime){
	if(threadRunning==true){ //do nothing if clock is being adjusted
		rt_printf("do nothing if clock is being adjusted\n");
		return;
	}
	if(threadWasRunning > 0){ //reset variables after clock has been adjusted
		threadWasRunning--;  // wait a few calls to make sure the clock stabilizes
		rt_printf("wait a few calls to make sure the clock stabilizes\n");
		return;
	}/*
	if(threadWasRunning==1){
		threadWasRunning=0;
		// reset offsets after a correction
		localOffset=aLocalCounter;
		remoteOffset=aRemoteCounter;
		timeOffset=aLastTime;
		rt_printf("reset variables after clock has been adjusted\n");
		return;
	}*/
	if (localOffset<=0 || remoteOffset<=0){ // probably this is the first run
		localOffset=aLocalCounter;
		remoteOffset=aRemoteCounter;
		timeOffset=aLastTime;
		rt_printf("First run of update(), localOffset: %d, remoteOffset: %d, timeOffset: %llu\n",
				localOffset, remoteOffset, timeOffset);
		return;
	}
	localCounter=aLocalCounter-localOffset;
	remoteCounter=aRemoteCounter-remoteOffset;
	lastTime=aLastTime-timeOffset;
	if (localCounter<=0 || remoteCounter<=0 || timeOffset<=0) {// did anything wrong happened?
		rt_printf("fourth\n");
		return;
	}
	// TODO: make sure we do not get actually adjust the clock too often (e.g.: limits on the timestamp)
	// Should keep track of last time a change has been made, so that when a large compensation is needed
	// (e.g.: as the program has just started), adjustClock() is called more frequently
	// but gets called less often later.
	// Should also try to avoid drastic correction after a while that the program
	// has started, so to avoid glitches. But this can maybe be handled by the thread itself.
	// TODO: should keep a log and maybe compensate for (and prevent) overshoot according to previous changes
	static RTIME lastlastTime=0;
//	rt_printf("interval: %f, \n",(double)(remoteCounter*300.0)/lastTime*1000000000.0);
	lastlastTime=lastTime;
	currentSamplingRate=gAudioCodec->getAudioSamplingRate();
	float T=1/currentSamplingRate;
	int elapsedSamples=remoteCounter*(NetworkBuffer::bufferLength-NetworkBuffer::headerLength);
	double expectedTimeUs=T*elapsedSamples*1000000;
	double actualTimeS=lastTime/1000000000.0;
	double actualTimeUs=lastTime/1000.0;
	static float averageBuffer[101]={0};
	static int averageBufferPointer=0;
	static float average=0;
	static int bufferFull=0;
	average-=averageBuffer[averageBufferPointer];
	averageBuffer[averageBufferPointer]=elapsedSamples/actualTimeS;
	average+=averageBuffer[averageBufferPointer];
	averageBufferPointer++;
	if(averageBufferPointer==101){
		averageBufferPointer=0;
		bufferFull++;
	}

//	rt_printf("Fs: %f, expectedTimeUs: %f, lastTime: %ul\n", expectedTimeUs, lastTime);
//	rt_printf("Fs: %.1f, actualTimeS: %4.6f, targetFs_ %.3f\n",
	if((averageBufferPointer&3)==0){
		static float oldTargetSamplingRate=0;
		targetSamplingRate=average/101;
		if(bufferFull>=3 && fabsf(targetSamplingRate-oldTargetSamplingRate) < 1){
			rt_printf("%.1f, %4.6f, %.3f;\n",
				gAudioCodec->getAudioSamplingRate(),  actualTimeS, targetSamplingRate, targetSamplingRate-oldTargetSamplingRate);
			adjustClock();
		}
		oldTargetSamplingRate=targetSamplingRate;
	}
}

void ClockSynchronizer::adjustClock(){
	if(fabsf(currentSamplingRate-targetSamplingRate)>0.7){ //TODO: actually check that the difference is less than the quantization error in the PLL
		BeagleRT_scheduleAuxiliaryTask(setClockTask);
	}
}