Index: /trunk/FACT++/src/ftmctrl.cc
===================================================================
--- /trunk/FACT++/src/ftmctrl.cc	(revision 16775)
+++ /trunk/FACT++/src/ftmctrl.cc	(revision 16776)
@@ -719,9 +719,9 @@
         return true;
     }
-    /*
+
     bool SetSelectedThresholds(const int32_t *th)
     {
         for (int i=0; i<FTM::StaticData::kMaxPatchIdx; i++)
-            if (th[i]<0 || th[i]>FTM::StaticData::kMaxDAC)
+            if (th[i]>FTM::StaticData::kMaxDAC)
                 return false;
 
@@ -730,20 +730,17 @@
         for (int i=0; i<=FTM::StaticData::kMaxPatchIdx; i++)
         {
-            if (fBufStaticData[i/4].fDac[i%4]!=th[i])
-            {
-
-                // Calculate offset in static data block
-                const uint16_t addr = (uintptr_t(&fStaticData[i/4].fDAC[i%4])-uintptr_t(&fStaticData))/2;
-
-                // From CmdSetRegister
-                const array<uint16_t, 2> data = {{ addr, uint16_t(th[i]) }};
-                PostCmd(data, FTM::kCmdWrite, FTM::kCmdRegister);
-
-                reinterpret_cast<uint16_t*>(&fBufStaticData)[addr] = th[i];
-            }
-        }
-
-        // Now execute change before the static data is requested back
-        PostCmd(FTM::kCmdConfigFTU, (patch/40) | (((patch/4)%10)<<8));
+            if (th[i]<0 || fBufStaticData[i/4].fDAC[i%4]==th[i])
+                continue;
+
+            // Calculate offset in static data block
+            const uint16_t addr = (uintptr_t(&fStaticData[i/4].fDAC[i%4])-uintptr_t(&fStaticData))/2;
+
+            reinterpret_cast<uint16_t*>(&fBufStaticData)[addr] = th[i];
+
+            // From CmdSetRegister
+            const array<uint16_t, 2> arr = {{ addr, uint16_t(th[i]) }};
+            PostCmd(arr, FTM::kCmdWrite, FTM::kCmdRegister);
+            PostCmd(FTM::kCmdConfigFTU, (i/40) | (((i/4)%10)<<8));
+        }
 
         //CmdGetRegister(addr);
@@ -751,6 +748,5 @@
 
         return true;
-    }*/
-
+    }
 
     bool SetAllThresholds(const int32_t *th)
@@ -1632,4 +1628,20 @@
     }
 
+    int SetSelectedThresholds(const EventImp &evt)
+    {
+        if (!CheckEventSize(evt.GetSize(), "SetSelectedThresholds", 160*4))
+            return T::kSM_FatalError;
+
+        const int32_t *data = evt.Ptr<int32_t>();
+        if (!fFTM.SetSelectedThresholds(data))
+        {
+            ostringstream msg;
+            msg << "SetAllThresholds - Value out of range, maximum 0xffff.";
+            T::Warn(msg);
+        }
+
+        return T::GetCurrentState();
+    }
+
     int SetAllThresholds(const EventImp &evt)
     {
@@ -2178,5 +2190,10 @@
              "|Threshold[counts]:Threshold to be set in binary counts");
 
-        T::AddEvent("SET_ALL_THRESHOLDS", "I:160", FTM::State::kIdle, FTM::State::kValid, FTM::State::kConfigured, FTM::State::kTriggerOn)
+        T::AddEvent("SET_SELECTED_THRESHOLDS", "I:160", FTM::State::kTriggerOn)
+            (bind(&StateMachineFTM::SetAllThresholds, this, placeholders::_1))
+            ("Set the comparator thresholds. Only thresholds which are different and >=0 are sent."
+             "|Thresholds[counts]:Threshold to be set in binary counts");
+
+        T::AddEvent("SET_ALL_THRESHOLDS", "I:160", FTM::State::kIdle, FTM::State::kValid, FTM::State::kConfigured)
             (bind(&StateMachineFTM::SetAllThresholds, this, placeholders::_1))
             ("Set the comparator thresholds"