Index: /trunk/FACT++/src/feedback.cc
===================================================================
--- /trunk/FACT++/src/feedback.cc	(revision 17026)
+++ /trunk/FACT++/src/feedback.cc	(revision 17027)
@@ -1,3 +1,4 @@
 #include <valarray>
+#include <algorithm>
 
 #include "Dim.h"
@@ -8,5 +9,4 @@
 #include "Configuration.h"
 #include "Console.h"
-#include "Converter.h"
 #include "externals/PixelMap.h"
 
@@ -15,5 +15,4 @@
 #include "LocalControl.h"
 
-#include "HeadersFAD.h"
 #include "HeadersFSC.h"
 #include "HeadersBIAS.h"
@@ -30,74 +29,111 @@
 {
 private:
-    enum control_t
-    {
-        kIdle,
-        kTemp,
-        kFeedback,
-        kFeedbackGlobal,
-        kCurrents,
-        kCurrentsNew,
-    };
-
-    control_t fControlType;
-
     PixelMap fMap;
 
+    bool fIsVerbose;
+    bool fEnableOldAlgorithm;
+
     DimVersion fDim;
-    DimDescribedState fDimFAD;
+
     DimDescribedState fDimFSC;
     DimDescribedState fDimBias;
 
-    DimDescribedService fDimReference;
-    DimDescribedService fDimDeviation;
     DimDescribedService fDimCalibration;
+    DimDescribedService fDimCalibration2;
+    DimDescribedService fDimCalibrationR8;
     DimDescribedService fDimCurrents;
+
+    vector<float>    fCalibCurrentMes[6]; // Measured calibration current at six different levels
+    vector<float>    fCalibVoltage[6];    // Corresponding voltage as reported by biasctrl
 
     vector<int64_t>  fCurrentsAvg;
     vector<int64_t>  fCurrentsRms;
 
+    vector<float>    fVoltGapd;     // Nominal breakdown voltage + 1.1V
+    vector<float>    fBiasVolt;     // Output voltage as reported by bias crate (voltage between R10 and R8)
+    vector<uint16_t> fBiasDac;      // Dac value corresponding to the voltage setting
+
     vector<float>    fCalibration;
-    vector<float>    fVoltGapd;
-    vector<float>    fBiasVolt;
-
-    vector<vector<float>> fData;
+    vector<float>    fCalibDeltaI;
+    vector<float>    fCalibR8;
 
      int64_t fCursorCur;
-    uint64_t fCursorAmpl;
-    uint64_t fCursorTemp;
-
-    Time fBiasLast;
-    Time fStartTime;
-    Time fCalibTime;
-
-    valarray<double> fPV[3];  // Process variable (intgerated/averaged amplitudes)
-    valarray<double> fSP;     // Set point        (target amplitudes)
-
-    double fKp;    // Proportional constant
-    double fKi;    // Integral     constant
-    double fKd;    // Derivative   constant
-    double fT;     // Time         constant (cycle time)
-    double fGain;  // Gain (conversion from a DRS voltage deviation into a BIAS voltage change at G-APD reference voltage)
-
-    double fT21;
-
-    double fBiasOffset;
+
+    Time fTimeCalib;
+    Time fTimeTemp;
+
+    double fUserOffset;
     double fTempOffset;
-    double fCalibrationOffset;
-    double fAppliedOffset;
+    double fTemp;
 
     uint16_t fCurrentRequestInterval;
     uint16_t fNumCalibIgnore;
     uint16_t fNumCalibRequests;
-
-    bool fOutputEnabled;
+    uint16_t fCalibStep;
+
+    // ============================= Handle Services ========================
+
+    int HandleBiasStateChange()
+    {
+        if (fDimBias.state()==BIAS::State::kVoltageOn && GetCurrentState()==Feedback::State::kCalibrating)
+        {
+            Dim::SendCommandNB("BIAS_CONTROL/REQUEST_STATUS");
+            Info("Starting calibration step "+to_string(fCalibStep));
+        }
+
+        if (fDimBias.state()==BIAS::State::kVoltageOff && GetCurrentState()==Feedback::State::kInProgress)
+            return Feedback::State::kCalibrated;
+
+        return GetCurrentState();
+    }
+    // ============================= Handle Services ========================
+
+    bool CheckEventSize(size_t has, const char *name, size_t size)
+    {
+        if (has==size)
+            return true;
+
+        // Disconnected
+        if (has==0)
+            return false;
+
+        ostringstream msg;
+        msg << name << " - Received event has " << has << " bytes, but expected " << size << ".";
+        Fatal(msg);
+        return false;
+    }
+
+    int HandleBiasNom(const EventImp &evt)
+    {
+        if (evt.GetSize()>=416*sizeof(float))
+        {
+            fVoltGapd.assign(evt.Ptr<float>(), evt.Ptr<float>()+416);
+            Info("Nominal bias voltages and calibration resistor received.");
+        }
+
+        return GetCurrentState();
+    }
+
+    int HandleBiasVoltage(const EventImp &evt)
+    {
+        if (evt.GetSize()>=416*sizeof(float))
+            fBiasVolt.assign(evt.Ptr<float>(), evt.Ptr<float>()+416);
+        return GetCurrentState();
+    }
+
+    int HandleBiasDac(const EventImp &evt)
+    {
+        if (evt.GetSize()>=416*sizeof(uint16_t))
+            fBiasDac.assign(evt.Ptr<uint16_t>(), evt.Ptr<uint16_t>()+416);
+        return GetCurrentState();
+    }
 
     int HandleCameraTemp(const EventImp &evt)
     {
-        if (fControlType!=kTemp && fControlType!=kCurrents && fControlType!=kCurrentsNew)
-            return GetCurrentState();
-
-        if (evt.GetSize()!=60*sizeof(float))
-            return GetCurrentState();
+        if (!CheckEventSize(evt.GetSize(), "HandleCameraTemp", 60*sizeof(float)))
+        {
+            fTimeTemp = Time(Time::none);
+            return GetCurrentState();
+        }
 
         const float *ptr = evt.Ptr<float>();
@@ -120,60 +156,271 @@
         avgt /= numt; // [deg C]
 
-        fTempOffset = (avgt-25)*4./70; // [V]
-
-        fCursorTemp++;
-
-        return fControlType==kCurrentsNew ? HandleCurrentControlNew() : HandleCurrentControl();
-    }
-
-    int HandleCurrentControlNew()
-    {
-        if (GetCurrentState()==Feedback::State::kCalibrating && fBiasOffset>fTempOffset-1.2)
-        {
-            fCursorTemp = 0;
-
-            ostringstream msg;
-            msg << " (applied calibration offset " << fBiasOffset << "V exceeds temperature correction " << fTempOffset << "V - 1.2V.";
-            Warn("Trying to calibrate above G-APD breakdown volatge!");
-            Warn(msg);
-            return GetCurrentState();
-        }
+        fTimeTemp   = evt.GetTime();
+        fTempOffset = (avgt-25)*0.0561765; // [V] From Hamamatsu datasheet
+        fTemp       = avgt;
+
+        return GetCurrentState();
+    }
+
+    pair<vector<float>, vector<float>> AverageCurrents(const int16_t *ptr, int n)
+    {
+        if (fCursorCur++>=0)
+        {
+            for (int i=0; i<BIAS::kNumChannels; i++)
+            {
+                fCurrentsAvg[i] += ptr[i];
+                fCurrentsRms[i] += ptr[i]*ptr[i];
+            }
+        }
+
+        if (fCursorCur<n)
+            return make_pair(vector<float>(), vector<float>());
+
+        const double conv = 5e-3/4096;
+
+        vector<float> rms(BIAS::kNumChannels);
+        vector<float> avg(BIAS::kNumChannels);
+        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]);
+        }
+
+        return make_pair(avg, rms);
+    }
+
+    int HandleCalibration(const EventImp &evt)
+    {
+        const uint16_t dac = 256+512*fCalibStep; // Command value
+
+        // Only the channels which are no spare channels are ramped
+        if (std::count(fBiasDac.begin(), fBiasDac.end(), dac)!=320)
+            return GetCurrentState();
+
+        const auto rc = AverageCurrents(evt.Ptr<int16_t>(), fNumCalibRequests);
+        if (rc.first.size()==0)
+        {
+            Dim::SendCommandNB("BIAS_CONTROL/REQUEST_STATUS");
+            return GetCurrentState();
+        }
+
+        const vector<float> &avg = rc.first;
+        const vector<float> &rms = rc.second;
+
+        // Current through resistor R8
+        fCalibCurrentMes[fCalibStep] = avg;       // [A]
+        fCalibVoltage[fCalibStep]    = fBiasVolt; // [V]
+
+        // ------------------------- Update calibration data --------------------
+
+        struct cal_data
+        {
+            uint32_t dac;
+            float    U[416];
+            float    Iavg[416];
+            float    Irms[416];
+
+            cal_data() { memset(this, 0, sizeof(cal_data)); }
+        } __attribute__((__packed__));
+
+        cal_data cal;
+        cal.dac = dac;
+        memcpy(cal.U,    fBiasVolt.data(), 416*sizeof(float));
+        memcpy(cal.Iavg, avg.data(),       416*sizeof(float));
+        memcpy(cal.Irms, rms.data(),       416*sizeof(float));
+
+        fDimCalibration2.setData(fCalibration);
+        fDimCalibration2.Update(fTimeCalib);
+
+        // -------------------- Start next calibration steo ---------------------
+
+        if (++fCalibStep<6)
+        {
+            fCursorCur  = -fNumCalibIgnore;
+            fCurrentsAvg.assign(BIAS::kNumChannels, 0);
+            fCurrentsRms.assign(BIAS::kNumChannels, 0);
+
+            Dim::SendCommandNB("BIAS_CONTROL/SET_GLOBAL_DAC", uint32_t(256+512*fCalibStep));
+
+            return GetCurrentState();
+        }
+
+        // --------------- Calculate old style calibration ----------------------
+
+        fCalibration.resize(BIAS::kNumChannels*4);
+
+        float *pavg  = fCalibration.data();
+        float *prms  = fCalibration.data()+BIAS::kNumChannels;
+        float *pres  = fCalibration.data()+BIAS::kNumChannels*2;
+        float *pUmes = fCalibration.data()+BIAS::kNumChannels*3;
+
+        for (int i=0; i<BIAS::kNumChannels; i++)
+        {
+            const double I = fCalibCurrentMes[5][i]; // [A]
+            const double U = fBiasVolt[i];           // [V]
+
+            pavg[i]  = I*1e6;                        // [uA]
+            prms[i]  = rms[i]*1e6;                   // [uA]
+            pres[i]  = U/I;                          // [Ohm]
+            pUmes[i] = U;                            // [V]
+        }
+
+        fDimCalibration.setData(fCalibration);
+        fDimCalibration.Update(fTimeCalib);
+
+        // -------------------- New style calibration --------------------------
+
+        fCalibDeltaI.resize(BIAS::kNumChannels);
+        fCalibR8.resize(BIAS::kNumChannels);
+
+        // Linear regression of the values at 256+512*N for N={ 3, 4, 5 }
+        for (int i=0; i<BIAS::kNumChannels; i++)
+        {
+            // x: Idac
+            // y: Iadc
+
+            double x  = 0;
+            double y  = 0;
+            double xx = 0;
+            double xy = 0;
+
+            const int beg = 3;
+            const int end = 5;
+            const int len = end-beg+1;
+
+            for (int j=beg; j<=end; j++)
+            {
+                const double Idac = (256+512*j)*1e-3/4096;
+
+                x  += Idac;
+                xx += Idac*Idac;
+                y  += fCalibCurrentMes[j][i];
+                xy += fCalibCurrentMes[j][i]*Idac;
+            }
+
+            const double m1 = xy - x*y / len;
+            const double m2 = xx - x*x / len;
+
+            const double m = m2==0 ? 0 : m1/m2;
+
+            const double t = (y - m*x) / len;
+
+            fCalibDeltaI[i] = t;     // [A]
+            fCalibR8[i]     = 100/m; // [Ohm]
+        }
+
+        vector<float> v(BIAS::kNumChannels*2);
+        v.insert(v.end(), fCalibDeltaI.begin(), fCalibDeltaI.end());
+        v.insert(v.end(), fCalibR8.begin(),     fCalibR8.end());
+
+        fDimCalibrationR8.setData(v);
+        fDimCalibrationR8.Update(fTimeCalib);
+
+        // ---------------------------------------------------------------------
+
+        Info("Calibration successfully done.");
+        Dim::SendCommandNB("BIAS_CONTROL/SET_ZERO_VOLTAGE");
+
+        return Feedback::State::kCalibrated;
+    }
+
+    int HandleBiasCurrent(const EventImp &evt)
+    {
+        if (!CheckEventSize(evt.GetSize(), "HandleBiasCurrent", BIAS::kNumChannels*sizeof(uint16_t)))
+            return Feedback::State::kConnected;
+
+        if (GetCurrentState()<Feedback::State::kCalibrating)
+            return GetCurrentState();
+
+        // FIXME? Allow for calibrated currents also during ramping?
+        if ((GetCurrentState()!=Feedback::State::kWaitingForData || fDimBias.state()!=BIAS::State::kVoltageOff) &&
+            fDimBias.state()!=BIAS::State::kVoltageOn)
+            return GetCurrentState();
+
+        // ------------------------------- HandleCalibration -----------------------------------
+        if (GetCurrentState()==Feedback::State::kCalibrating)
+            return HandleCalibration(evt);
+
+        // ---------------------- Calibrated, WaitingForData, InProgress -----------------------
+
+        // We are waiting but no valid temperature yet, go on waiting
+        if (GetCurrentState()==Feedback::State::kWaitingForData &&
+            (!fTimeTemp.IsValid() || Time()-fTimeTemp>boost::posix_time::minutes(5)))
+            return GetCurrentState();
+
+        // We are already in progress but no valid temperature update anymore
+        if (GetCurrentState()==Feedback::State::kInProgress &&
+            (!fTimeTemp.IsValid() || Time()-fTimeTemp>boost::posix_time::minutes(5)))
+        {
+            Warn("Current control in progress, but last received temperature older than 5min... switching voltage off.");
+            Dim::SendCommandNB("BIAS_CONTROL/SET_ZERO_VOLTAGE");
+            return Feedback::State::kCalibrated;
+        }
+
+        // ---------------------- Calibrated, WaitingForData, InProgress -----------------------
+
+        const vector<float> &Imes = AverageCurrents(evt.Ptr<int16_t>(), 3).first;
+        if (Imes.size()==0)
+            return GetCurrentState();
+
+        fCurrentsAvg.assign(416, 0);
+        fCurrentsRms.assign(416, 0);
+        fCursorCur = 0;
+
+        // -------------------------------------------------------------------------------------
+
+        // Nominal overvoltage (w.r.t. the bias setup values)
+        const double overvoltage = GetCurrentState()<Feedback::State::kWaitingForData ? 0 : fUserOffset;
 
         double avg[2] = {   0,   0 };
         double min[2] = {  90,  90 };
         double max[2] = { -90, -90 };
-        int    num[2] = {   0,   0 };
-
-        vector<double> med[2];
+        int    num[3] = {   0,   0,   0 };
+
+        vector<double> med[3];
         med[0].resize(416);
         med[1].resize(416);
-
-        //const float *Ravg = fCalibration.data()+BIAS::kNumChannels*2; // Measured resistance
-
-        vector<float> vec(2*BIAS::kNumChannels+2);
-
-        vec[BIAS::kNumChannels*2]   = fTempOffset;
-        vec[BIAS::kNumChannels*2+1] = fBiasOffset;
-
-        float *Uoff = vec.data()+BIAS::kNumChannels;
-
-        if (GetCurrentState()==Feedback::State::kCalibrating)
-            for (int i=0; i<BIAS::kNumChannels; i++)
-                Uoff[i] = fBiasOffset;
-        else
-            for (int i=0; i<BIAS::kNumChannels; i++)
-                Uoff[i] = fTempOffset+fBiasOffset;
-
-        if (fControlType==kCurrentsNew)
-        {
-            // Would be a devision by zero. We need informations first.
-            if (fCursorCur==0)
-                return GetCurrentState();
-
-            vector<double> dI;
-            vector<double> R8;
-            vector<double> R9;
-
-            for (int i=0; i<BIAS::kNumChannels; i++)
+        med[2].resize(416);
+
+        struct dim_data
+        {
+            float I[416];
+            float Iavg;
+            float Irms;
+            float Imed;
+            float Idev;
+            uint32_t N;
+            float Tdiff;
+            float Uov[416];
+            float Unom;
+            float dUtemp;
+
+            dim_data() { memset(this, 0, sizeof(dim_data)); }
+        } __attribute__((__packed__));
+
+        dim_data data;
+
+        data.Unom   = overvoltage;
+        data.dUtemp = fTempOffset;
+
+        vector<float> vec(416);
+
+        cout << setprecision(4) << endl;
+
+        if (fEnableOldAlgorithm)
+        {
+            // Pixel  583: 5 31 == 191 (5)  C2 B3 P3
+            // Pixel  830: 2  2 ==  66 (4)  C0 B8 P1
+            // Pixel 1401: 6  1 == 193 (5)  C2 B4 P0
+
+            // 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
+
+            for (int i=0; i<320; i++)
             {
                 const PixelMapEntry &hv = fMap.hv(i);
@@ -181,21 +428,94 @@
                     continue;
 
-                // Nominal breakdown voltage (includes overvoltage already)
-                double Ubd = fVoltGapd[i];
-
-                // Nominal breakdown voltage excluding overvoltage of 1.1V
-                Ubd -= 1.1;
-
-                // Correct breakdown voltage for temperature dependence
-                Ubd += fTempOffset;
+                // Average measured current
+                const double Im = Imes[i] * 1e6; // [uA]
+
+                // Group index (0 or 1) of the of the pixel (4 or 5 pixel patch)
+                const int g = hv.group();
+
+                // 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 Ubd = fVoltGapd[i] + fTempOffset;
+                const double U0  = Ubd + overvoltage - fCalibVoltage[5][i]; // appliedOffset-fCalibrationOffset;
+                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] : 0; // [uA]
+
+                // Make sure that the averaged resistor is valid
+                const double dU = Ravg[i]>10000 ? r*(I*1e-6 - dI) : 0;
+
+                if (i==2)
+                    cout << dU << endl;;
+
+                vec[i] = Ubd + overvoltage + dU;
+
+                // Calculate statistics only for channels with a valid calibration
+                if (Iavg[i]>0)
+                {
+                    med[g][num[g]] = dU;
+                    avg[g] += dU;
+                    num[g]++;
+
+                    if (dU<min[g])
+                        min[g] = dU;
+                    if (dU>max[g])
+                        max[g] = dU;
+
+                    data.I[i]  = Imes[i]*1e6 - fBiasVolt[i]/Ravg[i]*1e6;
+                    data.I[i] /= hv.count();
+
+                    if (i==66)
+                        data.I[i] /= 1.3;
+                    if (i==191 || i==193)
+                        data.I[i] /= 1.4;
+
+                    data.Iavg += data.I[i];
+                    data.Irms += data.I[i]*data.I[i];
+
+                    med[2][num[2]++] = data.I[i];
+                }
+            }
+        }
+        else
+        {
+            /* ================================= new ======================= */
+
+            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.group() ? 5 : 4;
+                const int N = hv.count();
 
                 // Average measured ADC value for this channel
-                const double adc = double(fCurrentsAvg[i])/fCursorCur * (5000/4096.); // [uA]
-
-                // Current through ~100Ohm measurement resistor
-                const double I8 = (adc-dI[i])*100/R8[i];
+                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)
@@ -211,23 +531,42 @@
                     R5 = 390/1.4;          // 379 = 1/(4/3900 + 1/390)
 
-                // Total resistance of branch with diode
+                // Total resistance of branch with diodes
                 const double R = R4+R5;
 
-                // Applied voltage at calibration resistors, according to
-                // biasctrl
-                const double U9 = fBiasVolt[i];
-
-                // Current through calibration resistors
-                // FIXME: Get that from biasctrl!!!
-                const double I9 = U9/R9[i];
-
-                // Current in R4/R5 branch
-                const double Iout = I8>I9 ? I8 - I9 : 0;
-
-                // Voltage drop in R4/R5 branch
+                // 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;
 
-                // Current overvoltage
+                // 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
@@ -235,5 +574,4 @@
                 double Iapd = Iout/N;
 
-                // This is assuming that the broken pixels have a 390 Ohm instead of 3900 Ohm serial resistor
                 // 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,
@@ -244,77 +582,75 @@
                 // increase the current flow as compared to the total current flow.
                 if (i==66)
-                    Iapd *= 1.3;
+                    Iapd /= 1.3;
                 if (i==191 || i==193)
-                    Iapd *= 1.4;
-
-                // If the G-APD voltage is above the breakdown voltage we have the current through the
-                // G-APD and the over-voltage applied to the G-APD to calculate its differential resistor.
-                if (Iapd>0)
+                    Iapd /= 1.4;
+
+                // 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 = pow(overvoltage, 1)/Rapd;
+
+                // Estimate set point for over-voltage (voltage drop at the target point)
+                //const double Uset = Ubd + overvoltage + R*Iov*N;
+                double Uset = Uov<0.3 ? Ubd + overvoltage + Udrp : Ubd + overvoltage + Udrp*pow(overvoltage/Uov, 1.66);
+
+                // Voltage set point
+                vec[i] = Uset;
+
+                // Calculate statistics only for channels with a valid calibration
+                //if (Uov>0)
                 {
-                    // 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 = (1.1+fBiasOffset)/Rapd;
-
-                    // Estimate set point for over-voltage
-                    const double Uset = (1.1+fBiasOffset) + Ubd + R*Iov*N;
-
-                    // Voltage set point as a difference between breakdown voltage and set point
-                    Uoff[i] = Uset - fBiasVolt[i];
+                    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;
                 }
-
-                 // 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;
-                 }
             }
