merge

3 files changed, 51 insertions, 8 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index 4f78d2134..5943c11c5 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -44,7 +44,6 @@
 
 Task_Handle Millisecond_Task_Handle;
 /******************** Definitions  ********************************************/
-#define INVALID_MSG_ID    0xFFFF
 
 /******************** STRUCTURES AND ENUMs  ********************************************/
 typedef struct MillisecMotorData
@@ -148,15 +147,17 @@ void MillisecStop(void)
     MillisecRestart = false;
     ADCAcquireStop();
 }
-
+#define HIGH_TASK_FREQUENCY 120000
 void MillisecStart(void)
 {
     MillisecRestart = true;
-    ROM_TimerLoadSet(Millisec_timerBase, TIMER_A,120000/*one millisecond*/);
+
+    ROM_TimerLoadSet(Millisec_timerBase, TIMER_A,HIGH_TASK_FREQUENCY);
     ROM_TimerEnable(Millisec_timerBase, TIMER_A);
     ROM_IntEnable(INT_TIMER1A);
     ROM_TimerIntEnable(Millisec_timerBase, TIMER_TIMA_TIMEOUT);
     ADCAcquireStart(0,1);
+
 }
 
 void OneMilliSecondMillisecInterrupt(UArg arg0)
@@ -167,7 +168,7 @@ void OneMilliSecondMillisecInterrupt(UArg arg0)
     ROM_TimerIntClear(Millisec_timerBase, TIMER_TIMA_TIMEOUT);  // Clear the timer interrupt
     if (MillisecRestart == true)
     {
-        ROM_TimerLoadSet(Millisec_timerBase, TIMER_A,120000/*one millisecond*/);
+        ROM_TimerLoadSet(Millisec_timerBase, TIMER_A,HIGH_TASK_FREQUENCY);
     }
     else
 	{
@@ -458,9 +459,12 @@ uint32_t MillisecLowLoop(uint32_t tick)
         {
             MillisecReadFromTempSensor(Sensor_i, NULL);
         }
-        if (watchdogCriticalAlarm == false)
+        if (GeneralHwReady == true)
         {
+            if (watchdogCriticalAlarm == false)
+            {
                 Control_WD(ENABLE,5);  //activate heaters/dispenser watchdog, 0.5 seconds
+            }
         }
     }
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index b7521d796..e4abc5e3c 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -56,7 +56,6 @@
 #include "control.h"
 #include "MillisecTask.h"
 /******************** Definitions  ********************************************/
-#define INVALID_MSG_ID    0xFFFF
 #define MAX_TANGO_CONTROL_DEVICES 100
 /******************** STRUCTURES AND ENUMs  ********************************************/
 
@@ -282,6 +281,42 @@ uint32_t  AddControlCallback( ControlCBFunction Callback, CTRL_TIMING_ENUM CtrlF
     return deviceId;
 
 }
+int SafeRemoveHighControlCallback(uint32_t deviceId , ControlCBFunction Callback)
+{
+    if (RemoveControlCallback(deviceId, Callback )!=OK)
+    {
+        Report("Fixing Remove control ",__FILE__,__LINE__,(int)GetControlDevice_i(),RpWarning,(int)deviceId,0);
+        if (RemoveControlCallback(GetControlDevice_i(),Callback)==OK)
+        {
+            Report("Remove control callback fixed",__FILE__,__LINE__,(int)GetControlDevice_i(),RpWarning,(int)deviceId,0);
+        }
+        else
+        {
+            Report("Remove control callback failed",__FILE__,__LINE__,(int)GetControlDevice_i(),RpWarning,(int)deviceId,0);
+            return ERROR;
+        }
+    }
+    return OK;
+}
+
+int  SafeRemoveControlCallback(uint32_t deviceId , ControlCBFunction Callback)
+{
+    if (RemoveControlCallback(deviceId, Callback )!=OK)
+    {
+        Report("Fixing Remove control ",__FILE__,__LINE__,(int)GetControlLowDevice_i(),RpWarning,(int)deviceId,0);
+        if (RemoveControlCallback(GetControlLowDevice_i(),Callback)==OK)
+        {
+            Report("Remove control callback fixed",__FILE__,__LINE__,(int)GetControlLowDevice_i(),RpWarning,(int)deviceId,0);
+        }
+        else
+        {
+            Report("Remove control callback failed",__FILE__,__LINE__,(int)GetControlLowDevice_i(),RpWarning,(int)deviceId,0);
+            return ERROR;
+        }
+    }
+    return OK;
+}
+
 int     RemoveControlCallback(uint32_t deviceId , ControlCBFunction Callback)
 {
     if (deviceId == 0xFF)
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.h b/Software/Embedded_SW/Embedded/Modules/Control/control.h
index d50868064..8d7e219ac 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.h
@@ -39,9 +39,13 @@ void        ControlStop(void);
 void        ControlStart(void);
 uint32_t    AddControlCallback( ControlCBFunction Callback, CTRL_TIMING_ENUM CtrlFrequency, DataReadCBFunction DriverfPtr, uint16_t IfIndex, uint32_t Parameter1, uint32_t Parameter2 );
 int         RemoveControlCallback(uint32_t deviceId, ControlCBFunction Callback );
+//The safe remove command can be used ONLY when called from the control callback itself
+int         SafeRemoveControlCallback(uint32_t deviceId , ControlCBFunction Callback);
+int         SafeRemoveHighControlCallback(uint32_t deviceId , ControlCBFunction Callback);
+
 uint32_t    TemplateDataReadCBFunction (uint32_t deviceId, uint32_t Parameter1);
-uint32_t GetControlDevice_i(void);
-uint32_t GetControlLowDevice_i(void);
+uint32_t    GetControlDevice_i(void);
+uint32_t    GetControlLowDevice_i(void);
 
 extern Task_Handle Control_Task_Handle;
 extern uint32_t millisecondCounter;