Merge branch 'master' of https://twinetfs.visualstudio.com/Tango/_git/Tango

3 files changed, 44 insertions, 3 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index 4f78d2134..e8a60a8c4 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
@@ -458,9 +457,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 0bcaecb03..8d7e219ac 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.h
@@ -39,6 +39,10 @@ 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);