# HG changeset patch # User Giulio Moro # Date 1444046774 -3600 # Node ID 8f98b32d0e23cce13d9fd2c3bf09be3e354fa12a # Parent e9c9404e3d1f67b7458eec65366c192eeb0617aa Last commit on this branch for a while. Overall not very succesful diff -r e9c9404e3d1f -r 8f98b32d0e23 core/ClockSync.cpp --- a/core/ClockSync.cpp Tue Sep 22 04:10:07 2015 +0100 +++ b/core/ClockSync.cpp Mon Oct 05 13:06:14 2015 +0100 @@ -3,6 +3,7 @@ void ClockSync::setVirtualClock(VirtualClock &aVirtualClock){ virtualClock=&aVirtualClock; } +Pid* gClockSyncPid; void ClockSync::init(bool thisIsSlave, int aPort, VirtualClock &aVirtualClock){ setVirtualClock(aVirtualClock); slave=thisIsSlave; @@ -15,6 +16,7 @@ receiveLoopTimeout=1e5; movingAverage.setLength(31); expectedClockSyncType=isSlave() ? kSync : kNone; + gClockSyncPid = &pid; } void ClockSync::resetTs(){ T1=-1; @@ -215,40 +217,9 @@ case kDelayResp: {//the clockSyncTimestamp is the instant when the master received the kDelayResp clockSync, the localTimestamp is meaningless T2p=clockSyncTimestamp; //TODO: evaluate things - double offset=(T1p-T1-T2p+T2)/2.0d; if(areTsValid()){ - processOffset(offset); - - /* - static int calls=0; - static double referenceOffset=0; - - if(calls<100){ // start by averaging everything - movingAverage.add(offset); - } else { //once we get an estimate, start discarding outliers - float maxOffsetDeviation=20; - float deviation=fabsf(movingAverage.getAverage()-offset); - if(deviation 1*expectedOffsetDifferenceStd && calls>51){ + float expectedOffsetDifference = 3.5; + static float pastNlOffset = 0; + static float pastIncrement = 0; + float offsetIncrement = offset-pastNlOffset; + static int filteredOut=0; + if (calls>51 && (fabsf(offsetIncrement) > expectedOffsetDifference && filteredOut < 8)){ // non-linear filtering: remove outliers - nlOffset = pastOffset; + nlOffset = pastNlOffset; +// printf("this actually happened\n"); +//nlOffset=offset; // BYPASS non linear filter // printf("%f %f 0,0,0,0,0,0\n", offset,pastOffset); + filteredOut++; } else { nlOffset=offset; + filteredOut = 0; } - pastOffset=nlOffset; - float iirOffset = iir.process(nlOffset); + pastIncrement = nlOffset - pastNlOffset; + pastNlOffset = nlOffset; + iirOffset = iir.process(nlOffset); static float maxOffset=0; static float pastIirOffset=0; if( calls > 0 ) { maxOffset=fabsf(iirOffset) > fabsf(maxOffset) ? iirOffset : maxOffset; pid.setError(iirOffset); - float correction=pid.getOutput()*0.0001; + float correction=pid.getOutput(); static float oldSamplingRate=44100; - printf("%10.3f, %10.3f, %10.3f, %10.3f, %10.3f, %10.3f\n", - offset, nlOffset, iirOffset, iirOffset - pastIirOffset, correction, oldSamplingRate); //unfiltered, filtered + if( (calls&0) == 0){ + printf("%10.3f, %10.3f, %10.3f, %10.3f, %10.3f, %10.3f, %10.3f, %10.3f, %10.3f, %10.3f, %10.3f\n", + offset, nlOffset, iirOffset, iirOffset - pastIirOffset, + pid.getProportionalGain(), pid.getIntegralGain(), pid.getDerivativeGain(), pid.getGlobalGain(), + correction, pid.getIntegralError(), oldSamplingRate); //unfiltered, filtered + } pastIirOffset = iirOffset; float targetSamplingRate; + // Applit PID + targetSamplingRate = oldSamplingRate - correction; + if(targetSamplingRate> 44110) //clip the Pid ! + targetSamplingRate = 44110; + if (targetSamplingRate < 44090) + targetSamplingRate = 44090; + + // or use a naive hysteresis comparator static int direction=1; float thresholdL = -1; -// targetSamplingRate = oldSamplingRate - correction; float thresholdH = 3; - targetSamplingRate = 44100; if( (iirOffset > thresholdH && direction == 1) || @@ -330,7 +321,7 @@ (iirOffset < -thresholdL && direction == 1) ) { - iirOffset = 44102; + iirOffset = 44103; direction = 1; } #ifndef USE_JUCE @@ -362,7 +353,7 @@ if(isSlave()==true){ //printf("Waiting for a sync clockSync\n"); } else { //if this is master - usleep(30000); //this times (roughly) how often sync clockSyncs are being sent. + usleep(10000); //this times (roughly) how often sync clockSyncs are being sent. int ret=masterSendSync(); if(ret<=0) return -1; @@ -373,4 +364,9 @@ return 1; } - +float ClockSync::getOffset(){ + return offset; +} +float ClockSync::getIirOffset(){ + return iirOffset; +} diff -r e9c9404e3d1f -r 8f98b32d0e23 core/ClockSyncThread.cpp --- a/core/ClockSyncThread.cpp Tue Sep 22 04:10:07 2015 +0100 +++ b/core/ClockSyncThread.cpp Mon Oct 05 13:06:14 2015 +0100 @@ -23,7 +23,10 @@ stopThread(); #endif /* USE_JUCE */ } + +ClockSync* gClockSync; void ClockSyncThread::init(bool isSlave, int aPort, VirtualClock &aVirtualClock){ + gClockSync = &clockSync; setVirtualClock(aVirtualClock); listening=false; clockSync.init(isSlave, aPort, *virtualClock); diff -r e9c9404e3d1f -r 8f98b32d0e23 core/Pid.cpp --- a/core/Pid.cpp Tue Sep 22 04:10:07 2015 +0100 +++ b/core/Pid.cpp Mon Oct 05 13:06:14 2015 +0100 @@ -13,8 +13,9 @@ error = 0; differentialError = 0; kp = 5; - ki = 2; - kd = 0.8; + ki = 2;// 2; + kd = 0.8; //0.8; + gain = 1; idleTimeout = 0; timeoutCount = 0; output = 0; @@ -32,12 +33,37 @@ ts = 1; //TODO: this should be passed as a parameter, unless setError() gets always called at constant intervals updateIntegralError(); updateDifferentialError(); - output= kp * error + ki * integralError + kd * differentialError; + output = gain * (kp * error + ki * integralError + kd * differentialError); // printf("%f %f %f %f %f %f %f\n", output, kp, error, ki, integralError, kd, differentialError); return output; } float Pid::getOutput(){ return output; } - - +void Pid::setProportionalGain(float proportionalGain){ + kp = proportionalGain; +} +void Pid::setDerivativeGain(float derivativeGain){ + kd = derivativeGain; +} +void Pid::setIntegralGain(float integralGain){ + ki = integralGain; +} +void Pid::setGlobalGain(float globalGain){ + gain = globalGain; +} +float Pid::getProportionalGain(){ + return kp; +} +float Pid::getDerivativeGain(){ + return kd; +} +float Pid::getIntegralGain(){ + return ki; +} +float Pid::getGlobalGain(){ + return gain; +} +float Pid::getIntegralError(){ + return integralError; +} diff -r e9c9404e3d1f -r 8f98b32d0e23 core/VirtualClock.cpp --- a/core/VirtualClock.cpp Tue Sep 22 04:10:07 2015 +0100 +++ b/core/VirtualClock.cpp Mon Oct 05 13:06:14 2015 +0100 @@ -80,11 +80,13 @@ } void VirtualClock::addOffset(double periodOffset){ +// printf("Clock was : %f ", elapsedPeriods); elapsedPeriods-=periodOffset; if(elapsedPeriods<0){ printf("ERROR: periodOffset adjustment of %f resulted in elapsedPeriods=%f\n", periodOffset, elapsedPeriods); exit(1); } +// printf("and now is: %f after a correction of %f", elapsedPeriods, periodOffset); movingAverage.reset(); } double VirtualClock::getPeriod(){ diff -r e9c9404e3d1f -r 8f98b32d0e23 include/ClockSync.h --- a/include/ClockSync.h Tue Sep 22 04:10:07 2015 +0100 +++ b/include/ClockSync.h Mon Oct 05 13:06:14 2015 +0100 @@ -51,7 +51,10 @@ VirtualClock *virtualClock; void resetTs(); bool areTsValid(); - void processOffset(double offset); + void processOffset(); + float offset; + float nlOffset; + float iirOffset; Pid pid; public: ClockSync(){}; @@ -67,6 +70,9 @@ void setType(int clockSyncType); void setTimestamp(myClock_t timestamp); void print(); + float getOffset(); + float getIirOffset(); + /** * sends a clockSync without blocking, checks results and returns the timestamp * immediately after the clockSync has been sent or -1 if there was an error or timeout expired. diff -r e9c9404e3d1f -r 8f98b32d0e23 include/Pid.h --- a/include/Pid.h Tue Sep 22 04:10:07 2015 +0100 +++ b/include/Pid.h Mon Oct 05 13:06:14 2015 +0100 @@ -19,6 +19,7 @@ float kp; float ki; float kd; + float gain; float ts; int idleTimeout; int timeoutCount; @@ -29,6 +30,15 @@ float setError(float anError); void setIdleTimeout(int aIdleTimeout); float getOutput(); + void setProportionalGain(float proportionalGain); + void setDerivativeGain(float derivativeGain); + void setIntegralGain(float integralGain); + void setGlobalGain(float globalGain); + float getProportionalGain(); + float getDerivativeGain(); + float getIntegralGain(); + float getGlobalGain(); + float getIntegralError(); }; diff -r e9c9404e3d1f -r 8f98b32d0e23 projects/scope/render.cpp --- a/projects/scope/render.cpp Tue Sep 22 04:10:07 2015 +0100 +++ b/projects/scope/render.cpp Mon Oct 05 13:06:14 2015 +0100 @@ -4,6 +4,7 @@ #include #include #include +#include float gPhase1, gPhase2; float gFrequency1, gFrequency2; @@ -56,7 +57,7 @@ gPhase1 = 0.0; gPhase2 = 0.0; - gFrequency1 = 200.0; + gFrequency1 = 441.0; gFrequency2 = 201.0; // testTime=BeagleRT_createAuxiliaryTask(testTimeFunction, 80, "testTimeTask"); @@ -67,7 +68,8 @@ // Input and output are given from the audio hardware and the other // ADCs and DACs (if available). If only audio is available, numMatrixFrames // will be 0. - +extern Pid* gClockSyncPid; +extern ClockSync* gClockSync; void render(BeagleRTContext *context, void *userData) { virtualClock.sync(context->audioFrames); @@ -101,12 +103,32 @@ float phaseInc=gFrequency1/44100.0*2*M_PI; // rt_printf("phaseInc: %f, phase: %f\n",phaseInc,phase); for(unsigned int n=0; naudioFrames; n++){ - context->audioOut[n*2]=sinf(phase);//context->audioIn[n*2]; - phase+=200.0/44100.0*2*M_PI; + context->audioOut[n*2]=sinf(phase) > 0 ? 1 : -1; + context->audioOut[n*2+1]=sinf(phase) > 0 ? 1 : -1; +// context->audioOut[n*2]= context->audioIn[n*2]; +// context->audioOut[n*2+1]= context->audioIn[n*2+1]; + phase+=phaseInc; if(phase>=2*M_PI) phase-=2*M_PI; - context->audioOut[n*2+1]=rand()/(float)RAND_MAX;//context->audioIn[n*2]; +// context->audioOut[n*2+1]=rand()/(float)RAND_MAX;//context->audioIn[n*2]; } + gClockSyncPid->setProportionalGain(context->analogIn[0]*10); +// gClockSyncPid->setIntegralGain(context->analogIn[1]); + gClockSyncPid->setIntegralGain(0); +// gClockSyncPid->setDerivativeGain(context->analogIn[2]); + gClockSyncPid->setDerivativeGain(0); + gClockSyncPid->setGlobalGain(context->analogIn[3]*1/0.828); + for (int n = 0; n < context->analogFrames; n++){ + context->analogOut[n*context->analogChannels + 0] = (gClockSync->getOffset() )/ 100 + 0.5; + context->analogOut[n*context->analogChannels + 1] = (gAudioCodec->getAudioSamplingRate()-44100)/20 + 0.5; + } + static bool pastSwitch = false; + bool thisSwitch = context->analogIn[4]>0.5; + if(thisSwitch && pastSwitch == false){ + rt_printf("--------------reset offset\n"); + virtualClock.addOffset(gClockSync->getIirOffset()); + } + pastSwitch = thisSwitch; count++; /* // if((count&262143)==0){