Index: /trunk/FACT++/src/feedback.cc
===================================================================
--- /trunk/FACT++/src/feedback.cc	(revision 12398)
+++ /trunk/FACT++/src/feedback.cc	(revision 12399)
@@ -56,4 +56,5 @@
         kStateTempCtrlRunning,
         kStateFeedbackCtrlRunning,
+        kStateCalibrating,
     };
 
@@ -80,4 +81,5 @@
     DimStampedInfo fFSC;
     DimStampedInfo fBias;
+    DimStampedInfo fBiasA;
 
     DimStampedInfo fBiasData;
@@ -86,4 +88,8 @@
     DimDescribedService fDimReference;
     DimDescribedService fDimDeviation;
+    DimDescribedService fDimCalibration;
+
+    vector<int64_t>  fCurrentsAvg;
+    vector<int64_t>  fCurrentsRms;
 
     vector<vector<float>> fData;
@@ -175,10 +181,8 @@
         if (curr==&fCameraTemp)
         {
-            if (curr->getSize()==0)
-                return;
             if (curr->getSize()!=60*sizeof(float))
                 return;
 
-            const float *ptr = (float*)curr->getData();
+            const float *ptr = static_cast<float*>(curr->getData());
 
             double avg = 0;
@@ -215,11 +219,44 @@
                                          (void*)&diff, sizeof(float));
             }
-            return;
+        }
+
+        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<int16_t*>(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;
+            }
+
+            vector<float> val(2*416, 0);
+            for (int i=0; i<416; i++)
+            {
+                val[i]      = double(fCurrentsAvg[i])/fCursor;
+                val[i+416] = sqrt(double(fCurrentsRms[i])/fCursor-val[i]*val[i]);
+            }
+
+            fDimCalibration.Update(val);
+
+            fOutputEnabled = false;
+            fControlType = kIdle;
         }
 
         if (curr==&fBiasData && fControlType==kFeedback)
         {
-            if (curr->getSize()==0)
-                return;
             if (curr->getSize()!=1440*sizeof(float))
                 return;
@@ -443,36 +480,4 @@
 
             }
-
-            /*
-            vector<float> correction(416);
-            vector<float> response(416);
-            vector<float> dev(416);
-
-            double gain = 0;
-
-            for (int j=0; j<416; j++)
-            {
-                //avg[j] /= fData.size();
-                //rms[j] /= sqrt((rms[j]*fData.size() - avg[j]*avg[j]))/fData.size();
-
-                dev[j] = avg[j] - target[j];
-
-                // Determine correction from response maxtrix and change voltages
-                correction[j] = -dev[j]*response[j]*gain;
-
-                if (correction[j]==0)
-                    continue;
-
-                // Limit voltage steps // Limit changes to 100 mV
-//                if (fabs(correction[j]) > 0.1)
-//                    correction[j] = 0.1*fabs(correction[j])/correction[j];
-
-                // Add voltage change command if not too noisy
-//                if (fabs(Average[i]) > 2*Sigma[i])
-//                    Cmd << fIDTable[i] << " " << std::showpos << Correction/Multiplicity[i] << " ";
-
-            }
-            */
-            return;
         }
 
@@ -624,4 +629,23 @@
     }
 
+    int CalibrateCurrents()
+    {
+//        if (!CheckEventSize(evt.GetSize(), "StartTempCtrl", 4))
+//            return kSM_FatalError;
+
+        ostringstream out;
+        out << "Starting temperature feedback with an offset of -2V";
+        Message(out);
+
+        fBiasOffset = -2;
+        fControlType = kTemp;
+        fCursor = 0;
+        fCurrentsAvg.assign(416, 0);
+        fCurrentsRms.assign(416, 0);
+        fStartTime = Time();
+        fOutputEnabled = true;
+
+        return kStateCalibrating;
+    }
 
     int Execute()
@@ -649,6 +673,46 @@
             return kStateConnecting;
 
+        if (GetCurrentState()==kStateCalibrating && fCursor<100)
+            return GetCurrentState();
+
+/*
         // All subsystems are connected
-
+        if (GetCurrentStatus()==kStateConfiguringStep1)
+        {
+            if (fCursor<1)
+                return kStateConfiguringStep1;
+
+            if (fCursor==1)
+            {
+                fStartTime = Time();
+                return kStateConfiguringStep2;
+            }
+        }
+        if (GetCurrentStatus()==kStateConfiguringStep2)
+        {
+            if (fCursor==1)
+            {
+                if ((Time()-fStartTime).total_microseconds()/1000000.<1.5)
+                    return kStateConfiguringStep2;
+
+                Dim::SendCommand("BIAS_CONTROL/REQUEST_STATUS");
+            }
+            if (fCursor==2)
+            {
+
+                int n=0;
+                double avg = 0;
+                for (size_t i=0; i<fCurrents.size(); i++)
+                    if (fCurrents[i]>=0)
+                    {
+                        avg += fCurrents[i];
+                        n++;
+                    }
+
+                cout << avg/n << endl;
+            }
+            return kStateConnected;
+        }
+*/
         if (fControlType==kFeedback)
             return fOutputEnabled ? kStateFeedbackCtrlRunning : kStateFeedbackCtrlIdle;
@@ -669,8 +733,10 @@
         fFSC("FSC_CONTROL/STATE",       (void*)NULL, 0, this),
         fBias("BIAS_CONTROL/STATE",     (void*)NULL, 0, this),
+        fBiasA("BIAS_CONTROL/CURRENT",  (void*)NULL, 0, this),
         fBiasData("FAD_CONTROL/FEEDBACK_DATA", (void*)NULL, 0, this),
         fCameraTemp("FSC_CONTROL/TEMPERATURE", (void*)NULL, 0, this),
         fDimReference("FEEDBACK/REFERENCE", "F:416",        ""),
         fDimDeviation("FEEDBACK/DEVIATION", "F:416;F:416",  ""),
+        fDimCalibration("FEEDBACK/CALIBRATION", "F:416;F:416",  ""),
         fKp(0), fKi(0), fKd(0), fT(-1), fOutputEnabled(false)
     {
@@ -707,4 +773,7 @@
                      "Temperature control activated and voltage output enabled.");
 
+        AddStateName(kStateCalibrating, "Calibrating",
+                     "Calibrating current offsets.");
+
         AddEvent("START_FEEDBACK_CONTROL", kStateConnected)
             (bind(&StateMachineFeedback::StartFeedback, this))
@@ -713,5 +782,6 @@
         AddEvent("START_TEMP_CONTROL", "F:1", kStateConnected)
             (bind(&StateMachineFeedback::StartTempCtrl, this, placeholders::_1))
-            ("Start the temperature control loop");
+            ("Start the temperature control loop"
+             "|offset[V]:Offset from the nominal temperature corrected value in Volts");
 
         // kStateTempCtrlIdle, kStateFeedbackCtrlIdle, kStateTempCtrlRunning, kStateFeedbackCtrlRunning
@@ -747,4 +817,8 @@
             (bind(&StateMachineFeedback::SetConstant, this, placeholders::_1, 3))
             ("Set time-constant. (-1 to use the cycle time, i.e. the time for the last average cycle, instead)");
+
+        AddEvent("CALIBRATE_CURRENTS", kStateConnected)//, kStateIdle)
+            (bind(&StateMachineFeedback::CalibrateCurrents, this))
+            ("");
 
         // Verbosity commands