view core/ClockSynchronizer.cpp @ 132:e24c531220ee scope-refactoring

Added some sort of synchronization, not working great though

/*
 * 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=3;
	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
	rt_printf("lastTime: %llu, remoteCounter: %d\n", lastTime, remoteCounter);

	currentSamplingRate=gAudioCodec->getAudioSamplingRate();
	float T=1/currentSamplingRate;
	int elapsedSamples=remoteCounter*(NetworkBuffer::bufferLength-NetworkBuffer::headerLength);
	double expectedTimeUs=T*elapsedSamples*1000000;
	double actualTimeUs=lastTime/1000.0;
	targetSamplingRate=expectedTimeUs/actualTimeUs*currentSamplingRate;
//	rt_printf("Fs: %f, expectedTimeUs: %f, lastTime: %ul\n", expectedTimeUs, lastTime);
	rt_printf("Fs: %.1f, expectedTimeUs: %4.3f, actualTimeUs: %4.3f, targetFs_ %.3f\n",
			gAudioCodec->getAudioSamplingRate(), expectedTimeUs/elapsedSamples, actualTimeUs/elapsedSamples, targetSamplingRate);
	adjustClock();
}

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