Index: trunk/FACT++/src/feedback.cc
===================================================================
--- trunk/FACT++/src/feedback.cc	(revision 12177)
+++ trunk/FACT++/src/feedback.cc	(revision 12183)
@@ -52,6 +52,8 @@
         kStateConnecting,
         kStateConnected,
-        kStateRunning,
-        kStateControlling,
+        kStateTempCtrlIdle,
+        kStateFeedbackCtrlIdle,
+        kStateTempCtrlRunning,
+        kStateFeedbackCtrlRunning,
     };
 
@@ -62,11 +64,14 @@
     pair<Time, int> fStatusDim;
     pair<Time, int> fStatusFAD;
+    pair<Time, int> fStatusFSC;
     pair<Time, int> fStatusBias;
 
     DimStampedInfo fDim;
     DimStampedInfo fFAD;
+    DimStampedInfo fFSC;
     DimStampedInfo fBias;
 
     DimStampedInfo *fBiasData;
+    DimStampedInfo *fCameraTemp;
 
     DimDescribedService fDimReference;
@@ -144,8 +149,53 @@
         }
 
+        if (curr==&fFSC)
+        {
+            fStatusFSC = GetNewState(fFSC);
+            return;
+        }
+
         if (curr==&fDim)
         {
             fStatusDim = GetNewState(fDim);
             fStatusDim.second = curr->getSize()==4 ? curr->getInt() : 0;
+            return;
+        }
+
+        if (curr==fCameraTemp)
+        {
+            if (curr->getSize()==0)
+                return;
+            if (curr->getSize()!=60*sizeof(float))
+                return;
+
+            const float *ptr = (float*)curr->getData();
+
+            double avg = 0;
+            int    num = 0;
+            for (int i=1; i<32; i++)
+                if (ptr[i]!=0)
+                {
+                    avg += ptr[i];
+                    num++;
+                }
+
+            if (num==0)
+                return;
+
+            const double diff = (25-avg)*0.057;
+
+            vector<float> vec(2*BIAS::kNumChannels);
+            for (int i=0; i<BIAS::kNumChannels; i++)
+                vec[i+416] = diff;
+
+            fDimDeviation.Update(vec);
+
+            if (fOutputEnabled)
+            {
+                Info("Sending correction to feedback.");
+
+                dic_cmnd_service((char*)"BIAS_CONTROL/SET_GAPD_REFERENCE_OFFSET",
+                                 (void*)&diff, sizeof(float));
+            }
             return;
         }
@@ -465,8 +515,24 @@
     }
 
+    int StartTempCtrl()
+    {
+        fCameraTemp = new DimStampedInfo("FSC_CONTROL/TEMPERATURE", (void*)NULL, 0, this);
+
+        return GetCurrentState();
+    }
+
     int StopFeedback()
     {
-        delete fBiasData;
-        fBiasData = 0;
+        if (fBiasData)
+        {
+            delete fBiasData;
+            fBiasData = 0;
+        }
+
+        if (fCameraTemp)
+        {
+            delete fCameraTemp;
+            fCameraTemp = 0;
+        }
 
         return GetCurrentState();
@@ -494,4 +560,24 @@
         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);
 
@@ -512,11 +598,25 @@
             return kStateDimNetworkNA;
 
-        if (fStatusFAD.second<FAD::kConnected && fStatusBias.second<BIAS::kConnecting)
+        // All subsystems are not connected
+        if (fStatusFAD.second<FAD::kConnected &&
+            fStatusBias.second<BIAS::kConnecting &&
+            fStatusFSC.second<2)
             return kStateDisconnected;
 
-        if (fStatusFAD.second<FAD::kConnected && fStatusBias.second<BIAS::kConnected)
+        // At least one subsystem is not connected
+        if (fStatusFAD.second<FAD::kConnected ||
+            fStatusBias.second<BIAS::kConnected ||
+            fStatusFSC.second<2)
             return kStateConnecting;
 