-
+        }
+
+        // ------------------------------- Update voltages ------------------------------------
+
+        if (GetCurrentState()!=Feedback::State::kCalibrated)
+        {
+            DimClient::sendCommandNB("BIAS_CONTROL/SET_ALL_CHANNELS_VOLTAGE",
+                                     vec.data(), BIAS::kNumChannels*sizeof(float));
+
+            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);
+            Info(msg);
+        }
+        else
+        {
+            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);
+            Info(msg);
+
+        }
+
+        // --------------------------------- Console out --------------------------------------
+
+        if (num[0]>0 && num[1]>0 && fIsVerbose)
+        {
             sort(med[0].begin(), med[0].begin()+num[0]);
             sort(med[1].begin(), med[1].begin()+num[1]);
 
-            fCurrentsAvg.assign(BIAS::kNumChannels, 0);
-            fCursorCur = 0;
-        }
-
-        fDimDeviation.setQuality(fControlType);
-        fDimDeviation.Update(vec);
-
-        // Warning: Here it is assumed that the ramp up and down is done properly
-        // within the time between two new temperatures and that the calibration
-        // is finished within that time.
-        if (GetCurrentState()!=Feedback::State::kCalibrating ||
-            fDimBias.state()!=BIAS::State::kVoltageOff ||
-            fCursorTemp!=1 || !fOutputEnabled)
-        {
-            if (!fOutputEnabled || fDimBias.state()!=BIAS::State::kVoltageOn)
-                return GetCurrentState();
-
-            // Trigger calibration
-            if (GetCurrentState()==Feedback::State::kCalibrating && fCursorTemp==2)
-            {
-                DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0);
-                return GetCurrentState();
-            }
-        }
-
-        ostringstream msg;
-        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);
-
-        if (fControlType==kCurrents && num[0]>0 && num[1]>0)
-        {
-            msg.str("");
+            ostringstream msg;
             msg << "   Avg0=" << setw(7) << avg[0]/num[0]    << "  |  Avg1=" << setw(7) << avg[1]/num[1];
             Debug(msg);
@@ -333,751 +669,45 @@
         }
 
