diff options
Diffstat (limited to 'Software/Embedded_SW/Embedded/Modules/Control/control.c')
| -rw-r--r-- | Software/Embedded_SW/Embedded/Modules/Control/control.c | 62 |
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; |