2 files changed, 12 insertions, 4 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index 5785bafe4..bee00c049 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -501,6 +501,7 @@ uint32_t MillisecLoop(uint32_t tick)
         for (Disp_i = 0;Disp_i < MAX_SYSTEM_DISPENSERS;Disp_i++)
         {
             CalculateDispenserPressure(Disp_i);
+            Read_MidTank_Pressure_Sensor(Disp_i);
         }
         FPGA_GetAllDispensersValveBusyOCD();
         DrawerFansStatus = Read_Fans_Tacho();
@@ -522,7 +523,7 @@ uint32_t MillisecLoop(uint32_t tick)
     }
     //ROM_IntMasterEnable();
 
-
+    //loop_Run_Read_Speed(); - just for testing
     return OK;
 }
 /******************************************************************************
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index 78cdd5324..2110029e4 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -202,7 +202,7 @@ uint32_t  AddControlCallback( ControlCBFunction Callback, CTRL_TIMING_ENUM CtrlF
     unsigned int key;
 
     uint32_t device_i;
-    uint32_t deviceId = 0xFFFFFFFF;
+    uint32_t deviceId = 0xFF;
     for(device_i = 0;device_i < MAX_TANGO_CONTROL_DEVICES;device_i++)
     {
         if (ControlArray[device_i].ControlActive == false)
@@ -211,8 +211,8 @@ uint32_t  AddControlCallback( ControlCBFunction Callback, CTRL_TIMING_ENUM CtrlF
             break;
         }
     }
-    if (deviceId == 0xFFFFFFFF)
-        return 0xFFFFFFFF;
+    if (deviceId == 0xFF)
+        return 0xFF;
     key = GateMutex_enter(gateControlDB);
     ControlArray[deviceId].ControlTiming = CtrlFrequency;
     ControlArray[deviceId].ControlCallbackPtr = Callback;
@@ -221,6 +221,7 @@ uint32_t  AddControlCallback( ControlCBFunction Callback, CTRL_TIMING_ENUM CtrlF
     ControlArray[deviceId].Parameter1 = Parameter1;
     ControlArray[deviceId].IfIndex = IfIndex;
     GateMutex_leave(gateControlDB, key);
+    //LOG_ERROR(deviceId, "Add Callback");
 
     return deviceId;
 
@@ -239,11 +240,17 @@ int     RemoveControlCallback(uint32_t deviceId , ControlCBFunction Callback)
         ControlArray[deviceId].ControlCallbackPtr = NULL;
         ControlArray[deviceId].ControlDataReadPtr = NULL;
         ControlArray[deviceId].ControlActive = false;
+        ControlArray[deviceId].Parameter1 = 0;
+        ControlArray[deviceId].IfIndex = 0;
+        //LOG_ERROR(deviceId, "Remove Callback ");
         GateMutex_leave(gateControlDB, key);
         return OK;
     }
     else
+    {
+        LOG_ERROR(deviceId, "Remove Callback failed");
         return ERROR;
+    }
 
 }