1 files changed, 32 insertions, 15 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index abc61972d..1d9686124 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -62,7 +62,8 @@ typedef struct
 {
   uint32_t PartId;  // the identity of the inspected/controlled part in the Devices enum.
   bool ControlActive;
-  uint32_t Parameter;
+  uint32_t Parameter1;
+  uint32_t Parameter2;
   DataReadCBFunction ControlDataReadPtr;
   ControlCBFunction ControlCallbackPtr;
   CTRL_TIMING_ENUM ControlTiming;
@@ -97,7 +98,7 @@ void OneMilliSecondFunction(UArg arg0);
 //**********************************************************************
 /******************** CODE  ********************************************/
 //**********************************************************************
-uint32_t TemplateDataReadCBFunction (uint32_t deviceID, uint32_t Parameter)
+uint32_t TemplateDataReadCBFunction (uint32_t deviceID, uint32_t Parameter1, uint32_t Parameter2)
 {
     return 0;
 }
@@ -160,23 +161,34 @@ void ControlStart(void)
  * both these callbacks can be removed. if a new call is arriving, it invalidates the previous one (no dual control or data)
  *
  ***************************************************************************************************************************************************/
-int  AddControlCallback(uint32_t deviceId, ControlCBFunction Callback, CTRL_TIMING_ENUM CtrlFrequency, DataReadCBFunction DriverfPtr, uint32_t Parameter )
+uint32_t  AddControlCallback( ControlCBFunction Callback, CTRL_TIMING_ENUM CtrlFrequency, DataReadCBFunction DriverfPtr, uint32_t Parameter1, uint32_t Parameter2 )
 {
-    assert(deviceId < MAX_TANGO_CONTROL_DEVICES);
     assert(Callback);
     assert(DriverfPtr);
     unsigned int key;
 
-
+    uint32_t device_i;
+    uint32_t deviceId = 0xFFFFFFFF;
+    for(device_i = 0;device_i < MAX_TANGO_CONTROL_DEVICES;device_i++)
+    {
+        if (ControlArray[device_i].ControlActive == false)
+        {
+            deviceId = device_i;
+            break;
+        }
+    }
+    if (deviceId == 0xFFFFFFFF)
+        return 0xFFFFFFFF;
     key = GateMutex_enter(gateControlDB);
     ControlArray[deviceId].ControlTiming = CtrlFrequency;
     ControlArray[deviceId].ControlCallbackPtr = Callback;
     ControlArray[deviceId].ControlActive = true;
     ControlArray[deviceId].ControlDataReadPtr = DriverfPtr;
-    ControlArray[deviceId].Parameter = Parameter;
+    ControlArray[deviceId].Parameter1 = Parameter1;
+    ControlArray[deviceId].Parameter2 = Parameter2;
     GateMutex_leave(gateControlDB, key);
 
-    return OK;
+    return deviceId;
 
 }
 int     RemoveControlCallback(uint32_t deviceId , ControlCBFunction Callback)
@@ -212,9 +224,6 @@ void OneMilliSecondControlInterrupt(UArg arg0)
     else
         ROM_TimerDisable(Control_timerBase, TIMER_A);
 
-    //trigger the ADC collection - check and set priorities to make sure handling timing is correct.
-    //we might want to call it from the task, afetr execution of other taks!!!
-    ADC_TriggerCollection();
     //send message to the control task
     Message.messageId = OneMillisec;
     Message.tick = UsersysTickGet();
@@ -243,33 +252,41 @@ uint32_t ControlLoop(uint32_t tick)
 
     for (Device_i = 0; Device_i < MAX_TANGO_CONTROL_DEVICES;Device_i++)
     {
-        if (ControlArray[Device_i].ControlDataReadPtr)
-            ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr(Device_i, ControlArray[Device_i].Parameter);
         if (ControlArray[Device_i].ControlActive)
         {
             switch (ControlArray[Device_i].ControlTiming)
             {
                 case eOneMillisecond:
+                        ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr(Device_i, ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
                         ControlArray[Device_i].ControlCallbackPtr(Device_i, ControlDatalog[Device_i]);
                     break;
                 case eTenMilliSecond:
                     if (Ten_msTick)
+                    {
+                        ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr(Device_i, ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
                         ControlArray[Device_i].ControlCallbackPtr(Device_i, ControlDatalog[Device_i]);
+                    }
                     break;
                 case eHunderdMillisecond:
                     if (Hundred_msTick)
+                    {
+                        ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr(Device_i, ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
                         ControlArray[Device_i].ControlCallbackPtr(Device_i, ControlDatalog[Device_i]);
+                    }
                     break;
                 case eOneSecond:
                     if (Onesecond_Tick)
+                    {
+                        ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr(Device_i, ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
                         ControlArray[Device_i].ControlCallbackPtr(Device_i, ControlDatalog[Device_i]);
+                    }
                     break;
                 default:
                     LOG_ERROR (ControlArray[Device_i].ControlTiming, "Invalid control timing value");
                     break;
-            }
-        }
-    }
+            } //switch
+        } //if control active
+    }  //for
 
     return OK;
 }