Index: trunk/FACT++/src/feedback.cc
===================================================================
--- trunk/FACT++/src/feedback.cc	(revision 13373)
+++ trunk/FACT++/src/feedback.cc	(revision 13453)
@@ -90,4 +90,5 @@
 
     DimStampedInfo fBiasData;
+    DimStampedInfo fBiasNom;
     DimStampedInfo fCameraTemp;
 
@@ -100,4 +101,5 @@
 
     vector<float>    fCalibration;
+    vector<float>    fVoltGapd;
 
     vector<vector<float>> fData;
@@ -122,4 +124,6 @@
 
     double fBiasOffset;
+    double fCalibrationOffset;
+    double fAppliedOffset;
 
     uint16_t fCurrentRequestInterval;
@@ -160,12 +164,25 @@
             return;
 
-        avgt /= numt;
-
-
-        const float diff = (avgt-25)*4./70 + fBiasOffset;
+        avgt /= numt; // [deg C]
+
+        const double dUt = (avgt-25)*4./70; // [V]
+
+        if (GetCurrentState()==kStateCalibrating && fBiasOffset>dUt-1.2)
+        {
+            ostringstream msg;
+            msg << " (applied calibration offset " << fBiasOffset << "V exceeds temperature correction " << avgt << "V - 1.2V.";
+            Warn("Trying to calibrate above G-APD breakdown volatge!");
+            Warn(msg);
+            return;
+        }
+
+        // FIXME: If calibrating do not wait for the temperature!
+        fAppliedOffset = fBiasOffset;
+        if (GetCurrentState()!=kStateCalibrating)
+            fAppliedOffset += dUt;
 
         vector<float> vec(2*BIAS::kNumChannels);
         for (int i=0; i<BIAS::kNumChannels; i++)
-            vec[i+BIAS::kNumChannels] = diff;
+            vec[i+BIAS::kNumChannels] = fAppliedOffset;
 
         double avg[2] = {   0,   0 };
@@ -186,8 +203,4 @@
             }
 
-            // Pixel  583: 5 31 == 191 (5)
-            // Pixel  830: 2  2 ==  66 (4)
-            // Pixel 1401: 6  1 == 193 (5)
-
             // Convert from DAC counts to uA
             const double conv = 5000e-6/4096;
@@ -195,4 +208,25 @@
             // 3900 Ohm/n + 1000 Ohm + 1100 Ohm  (with n=4 or n=5)
             const double R[2] = { 3075, 2870 };
+
+            const float *Iavg = fCalibration.data();                      // Offset at U=fCalibrationOffset
+            const float *Ravg = fCalibration.data()+BIAS::kNumChannels*2; // Measured resistance
+
+            // U0 = fCalibrationOffset
+            // dT = fAppliedVoltage
+
+            // Ifeedback = Im[i] - (U[i]-U0)/Ravg[i] - Iavg[i];
+            // dUapplied[i] + dUneu[i] = R[g] * (Im[i] - (dUapplied[i]+dUneu[i]-U0+dT)/Ravg[i] - Iavg[i])
+
+            // The assumption here is that the offset calculated from the temperature
+            // does not significanly change within a single step
+
+            // dU[i] := dUtotal[i] = dUapplied[i] + dUneu[i]
+            // dU[i] / R[g]               = Im[i] - (dU[i]+dT-U0)/Ravg[i] - Iavg[i]
+            // dU[i]/R[g] + dU[i]/Ravg[i] = Im[i] + U0/Ravg[i] - dT/Ravg[i] - Iavg[i]
+            // dU[i]*(1/R[g]+1/Ravg[i])   = Im[i] - Iavg[i] + U0/Ravg[i] - dT/Ravg[i]
+            // dU[i]   = (Im[i] - Iavg[i] + U0/Ravg[i] - dT/Ravg[i]) / (1/R[g]+1/Ravg[i])
+            // dU[i] = { Im[i] - Iavg[i] + (U0-dT)/Ravg[i] } * r    with   r := 1 / (1/R[g]+1/Ravg[i])
+
+            const double U0 = fCalibrationOffset-fAppliedOffset;
 
             for (int i=0; i<BIAS::kNumChannels; i++)
@@ -202,36 +236,44 @@
                     continue;
 
+                // Average measured current
+                const double Im = double(fCurrentsAvg[i])/fCursorCur; // [dac]
+
+                // Group index (0 or 1) of the of the pixel (4 or 5 pixel patch)
                 const int g = hv.group();
 
-                const double Im = double(fCurrentsAvg[i])/fCursorCur;
-                const double I  = Im>fCalibration[i] ? Im-fCalibration[i] : 0;
-
-                double U  = R[g] * I*conv;
-
-                // 510 / 390    1.30 ^1.66 =  1.55
-                // 470 / 380    1.23       =  1.41
-                // 450 / 360    1.25       =  1.45
-
-                // This is assuming that the broken pixels
-                // have a 1kOhm instead of 390 Ohm serial resistor
-                if (i==66)
-                    U *= 2400./R[0];    // (1k)2665 / (390)2400 / (~0)2100
-                if (i==191)
-                    U *= 2320./R[1];    // (1k)2794 / (390)2320 / (~0)2100
-                if (i==193)
-                    U *= 2320./R[1];    // (1k)2665 / (390)2400 / (~0)2100
-
-                vec[i+BIAS::kNumChannels] += U;
-
-                if (fCalibration[i]>0)
+                // Serial resistors in front of the G-APD
+                double Rg = R[g];
+
+                // This is assuming that the broken pixels have a 390 Ohm instead of 3900 Ohm serial resistor
+                if (i==66)                // Pixel 830(66)
+                    Rg = 2400;            // 2400 = (3/3900 + 1/390) + 1000 + 1100
+                if (i==191 || i==193)     // Pixel 583(191) / Pixel 1401(193)
+                    Rg = 2379;            // 2379 = (4/3900 + 1/390) + 1000 + 1100
+
+                const double r = 1./(1./Rg + 1./Ravg[i]); // [Ohm]
+
+                // Offset induced by the voltage above the calibration point
+                const double dI = U0/Ravg[i]; // [V/Ohm]
+
+                // Offset at the calibration point (make sure that the calibration is
+                // valid (Im[i]>Iavg[i]) and we operate above the calibration point)
+                const double I = Im>Iavg[i] ? (Im - Iavg[i])*conv : 0; // [A]
+
+                // Make sure that the averaged resistor is valid
+                const double dU = Ravg[i]>0 ? r*(I+dI) : 0;
+
+                vec[i+BIAS::kNumChannels] += dU;
+
+                // Calculate statistics only for channels with a valid calibration
+                if (Iavg[i]>0)
                 {
-                    med[g][num[g]] = U;
-                    avg[g] += U;
+                    med[g][num[g]] = dU;
+                    avg[g] += dU;
                     num[g]++;
 
-                    if (U<min[g])
-                        min[g] = U;
-                    if (U>max[g])
-                        max[g] = U;
+                    if (dU<min[g])
+                        min[g] = dU;
+                    if (dU>max[g])
+                        max[g] = dU;
                 }
             }
@@ -258,5 +300,5 @@
 
         ostringstream msg;
-        msg << setprecision(4) << "Sending new absolute offset (" << diff << "V+" << (num[0]+num[1]>0?(avg[0]+avg[1])/(num[0]+num[1]):0) << "V) to biasctrl.";
+        msg << setprecision(4) << "Sending new absolute offset (" << fAppliedOffset << "V+" << (num[0]+num[1]>0?(avg[0]+avg[1])/(num[0]+num[1]):0) << "V) to biasctrl.";
         Info(msg);
 
@@ -325,9 +367,17 @@
             return;
 
-        fCalibration.resize(BIAS::kNumChannels*2);
+        fCalibration.resize(BIAS::kNumChannels*3);
+
+        float *avg = fCalibration.data();
+        float *rms = fCalibration.data()+BIAS::kNumChannels;
+        float *res = fCalibration.data()+BIAS::kNumChannels*2;
+
         for (int i=0; i<BIAS::kNumChannels; i++)
         {
-            fCalibration[i]                    = double(fCurrentsAvg[i])/fCursorCur;
-            fCalibration[i+BIAS::kNumChannels] = sqrt(double(fCurrentsRms[i])/fCursorCur-fCalibration[i]*fCalibration[i]);
+            const double I = double(fCurrentsAvg[i])/fCursorCur;
+
+            res[i] = fVoltGapd[i]/I * 4096/5000e-6;
+            avg[i] = I;
+            rms[i] = sqrt(double(fCurrentsRms[i])/fCursorCur-I*I);
         }
 
@@ -720,4 +770,11 @@
         }
 
