diff options
| author | Shlomo Hecht <shlomo@twine-s.com> | 2020-02-04 18:51:57 +0200 |
|---|---|---|
| committer | Shlomo Hecht <shlomo@twine-s.com> | 2020-02-04 18:51:57 +0200 |
| commit | bee3522e23c95021a9637669a8091069af98e0c4 (patch) | |
| tree | 6b3f0148febe174de2b5e5a1baab359f7d1dd5e2 /Software/Embedded_SW/Embedded/Modules/Control/control.c | |
| parent | 3c241f9308d9a9f45278990ae19ef4b48c8240fe (diff) | |
| download | Tango-bee3522e23c95021a9637669a8091069af98e0c4.tar.gz Tango-bee3522e23c95021a9637669a8091069af98e0c4.zip | |
Version 1.4.6.8: remove most of head/whs ifdefs, checked to be working. fix control for missed milliseconds calls and more
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 |