-        return fBiasData ? kStateRunning : kStateConnected;
+        // All subsystems are connected
+
+        if (fBiasData)
+            return fOutputEnabled ? kStateFeedbackCtrlRunning : kStateFeedbackCtrlIdle;
+
+        if (fCameraTemp)
+            return fOutputEnabled ? kStateTempCtrlRunning : kStateTempCtrlIdle;
+
+        return kStateConnected;
     }
 
@@ -528,6 +628,7 @@
         fDim("DIS_DNS/VERSION_NUMBER",  (void*)NULL, 0, this),
         fFAD("FAD_CONTROL/STATE",       (void*)NULL, 0, this),
+        fFSC("FSC_CONTROL/STATE",       (void*)NULL, 0, this),
         fBias("BIAS_CONTROL/STATE",     (void*)NULL, 0, this),
-        fBiasData(0),
+        fBiasData(0), fCameraTemp(0),
         fDimReference("FEEDBACK/REFERENCE", "F:416",        ""),
         fDimDeviation("FEEDBACK/DEVIATION", "F:416;F:416",  ""),
@@ -543,25 +644,38 @@
         // State names
         AddStateName(kStateDimNetworkNA, "DimNetworkNotAvailable",
-                     ".");
+                     "The Dim DNS is not reachable.");
 
         AddStateName(kStateDisconnected, "Disconnected",
-                     ".");
+                     "The Dim DNS is reachable, but the required subsystems are not available.");
 
         AddStateName(kStateConnecting, "Connecting",
-                     ".");
+                     "Not all subsystems (FAD, FSC, Bias) are connected to their hardware.");
 
         AddStateName(kStateConnected, "Connected",
-                     ".");
-
-        AddStateName(kStateRunning, "Running",
-                     ".");
-
-        AddEvent("START")//, kStateIdle)
+                     "All needed subsystems are connected to their hardware, no action is performed.");
+
+        AddStateName(kStateFeedbackCtrlIdle, "FeedbackIdle",
+                     "Feedback control activated, but voltage output disabled.");
+
+        AddStateName(kStateTempCtrlIdle, "FeedbackIdle",
+                     "Temperature control activated, but voltage output disabled.");
+
+        AddStateName(kStateFeedbackCtrlRunning, "FeedbackControl",
+                     "Feedback control activated and voltage output enabled.");
+
+        AddStateName(kStateTempCtrlRunning, "TempControl",
+                     "Temperature control activated and voltage output enabled.");
+
+        AddEvent("START_FEEDBACK_CONTROL", kStateConnected)
             (bind(&StateMachineFeedback::StartFeedback, this))
-            ("Start control loop");
-
-        AddEvent("STOP")//, kStateIdle)
+            ("Start the feedback control loop");
+
+        AddEvent("START_TEMP_CONTROL", kStateConnected)
+            (bind(&StateMachineFeedback::StartTempCtrl, this))
+            ("Start the temperature control loop");
+
+        AddEvent("STOP", kStateTempCtrlIdle, kStateFeedbackCtrlIdle, kStateTempCtrlRunning, kStateFeedbackCtrlRunning)
             (bind(&StateMachineFeedback::StopFeedback, this))
-            ("Stop control loop");
+            ("Stop any control loop");
 
         AddEvent("ENABLE_OUTPUT", "B:1")//, kStateIdle)
@@ -572,4 +686,8 @@
             (bind(&StateMachineFeedback::StoreReference, this))
             ("Store the last (averaged) value as new reference (for debug purpose only)");
+
+        AddEvent("SET_REFERENCE", "F:1")//, kStateIdle)
+            (bind(&StateMachineFeedback::SetReference, this, placeholders::_1))
+            ("Set a new global reference value (for debug purpose only)");
 
         AddEvent("SET_Ki", "D:1")//, kStateIdle)
