Index: /trunk/FACT++/src/feedback.cc
===================================================================
--- /trunk/FACT++/src/feedback.cc	(revision 17172)
+++ /trunk/FACT++/src/feedback.cc	(revision 17173)
@@ -10,4 +10,5 @@
 #include "Console.h"
 #include "externals/PixelMap.h"
+#include "externals/Interpolator2D.h"
 
 #include "tools.h"
@@ -32,5 +33,4 @@
 
     bool fIsVerbose;
-    bool fEnableOldAlgorithm;
 
     DimVersion fDim;
@@ -52,4 +52,5 @@
     vector<float>    fVoltGapd;     // Nominal breakdown voltage + 1.1V
     vector<float>    fBiasVolt;     // Output voltage as reported by bias crate (voltage between R10 and R8)
+    vector<float>    fBiasR9;       // 
     vector<uint16_t> fBiasDac;      // Dac value corresponding to the voltage setting
 
@@ -64,5 +65,8 @@
 
     double fUserOffset;
-    double fTempOffset;
+    vector<double> fTempOffset;
+    float fTempOffsetAvg;
+    float fTempOffsetRms;
+    double fTempCoefficient;
     double fTemp;
 
@@ -109,4 +113,5 @@
         {
             fVoltGapd.assign(evt.Ptr<float>(), evt.Ptr<float>()+416);
+            fBiasR9.assign(evt.Ptr<float>()+2*416, evt.Ptr<float>()+3*416);
             Info("Nominal bias voltages and calibration resistor received.");
         }
