giuliomoro@135: #include "ClockSync.h" giuliomoro@135: giuliomoro@135: void ClockSync::setVirtualClock(VirtualClock &aVirtualClock){ giuliomoro@135: virtualClock=&aVirtualClock; giuliomoro@135: } giuliomoro@135: void ClockSync::init(bool thisIsSlave, int aPort, VirtualClock &aVirtualClock){ giuliomoro@135: setVirtualClock(aVirtualClock); giuliomoro@135: slave=thisIsSlave; giuliomoro@135: setPort(aPort); giuliomoro@135: // isSlave() ? client.setServer("127.0.0.1") : client.setServer("127.0.0.1"); giuliomoro@135: isSlave() ? client.setServer("192.168.7.1") : client.setServer("192.168.7.2"); giuliomoro@135: bufferLength=kSyncMessageLength; giuliomoro@141: resetTs(); giuliomoro@141: receiveLoopSleepUs=100; giuliomoro@141: receiveLoopTimeout=1e5; giuliomoro@141: movingAverage.setLength(31); giuliomoro@141: expectedClockSyncType=isSlave() ? kSync : kNone; giuliomoro@141: } giuliomoro@141: void ClockSync::resetTs(){ giuliomoro@135: T1=-1; giuliomoro@135: T1p=-1; giuliomoro@135: T2=-1; giuliomoro@135: T2p=-1; giuliomoro@141: } giuliomoro@141: bool ClockSync::areTsValid(){ giuliomoro@141: return T1>0 && T1p>0 && T2>0 && T2p>0; giuliomoro@135: } giuliomoro@135: ClockSync::ClockSync(bool thisIsSlave, int aPort, VirtualClock &aVirtualClock){ giuliomoro@135: init(thisIsSlave, aPort, aVirtualClock); giuliomoro@135: } giuliomoro@135: void* ClockSync::getBuffer(){ giuliomoro@135: return buffer; giuliomoro@135: } giuliomoro@135: bool ClockSync::isSlave(){ giuliomoro@135: return slave; giuliomoro@135: } giuliomoro@135: bool ClockSync::isMaster(){ giuliomoro@135: return !slave; giuliomoro@135: } giuliomoro@135: int ClockSync::getType(){ giuliomoro@135: return ((int*)buffer)[0]; giuliomoro@135: } giuliomoro@135: myClock_t ClockSync::getTimestamp(){ giuliomoro@135: return *((myClock_t*)((char*)buffer+sizeof(int))); giuliomoro@135: } giuliomoro@135: void ClockSync::setType(int clockSyncType){ giuliomoro@135: ((int*)buffer)[0]=clockSyncType; giuliomoro@135: } giuliomoro@135: void ClockSync::setTimestamp(myClock_t timestamp){ giuliomoro@139: // printf("setting timestamp: %lld\n", timestamp); giuliomoro@135: ((myClock_t*)((char*)buffer+sizeof(int)))[0]=timestamp; giuliomoro@135: } giuliomoro@135: void ClockSync::print(){ giuliomoro@135: //printf("type: %d, timestamp: %lld\n",*((int*)buffer),*((myClock_t*)(((int*)buffer)+1))); giuliomoro@135: } giuliomoro@135: void ClockSync::setPort(int aPort){ giuliomoro@135: if(aPort>0){ giuliomoro@135: int inPort=isSlave() ? aPort : aPort+1; giuliomoro@135: int outPort=isSlave() ? aPort+1: aPort; giuliomoro@135: server.bindToPort(inPort); giuliomoro@135: client.setPort(outPort); giuliomoro@135: //printf("Receiving on port %d\n", inPort); giuliomoro@135: //printf("Sending to port %d\n", outPort); giuliomoro@135: } giuliomoro@135: } giuliomoro@135: /** giuliomoro@135: * sends a clockSync without blocking, checks results and returns the timestamp giuliomoro@135: * immediately after the clockSync has been sent or -1 if there was an error or timeout expired. giuliomoro@135: */ giuliomoro@135: myClock_t ClockSync::send(){ giuliomoro@135: // print(); giuliomoro@135: int ret; giuliomoro@135: ret=client.waitUntilReady(false, 0); giuliomoro@135: if(ret<=0){ //don't retry giuliomoro@135: return -1; giuliomoro@135: } giuliomoro@135: ret=client.send(buffer, bufferLength); giuliomoro@135: myClock_t timestamp=(myClock_t)virtualClock->getNow(); giuliomoro@135: if(ret<0){ giuliomoro@135: //if there was an error while sending, don't retry giuliomoro@135: return -1; giuliomoro@135: } giuliomoro@135: return timestamp; //get the accurate time *after* you sent the sync clockSync giuliomoro@135: } giuliomoro@135: /** giuliomoro@135: * receives a clockSync without blocking, checks results and returns the timestamp giuliomoro@135: * immediately after the clockSync has been received, or -1 if there was an error giuliomoro@135: * or 0 if timeout expired. giuliomoro@135: */ giuliomoro@135: myClock_t ClockSync::receive(){ giuliomoro@135: int ret; giuliomoro@135: ret=server.waitUntilReady(true, 0); giuliomoro@135: if(ret<=0){ //don't retry giuliomoro@135: return 0; giuliomoro@135: } giuliomoro@135: ret=server.read(buffer, bufferLength, false); giuliomoro@135: myClock_t timestamp=(myClock_t)virtualClock->getNow(); giuliomoro@135: if(timestamp==0){ giuliomoro@135: //printf("The virtualClock period is <=0\n"); giuliomoro@135: return -3; giuliomoro@135: } giuliomoro@135: if(ret==-1){ giuliomoro@135: //if there was an error while receiving, don't retry giuliomoro@135: return -1; giuliomoro@135: } giuliomoro@135: if(ret!=bufferLength){ giuliomoro@135: //printf("Received a clockSync of the wrong size: %d\n", ret); giuliomoro@135: return -2; giuliomoro@135: } giuliomoro@135: // print(); giuliomoro@135: return timestamp; //get the accurate time *after* you sent the sync clockSync giuliomoro@135: } giuliomoro@135: giuliomoro@135: int ClockSync::masterSendSync(){ giuliomoro@135: //let's send a sync clockSync! giuliomoro@135: //printf("Sending a sync clockSync\n"); giuliomoro@135: setType(kSync); giuliomoro@135: setTimestamp(-1);//do not care about sending the timestamp, a more accurate one will be sent in the follow up giuliomoro@135: localTimestamp=send(); giuliomoro@135: if(localTimestamp<0){ giuliomoro@135: //printf("Could not send sync clockSync\n"); giuliomoro@135: return -1; //error, don't retry, start over giuliomoro@135: } giuliomoro@135: //let's send a followUp giuliomoro@135: //printf("sent a sync clockSync\n"); giuliomoro@135: setType(kFollowUp); giuliomoro@135: setTimestamp(localTimestamp); giuliomoro@135: if(localTimestamp<0){ giuliomoro@135: //printf("Could not send followup clockSync\n"); giuliomoro@135: return -2; //error, don't retry, start over giuliomoro@135: } giuliomoro@135: int ret=send(); giuliomoro@135: if(ret<0){ giuliomoro@135: //printf("Error while sending followup\n"); giuliomoro@135: return -3; giuliomoro@135: } giuliomoro@135: //printf("sent a followUp clockSync\n"); giuliomoro@135: expectedClockSyncType=kDelayReq; giuliomoro@135: return 1; giuliomoro@135: } giuliomoro@135: #ifdef USE_JUCE giuliomoro@135: #define NOTGSHOULDSTOP 1 giuliomoro@135: #else giuliomoro@135: extern bool gShouldStop; giuliomoro@135: #define NOTGSHOULDSTOP (!gShouldStop) giuliomoro@135: #endif /* USE_JUCE */ giuliomoro@135: int ClockSync::receiveLoop(){ giuliomoro@135: int receiveLoopElapsed=0; giuliomoro@135: while( NOTGSHOULDSTOP && (isSlave() || (receiveLoopElapsed giuliomoro@141: extern I2c_Codec* gAudioCodec; giuliomoro@141: void ClockSync::processOffset(double offset){ giuliomoro@141: static int calls=0; giuliomoro@141: // TODO: change the flow control below so that it can happen multiple times giuliomoro@141: //(base it upon the length of movingAverage rather than the number of calls) giuliomoro@141: if(calls<10) { //get an initial guess giuliomoro@141: movingAverage.add(offset); giuliomoro@141: // printf("-----------OFFSET IS : %04.4f samples, average: %04.4f samples\n", giuliomoro@141: // offset, movingAverage.getAverage()); giuliomoro@141: } else if (calls==10){ //then compensate for initial offset giuliomoro@141: // printf("compensating for offset: %f\n", offset); giuliomoro@141: virtualClock->addOffset(movingAverage.getAverage()); giuliomoro@141: movingAverage.reset(); giuliomoro@141: } else if (calls>=10){ //use IIR filter from now on giuliomoro@141: //filter coefficients obtained from Matlab : [B,A]=butter(2,0.005); giuliomoro@141: // static float B[3]={6.10061787580662e-05, 0.000122012357516132, 6.10061787580662e-05}; giuliomoro@141: // static float A[3]={1, -1.97778648377676, 0.978030508491796}; giuliomoro@141: static float B[3]={6.10061787580662e-05, 0.000122012357516132, 6.10061787580662e-05}; giuliomoro@141: static float A[3]={1, -1.97778648377676, 0.978030508491796}; giuliomoro@141: static float pastOut[3]={0,0,0}; giuliomoro@141: static float pastIn[3]={0,0,0}; giuliomoro@141: float in=offset; giuliomoro@141: float out= -pastOut[1]*A[1] -pastOut[2]*A[2] +in*B[0] +pastIn[1]*B[1] +pastIn[2]*B[2]; giuliomoro@141: pastOut[2]=pastOut[1]; giuliomoro@141: pastOut[1]=out; giuliomoro@141: pastIn[2]=pastIn[1]; giuliomoro@141: pastIn[1]=in; giuliomoro@141: offset=out; giuliomoro@141: static float maxOffset=0; giuliomoro@141: maxOffset=fabsf(offset) > fabsf(maxOffset) ? offset : maxOffset; giuliomoro@141: printf("%10.3f, %10.3f, %10.3f, %10.3f\n", in, offset, offset-pastOut[2], maxOffset); //unfiltered, filtered giuliomoro@141: if(fabsf(offset)>10 && calls>30){ giuliomoro@141: calls=11; giuliomoro@141: //TODO: correct for offset giuliomoro@141: float targetSamplingRate=offset>0 ? 44097 : 44103; giuliomoro@141: gAudioCodec->setAudioSamplingRate(targetSamplingRate); giuliomoro@141: // pastOut[1]=pastOut[2]=pastIn[1]=pastIn[2]=offset; giuliomoro@141: printf("------setAudioSmplingRate to %f\n", targetSamplingRate); giuliomoro@141: } giuliomoro@141: } giuliomoro@141: calls++; giuliomoro@141: } giuliomoro@135: int ClockSync::masterHandleMessage(){ giuliomoro@135: switch(clockSyncType){ giuliomoro@135: case kDelayReq: giuliomoro@135: //send kDelayResp giuliomoro@135: setType(kDelayResp); giuliomoro@135: setTimestamp(localTimestamp); giuliomoro@135: send(); giuliomoro@135: expectedClockSyncType=kNone; giuliomoro@141: return 1; giuliomoro@135: break; giuliomoro@141: default: giuliomoro@141: return -1; giuliomoro@135: } giuliomoro@135: } giuliomoro@135: giuliomoro@135: int ClockSync::sendReceiveLoop(){ giuliomoro@135: if(isSlave()==true){ giuliomoro@135: //printf("Waiting for a sync clockSync\n"); giuliomoro@135: } else { //if this is master giuliomoro@135: usleep(100000); //this times (roughly) how often sync clockSyncs are being sent. giuliomoro@135: int ret=masterSendSync(); giuliomoro@135: if(ret<=0) giuliomoro@135: return -1; giuliomoro@135: } giuliomoro@135: int ret=receiveLoop(); giuliomoro@135: if(ret<=0) giuliomoro@135: return -2; giuliomoro@135: return 1; giuliomoro@135: } giuliomoro@135: giuliomoro@135: