# HG changeset patch # User Edouard Tisserant # Date 1363337273 -32400 # Node ID cd8dadcef4265bfd94ace5cdcc2a33ea976b295f # Parent 2d03056993f6bee05d76bcbb2242e876b176ba74 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration diff -r 2d03056993f6 -r cd8dadcef426 ProjectController.py --- a/ProjectController.py Fri Mar 15 00:38:53 2013 +0100 +++ b/ProjectController.py Fri Mar 15 17:47:53 2013 +0900 @@ -777,7 +777,7 @@ return debug_code - def Generate_plc_common_main(self): + def Generate_plc_main(self): """ Use confnodes layout given in LocationCFilesAndCFLAGS to generate glue code that dispatch calls to all confnodes @@ -789,7 +789,7 @@ # Generate main, based on template if not self.BeremizRoot.getDisable_Extensions(): - plc_main_code = targets.GetCode("plc_common_main") % { + plc_main_code = targets.GetCode("plc_main_head") % { "calls_prototypes":"\n".join([( "int __init_%(s)s(int argc,char **argv);\n"+ "void __cleanup_%(s)s(void);\n"+ @@ -809,7 +809,7 @@ "__cleanup_%s();"%locstrs[i-1] for i in xrange(len(locstrs), 0, -1)]) } else: - plc_main_code = targets.GetCode("plc_common_main") % { + plc_main_code = targets.GetCode("plc_main_head") % { "calls_prototypes":"\n", "retrieve_calls":"\n", "publish_calls":"\n", @@ -817,6 +817,7 @@ "cleanup_calls":"\n" } plc_main_code += targets.GetTargetCode(self.GetTarget().getcontent()["name"]) + plc_main_code += targets.GetCode("plc_main_tail") return plc_main_code @@ -897,7 +898,7 @@ # debugger code (self.Generate_plc_debugger, "plc_debugger.c", "Debugger"), # init/cleanup/retrieve/publish, run and align code - (self.Generate_plc_common_main,"plc_common_main.c","Common runtime")]: + (self.Generate_plc_main,"plc_main.c","Common runtime")]: try: # Do generate code = generator() diff -r 2d03056993f6 -r cd8dadcef426 targets/Linux/plc_Linux_main.c --- a/targets/Linux/plc_Linux_main.c Fri Mar 15 00:38:53 2013 +0100 +++ b/targets/Linux/plc_Linux_main.c Fri Mar 15 17:47:53 2013 +0900 @@ -11,7 +11,6 @@ #include #include -extern unsigned long long common_ticktime__; static sem_t Run_PLC; long AtomicCompareExchange(long* atomicvar,long compared, long exchange) @@ -97,8 +96,6 @@ { struct sigevent sigev; setlocale(LC_NUMERIC, "C"); - /* Define Ttick to 1ms if common_ticktime not defined */ - Ttick = common_ticktime__?common_ticktime__:1000000; PLC_shutdown = 0; diff -r 2d03056993f6 -r cd8dadcef426 targets/Win32/plc_Win32_main.c --- a/targets/Win32/plc_Win32_main.c Fri Mar 15 00:38:53 2013 +0100 +++ b/targets/Win32/plc_Win32_main.c Fri Mar 15 17:47:53 2013 +0900 @@ -8,8 +8,6 @@ #include #include -/* provided by POUS.C */ -extern unsigned long long common_ticktime__; long AtomicCompareExchange(long* atomicvar, long compared, long exchange) { @@ -77,8 +75,6 @@ unsigned long thread_id = 0; BOOL tmp; setlocale(LC_NUMERIC, "C"); - /* Define Ttick to 1ms if common_ticktime not defined */ - Ttick = common_ticktime__?common_ticktime__:1000000; InitializeCriticalSection(&Atomic64CS); diff -r 2d03056993f6 -r cd8dadcef426 targets/Xenomai/plc_Xenomai_main.c --- a/targets/Xenomai/plc_Xenomai_main.c Fri Mar 15 00:38:53 2013 +0100 +++ b/targets/Xenomai/plc_Xenomai_main.c Fri Mar 15 17:47:53 2013 +0900 @@ -38,8 +38,6 @@ #define PYTHON_PIPE_MINOR 3 #define PIPE_SIZE 1 -/* provided by POUS.C */ -extern unsigned long common_ticktime__; long AtomicCompareExchange(long* atomicvar,long compared, long exchange) { @@ -170,9 +168,6 @@ /* no memory swapping for that process */ mlockall(MCL_CURRENT | MCL_FUTURE); - /* Define Ttick to 1ms if common_ticktime not defined */ - Ttick = common_ticktime__?common_ticktime__:1000000; - PLC_shutdown = 0; /*** RT Pipes creation and opening ***/ diff -r 2d03056993f6 -r cd8dadcef426 targets/plc_common_main.c --- a/targets/plc_common_main.c Fri Mar 15 00:38:53 2013 +0100 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,173 +0,0 @@ -/** - * Code common to all C targets - **/ - -#include "iec_types.h" -/* - * Prototypes of functions provided by generated C softPLC - **/ -void config_run__(unsigned long tick); -void config_init__(void); - -/* - * Prototypes of functions provided by generated target C code - * */ -void __init_debug(void); -void __cleanup_debug(void); -/*void __retrieve_debug(void);*/ -void __publish_debug(void); - -/* - * Variables used by generated C softPLC and plugins - **/ -IEC_TIME __CURRENT_TIME; -IEC_BOOL __DEBUG = 0; -unsigned long __tick = 0; - -/* - * Variable generated by C softPLC and plugins - **/ -extern unsigned long greatest_tick_count__; - -/* Help to quit cleanly when init fail at a certain level */ -static int init_level = 0; - -/* - * Prototypes of functions exported by plugins - **/ -%(calls_prototypes)s - -/* - * Retrieve input variables, run PLC and publish output variables - **/ -void __run(void) -{ - __tick++; - if (greatest_tick_count__) - __tick %%= greatest_tick_count__; - - %(retrieve_calls)s - - /*__retrieve_debug();*/ - - config_run__(__tick); - - __publish_debug(); - - %(publish_calls)s - -} - -/* - * Initialize variables according to PLC's default values, - * and then init plugins with that values - **/ -int __init(int argc,char **argv) -{ - int res = 0; - init_level = 0; - config_init__(); - __init_debug(); - %(init_calls)s - return res; -} -/* - * Calls plugin cleanup proc. - **/ -void __cleanup(void) -{ - %(cleanup_calls)s - __cleanup_debug(); -} - - -void PLC_GetTime(IEC_TIME *CURRENT_TIME); -void PLC_SetTimer(unsigned long long next, unsigned long long period); - -#define CALIBRATED -2 -#define NOT_CALIBRATED -1 -static int calibration_count = NOT_CALIBRATED; -static IEC_TIME cal_begin; -static long long Tsync = 0; -static long long FreqCorr = 0; -static int Nticks = 0; -static unsigned long last_tick = 0; -static long long Ttick = 0; -#define mod %% -/* - * Call this on each external sync, - * @param sync_align_ratio 0->100 : align ratio, < 0 : no align, calibrate period - **/ -void align_tick(int sync_align_ratio) -{ - /* - printf("align_tick(%%d)\n", calibrate); - */ - if(sync_align_ratio < 0){ /* Calibration */ - if(calibration_count == CALIBRATED) - /* Re-calibration*/ - calibration_count = NOT_CALIBRATED; - if(calibration_count == NOT_CALIBRATED) - /* Calibration start, get time*/ - PLC_GetTime(&cal_begin); - calibration_count++; - }else{ /* do alignment (if possible) */ - if(calibration_count >= 0){ - /* End of calibration */ - /* Get final time */ - IEC_TIME cal_end; - PLC_GetTime(&cal_end); - /*adjust calibration_count*/ - calibration_count++; - /* compute mean of Tsync, over calibration period */ - Tsync = ((long long)(cal_end.tv_sec - cal_begin.tv_sec) * (long long)1000000000 + - (cal_end.tv_nsec - cal_begin.tv_nsec)) / calibration_count; - if( (Nticks = (Tsync / Ttick)) > 0){ - FreqCorr = (Tsync mod Ttick); /* to be divided by Nticks */ - }else{ - FreqCorr = Tsync - (Ttick mod Tsync); - } - /* - printf("Tsync = %%ld\n", Tsync); - printf("calibration_count = %%d\n", calibration_count); - printf("Nticks = %%d\n", Nticks); - */ - calibration_count = CALIBRATED; - } - if(calibration_count == CALIBRATED){ - /* Get Elapsed time since last PLC tick (__CURRENT_TIME) */ - IEC_TIME now; - long long elapsed; - long long Tcorr; - long long PhaseCorr; - long long PeriodicTcorr; - PLC_GetTime(&now); - elapsed = (now.tv_sec - __CURRENT_TIME.tv_sec) * 1000000000 + now.tv_nsec - __CURRENT_TIME.tv_nsec; - if(Nticks > 0){ - PhaseCorr = elapsed - (Ttick + FreqCorr/Nticks)*sync_align_ratio/100; /* to be divided by Nticks */ - Tcorr = Ttick + (PhaseCorr + FreqCorr) / Nticks; - if(Nticks < 2){ - /* When Sync source period is near Tick time */ - /* PhaseCorr may not be applied to Periodic time given to timer */ - PeriodicTcorr = Ttick + FreqCorr / Nticks; - }else{ - PeriodicTcorr = Tcorr; - } - }else if(__tick > last_tick){ - last_tick = __tick; - PhaseCorr = elapsed - (Tsync*sync_align_ratio/100); - PeriodicTcorr = Tcorr = Ttick + PhaseCorr + FreqCorr; - }else{ - /*PLC did not run meanwhile. Nothing to do*/ - return; - } - /* DO ALIGNEMENT */ - PLC_SetTimer(Tcorr - elapsed, PeriodicTcorr); - } - } -} - -/** - * Prototypes for function provided by arch-specific code (main) - * is concatained hereafter - **/ diff -r 2d03056993f6 -r cd8dadcef426 targets/plc_debug.c --- a/targets/plc_debug.c Fri Mar 15 00:38:53 2013 +0100 +++ b/targets/plc_debug.c Fri Mar 15 17:47:53 2013 +0900 @@ -304,128 +304,3 @@ return wait_error; } - -/* LOGGING -*/ - -#define LOG_LEVELS 4 -#define LOG_CRITICAL 0 -#define LOG_WARNING 1 -#define LOG_INFO 2 -#define LOG_DEBUG 3 - -#ifndef LOG_BUFFER_SIZE -#define LOG_BUFFER_SIZE (1<<14) /*16Ko*/ -#endif -#define LOG_BUFFER_MASK (LOG_BUFFER_SIZE-1) -static char LogBuff[LOG_LEVELS][LOG_BUFFER_SIZE]; -void inline copy_to_log(uint8_t level, uint32_t buffpos, void* buf, uint32_t size){ - if(buffpos + size < LOG_BUFFER_SIZE){ - memcpy(&LogBuff[level][buffpos], buf, size); - }else{ - uint32_t remaining = LOG_BUFFER_SIZE - buffpos - 1; - memcpy(&LogBuff[level][buffpos], buf, remaining); - memcpy(LogBuff[level], buf + remaining, size - remaining); - } -} -void inline copy_from_log(uint8_t level, uint32_t buffpos, void* buf, uint32_t size){ - if(buffpos + size < LOG_BUFFER_SIZE){ - memcpy(buf, &LogBuff[level][buffpos], size); - }else{ - uint32_t remaining = LOG_BUFFER_SIZE - buffpos; - memcpy(buf, &LogBuff[level][buffpos], remaining); - memcpy(buf + remaining, LogBuff[level], size - remaining); - } -} - -/* Log buffer structure - - |<-Tail1.msgsize->|<-sizeof(mTail)->|<--Tail2.msgsize-->|<-sizeof(mTail)->|... - | Message1 Body | Tail1 | Message2 Body | Tail2 | - -*/ -typedef struct { - uint32_t msgidx; - uint32_t msgsize; - unsigned long tick; - IEC_TIME time; -} mTail; - -/* Log cursor : 64b - |63 ... 32|31 ... 0| - | Message | Buffer | - | counter | Index | */ -static uint64_t LogCursor[LOG_LEVELS] = {0x0,0x0,0x0,0x0}; - -/* Store one log message of give size */ -int LogMessage(uint8_t level, uint8_t* buf, uint32_t size){ - if(size < LOG_BUFFER_SIZE - sizeof(mTail)){ - uint32_t buffpos; - uint64_t new_cursor, old_cursor; - - mTail tail; - tail.msgsize = size; - tail.tick = __tick; - PLC_GetTime(&tail.time); - - /* We cannot increment both msg index and string pointer - in a single atomic operation but we can detect having been interrupted. - So we can try with atomic compare and swap in a loop until operation - succeeds non interrupted */ - do{ - old_cursor = LogCursor[level]; - buffpos = (uint32_t)old_cursor; - tail.msgidx = (old_cursor >> 32); - new_cursor = ((uint64_t)(tail.msgidx + 1)<<32) - | (uint64_t)((buffpos + size + sizeof(mTail)) & LOG_BUFFER_MASK); - }while(AtomicCompareExchange64( - (long long*)&LogCursor[level], - (long long)old_cursor, - (long long)new_cursor)!=old_cursor); - - copy_to_log(level, buffpos, buf, size); - copy_to_log(level, (buffpos + size) & LOG_BUFFER_MASK, &tail, sizeof(mTail)); - - return 1; /* Success */ - }else{ - uint8_t mstr[] = "Logging error : message too big"; - LogMessage(LOG_CRITICAL, mstr, sizeof(mstr)); - } - return 0; -} - -uint32_t GetLogCount(uint8_t level){ - return (uint64_t)LogCursor[level] >> 32; -} - -/* Return message size and content */ -uint32_t GetLogMessage(uint8_t level, uint32_t msgidx, char* buf, uint32_t max_size, uint32_t* tick, uint32_t* tv_sec, uint32_t* tv_nsec){ - uint64_t cursor = LogCursor[level]; - if(cursor){ - /* seach cursor */ - uint32_t stailpos = (uint32_t)cursor; - uint32_t smsgidx; - mTail tail; - tail.msgidx = cursor >> 32; - tail.msgsize = 0; - - /* Message search loop */ - do { - smsgidx = tail.msgidx; - stailpos = (stailpos - sizeof(mTail) - tail.msgsize ) & LOG_BUFFER_MASK; - copy_from_log(level, stailpos, &tail, sizeof(mTail)); - }while((tail.msgidx == smsgidx - 1) && (tail.msgidx > msgidx)); - - if(tail.msgidx == msgidx){ - uint32_t sbuffpos = (stailpos - tail.msgsize ) & LOG_BUFFER_MASK; - uint32_t totalsize = tail.msgsize; - *tick = tail.tick; - *tv_sec = tail.time.tv_sec; - *tv_nsec = tail.time.tv_nsec; - copy_from_log(level, sbuffpos, buf, - totalsize > max_size ? max_size : totalsize); - return totalsize; - } - } - return 0; -} diff -r 2d03056993f6 -r cd8dadcef426 targets/plc_main_head.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/targets/plc_main_head.c Fri Mar 15 17:47:53 2013 +0900 @@ -0,0 +1,96 @@ +/** + * Head of code common to all C targets + **/ + +#include "iec_types.h" +/* + * Prototypes of functions provided by generated C softPLC + **/ +void config_run__(unsigned long tick); +void config_init__(void); + +/* + * Prototypes of functions provided by generated target C code + * */ +void __init_debug(void); +void __cleanup_debug(void); +/*void __retrieve_debug(void);*/ +void __publish_debug(void); + +/* + * Variables used by generated C softPLC and plugins + **/ +IEC_TIME __CURRENT_TIME; +IEC_BOOL __DEBUG = 0; +unsigned long __tick = 0; + +/* + * Variable generated by C softPLC and plugins + **/ +extern unsigned long greatest_tick_count__; +/* Tick time provided by POUS.C */ +extern unsigned long long common_ticktime__; + +/* Effective tick time with 1ms default value */ +static long long Ttick = 1000000; + +/* Help to quit cleanly when init fail at a certain level */ +static int init_level = 0; + +/* + * Prototypes of functions exported by plugins + **/ +%(calls_prototypes)s + +/* + * Retrieve input variables, run PLC and publish output variables + **/ +void __run(void) +{ + __tick++; + if (greatest_tick_count__) + __tick %%= greatest_tick_count__; + + %(retrieve_calls)s + + /*__retrieve_debug();*/ + + config_run__(__tick); + + __publish_debug(); + + %(publish_calls)s + +} + +/* + * Initialize variables according to PLC's default values, + * and then init plugins with that values + **/ +int __init(int argc,char **argv) +{ + int res = 0; + init_level = 0; + + if(common_ticktime__) + Ttick = common_ticktime__; + + config_init__(); + __init_debug(); + %(init_calls)s + return res; +} +/* + * Calls plugin cleanup proc. + **/ +void __cleanup(void) +{ + %(cleanup_calls)s + __cleanup_debug(); +} + +void PLC_GetTime(IEC_TIME *CURRENT_TIME); +void PLC_SetTimer(unsigned long long next, unsigned long long period); + + + diff -r 2d03056993f6 -r cd8dadcef426 targets/plc_main_tail.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/targets/plc_main_tail.c Fri Mar 15 17:47:53 2013 +0900 @@ -0,0 +1,220 @@ +/** + * Tail of code common to all C targets + **/ + +/** + * LOGGING + **/ + +#define LOG_LEVELS 4 +#define LOG_CRITICAL 0 +#define LOG_WARNING 1 +#define LOG_INFO 2 +#define LOG_DEBUG 3 + +#ifndef LOG_BUFFER_SIZE +#define LOG_BUFFER_SIZE (1<<14) /*16Ko*/ +#endif +#ifndef LOG_BUFFER_ATTRS +#define LOG_BUFFER_ATTRS +#endif + +#define LOG_BUFFER_MASK (LOG_BUFFER_SIZE-1) + +static char LogBuff[LOG_LEVELS][LOG_BUFFER_SIZE] LOG_BUFFER_ATTRS; +void inline copy_to_log(uint8_t level, uint32_t buffpos, void* buf, uint32_t size){ + if(buffpos + size < LOG_BUFFER_SIZE){ + memcpy(&LogBuff[level][buffpos], buf, size); + }else{ + uint32_t remaining = LOG_BUFFER_SIZE - buffpos - 1; + memcpy(&LogBuff[level][buffpos], buf, remaining); + memcpy(LogBuff[level], buf + remaining, size - remaining); + } +} +void inline copy_from_log(uint8_t level, uint32_t buffpos, void* buf, uint32_t size){ + if(buffpos + size < LOG_BUFFER_SIZE){ + memcpy(buf, &LogBuff[level][buffpos], size); + }else{ + uint32_t remaining = LOG_BUFFER_SIZE - buffpos; + memcpy(buf, &LogBuff[level][buffpos], remaining); + memcpy(buf + remaining, LogBuff[level], size - remaining); + } +} + +/* Log buffer structure + + |<-Tail1.msgsize->|<-sizeof(mTail)->|<--Tail2.msgsize-->|<-sizeof(mTail)->|... + | Message1 Body | Tail1 | Message2 Body | Tail2 | + +*/ +typedef struct { + uint32_t msgidx; + uint32_t msgsize; + unsigned long tick; + IEC_TIME time; +} mTail; + +/* Log cursor : 64b + |63 ... 32|31 ... 0| + | Message | Buffer | + | counter | Index | */ +static uint64_t LogCursor[LOG_LEVELS] = {0x0,0x0,0x0,0x0}; + +/* Store one log message of give size */ +int LogMessage(uint8_t level, uint8_t* buf, uint32_t size){ + if(size < LOG_BUFFER_SIZE - sizeof(mTail)){ + uint32_t buffpos; + uint64_t new_cursor, old_cursor; + + mTail tail; + tail.msgsize = size; + tail.tick = __tick; + PLC_GetTime(&tail.time); + + /* We cannot increment both msg index and string pointer + in a single atomic operation but we can detect having been interrupted. + So we can try with atomic compare and swap in a loop until operation + succeeds non interrupted */ + do{ + old_cursor = LogCursor[level]; + buffpos = (uint32_t)old_cursor; + tail.msgidx = (old_cursor >> 32); + new_cursor = ((uint64_t)(tail.msgidx + 1)<<32) + | (uint64_t)((buffpos + size + sizeof(mTail)) & LOG_BUFFER_MASK); + }while(AtomicCompareExchange64( + (long long*)&LogCursor[level], + (long long)old_cursor, + (long long)new_cursor)!=old_cursor); + + copy_to_log(level, buffpos, buf, size); + copy_to_log(level, (buffpos + size) & LOG_BUFFER_MASK, &tail, sizeof(mTail)); + + return 1; /* Success */ + }else{ + uint8_t mstr[] = "Logging error : message too big"; + LogMessage(LOG_CRITICAL, mstr, sizeof(mstr)); + } + return 0; +} + +uint32_t GetLogCount(uint8_t level){ + return (uint64_t)LogCursor[level] >> 32; +} + +/* Return message size and content */ +uint32_t GetLogMessage(uint8_t level, uint32_t msgidx, char* buf, uint32_t max_size, uint32_t* tick, uint32_t* tv_sec, uint32_t* tv_nsec){ + uint64_t cursor = LogCursor[level]; + if(cursor){ + /* seach cursor */ + uint32_t stailpos = (uint32_t)cursor; + uint32_t smsgidx; + mTail tail; + tail.msgidx = cursor >> 32; + tail.msgsize = 0; + + /* Message search loop */ + do { + smsgidx = tail.msgidx; + stailpos = (stailpos - sizeof(mTail) - tail.msgsize ) & LOG_BUFFER_MASK; + copy_from_log(level, stailpos, &tail, sizeof(mTail)); + }while((tail.msgidx == smsgidx - 1) && (tail.msgidx > msgidx)); + + if(tail.msgidx == msgidx){ + uint32_t sbuffpos = (stailpos - tail.msgsize ) & LOG_BUFFER_MASK; + uint32_t totalsize = tail.msgsize; + *tick = tail.tick; + *tv_sec = tail.time.tv_sec; + *tv_nsec = tail.time.tv_nsec; + copy_from_log(level, sbuffpos, buf, + totalsize > max_size ? max_size : totalsize); + return totalsize; + } + } + return 0; +} + +#define CALIBRATED -2 +#define NOT_CALIBRATED -1 +static int calibration_count = NOT_CALIBRATED; +static IEC_TIME cal_begin; +static long long Tsync = 0; +static long long FreqCorr = 0; +static int Nticks = 0; +static unsigned long last_tick = 0; + +/* + * Called on each external periodic sync event + * make PLC tick synchronous with external sync + * ratio defines when PLC tick occurs between two external sync + * @param sync_align_ratio + * 0->100 : align ratio + * < 0 : no align, calibrate period + **/ +void align_tick(int sync_align_ratio) +{ + /* + printf("align_tick(%d)\n", calibrate); + */ + if(sync_align_ratio < 0){ /* Calibration */ + if(calibration_count == CALIBRATED) + /* Re-calibration*/ + calibration_count = NOT_CALIBRATED; + if(calibration_count == NOT_CALIBRATED) + /* Calibration start, get time*/ + PLC_GetTime(&cal_begin); + calibration_count++; + }else{ /* do alignment (if possible) */ + if(calibration_count >= 0){ + /* End of calibration */ + /* Get final time */ + IEC_TIME cal_end; + PLC_GetTime(&cal_end); + /*adjust calibration_count*/ + calibration_count++; + /* compute mean of Tsync, over calibration period */ + Tsync = ((long long)(cal_end.tv_sec - cal_begin.tv_sec) * (long long)1000000000 + + (cal_end.tv_nsec - cal_begin.tv_nsec)) / calibration_count; + if( (Nticks = (Tsync / Ttick)) > 0){ + FreqCorr = (Tsync % Ttick); /* to be divided by Nticks */ + }else{ + FreqCorr = Tsync - (Ttick % Tsync); + } + /* + printf("Tsync = %ld\n", Tsync); + printf("calibration_count = %d\n", calibration_count); + printf("Nticks = %d\n", Nticks); + */ + calibration_count = CALIBRATED; + } + if(calibration_count == CALIBRATED){ + /* Get Elapsed time since last PLC tick (__CURRENT_TIME) */ + IEC_TIME now; + long long elapsed; + long long Tcorr; + long long PhaseCorr; + long long PeriodicTcorr; + PLC_GetTime(&now); + elapsed = (now.tv_sec - __CURRENT_TIME.tv_sec) * 1000000000 + now.tv_nsec - __CURRENT_TIME.tv_nsec; + if(Nticks > 0){ + PhaseCorr = elapsed - (Ttick + FreqCorr/Nticks)*sync_align_ratio/100; /* to be divided by Nticks */ + Tcorr = Ttick + (PhaseCorr + FreqCorr) / Nticks; + if(Nticks < 2){ + /* When Sync source period is near Tick time */ + /* PhaseCorr may not be applied to Periodic time given to timer */ + PeriodicTcorr = Ttick + FreqCorr / Nticks; + }else{ + PeriodicTcorr = Tcorr; + } + }else if(__tick > last_tick){ + last_tick = __tick; + PhaseCorr = elapsed - (Tsync*sync_align_ratio/100); + PeriodicTcorr = Tcorr = Ttick + PhaseCorr + FreqCorr; + }else{ + /*PLC did not run meanwhile. Nothing to do*/ + return; + } + /* DO ALIGNEMENT */ + PLC_SetTimer(Tcorr - elapsed, PeriodicTcorr); + } + } +}