+        if (curr==&fBiasNom)
+        {
+            const float *ptr = reinterpret_cast<float*>(fBiasNom.getData());
+            fVoltGapd.assign(ptr, ptr+416);
+            return;
+        }
+
         if (curr==&fCameraTemp && (fControlType==kTemp || fControlType==kCurrents))
             HandleCameraTemp();
@@ -781,4 +838,10 @@
         }
 
+        const float *avg = fCalibration.data();
+        const float *rms = fCalibration.data()+BIAS::kNumChannels;
+        const float *res = fCalibration.data()+BIAS::kNumChannels*2;
+
+        Out() << "Average current at " << fCalibrationOffset << "V below G-APD operation voltage:\n";
+
         for (int k=0; k<13; k++)
             for (int j=0; j<8; j++)
@@ -786,7 +849,20 @@
                 Out() << setw(2) << k << "|" << setw(2) << j*4 << "|";
                 for (int i=0; i<4; i++)
-                    Out() << Tools::Form(" %6.1f+-%4.1f", fCalibration[k*32+j*4+i], fCalibration[k*32+j*4+i+BIAS::kNumChannels]);
-                Out() << endl;
+                    Out() << Tools::Form(" %6.1f+-%4.1f", avg[k*32+j*4+i], rms[k*32+j*4+i]);
+                Out() << '\n';
             }
+        Out() << '\n';
+
+        Out() << "Measured calibration resistor:\n";
+        for (int k=0; k<13; k++)
+            for (int j=0; j<4; j++)
+            {
+                Out() << setw(2) << k << "|" << setw(2) << j*8 << "|";
+                for (int i=0; i<8; i++)
+                    Out() << Tools::Form(" %5.0f", res[k*32+j*8+i]);
+                Out() << '\n';
+            }
+
+        Out() << flush;
 
         return GetCurrentState();
@@ -982,9 +1058,15 @@
         }
 
+        if (fVoltGapd.size()==0)
+        {
+            Error("No G-APD reference voltages received yet (BIAS_CONTROL/NOMINAL).");
+            return GetCurrentState();
+        }
+
         ostringstream out;
         out << "Starting temperature feedback for calibration with an offset of -2V";
         Message(out);
 
-        fBiasOffset = -2;
+        fBiasOffset = fCalibrationOffset;
         fControlType = kTemp;
         fCursorCur  = -fNumCalibIgnore;
@@ -1134,10 +1216,12 @@
                       "|DeltaAmpl[mV]:Amplitude offset measures"
                       "|DeltaBias[mV]:Correction value calculated"),
-        fDimCalibration("FEEDBACK/CALIBRATION", "F:416;F:416",
+        fDimCalibration("FEEDBACK/CALIBRATION", "F:416;F:416;F:416",
                         "Current offsets"
                         "|Avg[dac]:Average offset (5000uA/4096dac)"
                         "|Rms[dac]:Rms of offset (5000uA/4096dac)"),
+                        "|R[Ohm]:Measured calibration resistor",
         fSP(BIAS::kNumChannels),
         fKp(0), fKi(0), fKd(0), fT(-1),
+        fCalibrationOffset(-3),
         fCurrentRequestInterval(0),
         fNumCalibIgnore(30),
@@ -1300,6 +1384,7 @@
 
         fCurrentRequestInterval = conf.Get<uint16_t>("current-request-interval");
-        fNumCalibIgnore   = conf.Get<uint16_t>("num-calib-ignore");
-        fNumCalibRequests = conf.Get<uint16_t>("num-calib-average");
+        fNumCalibIgnore         = conf.Get<uint16_t>("num-calib-ignore");
+        fNumCalibRequests       = conf.Get<uint16_t>("num-calib-average");
+        fCalibrationOffset      = conf.Get<float>("calibration-offset");
 
         return -1;
@@ -1321,8 +1406,9 @@
     po::options_description control("Feedback options");
     control.add_options()
-        ("pixel-map-file",  var<string>("FACTmapV5a.txt"), "Pixel mapping file. Used here to get the default reference voltage.")
+        ("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.")
-        ("num-calib-ignore",  var<uint16_t>(30), "Number of current requests to be ignored before averaging")
-        ("num-calib-average",  var<uint16_t>(300), "Number of current requests to be averaged")
+        ("num-calib-ignore",    var<uint16_t>(30), "Number of current requests to be ignored before averaging")
+        ("num-calib-average",   var<uint16_t>(300), "Number of current requests to be averaged")
+        ("calibration-offset",  var<float>(-3), "Absolute offset relative to the G-APD operation voltage when calibrating")
         ;