fix continuous bug, alarm handling timing, control timing, initialization.

1 files changed, 30 insertions, 6 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index e85a36057..02a216ba6 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -65,9 +65,10 @@
 #include "MillisecTask.h"
 
 
-
+//#define CONTROL_DEBUG
+#define DURATION_LIMIT 5
 /******************** Definitions  ********************************************/
-#define MAX_TANGO_CONTROL_DEVICES 120
+#define MAX_TANGO_CONTROL_DEVICES 100
 /******************** STRUCTURES AND ENUMs  ********************************************/
 
 
@@ -383,6 +384,7 @@ uint32_t  AddControlCallback(char* Name, ControlCBFunction Callback, uint32_t Ct
     if (deviceId == 0xFF)
     {
         LOG_ERROR(deviceId, "Add Callback failed");
+        AlarmHandlingSetAlarm(EVENT_TYPE__FPGA_WATCHDOG_ACTIVATED,true);
         return 0xFF;
     }
     key = GateMutex_enter(gateControlDB);
@@ -564,12 +566,16 @@ void ResetControlTime(void)
 }
 uint32_t ControlLowLoop(uint32_t tick)
 {
-    uint32_t tempp,tempq,delta;
     uint32_t skipped_ticks = 0;
+#ifdef CONTROL_DEBUG
+    uint32_t tempp,tempq,delta;
+    uint32_t sys_ticks_start = msec_millisecondCounter,sys_ticks_end,max = 0,dev = 0;
+#endif
     if (tick-prevtick>1)
     {
         skipped_ticks = tick-prevtick-1;
-        //Report("ControlLowLoop skipped",__FILE__,tick,(int)prevtick,RpWarning,(int)skipped_ticks,0);
+        if (tick-prevtick>10)
+            Report("ControlLowLoop skipped",__FILE__,tick,(int)prevtick,RpWarning,(int)skipped_ticks,0);
     }
     prevtick = tick;
 
@@ -586,8 +592,9 @@ uint32_t ControlLowLoop(uint32_t tick)
                 ControlBacklog[backlogindex]=ControlLowDevice_i;
                 if ( ++backlogindex >= MAX_BACKLOG_SIZE)
                     backlogindex = 0;
-
+#ifdef CONTROL_DEBUG
                 tempp = HibernateRTCSSGet();
+#endif
                 if(ControlArray[ControlLowDevice_i].ControlDataReadPtr)
                     ControlDatalog[ControlLowDevice_i] = ControlArray[ControlLowDevice_i].ControlDataReadPtr( ControlArray[ControlLowDevice_i].Parameter1);
                 else
@@ -596,6 +603,7 @@ uint32_t ControlLowLoop(uint32_t tick)
                     ControlArray[ControlLowDevice_i].ControlCallbackPtr(ControlArray[ControlLowDevice_i].IfIndex, ControlDatalog[ControlLowDevice_i]);
                 else
                     LOG_ERROR (ControlLowDevice_i, "Invalid callback ptr");
+#ifdef CONTROL_DEBUG
                 tempq = HibernateRTCSSGet();
                 if (tempq < tempp)
                 {
@@ -603,14 +611,30 @@ uint32_t ControlLowLoop(uint32_t tick)
                 }
                 else
                   delta = tempq - tempp;
-
                 if (ControlTime[ControlLowDevice_i]<delta)
                 {
                     ControlTime[ControlLowDevice_i] = delta;
                 }
+#endif
             }
         } //if control active
     }  //for
+#ifdef CONTROL_DEBUG
+    sys_ticks_end= msec_millisecondCounter;
+    if (sys_ticks_end-sys_ticks_start > DURATION_LIMIT)
+    {
+        Report("ControlLowLoop long",__FILE__,sys_ticks_end-sys_ticks_start,(int)sys_ticks_end,RpWarning,(int)sys_ticks_start,0);
+        for (ControlLowDevice_i = 0; ControlLowDevice_i < MAX_TANGO_CONTROL_DEVICES;ControlLowDevice_i++)
+        {
+            if (ControlTime[ControlLowDevice_i]>max)
+            {
+                max = ControlTime[ControlLowDevice_i];
+                dev = ControlLowDevice_i;
+            }
+        }
+        Report(ControlArray[dev].Name,__FILE__,__LINE__,dev,RpWarning,max,0);
+    }
+#endif
     //ROM_IntMasterEnable();
 
     return OK;