Index: trunk/FACT++/src/feedback.cc =================================================================== --- trunk/FACT++/src/feedback.cc (revision 13059) +++ trunk/FACT++/src/feedback.cc (revision 13060) @@ -54,9 +54,11 @@ kStateConnectedFAD, kStateConnected, - kStateTempCtrlIdle, - kStateFeedbackCtrlIdle, - kStateTempCtrlRunning, - kStateFeedbackCtrlRunning, - kStateCalibrating, + kStateTempCtrlIdle, // 7 + kStateFeedbackCtrlIdle, // 8 + kStateCurrentCtrlIdle, // 9 + kStateTempCtrlRunning, // 9->10 + kStateFeedbackCtrlRunning, // 10->11 + kStateCurrentCtrlRunning, // 12 + kStateCalibrating, // 11->13 }; @@ -66,5 +68,6 @@ kTemp, kFeedback, - kFeedbackGlobal + kFeedbackGlobal, + kCurrents, }; @@ -100,5 +103,6 @@ vector> fData; - uint64_t fCursor; + uint64_t fCursorCur; + uint64_t fCursorAmpl; Time fBiasLast; @@ -117,4 +121,6 @@ double fBiasOffset; + + uint16_t fCurrentRequestInterval; bool fOutputEnabled; @@ -160,6 +166,25 @@ vec[i+416] = diff; - if (fControlType!=kTemp) - return; + if (fControlType==kCurrents) + { + if (fCursorCur==0) + { + DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0); + return; + } + + // Convert from DAC counts to uA + const double conv = 5000e-6/4096; + for (int i=0; i(fBiasA.getData()); + + for (int i=0; i<416; i++) + { + fCurrentsAvg[i] += ptr[i]; + fCurrentsRms[i] += ptr[i]*ptr[i]; + } + + fCursorCur++; + + return true; + } + + void HandleCalibration() { - if (fBiasA.getSize()!=416*sizeof(int16_t)) - return; - - if (fStatusBias.second==BIAS::kRamping) - return; - - const int16_t *ptr = static_cast(fBiasA.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) + const int rc = AverageCurrents(); + if (rc<0) + return; + + if (fCursorCur<100) { DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0); return; } + + if (rc==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]); + fCalibration[i] = double(fCurrentsAvg[i])/fCursorCur; + fCalibration[i+416] = sqrt(double(fCurrentsRms[i])/fCursorCur-fCalibration[i]*fCalibration[i]); } @@ -235,8 +274,8 @@ // -------- Store new event -------- - fData[fCursor%fData.size()].assign(reinterpret_cast(fBiasData.getData()), - reinterpret_cast(fBiasData.getData())+1440); - - if (++fCursor(fBiasData.getData()), + reinterpret_cast(fBiasData.getData())+1440); + + if (++fCursorAmpl0) + if (fCursorAmpl%fData.size()>0) return; @@ -322,5 +361,5 @@ ostringstream out; - out << "New " << fData.size() << " event received: " << fCursor << " / " << setprecision(3) << T21 << "s"; + out << "New " << fData.size() << " event received: " << fCursorAmpl << " / " << setprecision(3) << T21 << "s"; Info(out); @@ -441,8 +480,8 @@ 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); - // => 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; - */ + // ----- Calculate average currents ----- + + vector A(416); + for (int i=0; i<416; i++) + A[i] = double(fCurrentsAvg[i]) / fCursorCur; + + fCurrentsAvg.assign(416, 0); + fCursorCur = 0; + + // -------- Calculate correction -------- // correction = (fSP[0]-fPV[2])*fKi @@ -531,5 +571,6 @@ const valarray correction = 1./fGain/1000* ( - fKi*(pow(fSP[0], 1./1.6)-pow(fPV[2], 1./1.6)) + //fKi*(pow(fSP[0], 1./1.6)-pow(fPV[2], 1./1.6)) + fKi*(fSP[0]-fPV[2]) ); @@ -588,9 +629,12 @@ } - if (curr==&fCameraTemp) + if (curr==&fCameraTemp && (fControlType==kTemp || fControlType==kCurrents)) HandleCameraTemp(); if (curr==&fBiasA && fControlType==kTemp && GetCurrentState()==kStateCalibrating) HandleCalibration(); + + if (curr==&fBiasA && (fControlType==kFeedbackGlobal || fControlType==kCurrents)) + AverageCurrents(); if (curr==&fBiasData && fControlType==kFeedback) @@ -692,5 +736,7 @@ fData.assign(n>0 ? n : fData.size(), vector(0)); - fCursor = 0; + fCursorAmpl = 0; + fCursorCur = 0; + fStartTime = Time(); @@ -705,4 +751,7 @@ fPV[2].resize(0); + fCurrentsAvg.assign(416, 0); + fCurrentsRms.assign(416, 0); + if (fKp==0 && fKi==0 && fKd==0) Warn("Control loop parameters are all set to zero."); @@ -743,4 +792,31 @@ ostringstream out; out << "Starting temperature feedback with an offset of " << fBiasOffset << "V"; + Message(out); + + DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0); + + return GetCurrentState(); + } + + int StartCurrentCtrl(const EventImp &evt) + { + if (!CheckEventSize(evt.GetSize(), "StartCurrentCtrl", 4)) + return kSM_FatalError; + + if (fCalibration.size()==0) + { + ostringstream out; + out << "Current control needs a bias crate calibration first... command ignored."; + Error(out); + return GetCurrentState(); + } + + ResetData(0); + + fBiasOffset = evt.GetFloat(); + fControlType = kCurrents; + + ostringstream out; + out << "Starting current/temp feedback with an offset of " << fBiasOffset << "V"; Message(out); @@ -815,5 +891,5 @@ fBiasOffset = -2; fControlType = kTemp; - fCursor = 0; + fCursorCur = 0; fCurrentsAvg.assign(416, 0); fCurrentsRms.assign(416, 0); @@ -823,4 +899,16 @@ return kStateCalibrating; + } + + int SetCurrentRequestInterval(const EventImp &evt) + { + if (!CheckEventSize(evt.GetSize(), "SetCurrentRequestInterval", 2)) + return kSM_FatalError; + + fCurrentRequestInterval = evt.GetUShort(); + + Out() << "New current request interval: " << fCurrentRequestInterval << "ms" << endl; + + return GetCurrentState(); } @@ -901,8 +989,20 @@ if (fControlType==kTemp) { - if (GetCurrentState()==kStateCalibrating && fCursor<100) + if (GetCurrentState()==kStateCalibrating && fCursorCur<100) return GetCurrentState(); return fOutputEnabled ? kStateTempCtrlRunning : kStateTempCtrlIdle; + } + if (fControlType==kCurrents) + { + /* + static Time past; + if (fCurrentRequestInterval>0 && Time()-past>boost::posix_time::milliseconds(fCurrentRequestInterval)) + { + DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0); + past = Time(); + }*/ + + return fOutputEnabled ? kStateCurrentCtrlRunning : kStateCurrentCtrlIdle; } } @@ -938,8 +1038,10 @@ fDimCalibration("FEEDBACK/CALIBRATION", "F:416;F:416", "Current offsets" - "|Avg[nA]:Average offset" - "|Rms[nA]:Rms of offset"), + "|Avg[dac]:Average offset (5000uA/4096dac)" + "|Rms[dac]:Rms of offset (5000uA/4096dac)"), fSP(416), - fKp(0), fKi(0), fKd(0), fT(-1), fOutputEnabled(false) + fKp(0), fKi(0), fKd(0), fT(-1), + fCurrentRequestInterval(0), + fOutputEnabled(false) { // ba::io_service::work is a kind of keep_alive for the loop. @@ -971,14 +1073,15 @@ AddStateName(kStateFeedbackCtrlIdle, "FeedbackIdle", "Feedback control activated, but voltage output disabled."); - - AddStateName(kStateTempCtrlIdle, "FeedbackIdle", + AddStateName(kStateTempCtrlIdle, "TempCtrlIdle", "Temperature control activated, but voltage output disabled."); + AddStateName(kStateCurrentCtrlIdle, "CurrentCtrlIdle", + "Current control activated, but voltage output disabled."); AddStateName(kStateFeedbackCtrlRunning, "FeedbackControl", "Feedback control activated and voltage output enabled."); - AddStateName(kStateTempCtrlRunning, "TempControl", "Temperature control activated and voltage output enabled."); - + AddStateName(kStateCurrentCtrlRunning, "CurrentControl", + "Current/Temp control activated and voltage output enabled."); AddStateName(kStateCalibrating, "Calibrating", "Calibrating current offsets."); @@ -999,4 +1102,9 @@ "|offset[V]:Offset from the nominal temperature corrected value in Volts"); + AddEvent("START_CURRENT_CONTROL", "F:1", kStateConnectedFSC, kStateConnected) + (bind(&StateMachineFeedback::StartCurrentCtrl, this, placeholders::_1)) + ("Start the current/temperature control loop" + "|offset[V]:Offset from the nominal current/temperature corrected value in Volts"); + // kStateTempCtrlIdle, kStateFeedbackCtrlIdle, kStateTempCtrlRunning, kStateFeedbackCtrlRunning AddEvent("STOP") @@ -1035,4 +1143,8 @@ (bind(&StateMachineFeedback::CalibrateCurrents, this)) (""); + + AddEvent("SET_CURRENT_REQUEST_INTERVAL", kStateConnectedFSC, kStateConnected)//, kStateIdle) + (bind(&StateMachineFeedback::SetCurrentRequestInterval, this, placeholders::_1)) + ("|interval[ms]:Interval between two current requests in modes which need that."); // Verbosity commands @@ -1087,4 +1199,6 @@ Message(msg); + fCurrentRequestInterval = conf.Get("current-request-interval"); + return -1; } @@ -1106,4 +1220,5 @@ control.add_options() ("pixel-map-file", var("FACTmapV5a.txt"), "Pixel mapping file. Used here to get the default reference voltage.") + ("current-request-interval", var(1000), "Interval between two current requests.") ;