@@ -131,5 +136,5 @@
     int HandleCameraTemp(const EventImp &evt)
     {
-        if (!CheckEventSize(evt.GetSize(), "HandleCameraTemp", 60*sizeof(float)))
+        if (!CheckEventSize(evt.GetSize(), "HandleCameraTemp", 323*sizeof(float)))
         {
             fTimeTemp = Time(Time::none);
@@ -137,26 +142,18 @@
         }
 
-        const float *ptr = evt.Ptr<float>();
-
-        double avgt = 0;
-        int    numt = 0;
-        for (int i=1; i<32; i++)
-            if (ptr[i]!=0)
-            {
-                avgt += ptr[i];
-                numt++;
-            }
-
-        if (numt==0)
-        {
-            Warn("Received sensor temperatures all invalid.");
-            return GetCurrentState();
-        }
-
-        avgt /= numt; // [deg C]
-
-        fTimeTemp   = evt.GetTime();
-        fTempOffset = (avgt-25)*0.0561765; // [V] From Hamamatsu datasheet
-        fTemp       = avgt;
+        //fTempOffset = (avgt-25)*0.0561765; // [V] From Hamamatsu datasheet
+        //fTempOffset = (avgt-25)*0.05678; // [V] From Hamamatsu datasheet plus our own measurement (gein vs. temperature)
+
+        const float *ptr = evt.Ptr<float>(4);
+
+        fTimeTemp = evt.GetTime();
+        fTemp     = evt.Get<float>(321*4);
+
+        fTempOffsetAvg = (fTemp-25)*fTempCoefficient;
+        fTempOffsetRms =  evt.Get<float>(322*4)*fTempCoefficient;
+
+        fTempOffset.resize(320);
+        for (int i=0; i<320; i++)
+            fTempOffset[i] = (ptr[i]-25)*fTempCoefficient;
 
         return GetCurrentState();
@@ -183,8 +180,8 @@
         for (int i=0; i<BIAS::kNumChannels; i++)
         {
-            avg[i] = double(fCurrentsAvg[i])/fCursorCur * conv;
-            rms[i] = double(fCurrentsRms[i])/fCursorCur * conv * conv;
-
-            rms[i] = sqrt(rms[i]-avg[i]*avg[i]);
+            avg[i]  = double(fCurrentsAvg[i])/fCursorCur * conv;
+            rms[i]  = double(fCurrentsRms[i])/fCursorCur * conv * conv;
+            rms[i] -= avg[i]*avg[i];
+            rms[i]  = rms[i]<0 ? 0 : sqrt(rms[i]);
         }
 
@@ -235,5 +232,5 @@
         memcpy(cal.Irms, rms.data(),       416*sizeof(float));
 
-        fDimCalibration2.setData(fCalibration);
+        fDimCalibration2.setData(cal);
         fDimCalibration2.Update(fTimeCalib);
 
@@ -405,10 +402,12 @@
 
         data.Unom   = overvoltage;
-        data.dUtemp = fTempOffset;
+        data.dUtemp = fTempOffsetAvg;
 
         vector<float> vec(416);
 
+        /*
         if (fEnableOldAlgorithm)
         {
+            // ================================= old =======================
             // Pixel  583: 5 31 == 191 (5)  C2 B3 P3
             // Pixel  830: 2  2 ==  66 (4)  C0 B8 P1
@@ -445,5 +444,5 @@
 
                 // Offset induced by the voltage above the calibration point
-                const double Ubd = fVoltGapd[i] + fTempOffset;
+                const double Ubd = fVoltGapd[i] + fTempOffsets[i];
                 const double U0  = Ubd + overvoltage - fCalibVoltage[5][i]; // appliedOffset-fCalibrationOffset;
                 const double dI  = U0/Ravg[i]; // [V/Ohm]
@@ -455,7 +454,4 @@
                 // Make sure that the averaged resistor is valid
                 const double dU = Ravg[i]>10000 ? r*(I*1e-6 - dI) : 0;
-
-                if (i==2)
-                    cout << setprecision(4)<< dU << endl;;
 
                 vec[i] = Ubd + overvoltage + dU;
@@ -488,141 +484,145 @@
             }
         }
-        else
-        {
-            /* ================================= new ======================= */
-
-            for (int i=0; i<320/*BIAS::kNumChannels*/; i++)
+        */
+
+        for (int i=0; i<320/*BIAS::kNumChannels*/; i++)
+        {
+            const PixelMapEntry &hv = fMap.hv(i);
+            if (!hv)
+                continue;
+
+            // Number of G-APDs in this patch
+            const int N = hv.count();
+
+            // Average measured ADC value for this channel
+            const double adc = Imes[i]/* * (5e-3/4096)*/; // [A]
+
+            // Current through ~100 Ohm measurement resistor
+            const double I8 = (adc-fCalibDeltaI[i])*fCalibR8[i]/100;
+
+            // Current through calibration resistors (R9)
+                // This is uncalibrated, biut since the corresponding calibrated
+            // value I8 is subtracted, the difference should yield a correct value
+            const double I9 = fBiasDac[i] * (1e-3/4096);//U9/R9;   [A]
+
+            // Current in R4/R5 branch
+            const double Iout = I8>I9 ? I8 - I9 : 0;
+
+            // Applied voltage at calibration resistors, according to biasctrl
+            const double U9 = fBiasVolt[i];
+
+            // Serial resistors (one 1kOhm at the output of the bias crate, one 1kOhm in the camera)
+            const double R4 = 2000;
+
+            // Serial resistor of the individual G-APDs
+            double R5 = 3900./N;
+
+            // This is assuming that the broken pixels have a 390 Ohm instead of 3900 Ohm serial resistor
+            if (i==66)                 // Pixel 830(66)
+                R5 = 300;              // 2400 = 1/(3/3900 + 1/390)
+            if (i==191 || i==193)      // Pixel 583(191) / Pixel 1401(193)
+                R5 = 390/1.4;          // 379 = 1/(4/3900 + 1/390)
+
+            // The measurement resistor
+            const double R8 = 100;
+
+            // Total resistance of branch with diodes (R4+R5)
+            // Assuming that the voltage output of the OpAMP is linear
+            // with the DAC setting and not the voltage at R9, the
+            // additional voltage drop at R8 must be taken into account
+            const double R = R4 + R5 + R8;
+
+            // For the patches with a broken resistor - ignoring the G-APD resistance -
+            // we get:
+            //
+            // I[R=3900] =  Iout *      1/(10+(N-1))   = Iout        /(N+9)
+            // I[R= 390] =  Iout * (1 - 1/ (10+(N-1))) = Iout * (N+8)/(N+9)
+            //
+            // I[R=390] / I[R=3900] = N+8
+            //
+            // Udrp = Iout*3900/(N+9) + Iout*1000 + Iout*1000 = Iout * R
+
+            // Voltage drop in R4/R5 branch (for the G-APDs with correct resistor)
+            const double Udrp = R*Iout;
+
+            // Nominal breakdown voltage with correction for temperature dependence
+            const double Ubd = fVoltGapd[i] + fTempOffset[i];
+
+            // Current overvoltage (at a G-APD with the correct 3900 Ohm resistor)
+            const double Uov = U9-Udrp-Ubd>0 ? U9-Udrp-Ubd : 0;
+
+            // Iout linear with U9 above Ubd
+            //
+            //  Rx = (U9-Ubd)/Iout
+            //  I' = (U9'-Ubd) / Rx
+            //  Udrp' = R*I'
+            //  Uov = U9' - Udrp' - Ubd
+            //  Uov = overvoltage
+            //
+            //  overvoltage = U9' - Udrp' - Ubd
+            //  overvoltage = U9' - R*I' - Ubd
+            //  overvoltage = U9' - R*((U9'-Ubd)/Rx) - Ubd
+            //  overvoltage = U9' - U9'*R/Rx + Ubd*R/Rx - Ubd
+            //  overvoltage = U9'*(1 - R/Rx) + Ubd*R/Rx - Ubd
+            //  overvoltage - Ubd*R/Rx +Ubd = U9'*(1 - R/Rx)
+            //  U9' = [ overvoltage - Ubd*R/Rx +Ubd ] / (1 - R/Rx)
+            //
+
+            // The current through one G-APD is the sum divided by the number of G-APDs
+            // (assuming identical serial resistors)
+            double Iapd = Iout/N;
+
+            // In this and the previosu case we neglect the resistance of the G-APDs, but we can make an
+            // assumption: The differential resistance depends more on the NSB than on the PDE,
+            // thus it is at least comparable for all G-APDs in the patch. In addition, although the
+            // G-APD with the 390Ohm serial resistor has the wrong voltage applied, this does not
+            // significantly influences the ohmic resistor or the G-APD because the differential
+            // resistor is large enough that the increase of the overvoltage does not dramatically
+            // increase the current flow as compared to the total current flow.
+            if (i==66 || i==191 || i==193)
+                Iapd = Iout/(N+9); // Iapd = R5*Iout/3900;
+
+            // The differential resistance of the G-APD, i.e. the dependence of the
+            // current above the breakdown voltage, is given by
+            //const double Rapd = Uov/Iapd;
+            // This allows us to estimate the current Iov at the overvoltage we want to apply
+            //const double Iov = overvoltage/Rapd;
+
+            // Estimate set point for over-voltage (voltage drop at the target point)
+            //const double Uset = Ubd + overvoltage + R*Iov*N;
+            const double Uset = Uov<0.3 ? Ubd + overvoltage + Udrp : Ubd + overvoltage + Udrp*pow(overvoltage/Uov, 1.66);
+
+            // Voltage set point
+            vec[i] = Uset;
+
+            /*
+             if (fDimBias.state()==BIAS::State::kVoltageOn && GetCurrentState()==Feedback::State::kInProgress &&
+             fabs(Uov-overvoltage)>0.033)
+             cout << setprecision(4) << setw(3) << i << ": Uov=" << Uov << " Udrp=" << Udrp << " Iapd=" << Iapd*1e6 << endl;
+             */
+
+            // Calculate statistics only for channels with a valid calibration
+            //if (Uov>0)
             {
-                const PixelMapEntry &hv = fMap.hv(i);
-                if (!hv)
-                    continue;
-
-                // Number of G-APDs in this patch
-                const int N = hv.count();
-
-                // Average measured ADC value for this channel
-                const double adc = Imes[i]/* * (5e-3/4096)*/; // [A]
-
-                // Current through ~100 Ohm measurement resistor
-                const double I8 = (adc-fCalibDeltaI[i])*fCalibR8[i]/100;
-
-                // Applied voltage at calibration resistors, according to biasctrl
-                const double U9 = fBiasVolt[i];
-
-                // Current through calibration resistors (R9)
-                // This is uncalibrated, biut since the corresponding calibrated
-                // value I8 is subtracted, the difference should yield a correct value
-                const double I9 = fBiasDac[i] * (1e-3/4096);//U9/R9;   [A]
-
-                // Current in R4/R5 branch
-                const double Iout = I8>I9 ? I8 - I9 : 0;
-
-                // Serial resistors (one 1kOhm at the output of the bias crate, one 1kOhm in the camera)
-                const double R4 = 2000;
-
-                // Serial resistor of the individual G-APDs
-                double R5 = 3900./N;
-
-                // This is assuming that the broken pixels have a 390 Ohm instead of 3900 Ohm serial resistor
-                if (i==66)                 // Pixel 830(66)
-                    R5 = 300;              // 2400 = 1/(3/3900 + 1/390)
-                if (i==191 || i==193)      // Pixel 583(191) / Pixel 1401(193)
-                    R5 = 390/1.4;          // 379 = 1/(4/3900 + 1/390)
-
-                // Total resistance of branch with diodes
-                const double R = R4+R5;
-
-                // For the patches with a broken resistor - ignoring the G-APD resistance -
-                // we get:
-                //
-                // I[R=3900] =  Iout *      1/(10+(N-1))   = Iout        /(N+9)
-                // I[R= 390] =  Iout * (1 - 1/ (10+(N-1))) = Iout * (N+8)/(N+9)
-                //
-                // I[R=390] / I[R=3900] = N+8
-                //
-                // Udrp = Iout*3900/(N+9) + Iout*1000 + Iout*1000 = Iout * R
-
-                // Voltage drop in R4/R5 branch (for the G-APDs with correct resistor)
-                const double Udrp = R*Iout;
-
-                // Nominal breakdown voltage with correction for temperature dependence
-                const double Ubd = fVoltGapd[i] + fTempOffset;
-
-                // Current overvoltage (at a G-APD with the correct 3900 Ohm resistor)
-                const double Uov = U9-Udrp-Ubd>0 ? U9-Udrp-Ubd : 0;
-
-                // Iout linear with U9 above Ubd
-                //
-                //  Rx = (U9-Ubd)/Iout
-                //  I' = (U9'-Ubd) / Rx
-                //  Udrp' = R*I'
-                //  Uov = U9' - Udrp' - Ubd
-                //  Uov = overvoltage
-                //
-                //  overvoltage = U9' - Udrp' - Ubd
-                //  overvoltage = U9' - R*I' - Ubd
-                //  overvoltage = U9' - R*((U9'-Ubd)/Rx) - Ubd
-                //  overvoltage = U9' - U9'*R/Rx + Ubd*R/Rx - Ubd
-                //  overvoltage = U9'*(1 - R/Rx) + Ubd*R/Rx - Ubd
-                //  overvoltage - Ubd*R/Rx +Ubd = U9'*(1 - R/Rx)
-                //  U9' = [ overvoltage - Ubd*R/Rx +Ubd ] / (1 - R/Rx)
-                //
-
-                // The current through one G-APD is the sum divided by the number of G-APDs
-                // (assuming identical serial resistors)
-                double Iapd = Iout/N;
-
-                // In this and the previosu case we neglect the resistance of the G-APDs, but we can make an
-                // assumption: The differential resistance depends more on the NSB than on the PDE,
-                // thus it is at least comparable for all G-APDs in the patch. In addition, although the
-                // G-APD with the 390Ohm serial resistor has the wrong voltage applied, this does not
-                // significantly influences the ohmic resistor or the G-APD because the differential
-                // resistor is large enough that the increase of the overvoltage does not dramatically
-                // increase the current flow as compared to the total current flow.
-                if (i==66 || i==191 || i==193)
-                    Iapd = Iout/(N+9); // Iapd = R5*Iout/3900;
-
-                // The differential resistance of the G-APD, i.e. the dependence of the
-                // current above the breakdown voltage, is given by
-                //const double Rapd = Uov/Iapd;
-                // This allows us to estimate the current Iov at the overvoltage we want to apply
-                //const double Iov = overvoltage/Rapd;
-
-                // Estimate set point for over-voltage (voltage drop at the target point)
-                //const double Uset = Ubd + overvoltage + R*Iov*N;
-                const double Uset = Uov<0.3 ? Ubd + overvoltage + Udrp : Ubd + overvoltage + Udrp*pow(overvoltage/Uov, 1.66);
-
-                // Voltage set point
-                vec[i] = Uset;
-
-                if (fDimBias.state()==BIAS::State::kVoltageOn && GetCurrentState()==Feedback::State::kInProgress &&
-                    fabs(Uov-overvoltage)>0.033)
-                    cout << setprecision(4) << setw(3) << i << ": Uov=" << Uov << " Udrp=" << Udrp << " Iapd=" << Iapd*1e6 << endl;
-
-
-                // Calculate statistics only for channels with a valid calibration
-                //if (Uov>0)
-                {
-                    const int g = hv.group();
-
-                    med[g][num[g]] = Uov;
-                    avg[g] += Uov;
-                    num[g]++;
-
-                    if (Uov<min[g])
-                        min[g] = Uov;
-                    if (Uov>max[g])
-                        max[g] = Uov;
-
-                    const double iapd = Iapd*1e6; // A --> uA
-
-                    data.I[i]  = iapd;
-                    data.Iavg += iapd;
-                    data.Irms += iapd*iapd;
-
-                    data.Uov[i] = Uov;
-
-                    med[2][num[2]++] = iapd;
-                }
+                const int g = hv.group();
+
+                med[g][num[g]] = Uov;
+                avg[g] += Uov;
+                num[g]++;
+
+                if (Uov<min[g])
+                    min[g] = Uov;
+                if (Uov>max[g])
+                    max[g] = Uov;
+
+                const double iapd = Iapd*1e6; // A --> uA
+
+                data.I[i]  = iapd;
+                data.Iavg += iapd;
+                data.Irms += iapd*iapd;
+
+                data.Uov[i] = Uov;
+
+                med[2][num[2]++] = iapd;
             }
         }
@@ -638,5 +638,5 @@
 
                 ostringstream msg;
-                msg << setprecision(4) << "Sending new absolute offset: dU(" << fTemp << "degC)=" << fTempOffset << "V, Unom=" << overvoltage << "V, Uov=" << (num[0]+num[1]>0?(avg[0]+avg[1])/(num[0]+num[1]):0);
+                msg << setprecision(4) << "Sending new absolute offset: dU(" << fTemp << "degC)=" << fTempOffsetAvg << "V+-" << fTempOffsetRms << ", Unom=" << overvoltage << "V, Uov=" << (num[0]+num[1]>0?(avg[0]+avg[1])/(num[0]+num[1]):0);
                 Info(msg);
             }
