merge

1 files changed, 58 insertions, 4 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index e093815a9..bea68e314 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -66,8 +66,9 @@
 
 
 
+#define DURATION_LIMIT 6
 /******************** Definitions  ********************************************/
-#define MAX_TANGO_CONTROL_DEVICES 120
+#define MAX_TANGO_CONTROL_DEVICES 100
 /******************** STRUCTURES AND ENUMs  ********************************************/
 
 
@@ -108,6 +109,7 @@ ControlDeviceStruc      ControlArray[MAX_TANGO_CONTROL_DEVICES];
 uint32_t                ControlDatalog[MAX_TANGO_CONTROL_DEVICES];
 #define MAX_BACKLOG_SIZE 100
 uint16_t ControlBacklog[MAX_BACKLOG_SIZE]={0};
+uint32_t ControlTime[MAX_TANGO_CONTROL_DEVICES]={0};
 uint16_t backlogindex = 0;
 uint32_t        Control_timerBase = TIMER0_BASE;        //Timer handle
 uint32_t MaxHighDevices = 0xFF;
@@ -382,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);
@@ -556,22 +559,44 @@ uint32_t ControlLoop(uint32_t tick)
 
     return OK;
 }
+uint32_t prevtick = 0;
+#ifdef CONTROL_DEBUG
+void ResetControlTime(void)
+{
+    memset(ControlTime,0,sizeof(ControlTime));
+}
+#endif
 uint32_t ControlLowLoop(uint32_t tick)
 {
+    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;
+        //if (tick-prevtick>10)
+         //   Report("ControlLowLoop skipped",__FILE__,tick,(int)prevtick,RpWarning,(int)skipped_ticks,0);
+    }
+    prevtick = tick;
+
     for (ControlLowDevice_i = 0; ControlLowDevice_i < MAX_TANGO_CONTROL_DEVICES;ControlLowDevice_i++)
     {
         if (ControlArray[ControlLowDevice_i].ControlActive)
         {
-            if (tick == ControlArray[ControlLowDevice_i].StartTick)
+            if (tick - ControlArray[ControlLowDevice_i].StartTick<=skipped_ticks)
                 continue;
             if (ControlArray[ControlLowDevice_i].ControlTiming == eOneMillisecond)
                 continue;
-            if (((tick - ControlArray[ControlLowDevice_i].StartTick)%ControlArray[ControlLowDevice_i].ControlTiming)==0) // run the control on exact intervals
+            if (((tick - ControlArray[ControlLowDevice_i].StartTick)%ControlArray[ControlLowDevice_i].ControlTiming)<=skipped_ticks) // run the control on exact intervals
             {
                 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
@@ -580,9 +605,38 @@ 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)
+                {
+                  delta = (32768 - tempp) + tempq + 1;
+                }
+                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;