Changeset 13060 for trunk

Timestamp:

03/11/12 13:10:02 (13 years ago)

Author:

tbretz

Message:

Implemented the possibility of a current feedback which corrects the applied voltage according to the measured currents.

File:

• trunk/FACT++/src/feedback.cc (modified) (26 diffs)

trunk/FACT++/src/feedback.cc

@@ -54 +54 @@
         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 +68 @@
         kTemp,
         kFeedback,
-        kFeedbackGlobal
+        kFeedbackGlobal,
+        kCurrents,
     };
 
@@ -100 +103 @@
     vector<vector<float>> fData;
 
-    uint64_t fCursor;
+    uint64_t fCursorCur;
+    uint64_t fCursorAmpl;
 
     Time fBiasLast;
@@ -117 +121 @@
 
     double fBiasOffset;
+
+    uint16_t fCurrentRequestInterval;
 
     bool fOutputEnabled;
@@ -160 +166 @@
             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<BIAS::kNumChannels; i++)
+            {
+                // 3900 Ohm/n  +  1000 Ohm  +  1200 Ohm
+                const double R = fMap.hv(i).group()==0 ? 3175 : 2980;
+                const double I = double(fCurrentsAvg[i])/fCursorCur - fCalibration[i];
+                vec[i+416] += R * I*conv;
+            }
+
+            fCurrentsAvg.assign(416, 0);
+            fCursorCur = 0;
+        }
 
         fDimDeviation.setQuality(fControlType);
@@ -175 +200 @@
     }
 
+    int AverageCurrents()
+    {
+        if (fBiasA.getSize()!=416*sizeof(int16_t))
+            return -1;
+
+        if (fStatusBias.second==BIAS::kRamping)
+            return false;
+
+        const int16_t *ptr = static_cast<int16_t*>(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<int16_t*>(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 +274 @@
         // -------- Store new event --------
 
-        fData[fCursor%fData.size()].assign(reinterpret_cast<float*>(fBiasData.getData()),
-                                           reinterpret_cast<float*>(fBiasData.getData())+1440);
-
-        if (++fCursor<fData.size())
+        fData[fCursorAmpl%fData.size()].assign(reinterpret_cast<float*>(fBiasData.getData()),
+                                               reinterpret_cast<float*>(fBiasData.getData())+1440);
+
+        if (++fCursorAmpl<fData.size())
             return;
 
@@ -308 +347 @@
         // CO(k)-CO(k-1) = - Kp[ PV(k) - PV(k-1) ] + Ki * T * (SP(k)-PV(k)) - Kd/T [ PV(k) - 2PV(k-1) + PV(k-2) ]
 
-        if (fCursor%fData.size()>0)
+        if (fCursorAmpl%fData.size()>0)
             return;
 
@@ -322 +361 @@
 
         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 +480 @@
         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<fData.size())
+        fData[fCursorAmpl%fData.size()].resize(1); //assign(&med, &med);
+        fData[fCursorAmpl%fData.size()][0] = med;  //assign(&med, &med);
+
+        if (++fCursorAmpl<fData.size())
             return;
 
@@ -464 +503 @@
         // -------- Calculate correction --------
 
-        if (fCursor%fData.size()!=0)
+        if (fCursorAmpl%fData.size()!=0)
             return;
 
@@ -504 +543 @@
         fPV[2] = valarray<double>(&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<float> 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 +571 @@
         const valarray<double> 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 +629 @@
         }
 
-        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 +736 @@
         fData.assign(n>0 ? n : fData.size(), vector<float>(0));
 
-        fCursor = 0;
+        fCursorAmpl = 0;
+        fCursorCur  = 0;
+
         fStartTime = Time();
 
@@ -705 +751 @@
         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 +792 @@
         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 +891 @@
         fBiasOffset = -2;
         fControlType = kTemp;
-        fCursor = 0;
+        fCursorCur = 0;
         fCurrentsAvg.assign(416, 0);
         fCurrentsRms.assign(416, 0);
@@ -823 +899 @@
 
         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 +989 @@
             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 +1038 @@
         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 +1073 @@
         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 +1102 @@
              "|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 +1143 @@
             (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 +1199 @@
         Message(msg);
 
+        fCurrentRequestInterval = conf.Get<uint16_t>("current-request-interval");
+
         return -1;
     }
@@ -1106 +1220 @@
     control.add_options()
         ("pixel-map-file",  var<string>("FACTmapV5a.txt"), "Pixel mapping file. Used here to get the default reference voltage.")
+        ("current-request-interval",  var<uint16_t>(1000), "Interval between two current requests.")
         ;