@@ -647,5 +647,5 @@
             {
                 ostringstream msg;
-                msg << setprecision(4) << "Current status: dU(" << fTemp << "degC)=" << fTempOffset << "V, Unom=" << overvoltage << "V, Uov=" << (num[0]+num[1]>0?(avg[0]+avg[1])/(num[0]+num[1]):0);
+                msg << setprecision(4) << "Current status: dU(" << fTemp << "degC)=" << fTempOffsetAvg << "V+-" << fTempOffsetRms << ", Unom=" << overvoltage << "V, Uov=" << (num[0]+num[1]>0?(avg[0]+avg[1])/(num[0]+num[1]):0);
                 Info(msg);
             }
@@ -687,11 +687,12 @@
             data.Iavg /= num[2];
             data.Irms /= num[2];
+            data.Irms -= data.Iavg*data.Iavg;
 
             data.N = num[2];
-            data.Irms = sqrt(data.Irms-data.Iavg*data.Iavg);
+            data.Irms = data.Irms<0 ? 0: sqrt(data.Irms);
 
             sort(med[2].data(), med[2].data()+num[2]);
 
-            data.Imed = num[2]%2 ? (med[2][num[2]/2-1]+med[2][num[2]/2])/2 : med[2][num[2]/2];
+            data.Imed = num[2]%2 ? med[2][num[2]/2] : (med[2][num[2]/2-1]+med[2][num[2]/2])/2;
 
             for (int i=0; i<num[2]; i++)
