Mercurial > hg > beaglert

changeset 132:e24c531220ee scope-refactoring

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Added some sort of synchronization, not working great though
author Giulio Moro <giuliomoro@yahoo.it>
date Thu, 27 Aug 2015 01:42:04 +0100
parents ff28e56e5b7e
children 04b1678614c9
files .cproject core/ClockSynchronizer.cpp core/I2c_Codec.cpp core/NetworkSend.cpp core/ReceiveAudioThread.cpp include/ClockSynchronizer.h include/I2c_Codec.h include/NetworkSend.h include/ReceiveAudioThread.h projects/scope/render.cpp resources/network/udp-server.c
diffstat 11 files changed, 258 insertions(+), 22 deletions(-) [+]
line wrap: on
line diff
--- a/.cproject	Wed Aug 26 02:02:10 2015 +0100
+++ b/.cproject	Thu Aug 27 01:42:04 2015 +0100
@@ -108,7 +108,7 @@
 				</extensions>
 			</storageModule>
 			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
-				<configuration artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.release,org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe" cleanCommand="rm -rf" description="" id="cdt.managedbuild.config.gnu.exe.release.1521194538" name="Release" parent="cdt.managedbuild.config.gnu.exe.release" postannouncebuildStep="Stopping process on BBB and copying new binary" postbuildStep="ssh root@192.168.7.2 &quot;kill -s 2 \`pidof ${BuildArtifactFileName}\` 2&gt;/dev/null&quot;; scp ${BuildArtifactFilePrefix}${BuildArtifactFileName} root@192.168.7.2:~/beaglert/ ; echo 'done copying' | wall">
+				<configuration artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.release,org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe" cleanCommand="rm -rf" description="" id="cdt.managedbuild.config.gnu.exe.release.1521194538" name="Release" parent="cdt.managedbuild.config.gnu.exe.release" postannouncebuildStep="Stopping process on BBB and copying new binary" postbuildStep="ssh root@192.168.7.2 &quot;kill -s 9 \`pidof ${BuildArtifactFileName}\` 2&gt;/dev/null; sleep 0.5; &quot;; scp ${PWD}/${BuildArtifactFileName} root@192.168.7.2:~/beaglert/ &amp;&amp; echo 'done copying\n' | wall || echo 'error'|wall">
 					<folderInfo id="cdt.managedbuild.config.gnu.exe.release.1521194538." name="/" resourcePath="">
 						<toolChain id="cdt.managedbuild.toolchain.gnu.exe.release.1612059942" name="Linux GCC" superClass="cdt.managedbuild.toolchain.gnu.exe.release">
 							<targetPlatform id="cdt.managedbuild.target.gnu.platform.exe.release.908983575" name="Debug Platform" superClass="cdt.managedbuild.target.gnu.platform.exe.release"/>
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/core/ClockSynchronizer.cpp	Thu Aug 27 01:42:04 2015 +0100
@@ -0,0 +1,114 @@
+/*
+ * 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);
+	}
+}
--- a/core/I2c_Codec.cpp	Wed Aug 26 02:02:10 2015 +0100
+++ b/core/I2c_Codec.cpp	Thu Aug 27 01:42:04 2015 +0100
@@ -44,11 +44,15 @@
 		return 1;
 	if(writeRegister(0x03, 0x91))	// PLL register A: enable
 		return 1;
-	if(writeRegister(0x04, 0x1C))	// PLL register B
+//	if(writeRegister(0x04, 0x1C))	// PLL register B
+//		return 1;
+//	if(writeRegister(0x05, 0x52))	// PLL register C
+//		return 1;
+//	if(writeRegister(0x06, 0x40))	// PLL register D
+//		return 1;
+	if(setPllD(5264)) //7.5264 gives 44.1kHz nominal value with a 12MHz master clock
 		return 1;
-	if(writeRegister(0x05, 0x52))	// PLL register C
-		return 1;
-	if(writeRegister(0x06, 0x40))	// PLL register D
+	if(setPllJ(7))
 		return 1;
 	if(dual_rate) {
 		if(writeRegister(0x07, 0xEA))	// Codec datapath register: 44.1kHz; dual rate; standard datapath
@@ -120,7 +124,7 @@
 //set the numerator multiplier for the PLL
 int I2c_Codec::setPllK(float k){
 	short unsigned int j=(int)k;
-	unsigned int d=(k-j+0.5)*10000; //fractionary part, between 0 and 9999
+	unsigned int d=(int)(0.5+(k-j)*10000); //fractional part, between 0 and 9999
 	if(setPllJ(j)>0)
 		return 1;
 	if(setPllD(d)>0)
@@ -138,6 +142,7 @@
 		printf("I2C error while writing PLL j: %d", j);
 		return 1;
 	}
+	pllJ=j;
 	return 0;
 }
 
@@ -153,8 +158,40 @@
 		printf("I2C error while writing PLL d part 2 : %d", d);
 		return 1;
 	}
+	pllD=d;
 	return 0;
 }
+
+int I2c_Codec::setAudioSamplingRate(float newSamplingRate){
+	int pllP=1; //TODO: create get/set for pllP and pllR
+	int pllR=1;
+	long int PLLCLK_IN=12000000;
+	//	f_{S(ref)} = (PLLCLK_IN × K × R)/(2048 × P)
+	float k = ((double)(newSamplingRate * pllP * 2048.0f/(float)pllR)) / PLLCLK_IN ;
+	return (setPllK(k));
+}
+
+short unsigned int I2c_Codec::getPllJ(){
+	return pllJ;
+}
+unsigned int I2c_Codec::getPllD(){
+	return pllD;
+}
+float I2c_Codec::getPllK(){
+	float j=getPllJ();
+	float d=getPllD();
+	float k=j+d/10000.0f;
+	return k;
+}
+
+float I2c_Codec::getAudioSamplingRate(){
+	int pllP=1; //TODO: create get/set for pllP and pllR
+	int pllR=1;
+	long int PLLCLK_IN=12000000;
+	//	f_{S(ref)} = (PLLCLK_IN × K × R)/(2048 × P)
+	float fs = (PLLCLK_IN/2048.0f) * getPllK()*pllR/(float)pllP;
+	return fs;
+}
 // Set the volume of the DAC output
 int I2c_Codec::setDACVolume(int halfDbSteps)
 {
--- a/core/NetworkSend.cpp	Wed Aug 26 02:02:10 2015 +0100
+++ b/core/NetworkSend.cpp	Thu Aug 27 01:42:04 2015 +0100
@@ -194,6 +194,10 @@
 	return channel.channelNumber;
 };
 
+int NetworkSend::getTimestamp(){
+	return channel.buffers[channel.readBuffer][channel.headerTimestampIndex];
+}
+
 void NetworkSend::sendData(){
 	if(channel.enabled==false)
 		return;
--- a/core/ReceiveAudioThread.cpp	Wed Aug 26 02:02:10 2015 +0100
+++ b/core/ReceiveAudioThread.cpp	Thu Aug 27 01:42:04 2015 +0100
@@ -59,6 +59,11 @@
     if(writePointer<0)
         return 0;
     if(socket.waitUntilReady(true, waitForSocketTime)){// TODO: if waitForSocketTime here is >>5, the
+#ifdef USE_JUCE
+#else
+		lastTime=rt_timer_read();
+//        rt_printf("lastTimeread= %llu\n", lastTime);
+#endif /* USE_JUCE */
     	// destructor (always or sometimes) never actually gets called, despite run() returns ...see issue #1381
         pushPayload(writePointer); //backup headerLength samples. This could be skipped if writePointer==0
         //read header+payload
@@ -80,7 +85,6 @@
 //        	printf("I am channel %d, but I received data for channel %d\n", channel, (int)buffer[writePointer]);
         	return -5;
         }
-        static int timestamp=0;
         if(buffer[writePointer+1]!=timestamp+1)
         	printf("missing a timestamp: %d\n",timestamp+1);
         timestamp=buffer[writePointer+1];
@@ -161,6 +165,7 @@
   writePointer=-1;
   readPointer=0;
   sleepTime=payloadLength/(float)44100 /4.0; //set sleepTime so that you do not check too often or too infrequently
+  timestamp=0;
 #ifdef USE_JUCE
   startThread(threadPriority);
 #else
@@ -241,7 +246,13 @@
 bool ReceiveAudioThread::threadShouldExit(){
 	return(gShouldStop || threadIsExiting );
 }
+RTIME ReceiveAudioThread::getLastTime(){
+	return lastTime;
+}
 #endif /* USE_JUCE */
+int ReceiveAudioThread::getTimestamp(){
+	return timestamp;
+}
 void ReceiveAudioThread::run(){
     //  fd2=fopen("buffer.m","w"); //DEBUG
     //  fprintf(fd2, "buf=["); //DEBUG
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/ClockSynchronizer.h	Thu Aug 27 01:42:04 2015 +0100
@@ -0,0 +1,42 @@
+/*
+ * ClockSynchronizer.h
+ *
+ *  Created on: 26 Aug 2015
+ *      Author: giulio
+ */
+
+#ifndef CLOCKSYNCHRONIZER_H_
+#define CLOCKSYNCHRONIZER_H_
+
+#include <BeagleRT.h>
+#include <I2c_Codec.h>
+#include <NetworkSend.h>
+#include <native/timer.h>
+extern I2c_Codec *gAudioCodec;
+
+class ClockSynchronizer {
+private:
+	int localOffset;
+	int remoteOffset;
+	RTIME timeOffset;
+	int localCounter;
+	int remoteCounter;
+	RTIME lastTime;
+	void reset();
+	static bool staticConstructed;
+	static void staticConstructor();
+	static float currentSamplingRate;
+	static float targetSamplingRate;
+	static bool threadRunning;
+	static int threadWasRunning;
+	static AuxiliaryTask setClockTask;
+public:
+	ClockSynchronizer();
+	~ClockSynchronizer();
+	void setup();
+	void update(int aLocalCounter, int aRemoteCounter, RTIME aLastTime);
+	void adjustClock();
+	static void setClock();
+};
+
+#endif /* CLOCKSYNCHRONIZER_H_ */
--- a/include/I2c_Codec.h	Wed Aug 26 02:02:10 2015 +0100
+++ b/include/I2c_Codec.h	Thu Aug 27 01:42:04 2015 +0100
@@ -21,6 +21,8 @@
 
 class I2c_Codec : public I2c
 {
+	short unsigned int pllJ;
+	short unsigned int pllD;
 public:
 	int writeRegister(unsigned int reg, unsigned int value);
 
@@ -31,6 +33,11 @@
 	int setPllJ(short unsigned int j);
 	int setPllD(unsigned int d);
 	int setPllK(float k);
+	int setAudioSamplingRate(float newSamplingRate);
+	short unsigned int getPllJ();
+	unsigned int getPllD();
+	float getPllK();
+	float getAudioSamplingRate();
 	int setDACVolume(int halfDbSteps);
 	int writeDACVolumeRegisters(bool mute);
 	int setADCVolume(int halfDbSteps);
--- a/include/NetworkSend.h	Wed Aug 26 02:02:10 2015 +0100
+++ b/include/NetworkSend.h	Thu Aug 27 01:42:04 2015 +0100
@@ -76,6 +76,7 @@
 	void setServer(const char* aServer);
 	void setChannelNumber(int aChannelNumber);
 	int getChannelNumber();
+	int getTimestamp();
 #ifdef USE_JUCE
 	void run();
 #else
--- a/include/ReceiveAudioThread.h	Wed Aug 26 02:02:10 2015 +0100
+++ b/include/ReceiveAudioThread.h	Thu Aug 27 01:42:04 2015 +0100
@@ -4,12 +4,14 @@
 #ifdef USE_JUCE
 #include <JuceHeader.h>
 #else
+#include <BeagleRT.h>
+#include <UdpServer.h>
 #include <vector>
 #include <iostream>
-#include <UdpServer.h>
-#include <BeagleRT.h>
 #include <native/task.h>
+#include <native/timer.h>
 #include <math.h>
+
 #endif /*USE_JUCE*/
 
 #ifdef USE_JUCE
@@ -50,6 +52,7 @@
     int bytesToRead;
     int threadPriority;
     int channel;
+    int timestamp;
     void dealloc();
     void wrapWritePointer();
     void pushPayload(int startIndex);
@@ -57,6 +60,7 @@
     int readUdpToBuffer();
 #ifdef USE_JUCE
 #else
+    RTIME lastTime; // Used for clock synchronization
     static bool threadShouldExit();
 	static bool staticConstructed;
     static void staticConstructor();
@@ -85,10 +89,12 @@
     		float samplingRateRatio, int numChannelsInDestination,
     		int channelToWriteTo);
     bool isBufferReady();
+    int getTimestamp();
 #ifdef USE_JUCE // if we are in Juce, then we run a separate thread for each receiver
     		// (as each of them are typically receiving on a mono or stereo track)
     void run();
 #else
+    RTIME getLastTime();
     void static run(); //while in BeagleRT we have a single thread that receives for all the instances.
     //TODO: make run() private in BeagleRT
     static void startThread();
--- a/projects/scope/render.cpp	Wed Aug 26 02:02:10 2015 +0100
+++ b/projects/scope/render.cpp	Thu Aug 27 01:42:04 2015 +0100
@@ -1,6 +1,7 @@
 #include <BeagleRT.h> 
 #include <NetworkSend.h>
 #include <ReceiveAudioThread.h>
+#include <ClockSynchronizer.h>
 #include <cmath>
 
 float gPhase1, gPhase2;
@@ -20,6 +21,8 @@
 // Return true on success; returning false halts the program.
 ReceiveAudioThread receiveAudio0;
 //ReceiveAudioThread receiveAudio1;
+ClockSynchronizer clockSynchronizer;
+extern I2c_Codec* gAudioCodec;
 bool setup(BeagleRTContext *context, void *userData)
 {
 	receiveAudio0.init(10000, context->audioFrames, 0);
@@ -29,7 +32,7 @@
 //	scope.setPort(0, 9999);
 //	scope.setPort(1, 10000);
 	networkSend.setup(context->audioSampleRate, context->audioFrames, 0, 9999, "192.168.7.1");
-	 
+	clockSynchronizer.setup();
 	gInverseSampleRate = 1.0/context->audioSampleRate;
 	
 	gPhase1 = 0.0;
@@ -49,7 +52,19 @@
 void render(BeagleRTContext *context, void *userData)
 {
 	static int count=0;
+//	if((count&262143)==0){
+//	static int nextCall=160000;
+	if( ((count&(16384-1))==0 /*&& count>200000*/)){
+//		rt_printf("b %d\n", count);
+		clockSynchronizer.update(networkSend.getTimestamp(), receiveAudio0.getTimestamp(), receiveAudio0.getLastTime());
+//		nextCall=count+100000;
+//		rt_printf("a %d\n", count);
+	}
+//	if(count == nextCall){
+//		clockSynchronizer.update(networkSend.getTimestamp(), receiveAudio0.getTimestamp(), receiveAudio0.getLastTime());
+//	}
 	if(count==0){
+		gAudioCodec->setAudioSamplingRate(44080);
 		printf("startHread\n");
 		ReceiveAudioThread::startThread();
 	}
@@ -63,8 +78,8 @@
 
     //  float chn4 = context->analogIn[(int)n/2*8 + 0];
     //  float chn5 = context->analogIn[(int)n/2*8 + 1];
-		networkSend.log(context->audioIn[n]);
-//		networkSend.log(chn0);
+//		networkSend.log(context->audioIn[n]);
+		networkSend.log(chn0);
 //		scope.log(0, chn0);
 //		scope.log(1, chn1);
 		//  scope.log(2, chn2);
@@ -79,13 +94,13 @@
 	    //to view, click the 'oscilloscope' button on the toolbar while BeagleRT is NOT running
 	    //then click the big red button on the toolbar on this page
 	    
-	    gPhase1 += 2.0 * M_PI * gFrequency1 * gInverseSampleRate * ((count&4095)/4096.0+1);
+	    gPhase1 += 2.0 * M_PI * gFrequency1 * gInverseSampleRate * ((count&65535)/65535.0+1);
 			gPhase2 += 2.0 * M_PI * gFrequency2 * gInverseSampleRate;
 		if(gPhase1 > 2.0 * M_PI)
 			gPhase1 -= 2.0 * M_PI;
 		if(gPhase2 > 2.0 * M_PI)
 			gPhase2 -= 2.0 * M_PI;
-		
+		count++;
 	}
 	if(count>0){
 		float samplingRateRatio=1;
@@ -93,15 +108,14 @@
 		int channelToWriteTo=0;
 		int length=receiveAudio0.getSamplesSrc(context->audioOut, context->audioFrames,
 				samplingRateRatio, channelsInDestinationBuffer, channelToWriteTo);
-		if(length!=context->audioFrames){
+		if((unsigned int)length!=context->audioFrames){
 			rt_printf("Length mismatch: %d\n", length);
 		}
 //		int readPointer1=receiveAudio1.getSamplesSrc(context->audioOut, context->audioFrames, 1, 2, 1);
 	}
-	for(int n=0; n<context->audioFrames; n++){
+	for(unsigned int n=0; n<context->audioFrames; n++){
 		context->audioOut[n*2+1]=context->audioOut[n*2];
 	}
-	count++;
 }
 
 // cleanup_render() is called once at the end, after the audio has stopped.
--- a/resources/network/udp-server.c	Wed Aug 26 02:02:10 2015 +0100
+++ b/resources/network/udp-server.c	Thu Aug 27 01:42:04 2015 +0100
@@ -48,11 +48,11 @@
        if (n < 0) error("recvfrom");
        printf("Received a datagram of size %d: \n", n);
        printf("Header: channel: %d, timestamp: %d\n", (int)buf[0], (int)buf[1]);
-       for(i=2; i<n/sizeof(float); i+=8)
-	       printf("%+f, %+f, %+f, %+f, %+f, %+f, %+f, %+f\n",i,buf[0+i],buf[1+i],buf[2+i],buf[3+i],buf[4+i],buf[5+i],buf[6+i],buf[7+i]);
-       n = sendto(sock,"Got your message\n",17,
-                  0,(struct sockaddr *)&from,fromlen);
-       if (n  < 0) error("sendto");
+       //  for(i=2; i<n/sizeof(float); i+=8)
+	       //  printf("%+f, %+f, %+f, %+f, %+f, %+f, %+f, %+f\n",buf[0+i],buf[1+i],buf[2+i],buf[3+i],buf[4+i],buf[5+i],buf[6+i],buf[7+i]);
+       //  n = sendto(sock,"Got your message\n",17,
+                  //  0,(struct sockaddr *)&from,fromlen);
+       //  if (n  < 0) error("sendto");
    }
    return 0;
  }