-        DimClient::sendCommandNB("BIAS_CONTROL/SET_ALL_CHANNELS_OFFSET",
-                                 vec.data()+BIAS::kNumChannels, BIAS::kNumChannels*sizeof(float));
-
-        return GetCurrentState();
-    }
-
-    int HandleCurrentControl()
-    {
-        const double dUt = fTempOffset; // [V]
-
-        if (GetCurrentState()==Feedback::State::kCalibrating && fBiasOffset>dUt-1.2)
-        {
-            fCursorTemp = 0;
-
-            ostringstream msg;
-            msg << " (applied calibration offset " << fBiasOffset << "V exceeds temperature correction " << fTempOffset << "V - 1.2V.";
-            Warn("Trying to calibrate above G-APD breakdown volatge!");
-            Warn(msg);
-            return GetCurrentState();
-        }
-
-        // FIXME: If calibrating do not wait for the temperature!
-        fAppliedOffset = fBiasOffset;
-        if (GetCurrentState()!=Feedback::State::kCalibrating)
-            fAppliedOffset += dUt;
-
-        vector<float> vec(2*BIAS::kNumChannels+2);
-        for (int i=0; i<BIAS::kNumChannels; i++)
-            vec[i+BIAS::kNumChannels] = fAppliedOffset;
-
-        vec[BIAS::kNumChannels*2]   = dUt;
-        vec[BIAS::kNumChannels*2+1] = fBiasOffset;
-
-        double avg[2] = {   0,   0 };
-        double min[2] = {  90,  90 };
-        double max[2] = { -90, -90 };
-        int    num[2] = {   0,   0 };
-
-        vector<double> med[2];
-        med[0].resize(416);
-        med[1].resize(416);
-
-        if (fControlType==kCurrents)
-        {
-            if (fCursorCur==0)
-            {
-                //DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0);
-                return GetCurrentState();
-            }
-
-            // Pixel  583: 5 31 == 191 (5)  C2 B3 P3
-            // Pixel  830: 2  2 ==  66 (4)  C0 B8 P1
-            // Pixel 1401: 6  1 == 193 (5)  C2 B4 P0
-
-            // Convert from DAC counts to uA
-            const double conv = 5000./4096;
-
-            // 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 = fAppliedOffset-fCalibrationOffset;
-
-            for (int i=0; i<BIAS::kNumChannels; i++)
-            {
-                const PixelMapEntry &hv = fMap.hv(i);
-                if (!hv)
-                    continue;
-
-                // Average measured current
-                const double Im = double(fCurrentsAvg[i])/fCursorCur * conv; // [uA]
-
-                // Group index (0 or 1) of the of the pixel (4 or 5 pixel patch)
-                const int g = hv.group();
-
-                // 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] : 0; // [A]
-
-                // Make sure that the averaged resistor is valid
-                const double dU = Ravg[i]>10000 ? r*(I*1e-6 - dI) : 0;
-
-                vec[i+BIAS::kNumChannels] += dU;
-
-                // Angelegte Spannung:    U0+dU
-                // Gemessener Strom:      Im - Iavg
-                // Strom offset:          (U0+dU) / Ravg
-                // Fliessender Strom:     Im-Iavg - (U0+dU)/Ravg
-                // Korrektur:             [ Im-Iavg - (U0+dU)/Ravg ] * Rg
-
-                // Aufgeloest nach dU:    dU =  ( Im-Iavg - dU/Ravg ) / ( 1/Rg + 1/Ravg )
-                // Equivalent zu:         dU  = ( I*Ravg - U0 ) / ( Ravg/Rg+1 )
-
-                // Calculate statistics only for channels with a valid calibration
-                if (Iavg[i]>0)
-                {
-                    med[g][num[g]] = dU;
-                    avg[g] += dU;
-                    num[g]++;
-
-                    if (dU<min[g])
-                        min[g] = dU;
-                    if (dU>max[g])
-                        max[g] = dU;
-                }
-            }
-
-            sort(med[0].begin(), med[0].begin()+num[0]);
-            sort(med[1].begin(), med[1].begin()+num[1]);
-
-            fCurrentsAvg.assign(BIAS::kNumChannels, 0);
-            fCursorCur = 0;
-        }
-
-        fDimDeviation.setQuality(fControlType);
-        fDimDeviation.Update(vec);
-
-        // Warning: Here it is assumed that the ramp up and down is done properly
-        // within the time between two new temperatures and that the calibration
-        // is finished within that time.
-        if (!(GetCurrentState()==Feedback::State::kCalibrating && fCursorTemp==1 && fOutputEnabled && fDimBias.state()==BIAS::State::kVoltageOff))
-        {
-            if (!fOutputEnabled || fDimBias.state()!=BIAS::State::kVoltageOn)
-                return GetCurrentState();
-
-            // Trigger calibration
-            if (GetCurrentState()==Feedback::State::kCalibrating && fCursorTemp==2)
-            {
-                DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0);
-                return GetCurrentState();
-            }
-        }
-
-        ostringstream msg;
-        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);
-
-        if (fControlType==kCurrents && num[0]>0 && num[1]>0)
-        {
-            msg.str("");
-            msg << "   Avg0=" << setw(7) << avg[0]/num[0]    << "  |  Avg1=" << setw(7) << avg[1]/num[1];
-            Debug(msg);
-
-            msg.str("");
-            msg << "   Med0=" << setw(7) << med[0][num[0]/2] << "  |  Med1=" << setw(7) << med[1][num[1]/2];
-            Debug(msg);
-
-            msg.str("");
-            msg << "   Min0=" << setw(7) << min[0]           << "  |  Min1=" << setw(7) << min[1];
-            Debug(msg);
-
-            msg.str("");
-            msg << "   Max0=" << setw(7) << max[0]           << "  |  Max1=" << setw(7) << max[1];
-            Debug(msg);
-        }
-
-        DimClient::sendCommandNB("BIAS_CONTROL/SET_ALL_CHANNELS_OFFSET",
-                                 vec.data()+BIAS::kNumChannels, BIAS::kNumChannels*sizeof(float));
-
-        return GetCurrentState();
-    }
-
-    int AverageCurrents(const EventImp &evt)
-    {
-        if (evt.GetSize()!=BIAS::kNumChannels*sizeof(int16_t))
-            return -1;
-
-        if (fDimBias.state()!=BIAS::State::kVoltageOn)
-            return false;
-
-        if (fCursorCur++<0)
-            return true;
-
-        const int16_t *ptr = evt.Ptr<int16_t>();
-
-        for (int i=0; i<BIAS::kNumChannels; i++)
-        {
-            fCurrentsAvg[i] += ptr[i];
-            fCurrentsRms[i] += ptr[i]*ptr[i];
-        }
-
-        return true;
-    }
-
-
-    void HandleCalibration(const EventImp &evt)
-    {
-        const int rc = AverageCurrents(evt);
-        if (rc<0)
-            return;
-
-        if (fCursorCur<fNumCalibRequests)
-        {
-            if (fDimBias.state()==BIAS::State::kVoltageOn)
-                DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0);
-            return;
-        }
-
-        if (rc==0)
-            return;
-
-        fCalibration.resize(BIAS::kNumChannels*3);
-
-        float *avg = fCalibration.data();
-        float *rms = fCalibration.data()+BIAS::kNumChannels;
-        float *res = fCalibration.data()+BIAS::kNumChannels*2;
-
-        const double conv = 5000./4096;
-
-        for (int i=0; i<BIAS::kNumChannels; i++)
-        {
-            const double I = double(fCurrentsAvg[i])/fCursorCur;
-
-            res[i] = (fVoltGapd[i]+fCalibrationOffset)/I / conv * 1e6;
-            avg[i] = conv * I;
-            rms[i] = conv * sqrt(double(fCurrentsRms[i])/fCursorCur-I*I);
-        }
-
-        fCalibTime = Time();
-
-        fDimCalibration.setData(fCalibration);
-        fDimCalibration.Update(fCalibTime);
-
-        fOutputEnabled = false;
-        fControlType = kIdle;
-
-        Info("Calibration successfully done.");
-
-        if (fDimBias.state()==BIAS::State::kVoltageOn)
-            DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0);
-    }
-
-    void HandleFeedback(const EventImp &evt)
-    {
-        if (evt.GetSize()!=1440*sizeof(float))
-            return;
-
-        // -------- Check age of last stored event --------
-
-        const Time tm(evt.GetTime());
-
-        if (Time()-fBiasLast>boost::posix_time::seconds(30))
-        {
-            Warn("Last received event data older than 30s... resetting average calculation.");
-            ResetData();
-        }
-        fBiasLast = tm;
-
-        // -------- Store new event --------
-
-        fData[fCursorAmpl%fData.size()].assign(evt.Ptr<float>(), evt.Ptr<float>()+1440);
-        if (++fCursorAmpl<fData.size())
-            return;
-
-        // -------- Calculate statistics --------
-
-        valarray<double> med(1440);
-
-        for (int ch=0; ch<1440; ch++)
-        {
-            vector<float> arr(fData.size());
-            for (size_t i=0; i<fData.size(); i++)
-                arr[i] = fData[i][ch];
-
-            sort(arr.begin(), arr.end());
-
-            med[ch] = arr[arr.size()/2];
-        }
-
-        /*
-            vector<float> med(1440);
-            vector<float> rms(1440);
-            for (size_t i=0; i<fData.size(); i++)
-            {
-                if (fData[i].size()==0)
-                    return;
-
-                for (int j=0; j<1440; j++)
-                {
-                    med[j] += fData[i][j];
-                    rms[j] += fData[i][j]*fData[i][j];
-                }
-            }
-            */
-
-        vector<double> avg(BIAS::kNumChannels);
-        vector<int>    num(BIAS::kNumChannels);
-        for (int i=0; i<1440; i++)
-        {
-            const PixelMapEntry &ch = fMap.hw(i);
-
-            // FIXME: Add a consistency check if the median makes sense...
-            // FIXME: Add a consistency check to remove pixels with bright stars (median?)
-
-            avg[ch.hv()] += med[i];
-            num[ch.hv()]++;
-        }
-
-        for (int i=0; i<BIAS::kNumChannels; i++)
-        {
-            if (num[i])
-                avg[i] /= num[i];
-
-        }
-
-        // -------- Calculate correction --------
-
-        // http://bestune.50megs.com/typeABC.htm
-
-        // CO: Controller output
-        // PV: Process variable
-        // SP: Set point
-        // T:  Sampling period (loop update period)
-        // e = SP - PV
-        //
-        // Kp : No units
-        // Ki : per seconds
-        // Kd : seconds
-
-        // 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 (fCursorAmpl%fData.size()>0)
-            return;
-
-        // FIXME: Take out broken / dead boards.
-
-        const Time tm0 = Time();
-
-        /*const*/ double T21 = fT>0 ? fT : (tm0-fStartTime).total_microseconds()/1000000.;
-        const double T10 = fT21;
-        fT21 = T21;
-
-        fStartTime = tm0;
-
-        ostringstream out;
-        out << "New " << fData.size() << " event received: " << fCursorAmpl << " / " << setprecision(3) << T21 << "s";
-        Info(out);
-
-        if (fPV[0].size()==0)
-        {
-            fPV[0].resize(avg.size());
-            fPV[0] = valarray<double>(avg.data(), avg.size());
-            return;
-        }
-
-        if (fPV[1].size()==0)
-        {
-            fPV[1].resize(avg.size());
-            fPV[1] = valarray<double>(avg.data(), avg.size());
-            return;
-        }
-
-        if (fPV[2].size()==0)
-        {
-            fPV[2].resize(avg.size());
-            fPV[2] = valarray<double>(avg.data(), avg.size());
-            return;
-        }
-
-        fPV[0] = fPV[1];
-        fPV[1] = fPV[2];
-
-        fPV[2].resize(avg.size());
-        fPV[2] = valarray<double>(avg.data(), avg.size());
-
-        if (T10<=0 || T21<=0)
-            return;
-
-        //cout << "Calculating (" << fCursor << ":" << T21 << ")... " << endl;
-
-        // fKi[j] = response[j]*gain;
-        // Kp = 0;
-        // Kd = 0;
-
-        // => 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;
-        T21 = 1;
-        */
-
-        //valarray<double> correction = - Kp*(PV[2] - PV[1]) + Ki * dT * (SP-PV[2]) - Kd/dT * (PV[2] - 2*PV[1] + PV[0]);
-        //valarray<double> correction =
-        // -      Kp * (PV[2] - PV[1])
-        // + dT * Ki * (SP    - PV[2])
-        // - Kd / dT * (PV[2] - 2*PV[1] + PV[0]);
-        //
-        // - (Kp+Kd/dT1) * (PV[2] - PV[1])
-        // + dT2 * Ki * (SP    - PV[2])
-        // + Kd / dT1 * (PV[1] - PV[0]);
-        //
-        // - Kp * (PV[2] - PV[1])
-        // + Ki * (SP    - PV[2])*dT
-        // - Kd * (PV[2] - PV[1])/dT
-        // + Kd * (PV[1] - PV[0])/dT;
-        //
-        //valarray<double> correction =
-        //    - Kp*(PV[2] - PV[1]) + Ki * T21 * (SP-PV[2]) - Kd*(PV[2]-PV[1])/T21 - Kd*(PV[0]-PV[1])/T01;
-        const valarray<double> correction = 1./fGain/1000*
-            (
-             - (fKp+fKd/T21)*(fPV[2] - fPV[1])
-             +  fKi*T21*(fSP-fPV[2])
-             +  fKd/T10*(fPV[1]-fPV[0])
-            );
-
-        /*
-         integral = 0
-         start:
-         integral += (fSP - fPV[2])*dt
-
-         output = Kp*(fSP - fPV[2]) + Ki*integral - Kd*(fPV[2] - fPV[1])/dt
-
-         wait(dt)
-
-         goto start
-         */
-
-        vector<float> vec(2*BIAS::kNumChannels+2);
-        for (int i=0; i<BIAS::kNumChannels; i++)
-            vec[i] = fPV[2][i]-fSP[i];
-
-        for (int i=0; i<BIAS::kNumChannels; i++)
-            vec[i+BIAS::kNumChannels] = avg[i]<5*2.5 ? 0 : correction[i];
-
-        fDimDeviation.setQuality(fControlType);
-        fDimDeviation.Update(vec);
-
-        if (!fOutputEnabled || fDimBias.state()!=BIAS::State::kVoltageOn)
-            return;
-
-        Info("Sending new relative offset to biasctrl.");
-
-        DimClient::sendCommandNB("BIAS_CONTROL/INCREASE_ALL_CHANNELS_VOLTAGE",
-                                 vec.data()+BIAS::kNumChannels, BIAS::kNumChannels*sizeof(float));
-    }
-
-    void HandleGlobalFeedback(const EventImp &evt)
-    {
-        if (evt.GetSize()!=1440*sizeof(float))
-            return;
-
-        // -------- Store new event --------
-
-        vector<float> arr(evt.Ptr<float>(), evt.Ptr<float>()+1440);
-
-        sort(arr.begin(), arr.end());
-
-        const float med = arr[arr.size()/2];
-
-        fData[fCursorAmpl%fData.size()].resize(1); //assign(&med, &med);
-        fData[fCursorAmpl%fData.size()][0] = med;  //assign(&med, &med);
-
-        if (++fCursorAmpl<fData.size())
-            return;
-
-        // -------- Calculate statistics --------
-
-        double avg=0;
-        double rms=0;
-        for (size_t i=0; i<fData.size(); i++)
-        {
-            avg += fData[i][0];
-            rms += fData[i][0]*fData[i][0];
-        }
-
-        avg /= fData.size();
-        rms /= fData.size();
-
-        rms  = sqrt(rms-avg*avg);
-
-        // -------- Calculate correction --------
-
-        if (fCursorAmpl%fData.size()!=0)
-            return;
-
-        Out() << "Amplitude: " << avg << " +- " << rms << endl;
-
-        // FIXME: Take out broken / dead boards.
-
-        /*
-        ostringstream out;
-        out << "New " << fData.size() << " event received: " << fCursor << " / " << setprecision(3) << T21 << "s";
-        Info(out);
-        */
-
-        if (fPV[0].size()==0)
-        {
-            fPV[0].resize(1);
-            fPV[0] = valarray<double>(&avg, 1);
-            return;
-        }
-
-        if (fPV[1].size()==0)
-        {
-            fPV[1].resize(1);
-            fPV[1] = valarray<double>(&avg, 1);
-            return;
-        }
-
-        if (fPV[2].size()==0)
-        {
-            fPV[2].resize(1);
-            fPV[2] = valarray<double>(&avg, 1);
-            return;
-        }
-
-        fPV[0] = fPV[1];
-        fPV[1] = fPV[2];
-
-        fPV[2].resize(1);
-        fPV[2] = valarray<double>(&avg, 1);
-
-        // ----- Calculate average currents -----
-
-        vector<float> A(BIAS::kNumChannels);
-        for (int i=0; i<BIAS::kNumChannels; i++)
-            A[i] = double(fCurrentsAvg[i]) / fCursorCur;
-
-        fCurrentsAvg.assign(BIAS::kNumChannels, 0);
-        fCursorCur = 0;
-
-        // -------- Calculate correction --------
-
-        // correction = (fSP[0]-fPV[2])*fKi
-        /*
-        const double T21 = 1; // feedback is  1s
-        const double T10 = 1; // feedback is 20s
-
-        const valarray<double> correction = 1./fGain/1000*
-            (
-             - (fKp+fKd/T21)*(fPV[2] - fPV[1])
-             +  fKi*T21*(fSP[0]-fPV[2])
-             +  fKd/T10*(fPV[1]-fPV[0])
-            );
-        */
-
-        // pow of 1.6 comes from the non-linearity of the
-        // amplitude vs bias voltage
-        const valarray<double> correction = 1./fGain/1000*
-            (
-             //fKi*(pow(fSP[0], 1./1.6)-pow(fPV[2], 1./1.6))
-             fKi*(fSP[0]-fPV[2])
-            );
-
-        Out() << "Correction: " << correction[0] << "V (" << fSP[0] << ")" << endl;
-
-        const int nch = BIAS::kNumChannels;
-
-        // FIXME: Sanity check!
-
-        vector<float> vec;
-        vec.reserve(2*nch+2);
-        vec.insert(vec.begin(),     nch, fPV[2][0]-fSP[0]);
-        vec.insert(vec.begin()+nch, nch, correction[0]);
-        vec.push_back(0);
-        vec.push_back(0);
-
-        fDimDeviation.setQuality(fControlType);
-        fDimDeviation.Update(vec);
-
-        if (!fOutputEnabled || fDimBias.state()!=BIAS::State::kVoltageOn)
-            return;
-
-        Info("Sending new global relative offset to biasctrl.");
-
-        DimClient::sendCommandNB("BIAS_CONTROL/INCREASE_ALL_CHANNELS_VOLTAGE",
-                                 vec.data()+BIAS::kNumChannels, BIAS::kNumChannels*sizeof(float));
-    }
-
-    void HandleCalibrateCurrents(const EventImp &evt)
-    {
-        if (fBiasVolt.empty() || fCalibration.empty() || evt.GetSize()<416*sizeof(int16_t))
-            return;
-
-        struct dim_data {
-            float I[416];
-            float Iavg;
-            float Irms;
-            float Imed;
-            float Idev;
-            uint32_t N;
-            float Tdiff;
-
-            dim_data() { memset(this, 0, sizeof(dim_data)); }
-        } __attribute__((__packed__));
-
-        const int16_t *I = evt.Ptr<int16_t>();
-        const float   *R = fCalibration.data()+BIAS::kNumChannels*2;
-        const float   *U = fBiasVolt.data();
-
-        vector<float> med(416);
-        uint16_t cnt = 0;
-
-        double avg = 0;
-        double rms = 0;
-
-        dim_data data;
-        for (int i=0; i<416; i++)
-        {
-            const PixelMapEntry &hv = fMap.hv(i);
-            if (!hv)
-                continue;
-
-            if (R[i]<=0)
-                continue;
-
-            data.I[i]  = I[i]*5000./4096 - U[i]/R[i]*1e6;
-            data.I[i] /= hv.group() ? 5 : 4;
-
-            avg += data.I[i];
-            rms += data.I[i]*data.I[i];
-
-            if (i>=320)
-                continue;
-
-            med[cnt++] = data.I[i];
-        }
-
-        if (cnt==0)
-            return;
-
-        avg /= cnt;
-        rms /= cnt;
-
-        data.N = cnt;
-        data.Iavg = avg;
-        data.Irms = sqrt(rms-avg*avg);
-
-        sort(med.data(), med.data()+cnt);
-
-        data.Imed = cnt%2 ? (med[cnt/2-1]+med[cnt/2])/2 : med[cnt/2];
-
-        for (int i=0; i<cnt; i++)
-            med[i] = fabs(med[i]-data.Imed);
-
-        sort(med.data(), med.data()+cnt);
-
-        data.Idev = med[uint32_t(0.682689477208650697*cnt)];
-
-        data.Tdiff = evt.GetTime().UnixTime()-fCalibTime.UnixTime();
-
-        fDimCurrents.setData(&data, sizeof(dim_data));
-        fDimCurrents.Update(evt.GetTime());
-    }
-
-    int HandleBiasCurrent(const EventImp &evt)
-    {
-        if (fControlType==kTemp && GetCurrentState()==Feedback::State::kCalibrating)
-            HandleCalibration(evt);
-
-        if (fControlType==kFeedbackGlobal || fControlType==kCurrents || fControlType==kCurrentsNew)
-            AverageCurrents(evt);
-
-        /*
-        if (fControlType==kCurrents && fCursorTemp>0 && fCursorCur>0)
-        {
-            // fCursorTemp: 1 2 3 4 5 6 7 8
-            // fCursor%x:   1 1 1 2 2 2 3 3    // 9 steps in ~15s
-            //if (fCursorTemp<3 && fCursorCur%(fCursorTemp/3+1)==0)
-                HandleCurrentControl();
-        }*/
-
-        HandleCalibrateCurrents(evt);
-
-        return GetCurrentState();
-    }
-
-    int HandleBiasData(const EventImp &evt)
-    {
-        if (fControlType==kFeedback)
-            HandleFeedback(evt);
-
-        if (fControlType==kFeedbackGlobal)
-            HandleGlobalFeedback(evt);
-
-        return GetCurrentState();
-    }
-
-    int HandleBiasNom(const EventImp &evt)
-    {
-        if (evt.GetSize()>=416*sizeof(float))
-        {
-            fVoltGapd.assign(evt.Ptr<float>(), evt.Ptr<float>()+416);
-            Info("Nominal bias voltages received.");
-        }
-
-        return GetCurrentState();
-    }
-
-    int HandleBiasVoltage(const EventImp &evt)
-    {
-        if (evt.GetSize()>=416*sizeof(float))
-            fBiasVolt.assign(evt.Ptr<float>(), evt.Ptr<float>()+416);
-        return GetCurrentState();
-    }
-
-    bool CheckEventSize(size_t has, const char *name, size_t size)
-    {
-        if (has==size)
-            return true;
-
-        ostringstream msg;
-        msg << name << " - Received event has " << has << " bytes, but expected " << size << ".";
-        Fatal(msg);
-        return false;
-    }
+        // ---------------------------- Calibrated Currents -----------------------------------
+
+        if (num[2]>0)
+        {
+            data.Iavg /= num[2];
+            data.Irms /= num[2];
+
+            data.N = num[2];
+            data.Irms = sqrt(data.Irms-data.Iavg*data.Iavg);
+
+            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];
+
+            for (int i=0; i<num[2]; i++)
+                med[2][i] = fabs(med[2][i]-data.Imed);
+
+            sort(med[2].data(), med[2].data()+num[2]);
+
+            data.Idev = med[2][uint32_t(0.682689477208650697*num[2])];
+
+            data.Tdiff = evt.GetTime().UnixTime()-fTimeCalib.UnixTime();
+
+            // FIXME:
+            //  + Current overvoltage
+            //  + Temp offset
+            //  + User offset
+            //  + Command overvoltage
+            fDimCurrents.setQuality(GetCurrentState());
+            fDimCurrents.setData(&data, sizeof(dim_data));
+            fDimCurrents.Update(evt.GetTime());
+        }
+
+        return GetCurrentState()==Feedback::State::kCalibrated ? Feedback::State::kCalibrated : Feedback::State::kInProgress;
+    }
+
+    // ======================================================================
 
     int Print() const
     {
         Out() << fDim << endl;
-        Out() << fDimFAD << endl;
         Out() << fDimFSC << endl;
         Out() << fDimBias << endl;
@@ -1088,5 +718,6 @@
     int PrintCalibration()
     {
-        if (fCalibration.empty())
+        /*
+        if (fCalibration.size()==0)
         {
             Out() << "No calibration performed so far." << endl;
@@ -1121,441 +752,178 @@
 
         Out() << flush;
-
+        */
         return GetCurrentState();
     }
 
-    void WarnState(bool needfsc, bool needfad)
-    {
+    int EnableOldAlgorithm(const EventImp &evt)
+    {
+        if (!CheckEventSize(evt.GetSize(), "EnableOldAlgorithm", 1))
+            return kSM_FatalError;
+
+        fEnableOldAlgorithm = evt.GetBool();
+
+        return GetCurrentState();
+    }
+
+    int SetVerbosity(const EventImp &evt)
+    {
+        if (!CheckEventSize(evt.GetSize(), "SetVerbosity", 1))
+            return kSM_FatalError;
+
+        fIsVerbose = evt.GetBool();
+
+        return GetCurrentState();
+    }
+
+    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();
+    }
+
+    int Calibrate()
+    {
+        if (fDimBias.state()!=BIAS::State::kVoltageOff)
+        {
+            Warn("Calibration can only be started when biasctrl is in state VoltageOff.");
+            return GetCurrentState();
+        }
+
+        Message("Starting calibration (ignore="+to_string(fNumCalibIgnore)+", N="+to_string(fNumCalibRequests)+")");
+
+        fCursorCur  = -fNumCalibIgnore;
+        fCurrentsAvg.assign(BIAS::kNumChannels, 0);
+        fCurrentsRms.assign(BIAS::kNumChannels, 0);
+
+        fBiasDac.assign(BIAS::kNumChannels, 0);
+
+        fCalibStep = 3;
+        fTimeCalib = Time();
+
+        Dim::SendCommandNB("BIAS_CONTROL/SET_GLOBAL_DAC", uint32_t(256+512*fCalibStep));
+
+        return Feedback::State::kCalibrating;
+    }
+
+    int Start(const EventImp &evt)
+    {
+        if (!CheckEventSize(evt.GetSize(), "Start", 4))
+            return kSM_FatalError;
+
+        fUserOffset = evt.GetFloat();
+
+        fCursorCur = 0;
+
+        fCurrentsAvg.assign(BIAS::kNumChannels, 0);
+        fCurrentsRms.assign(BIAS::kNumChannels, 0);
+
+        ostringstream out;
+        out << "Starting feedback with an offset of " << fUserOffset << "V";
+        Message(out);
+
+        return Feedback::State::kWaitingForData;
+    }
+
+    int StopFeedback()
+    {
+        if (GetCurrentState()==Feedback::State::kCalibrating)
+            return Feedback::State::kConnected;
+
+        if (GetCurrentState()>Feedback::State::kCalibrated)
+            return Feedback::State::kCalibrated;
+
+        return GetCurrentState();
+    }
+
+    int Execute()
+    {
+        if (!fDim.online())
+            return Feedback::State::kDimNetworkNA;
+
         const bool bias = fDimBias.state() >= BIAS::State::kConnecting;
         const bool fsc  = fDimFSC.state()  >= FSC::State::kConnected;
-        const bool fad  = fDimFAD.state()  >= FAD::State::kConnected;
-
-        if (!bias)
-            Warn("Bias control not yet ready.");
-        if (needfsc && !fsc)
-            Warn("FSC control not yet ready.");
-        if (needfad && !fad)
-            Warn("FAD control not yet ready.");
-    }
-
-    int SetConstant(const EventImp &evt, int constant)
-    {
-        if (!CheckEventSize(evt.GetSize(), "SetConstant", 8))
-            return kSM_FatalError;
-
-        switch (constant)
-        {
-        case 0: fKi   = evt.GetDouble(); break;
-        case 1: fKp   = evt.GetDouble(); break;
-        case 2: fKd   = evt.GetDouble(); break;
-        case 3: fT    = evt.GetDouble(); break;
-        case 4: fGain = evt.GetDouble(); break;
-        default:
-            Fatal("SetConstant got an unexpected constant id -- this is a program bug!");
-            return kSM_FatalError;
+
+        // All subsystems are not connected
+        if (!bias && !fsc)
+            return Feedback::State::kDisconnected;
+
+        // Not all subsystems are yet connected
+        if (!bias || !fsc)
+            return Feedback::State::kConnecting;
+
+        if (GetCurrentState()<Feedback::State::kCalibrating)
+            return Feedback::State::kConnected;
+
+        if (GetCurrentState()==Feedback::State::kConnected)
+            return GetCurrentState();
+        if (GetCurrentState()==Feedback::State::kCalibrating)
+            return GetCurrentState();
+
+        // kCalibrated, kWaitingForData, kInProgress
+
+        if (fDimBias.state()==BIAS::State::kVoltageOn || (fDimBias.state()==BIAS::State::kVoltageOff && GetCurrentState()==Feedback::State::kWaitingForData))
+        {
+            static Time past;
+            if (fCurrentRequestInterval>0 && Time()-past>boost::posix_time::milliseconds(fCurrentRequestInterval))
+            {
+                Dim::SendCommandNB("BIAS_CONTROL/REQUEST_STATUS");
+                past = Time();
+            }
         }
 
         return GetCurrentState();
-    }
-
-    int EnableOutput(const EventImp &evt)
-    {
-        if (!CheckEventSize(evt.GetSize(), "EnableOutput", 1))
-            return kSM_FatalError;
-
-        fOutputEnabled = evt.GetBool();
-
-        if (fControlType==kCurrents || fControlType==kCurrentsNew)
-            if (fCursorTemp>1)
-                fCursorTemp = 1;
-
-        return GetCurrentState();
-    }
-
-    void ResetData(int16_t n=-1)
-    {
-        fData.assign(n>0 ? n : fData.size(), vector<float>(0));
-
-        fCursorAmpl = 0;
-        fCursorCur  = 0;
-        fCursorTemp = 0;
-
-        fStartTime = Time();
-
-        fSP = valarray<double>(0., BIAS::kNumChannels);
-
-        vector<float> vec(2*BIAS::kNumChannels+2, fBiasOffset);
-        vec[2*BIAS::kNumChannels]   = 0;
-        fDimDeviation.setQuality(kIdle);
-        fDimDeviation.Update(vec);
-
-        fPV[0].resize(0);
-        fPV[1].resize(0);
-        fPV[2].resize(0);
-
-        fCurrentsAvg.assign(BIAS::kNumChannels, 0);
-        fCurrentsRms.assign(BIAS::kNumChannels, 0);
-
-        if (fKp==0 && fKi==0 && fKd==0)
-            Warn("Control loop parameters are all set to zero.");
-    }
-
-    int StartFeedback(const EventImp &evt)
-    {
-        if (!CheckEventSize(evt.GetSize(), "StartFeedback", 2))
-            return kSM_FatalError;
-
-        WarnState(false, true);
-
-        fBiasOffset = 0;
-        ResetData(evt.GetShort());
-
-        fControlType = kFeedback;
-
-        return GetCurrentState();
-    }
-
-    int StartFeedbackGlobal(const EventImp &evt)
-    {
-        if (!CheckEventSize(evt.GetSize(), "StartFeedbackGlobal", 2))
-            return kSM_FatalError;
-
-        WarnState(false, true);
-
-        fBiasOffset = 0;
-        ResetData(evt.GetShort());
-
-        fControlType = kFeedbackGlobal;
-
-        return GetCurrentState();
-    }
-
-    int StartTempCtrl(const EventImp &evt)
-    {
-        if (!CheckEventSize(evt.GetSize(), "StartTempCtrl", 4))
-            return kSM_FatalError;
-
-        WarnState(true, false);
-
-        fBiasOffset = evt.GetFloat();
-        fControlType = kTemp;
-
-        ostringstream out;
-        out << "Starting temperature feedback with an offset of " << fBiasOffset << "V";
-        Message(out);
-
-        if (fDimBias.state()==BIAS::State::kVoltageOn)
-            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.empty())
-        {
-            Warn("Current control needs a bias crate calibration first... command ignored.");
-            return GetCurrentState();
-        }
-
-        WarnState(true, false);
-
-        fBiasOffset = evt.GetFloat();
-        fTempOffset = -3;
-        ResetData(0);
-        fControlType = kCurrents;
-
-        ostringstream out;
-        out << "Starting current/temp feedback with an offset of " << fBiasOffset << "V";
-        Message(out);
-
-        return GetCurrentState();
-    }
-
-    int StartNewCurrentCtrl(const EventImp &evt)
-    {
-        if (!CheckEventSize(evt.GetSize(), "StartNewCurrentCtrl", 4))
-            return kSM_FatalError;
-
-        if (fCalibration.empty())
-        {
-            Warn("Current control needs a bias crate calibration first... command ignored.");
-            return GetCurrentState();
-        }
-
-        WarnState(true, false);
-
-        fBiasOffset = evt.GetFloat();
-        fTempOffset = -3;
-        ResetData(0);
-        fControlType = kCurrentsNew;
-
-        ostringstream out;
-        out << "Starting new current/temp feedback with an offset of " << fBiasOffset << "V";
-        Message(out);
-
-        return GetCurrentState();
-    }
-
-    int StopFeedback()
-    {
-        fControlType = kIdle;
-
-        return GetCurrentState();
-    }
-
-    int StoreReference()
-    {
-        if (!fPV[0].size() && !fPV[1].size() && !fPV[2].size())
-        {
-            Warn("No values in memory. Take enough events first!");
-            return GetCurrentState();
-        }
-
-        // FIXME: Check age
-
-        if (!fPV[1].size() && !fPV[2].size())
-            fSP = fPV[0];
-
-        if (!fPV[2].size())
-            fSP = fPV[1];
-        else
-            fSP = fPV[2];
-
-        vector<float> vec(BIAS::kNumChannels);
-        for (int i=0; i<BIAS::kNumChannels; i++)
-            vec[i] = fSP[i];
-        fDimReference.Update(vec);
-
-        return GetCurrentState();
-    }
-
-    int SetReference(const EventImp &evt)
-    {
-        if (!CheckEventSize(evt.GetSize(), "SetReference", 4))
-            return kSM_FatalError;
-
-        const float val = evt.GetFloat();
-        /*
-        if (!fPV[0].size() && !fPV[1].size() && !fPV[2].size())
-        {
-            Warn("No values in memory. Take enough events first!");
-            return GetCurrentState();
-        }*/
-
-        vector<float> vec(BIAS::kNumChannels);
-        for (int i=0; i<BIAS::kNumChannels; i++)
-            vec[i] = fSP[i] = val;
-        fDimReference.Update(vec);
-
-        Out() << "New global reference value: " << val << "mV" << endl;
-
-        return GetCurrentState();
-    }
-
-    int CalibrateCurrents()
-    {
-//        if (!CheckEventSize(evt.GetSize(), "StartTempCtrl", 4))
-//            return kSM_FatalError;
-
-        if (fDimBias.state()==BIAS::State::kRamping)
-        {
-            Warn("Calibration cannot be started when biasctrl is in state Ramping.");
-            return GetCurrentState();
-        }
-
-        if (fVoltGapd.empty())
-        {
-            Error("No G-APD reference voltages received yet (BIAS_CONTROL/NOMINAL).");
-            return GetCurrentState();
-        }
-
-        WarnState(true, false);
-
-        ostringstream out;
-        out << "Starting temperature feedback for calibration with an offset of " << fCalibrationOffset << "V";
-        Message(out);
-
-        fBiasOffset = fCalibrationOffset;
-        fControlType = kTemp;
-        fCursorCur  = -fNumCalibIgnore;
-        fCursorTemp = 0;
-        fCurrentsAvg.assign(BIAS::kNumChannels, 0);
-        fCurrentsRms.assign(BIAS::kNumChannels, 0);
-        fCalibration.resize(0);
-        fStartTime = Time();
-        fOutputEnabled = true;
-
-        return Feedback::State::kCalibrating;
-    }
-
-    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();
-    }
-
-    int Execute()
-    {
-        // Dispatch (execute) at most one handler from the queue. In contrary
-        // to run_one(), it doesn't wait until a handler is available
-        // which can be dispatched, so poll_one() might return with 0
-        // handlers dispatched. The handlers are always dispatched/executed
-        // synchronously, i.e. within the call to poll_one()
-        //poll_one();
-
-        if (!fDim.online())
-            return Feedback::State::kDimNetworkNA;
-
-        const bool bias = fDimBias.state() >= BIAS::State::kConnecting;
-        const bool fad  = fDimFAD.state()  >= FAD::State::kConnected;
-        const bool fsc  = fDimFSC.state()  >= FSC::State::kConnected;
-
-        // All subsystems are not connected
-        if (!bias && !fad && !fsc)
-            return Feedback::State::kDisconnected;
-
-        // At least one subsystem apart from bias is connected
-        if (bias && !fad && !fsc)
-            return Feedback::State::kConnecting;
-
-/*
-        // All subsystems are connected
-        if (GetCurrentStatus()==Feedback::State::kConfiguringStep1)
-        {
-            if (fCursor<1)
-                return Feedback::State::kConfiguringStep1;
-
-            if (fCursor==1)
-            {
-                fStartTime = Time();
-                return Feedback::State::kConfiguringStep2;
-            }
-        }
-        if (GetCurrentStatus()==Feedback::State::kConfiguringStep2)
-        {
-            if (fCursor==1)
-            {
-                if ((Time()-fStartTime).total_microseconds()/1000000.<1.5)
-                    return Feedback::State::kConfiguringStep2;
-
-                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 Feedback::State::kConnected;
-        }
-        */
-
-        // Needs connection of FAD and BIAS
-        if (bias && fad)
-        {
-            if (fControlType==kFeedback || fControlType==kFeedbackGlobal)
-                return fOutputEnabled ? Feedback::State::kFeedbackCtrlRunning : Feedback::State::kFeedbackCtrlIdle;
-        }
-
-        // Needs connection of FSC and BIAS
-        if (bias && fsc)
-        {
-            if (fControlType==kTemp)
-            {
-                if (GetCurrentState()==Feedback::State::kCalibrating && fCursorCur<fNumCalibRequests)
-                    return GetCurrentState();
-
-                return fOutputEnabled ? Feedback::State::kTempCtrlRunning : Feedback::State::kTempCtrlIdle;
-            }
-            if (fControlType==kCurrents || fControlType==kCurrentsNew)
-            {
-                static Time past;
-                if (fCurrentRequestInterval>0 && Time()-past>boost::posix_time::milliseconds(fCurrentRequestInterval))
-                {
-                    if (fDimBias.state()==BIAS::State::kVoltageOn)
-                        DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0);
-                    past = Time();
-                }
-
-                return fOutputEnabled && fCursorTemp>0 ? Feedback::State::kCurrentCtrlRunning : Feedback::State::kCurrentCtrlIdle;
-            }
-        }
-
-        if (bias && fad && !fsc)
-            return Feedback::State::kConnectedFAD;
-
-        if (bias && fsc && !fad)
-            return Feedback::State::kConnectedFSC;
-
-        return Feedback::State::kConnected;
     }
 
 public:
     StateMachineFeedback(ostream &out=cout) : StateMachineDim(out, "FEEDBACK"),
