Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
--- 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()
--- 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 <locale.h>
#include <semaphore.h>
-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;
--- 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 <windows.h>
#include <locale.h>
-/* 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);
--- 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 ***/
--- 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
- **/
--- 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;
-}
--- /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);
+
+
+
--- /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);
+ }
+ }
+}