Index: /trunk/FACT++/src/ratecontrol.cc
===================================================================
--- /trunk/FACT++/src/ratecontrol.cc	(revision 15026)
+++ /trunk/FACT++/src/ratecontrol.cc	(revision 15027)
@@ -14,4 +14,5 @@
 
 #include "HeadersFTM.h"
+#include "HeadersDrive.h"
 #include "HeadersRateScan.h"
 #include "HeadersRateControl.h"
@@ -40,4 +41,5 @@
     DimDescribedState fDimFTM;
     DimDescribedState fDimRS;
+    DimDescribedState fDimDrive;
 
     DimDescribedService fDimThreshold;
@@ -49,5 +51,8 @@
     uint16_t fThresholdReference;
 
+    deque<pair<Time,float>> fCurrents;
+
     bool fVerbose;
+    bool fCalibrateByCurrent;
 
     uint64_t fCounter;
@@ -362,5 +367,5 @@
         const FTM::DimTriggerRates &sdata = *static_cast<const FTM::DimTriggerRates*>(evt.GetData());
 
-        if (GetCurrentState()==RateControl::State::kSettingGlobalThreshold)
+        if (GetCurrentState()==RateControl::State::kSettingGlobalThreshold && !fCalibrateByCurrent)
             return ProcessCamera(sdata);
 
@@ -369,4 +374,64 @@
 
         return GetCurrentState();
+    }
+
+    int HandleCalibratedCurrents(const EventImp &evt)
+    {
+        // Check if received event is valid
+        if (!CheckEventSize(evt, (416+6)*4))
+            return GetCurrentState();
+
+        // Record only currents when the drive is tracking to avoid
+        // bias from the movement
+        if (fDimDrive.state()<Drive::State::kTracking)
+            return GetCurrentState();
+
+        // Get time and median current (FIXME: check N?)
+        const Time &time = evt.GetTime();
+        const float med  = evt.Get<float>(416*4+4+4);
+
+        // Keep all median currents of the past 10 seconds
+        fCurrents.push_back(make_pair(time, med));
+        while (fCurrents.size()>0)
+            if (time-fCurrents.front().first>boost::posix_time::seconds(10))
+                fCurrents.pop_front();
+
+        // If we are not doing a calibration no further action necessary
+        if (!fCalibrateByCurrent)
+            return GetCurrentState();
+
+        if (GetCurrentState()!=RateControl::State::kSettingGlobalThreshold)
+            return GetCurrentState();
+
+        // We want at least 8 values for averaging
+        if (fCurrents.size()<8)
+            return GetCurrentState();
+
+        // Calculate avera and rms of median
+        double avg = 0;
+        double rms = 0;
+        for (auto it=fCurrents.begin(); it!=fCurrents.end(); it++)
+        {
+            avg += it->second;
+            rms += it->second*it->second;
+        }
+        avg /= fCurrents.size();
+        rms /= fCurrents.size();
+
+        rms = sqrt(rms-avg*avg);
+
+        fThresholdMin = 36.0833*pow(avg, 0.638393)+184.037;
+        fThresholds.assign(160, fThresholdMin);
+
+        const RateControl::DimThreshold data = { fThresholdMin, fCalibrationTimeStart.Mjd(), Time().Mjd() };
+        fDimThreshold.setQuality(0);
+        fDimThreshold.Update(data);
+
+        ostringstream out;
+        out << setprecision(3);
+        out << "Measured average current " << avg << "uA +- " << rms << "uA [N=" << fCurrents.size() << "]... mininum threshold set to " << fThresholdMin;
+        Info(out);
+
+        return RateControl::State::kGlobalThresholdSet;
     }
 
@@ -388,4 +453,5 @@
         fCounter      = 0;
 
+        fCalibrateByCurrent = false;
         fCalibrationTimeStart = Time();
 
@@ -397,4 +463,26 @@
     }
 
+    int CalibrateByCurrent()
+    {
+        if (!fTriggerOn)
+        {
+            Info("Physics trigger not enabled... CALIBRATE command ignored.");
+            return RateControl::State::kGlobalThresholdSet;
+        }
+
+        if (fDimDrive.state()<Drive::State::kMoving)
+            Warn("Drive not even moving...");
+
+        fCounter = 0;
+        fCalibrateByCurrent = true;
+        fCalibrationTimeStart = Time();
+
+        ostringstream out;
+        out << "Rate calibration by current " << fThresholdReference << " with a target rate of " << fTargetRate << " Hz";
+        Info(out);
+
+        return RateControl::State::kSettingGlobalThreshold;
+    }
+
     int StopRC()
     {
@@ -429,4 +517,5 @@
         Out() << fDimFTM << endl;
         Out() << fDimRS << endl;
+        Out() << fDimDrive << endl;
 
         return GetCurrentState();
@@ -449,5 +538,5 @@
 
         // All subsystems are not connected
-        if (fDimFTM.state()<FTM::State::kConnected)
+        if (fDimFTM.state()<FTM::State::kConnected || fDimDrive.state()<Drive::State::kConnected)
             return RateControl::State::kDisconnected;
 
@@ -476,4 +565,5 @@
         fDimFTM("FTM_CONTROL"),
         fDimRS("RATE_SCAN"),
+        fDimDrive("DRIVE_CONTROL"),
 
         fDimThreshold("RATE_CONTROL/THRESHOLD", "S:1;D:1;D:1",
@@ -498,4 +588,6 @@
         Subscribe("FTM_CONTROL/STATIC_DATA")
             (bind(&StateMachineRateControl::HandleStaticData,   this, placeholders::_1));
+        Subscribe("FEEDBACK/CALIBRATED_CURRENTS")
+            (bind(&StateMachineRateControl::HandleCalibratedCurrents, this, placeholders::_1));
 
         // State names
@@ -521,4 +613,8 @@
             (bind(&StateMachineRateControl::Calibrate, this))
             ("Start a search for a reasonable minimum global threshold");
+
+        AddEvent("CALIBRATE_BY_CURRENT")
+            (bind(&StateMachineRateControl::CalibrateByCurrent, this))
+            ("Set the global threshold from the median current");
 
         AddEvent("STOP", RateControl::State::kSettingGlobalThreshold, RateControl::State::kGlobalThresholdSet, RateControl::State::kInProgress)