+        fIsVerbose(false), fEnableOldAlgorithm(true),
         //---
-        fDimFAD("FAD_CONTROL"),
         fDimFSC("FSC_CONTROL"),
         fDimBias("BIAS_CONTROL"),
         //---
-        fDimReference("FEEDBACK/REFERENCE", "F:416",
-                      "Amplitude reference value(s)"
-                      "Vref[mV]:Amplitude reference"),
-        fDimDeviation("FEEDBACK/DEVIATION", "F:416;F:416;F:1;F:1",
-                      "Control loop information"
-                      "|DeltaAmpl[mV]:Amplitude offset measures"
-                      "|DeltaBias[mV]:Correction value calculated"
-                      "|DeltaTemp[mV]:Correction calculated from temperature"
-                      "|DeltaUser[mV]:Additional offset specified by user"),
-        fDimCalibration("FEEDBACK/CALIBRATION", "F:416;F:416;F:416",
+        fDimCalibration("FEEDBACK/CALIBRATION", "F:416;F:416;F:416;F:416",
                         "Current offsets"
-                        "|Avg[uA]:Average offset"
-                        "|Rms[uA]:Rms of offset"
-                        "|R[Ohm]:Measured calibration resistor"),
-        fDimCurrents("FEEDBACK/CALIBRATED_CURRENTS", "F:416;F:1;F:1;F:1;F:1;I:1;F:1",
+                        "|Avg[uA]:Average offset at dac=256+5*512"
+                        "|Rms[uA]:Rms of Avg"
+                        "|R[Ohm]:Measured calibration resistor"
+                        "|U[V]:Corresponding voltage reported by biasctrl"),
+        fDimCalibration2("FEEDBACK/CALIBRATION_STEPS", "I:1;F:416;F:416;F:416",
+                        "Calibration of the R8 resistor"
+                        "|DAC[dac]:DAC setting"
+                        "|U[V]:Corresponding voltages reported by biasctrl"
+                        "|Iavg[uA]:Averaged measured current"
+                        "|Irms[uA]:Rms measured current"),
+        fDimCalibrationR8("FEEDBACK/CALIBRATION_R8", "F:416;F:416",
+                          "Calibration of R8"
+                          "|DeltaI[uA]:Average offset"
+                          "|R8[uA]:Measured effective resistor R8"),
+        fDimCurrents("FEEDBACK/CALIBRATED_CURRENTS", "F:416;F:1;F:1;F:1;F:1;I:1;F:1;F:416;F:1;F:1",
                      "Calibrated currents"
-                     "|I[uA]:Calibrated currents"
-                     "|I_avg[uA]:Average calibrated current (320 channels)"
-                     "|I_rms[uA]:Rms of calibrated current (320 channels)"
-                     "|I_med[uA]:Median calibrated current (320 channels)"
-                     "|I_dev[uA]:Deviation of calibrated current (320 channels)"
+                     "|I[uA]:Calibrated currents per pixel"
+                     "|I_avg[uA]:Average calibrated current (N channels)"
+                     "|I_rms[uA]:Rms of calibrated current (N channels)"
+                     "|I_med[uA]:Median calibrated current (N channels)"
+                     "|I_dev[uA]:Deviation of calibrated current (N channels)"
                      "|N[uint16]:Number of valid values"
-                     "|T_diff[s]:Time difference to calibration"),
-        fSP(BIAS::kNumChannels),
-        fKp(0), fKi(0), fKd(0), fT(-1),
-        fCalibrationOffset(-3),
+                     "|T_diff[s]:Time difference to calibration"
+                     "|U_ov[V]:Calculated overvoltage"
+                     "|U_nom[V]:Nominal overvoltage"
+                     "|dU_temp[V]:Correction calculated from temperature"
+                    ),
         fCurrentRequestInterval(0),
         fNumCalibIgnore(30),
