comparison 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);
+	}
+}