diff options
Diffstat (limited to 'Software/Embedded_SW/Embedded/Modules/Control/control.c')
| -rw-r--r-- | Software/Embedded_SW/Embedded/Modules/Control/control.c | 30 |
1 files changed, 29 insertions, 1 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c index e093815a9..e85a36057 100644 --- a/Software/Embedded_SW/Embedded/Modules/Control/control.c +++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c @@ -108,6 +108,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; @@ -556,8 +557,22 @@ uint32_t ControlLoop(uint32_t tick) return OK; } +uint32_t prevtick = 0; +void ResetControlTime(void) +{ + memset(ControlTime,0,sizeof(ControlTime)); +} uint32_t ControlLowLoop(uint32_t tick) { + uint32_t tempp,tempq,delta; + uint32_t skipped_ticks = 0; + if (tick-prevtick>1) + { + skipped_ticks = tick-prevtick-1; + //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) @@ -566,12 +581,13 @@ uint32_t ControlLowLoop(uint32_t tick) 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; + tempp = HibernateRTCSSGet(); if(ControlArray[ControlLowDevice_i].ControlDataReadPtr) ControlDatalog[ControlLowDevice_i] = ControlArray[ControlLowDevice_i].ControlDataReadPtr( ControlArray[ControlLowDevice_i].Parameter1); else @@ -580,6 +596,18 @@ 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"); + tempq = HibernateRTCSSGet(); + if (tempq < tempp) + { + delta = (32768 - tempp) + tempq + 1; + } + else + delta = tempq - tempp; + + if (ControlTime[ControlLowDevice_i]<delta) + { + ControlTime[ControlLowDevice_i] = delta; + } } } //if control active } //for |