-        fNumCalibRequests(300),
-        fOutputEnabled(false)
-    {
-        // ba::io_service::work is a kind of keep_alive for the loop.
-        // It prevents the io_service to go to stopped state, which
-        // would prevent any consecutive calls to run()
-        // or poll() to do nothing. reset() could also revoke to the
-        // previous state but this might introduce some overhead of
-        // deletion and creation of threads and more.
-
+        fNumCalibRequests(300)
+    {
         fDim.Subscribe(*this);
-        fDimFAD.Subscribe(*this);
         fDimFSC.Subscribe(*this);
         fDimBias.Subscribe(*this);
+
+        fDimBias.SetCallback(bind(&StateMachineFeedback::HandleBiasStateChange, this));
 
         Subscribe("BIAS_CONTROL/CURRENT")
@@ -1563,6 +931,6 @@
         Subscribe("BIAS_CONTROL/VOLTAGE")
             (bind(&StateMachineFeedback::HandleBiasVoltage, this, placeholders::_1));
-        Subscribe("BIAS_CONTROL/FEEDBACK_DATA")
-            (bind(&StateMachineFeedback::HandleBiasData,    this, placeholders::_1));
+        Subscribe("BIAS_CONTROL/DAC")
+            (bind(&StateMachineFeedback::HandleBiasDac,     this, placeholders::_1));
         Subscribe("BIAS_CONTROL/NOMINAL")
             (bind(&StateMachineFeedback::HandleBiasNom,     this, placeholders::_1));
@@ -1576,109 +944,62 @@
         AddStateName(Feedback::State::kDisconnected, "Disconnected",
                      "The Dim DNS is reachable, but the required subsystems are not available.");
-
         AddStateName(Feedback::State::kConnecting, "Connecting",
-                     "Only biasctrl is available and connected with its hardware.");
-
-        AddStateName(Feedback::State::kConnectedFSC, "ConnectedFSC",
+                     "Either biasctrl or fscctrl not connected.");
+        AddStateName(Feedback::State::kConnected, "Connected",
                      "biasctrl and fscctrl are available and connected with their hardware.");
-        AddStateName(Feedback::State::kConnectedFAD, "ConnectedFAD",
-                     "biasctrl and fadctrl are available and connected with their hardware.");
-        AddStateName(Feedback::State::kConnected, "Connected",
-                     "biasctrl, fadctrl and fscctrl are available and connected with their hardware.");
-
-        AddStateName(Feedback::State::kFeedbackCtrlIdle, "FeedbackIdle",
-                     "Feedback control activated, but voltage output disabled.");
-        AddStateName(Feedback::State::kTempCtrlIdle, "TempCtrlIdle",
-                     "Temperature control activated, but voltage output disabled.");
-        AddStateName(Feedback::State::kCurrentCtrlIdle, "CurrentCtrlIdle",
-                     "Current control activated, but voltage output disabled.");
-
-        AddStateName(Feedback::State::kFeedbackCtrlRunning, "FeedbackControl",
-                     "Feedback control activated and voltage output enabled.");
-        AddStateName(Feedback::State::kTempCtrlRunning, "TempControl",
-                     "Temperature control activated and voltage output enabled.");
-        AddStateName(Feedback::State::kCurrentCtrlRunning, "CurrentControl",
-                     "Current/Temp control activated and voltage output enabled.");
+
         AddStateName(Feedback::State::kCalibrating, "Calibrating",
-                     "Calibrating current offsets.");
-
-        AddEvent("START_FEEDBACK_CONTROL", "S:1", Feedback::State::kConnectedFAD, Feedback::State::kConnected)
-            (bind(&StateMachineFeedback::StartFeedback, this, placeholders::_1))
-            ("Start the feedback control loop"
-             "|Num[short]:Number of events 'medianed' to calculate the correction value");
-
-        AddEvent("START_GLOBAL_FEEDBACK", "S:1", Feedback::State::kConnectedFAD, Feedback::State::kConnected)
-            (bind(&StateMachineFeedback::StartFeedbackGlobal, this, placeholders::_1))
-            ("Start the global feedback control loop"
-             "Num[short]:Number of events averaged to calculate the correction value");
-
-        AddEvent("START_TEMP_CONTROL", "F:1", Feedback::State::kConnectedFSC, Feedback::State::kConnected)
-            (bind(&StateMachineFeedback::StartTempCtrl, this, placeholders::_1))
-            ("Start the temperature control loop"
-             "|offset[V]:Offset from the nominal temperature corrected value in Volts");
-
-        AddEvent("START_CURRENT_CONTROL", "F:1", Feedback::State::kConnectedFSC, Feedback::State::kConnected)
-            (bind(&StateMachineFeedback::StartCurrentCtrl, this, placeholders::_1))
+                     "Bias crate calibrating in progress.");
+        AddStateName(Feedback::State::kCalibrated, "Calibrated",
+                     "Bias crate calibrated.");
+
+        AddStateName(Feedback::State::kWaitingForData, "WaitingForData",
+                     "Current control started, waiting for valid temperature and current data.");
+        AddStateName(Feedback::State::kInProgress, "InProgress",
+                     "Current control in progress.");
+
+
+        /*
+        AddEvent("SET_CURRENT_REQUEST_INTERVAL")
+            (bind(&StateMachineFeedback::SetCurrentRequestInterval, this, placeholders::_1))
+            ("|interval[ms]:Interval between two current requests in modes which need that.");
+        */
+
+        AddEvent("CALIBRATE", Feedback::State::kConnected, Feedback::State::kCalibrated)
+            (bind(&StateMachineFeedback::Calibrate, this))
+            ("");
+
+        AddEvent("START", "F:1", Feedback::State::kCalibrated)
+            (bind(&StateMachineFeedback::Start, this, placeholders::_1))
             ("Start the current/temperature control loop"
-             "|offset[V]:Offset from the nominal current/temperature corrected value in Volts");
-
-        // Feedback::State::kTempCtrlIdle, Feedback::State::kFeedbackCtrlIdle, Feedback::State::kTempCtrlRunning, Feedback::State::kFeedbackCtrlRunning
+             "|Uov[V]:Overvoltage to be applied (standard value is 1.1V)");
+
         AddEvent("STOP")
             (bind(&StateMachineFeedback::StopFeedback, this))
             ("Stop any control loop");
 