@@ -768,14 +769,4 @@
     }
 
-    int EnableOldAlgorithm(const EventImp &evt)
-    {
-        if (!CheckEventSize(evt.GetSize(), "EnableOldAlgorithm", 1))
-            return kSM_FatalError;
-
-        fEnableOldAlgorithm = evt.GetBool();
-
-        return GetCurrentState();
-    }
-
     int SetVerbosity(const EventImp &evt)
     {
@@ -795,5 +786,5 @@
         fCurrentRequestInterval = evt.GetUShort();
 
-        Out() << "New current request interval: " << fCurrentRequestInterval << "ms" << endl;
+        Info("New current request interval: "+to_string(fCurrentRequestInterval)+"ms");
 
         return GetCurrentState();
@@ -901,5 +892,5 @@
 public:
     StateMachineFeedback(ostream &out=cout) : StateMachineDim(out, "FEEDBACK"),
-        fIsVerbose(false), fEnableOldAlgorithm(true),
+        fIsVerbose(false), 
         //---
         fDimFSC("FSC_CONTROL"),
@@ -953,5 +944,5 @@
         Subscribe("BIAS_CONTROL/NOMINAL")
             (bind(&StateMachineFeedback::HandleBiasNom,     this, placeholders::_1));
-        Subscribe("FSC_CONTROL/TEMPERATURE")
+        Subscribe("FSC_CONTROL/BIAS_TEMP")
             (bind(&StateMachineFeedback::HandleCameraTemp,  this, placeholders::_1));
 
@@ -998,8 +989,4 @@
 
 
-        AddEvent("ENABLE_OLD_ALRGORITHM", "B:1", Feedback::State::kConnected, Feedback::State::kCalibrated)
-            (bind(&StateMachineFeedback::EnableOldAlgorithm, this, placeholders::_1));
-
-
         AddEvent("PRINT")
             (bind(&StateMachineFeedback::Print, this))
@@ -1029,4 +1016,5 @@
         fNumCalibIgnore         = conf.Get<uint16_t>("num-calib-ignore");
         fNumCalibRequests       = conf.Get<uint16_t>("num-calib-average");
+        fTempCoefficient        = conf.Get<double>("temp-coefficient");
 
         return -1;
@@ -1053,4 +1041,5 @@
         ("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")
+        ("temp-coefficient",    var<double>()->required(), "Temp. coefficient [V/K]")
         ;
 
