Index: trunk/FACT++/src/feedback.cc =================================================================== --- trunk/FACT++/src/feedback.cc (revision 12878) +++ trunk/FACT++/src/feedback.cc (revision 12879) @@ -63,5 +63,6 @@ kIdle, kTemp, - kFeedback + kFeedback, + kFeedbackGlobal }; @@ -129,182 +130,128 @@ } - void ResetData() - { - fData.clear(); - fData.resize(500); - fCursor = 0; - fStartTime = Time(); - - fSP = valarray(0., 416); + void HandleCameraTemp() + { + if (fCameraTemp.getSize()!=60*sizeof(float)) + return; + + const float *ptr = static_cast(fCameraTemp.getData()); + + double avg = 0; + int num = 0; + for (int i=1; i<32; i++) + if (ptr[i]!=0) + { + avg += ptr[i]; + num++; + } + + if (num==0) + return; + + avg /= num; + + + const float diff = (avg-25)*4./70 + fBiasOffset; vector vec(2*BIAS::kNumChannels); + for (int i=0; igetSize()==4 ? curr->getInt() : 0; - return; - } - - if (curr==&fCameraTemp) - { - if (curr->getSize()!=60*sizeof(float)) - return; - - const float *ptr = static_cast(curr->getData()); - - double avg = 0; - int num = 0; - for (int i=1; i<32; i++) - if (ptr[i]!=0) - { - avg += ptr[i]; - num++; - } - - if (num==0) - return; - - avg /= num; - - - const float diff = (avg-25)*4./70 + fBiasOffset; - - vector vec(2*BIAS::kNumChannels); - for (int i=0; i(fBiasA.getData()); + + for (int i=0; i<416; i++) + if (ptr[i]>0) { - Info("Sending correction to feedback."); - - DimClient::sendCommandNB((char*)"BIAS_CONTROL/SET_GAPD_REFERENCE_OFFSET", - (void*)&diff, sizeof(float)); + fCurrentsAvg[i] += ptr[i]; + fCurrentsRms[i] += ptr[i]*ptr[i]; } - } - - if (curr==&fBiasA && fControlType==kTemp && GetCurrentState()==kStateCalibrating) - { - if (curr->getSize()!=416*sizeof(int16_t)) - return; - - if (fStatusBias.second==BIAS::kRamping) - return; - - const int16_t *ptr = static_cast(curr->getData()); - - for (int i=0; i<416; i++) - if (ptr[i]>0) - { - fCurrentsAvg[i] += ptr[i]; - fCurrentsRms[i] += ptr[i]*ptr[i]; - } - - if (++fCursor<100) - { - DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0); - return; - } - - fCalibration.resize(416*2); - for (int i=0; i<416; i++) - { - fCalibration[i] = double(fCurrentsAvg[i])/fCursor; - fCalibration[i+416] = sqrt(double(fCurrentsRms[i])/fCursor-fCalibration[i]*fCalibration[i]); - } - - fDimCalibration.Update(fCalibration); - - fOutputEnabled = false; - fControlType = kIdle; - + + if (++fCursor<100) + { DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0); - } - - if (curr==&fBiasData && fControlType==kFeedback) - { - if (curr->getSize()!=1440*sizeof(float)) - return; - - // -------- Check age of last stored event -------- - - // Must be called in this order - const int tsec = curr->getTimestamp(); - const int tms = curr->getTimestampMillisecs(); - - const Time tm(tsec, tms*1000); - - if (Time()-fBiasLast>boost::posix_time::seconds(30)) - { - Warn("Last received event data older than 30s... resetting average calculation."); - ResetData(); - } - fBiasLast = tm; - - // -------- Store new event -------- - - fData[fCursor%fData.size()].assign(reinterpret_cast(curr->getData()), - reinterpret_cast(curr->getData())+1440); - - - fCursor++; - - if (fCursor med(1440); - - for (int ch=0; ch<1440; ch++) - { - vector arr(fData.size()); - for (size_t i=0; iboost::posix_time::seconds(30)) + { + Warn("Last received event data older than 30s... resetting average calculation."); + ResetData(); + } + fBiasLast = tm; + + // -------- Store new event -------- + + fData[fCursor%fData.size()].assign(reinterpret_cast(fBiasData.getData()), + reinterpret_cast(fBiasData.getData())+1440); + + if (++fCursor med(1440); + + for (int ch=0; ch<1440; ch++) + { + vector arr(fData.size()); + for (size_t i=0; i med(1440); vector rms(1440); @@ -322,167 +269,319 @@ */ - vector avg(BIAS::kNumChannels); - vector num(BIAS::kNumChannels); - for (int i=0; i<1440; i++) - { - const PixelMapEntry &ch = fMap.hw(i); - - // FIXME: Add a consistency check if the median makes sense... - // FIXME: Add a consistency check to remove pixels with bright stars (median?) - - avg[ch.hv()] += med[i]; - num[ch.hv()]++; - } - - for (int i=0; i0 ? fT : (tm0-fStartTime).total_microseconds()/1000000.; - const double T10 = fT21; - fT21 = T21; - - fStartTime = tm0; - - ostringstream out; - out << "New " << fData.size() << " event received: " << fCursor << " / " << setprecision(3) << T21 << "s"; - Info(out); - - if (fPV[0].size()==0) - { - fPV[0].resize(avg.size()); - fPV[0] = valarray(avg.data(), avg.size()); - } - else - if (fPV[1].size()==0) - { - fPV[1].resize(avg.size()); - fPV[1] = valarray(avg.data(), avg.size()); - } - else - if (fPV[2].size()==0) - { - fPV[2].resize(avg.size()); - fPV[2] = valarray(avg.data(), avg.size()); - } - else - { - fPV[0] = fPV[1]; - fPV[1] = fPV[2]; - - fPV[2].resize(avg.size()); - fPV[2] = valarray(avg.data(), avg.size()); - - if (T10<=0 || T21<=0) - return; - - cout << "Calculating (" << fCursor << ":" << T21 << ")... " << endl; - - // fKi[j] = response[j]*gain; - // Kp = 0; - // Kd = 0; - - // => Kp = 0.01 * gain = 0.00005 - // => Ki = 0.8 * gain/20s = 0.00025 - // => Kd = 0.1 * gain/20s = 0.00003 - - fKp = 0; - fKd = 0; - fKi = 0.00003*20; - T21 = 1; - - //valarray correction = - Kp*(PV[2] - PV[1]) + Ki * dT * (SP-PV[2]) - Kd/dT * (PV[2] - 2*PV[1] + PV[0]); - //valarray correction = - // - Kp * (PV[2] - PV[1]) - // + dT * Ki * (SP - PV[2]) - // - Kd / dT * (PV[2] - 2*PV[1] + PV[0]); - // - // - (Kp+Kd/dT1) * (PV[2] - PV[1]) - // + dT2 * Ki * (SP - PV[2]) - // + Kd / dT1 * (PV[1] - PV[0]); - // - // - Kp * (PV[2] - PV[1]) - // + Ki * (SP - PV[2])*dT - // - Kd * (PV[2] - PV[1])/dT - // + Kd * (PV[1] - PV[0])/dT; - // - //valarray correction = - // - Kp*(PV[2] - PV[1]) + Ki * T21 * (SP-PV[2]) - Kd*(PV[2]-PV[1])/T21 - Kd*(PV[0]-PV[1])/T01; - const valarray correction = fGain/1000* - ( - - (fKp+fKd/T21)*(fPV[2] - fPV[1]) - + fKi*T21*(fSP-fPV[2]) - + fKd/T10*(fPV[1]-fPV[0]) - ); - - /* - integral = 0 - start: - integral += (fSP - fPV[2])*dt - - output = Kp*(fSP - fPV[2]) + Ki*integral - Kd*(fPV[2] - fPV[1])/dt - - wait(dt) - - goto start - */ - - vector vec(2*BIAS::kNumChannels); - for (int i=0; i avg(BIAS::kNumChannels); + vector num(BIAS::kNumChannels); + for (int i=0; i<1440; i++) + { + const PixelMapEntry &ch = fMap.hw(i); + + // FIXME: Add a consistency check if the median makes sense... + // FIXME: Add a consistency check to remove pixels with bright stars (median?) + + avg[ch.hv()] += med[i]; + num[ch.hv()]++; + } + + for (int i=0; i0) + return; + + // FIXME: Take out broken / dead boards. + + const Time tm0 = Time(); + + /*const*/ double T21 = fT>0 ? fT : (tm0-fStartTime).total_microseconds()/1000000.; + const double T10 = fT21; + fT21 = T21; + + fStartTime = tm0; + + ostringstream out; + out << "New " << fData.size() << " event received: " << fCursor << " / " << setprecision(3) << T21 << "s"; + Info(out); + + if (fPV[0].size()==0) + { + fPV[0].resize(avg.size()); + fPV[0] = valarray(avg.data(), avg.size()); + return; + } + + if (fPV[1].size()==0) + { + fPV[1].resize(avg.size()); + fPV[1] = valarray(avg.data(), avg.size()); + return; + } + + if (fPV[2].size()==0) + { + fPV[2].resize(avg.size()); + fPV[2] = valarray(avg.data(), avg.size()); + return; + } + + fPV[0] = fPV[1]; + fPV[1] = fPV[2]; + + fPV[2].resize(avg.size()); + fPV[2] = valarray(avg.data(), avg.size()); + + if (T10<=0 || T21<=0) + return; + + //cout << "Calculating (" << fCursor << ":" << T21 << ")... " << endl; + + // fKi[j] = response[j]*gain; + // Kp = 0; + // Kd = 0; + + // => Kp = 0.01 * gain = 0.00005 + // => Ki = 0.8 * gain/20s = 0.00025 + // => Kd = 0.1 * gain/20s = 0.00003 + + /* + fKp = 0; + fKd = 0; + fKi = 0.00003*20; + T21 = 1; + */ + + //valarray correction = - Kp*(PV[2] - PV[1]) + Ki * dT * (SP-PV[2]) - Kd/dT * (PV[2] - 2*PV[1] + PV[0]); + //valarray correction = + // - Kp * (PV[2] - PV[1]) + // + dT * Ki * (SP - PV[2]) + // - Kd / dT * (PV[2] - 2*PV[1] + PV[0]); + // + // - (Kp+Kd/dT1) * (PV[2] - PV[1]) + // + dT2 * Ki * (SP - PV[2]) + // + Kd / dT1 * (PV[1] - PV[0]); + // + // - Kp * (PV[2] - PV[1]) + // + Ki * (SP - PV[2])*dT + // - Kd * (PV[2] - PV[1])/dT + // + Kd * (PV[1] - PV[0])/dT; + // + //valarray correction = + // - Kp*(PV[2] - PV[1]) + Ki * T21 * (SP-PV[2]) - Kd*(PV[2]-PV[1])/T21 - Kd*(PV[0]-PV[1])/T01; + const valarray correction = 1./fGain/1000* + ( + - (fKp+fKd/T21)*(fPV[2] - fPV[1]) + + fKi*T21*(fSP-fPV[2]) + + fKd/T10*(fPV[1]-fPV[0]) + ); + + /* + integral = 0 + start: + integral += (fSP - fPV[2])*dt + + output = Kp*(fSP - fPV[2]) + Ki*integral - Kd*(fPV[2] - fPV[1])/dt + + wait(dt) + + goto start + */ + + vector vec(2*BIAS::kNumChannels); + for (int i=0; i arr(reinterpret_cast(fBiasData.getData()), + reinterpret_cast(fBiasData.getData())+1440); + + sort(arr.begin(), arr.end()); + + const float med = arr[arr.size()/2]; + + fData[fCursor%fData.size()].resize(1); //assign(&med, &med); + fData[fCursor%fData.size()][0] = med; //assign(&med, &med); + + if (++fCursor(&avg, 1); + return; + } + + if (fPV[1].size()==0) + { + fPV[1].resize(1); + fPV[1] = valarray(&avg, 1); + return; + } + + if (fPV[2].size()==0) + { + fPV[2].resize(1); + fPV[2] = valarray(&avg, 1); + return; + } + + fPV[0] = fPV[1]; + fPV[1] = fPV[2]; + + fPV[2].resize(1); + fPV[2] = valarray(&avg, 1); + + const double T21 = 1; // feedback is 1s + const double T10 = 1; // feedback is 20s + + // => Kp = 0.01 * gain = 0.00005 + // => Ki = 0.8 * gain/20s = 0.00025 + // => Kd = 0.1 * gain/20s = 0.00003 + + /* + fKp = 0; + fKd = 0; + fKi = 0.00003*20; + */ + + // correction = (fSP[0]-fPV[2])*fKi + const valarray correction = 1./fGain/1000* + ( + - (fKp+fKd/T21)*(fPV[2] - fPV[1]) + + fKi*T21*(fSP[0]-fPV[2]) + + fKd/T10*(fPV[1]-fPV[0]) + ); + + Out() << "Correction: " << correction[0] << "V (" << fSP[0] << ")" << endl; + + const int nch = BIAS::kNumChannels; + + // FIXME: Sanity check! + + vector vec; + vec.reserve(2*nch); + vec.insert(vec.begin(), nch, fPV[2][0]-fSP[0]); + vec.insert(vec.begin()+nch, nch, correction[0]); + + fDimDeviation.Update(vec); + + if (!fOutputEnabled || fStatusBias.second!=BIAS::kVoltageOn) + return; + + Info("Sending global correction to feedback."); + DimClient::sendCommandNB((char*)"BIAS_CONTROL/ADD_REFERENCE_VOLTAGES", + (void*)(vec.data()+416), 416*sizeof(float)); + } + + void infoHandler() + { + DimInfo *curr = getInfo(); // get current DimInfo address + if (!curr) + return; + + if (curr==&fBias) + { + fStatusBias = GetNewState(fBias); + return; + } + + if (curr==&fFAD) + { + fStatusFAD = GetNewState(fFAD); + return; + } + + if (curr==&fFSC) + { + fStatusFSC = GetNewState(fFSC); + return; + } + + if (curr==&fDim) + { + fStatusDim = GetNewState(fDim); + fStatusDim.second = curr->getSize()==4 ? curr->getInt() : 0; + return; + } + + if (curr==&fCameraTemp) + HandleCameraTemp(); + + if (curr==&fBiasA && fControlType==kTemp && GetCurrentState()==kStateCalibrating) + HandleCalibration(); + + if (curr==&fBiasData && fControlType==kFeedback) + HandleFeedback(); + + if (curr==&fBiasData && fControlType==kFeedbackGlobal) + HandleGlobalFeedback(); } @@ -574,4 +673,24 @@ } + void ResetData() + { + fData.clear(); + fData.resize(500); + fCursor = 0; + fStartTime = Time(); + + fSP = valarray(0., 416); + + vector vec(2*BIAS::kNumChannels); + fDimDeviation.Update(vec); + + fPV[0].resize(0); + fPV[1].resize(0); + fPV[2].resize(0); + + if (fKp==0 && fKi==0 && fKd==0) + Warn("Control loop parameters are all set to zero."); + } + int StartFeedback() { @@ -579,4 +698,14 @@ fControlType = kFeedback; + + return GetCurrentState(); + } + + int StartFeedbackGlobal() + { + ResetData(); + fData.resize(5); + + fControlType = kFeedbackGlobal; return GetCurrentState(); @@ -587,11 +716,11 @@ if (!CheckEventSize(evt.GetSize(), "StartTempCtrl", 4)) return kSM_FatalError; + + fBiasOffset = evt.GetFloat(); + fControlType = kTemp; ostringstream out; out << "Starting temperature feedback with an offset of " << fBiasOffset << "V"; Message(out); - - fBiasOffset = evt.GetFloat(); - fControlType = kTemp; return GetCurrentState(); @@ -648,4 +777,6 @@ vec[i] = fSP[i] = val; fDimReference.Update(vec); + + Out() << "New global reference value: " << val << "mV" << endl; return GetCurrentState(); @@ -736,5 +867,5 @@ } */ - if (fControlType==kFeedback) + if (fControlType==kFeedback || fControlType==kFeedbackGlobal) return fOutputEnabled ? kStateFeedbackCtrlRunning : kStateFeedbackCtrlIdle; @@ -809,4 +940,8 @@ ("Start the feedback control loop"); + AddEvent("START_GLOBAL_FEEDBACK", kStateConnected) + (bind(&StateMachineFeedback::StartFeedbackGlobal, this)) + ("Start the global feedback control loop"); + AddEvent("START_TEMP_CONTROL", "F:1", kStateConnected) (bind(&StateMachineFeedback::StartTempCtrl, this, placeholders::_1)) @@ -874,19 +1009,20 @@ } - // -110 / -110 (-23 DAC / -0.51V) - // Reference voltage: -238 / -203 - // -360 / -343 ( 23 DAC / 0.51V) - - // 0.005 A/V - // 220 Amplitude / 1V - - // Gain = 1V / 200 = 0.005 - - fGain = 5; // (BIAS)V / (DRS)V ( 1V / 0.22V ) + fGain = 0.1; // V(Amplitude) / V(Bias) + + // 148 -> 248 + + // 33 : 10s < 2% + // 50 : 5s < 2% + // 66 : 3s < 2% + // 85 : 2s < 2% fKp = 0; fKd = 0; - fKi = 0.12; + fKi = 0.66; fT = 1; + + // Is that independent of the aboslute real amplitude of + // the light pulser? ostringstream msg; @@ -897,5 +1033,5 @@ else msg << ""; - msg << ", Gain(BIAS/DRS)=" << fGain << "V/V"; + msg << ", Gain(DRS/BIAS)=" << fGain << "V/V"; Message(msg);