-        AddEvent("ENABLE_OUTPUT", "B:1")//, Feedback::State::kIdle)
-            (bind(&StateMachineFeedback::EnableOutput, this, placeholders::_1))
-            ("Enable sending of correction values caluclated by the control loop to the biasctrl");
-
-        AddEvent("STORE_REFERENCE")//, Feedback::State::kIdle)
-            (bind(&StateMachineFeedback::StoreReference, this))
-            ("Store the last (averaged) value as new reference (for debug purpose only)");
-
-        AddEvent("SET_REFERENCE", "F:1")//, Feedback::State::kIdle)
-            (bind(&StateMachineFeedback::SetReference, this, placeholders::_1))
-            ("Set a new global reference value (for debug purpose only)");
-
-        AddEvent("SET_Ki", "D:1")//, Feedback::State::kIdle)
-            (bind(&StateMachineFeedback::SetConstant, this, placeholders::_1, 0))
-            ("Set integral constant Ki");
-
-        AddEvent("SET_Kp", "D:1")//, Feedback::State::kIdle)
-            (bind(&StateMachineFeedback::SetConstant, this, placeholders::_1, 1))
-            ("Set proportional constant Kp");
-
-        AddEvent("SET_Kd", "D:1")//, Feedback::State::kIdle)
-            (bind(&StateMachineFeedback::SetConstant, this, placeholders::_1, 2))
-            ("Set derivative constant Kd");
-
-        AddEvent("SET_T", "D:1")//, Feedback::State::kIdle)
-            (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", Feedback::State::kConnectedFSC, Feedback::State::kConnected)//, Feedback::State::kIdle)
-            (bind(&StateMachineFeedback::CalibrateCurrents, this))
-            ("");
-
-        AddEvent("SET_CURRENT_REQUEST_INTERVAL", Feedback::State::kConnectedFSC, Feedback::State::kConnected)//, Feedback::State::kIdle)
-            (bind(&StateMachineFeedback::SetCurrentRequestInterval, this, placeholders::_1))
-            ("|interval[ms]:Interval between two current requests in modes which need that.");
-
-        // Verbosity commands
-//        AddEvent("SET_VERBOSE", "B:1")
-//            (bind(&StateMachineMCP::SetVerbosity, this, placeholders::_1))
-//            ("set verbosity state"
-//             "|verbosity[bool]:disable or enable verbosity for received data (yes/no), except dynamic data");
+
+        AddEvent("ENABLE_OLD_ALRGORITHM", "B:1", Feedback::State::kConnected, Feedback::State::kCalibrated)
+            (bind(&StateMachineFeedback::EnableOldAlgorithm, this, placeholders::_1));
+
 
         AddEvent("PRINT")
             (bind(&StateMachineFeedback::Print, this))
             ("");
-
         AddEvent("PRINT_CALIBRATION")
             (bind(&StateMachineFeedback::PrintCalibration, this))
             ("");
+
+        // Verbosity commands
+        AddEvent("SET_VERBOSE", "B:1")
+            (bind(&StateMachineFeedback::SetVerbosity, this, placeholders::_1))
+            ("set verbosity state"
+             "|verbosity[bool]:disable or enable verbosity when calculating overvoltage");
     }
 
     int EvalOptions(Configuration &conf)
     {
+        fIsVerbose = !conf.Get<bool>("quiet");
+
         if (!fMap.Read(conf.Get<string>("pixel-map-file")))
         {
@@ -1686,37 +1007,8 @@
             return 1;
         }
-
-        fGain = 0.1; // V(Amplitude) / V(Bias)
-
-        // 148 -> 248
-
-        // 33 : 10s  < 2%
-        // 50 :  5s  < 2%
-        // 66 :  3s  < 2%
-        // 85 :  2s  < 2%
-
-        fKp = 0;
-        fKd = 0;
-        fKi = 0.75;
-        fT  = 1;
-
-        // Is that independent of the aboslute real amplitude of
-        // the light pulser?
-
-        ostringstream msg;
-        msg << "Control loop parameters: ";
-        msg << "Kp=" << fKp << ", Kd=" << fKd << ", Ki=" << fKi << ", ";
-        if (fT>0)
-            msg << fT;
-        else
-            msg << "<auto>";
-        msg << ", Gain(DRS/BIAS)=" << fGain << "V/V";
-
-        Message(msg);
 
         fCurrentRequestInterval = conf.Get<uint16_t>("current-request-interval");
         fNumCalibIgnore         = conf.Get<uint16_t>("num-calib-ignore");
         fNumCalibRequests       = conf.Get<uint16_t>("num-calib-average");
-        fCalibrationOffset      = conf.Get<float>("calibration-offset");
 
         return -1;
@@ -1738,9 +1030,9 @@
     po::options_description control("Feedback options");
     control.add_options()
+        ("quiet,q", po_bool(true), "Disable printing more information on average overvoltagecontents of all received messages (except dynamic data) in clear text.")
         ("pixel-map-file",      var<string>()->required(), "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")
-        ("calibration-offset",  var<float>(-3), "Absolute offset relative to the G-APD operation voltage when calibrating")
         ;