targets/plc_main_tail.c
author Mario de Sousa <msousa@fe.up.pt>
Sat, 02 Jun 2018 11:56:01 +0100
changeset 2479 7f08b03a5d92
parent 1479 8f41aa88aa46
child 1800 1711339585ce
permissions -rw-r--r--
Modbus plugin: reset periodic activation timer when overrun occurs (delays due to communication errors, ...)
985
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
     1
/**
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
     2
 * Tail of code common to all C targets
209
08dc3d064cb5 Moved template C code to targets dir. Cleaned up some forgotten print.
etisserant
parents:
diff changeset
     3
 **/
985
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
     4
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
     5
/** 
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
     6
 * LOGGING
209
08dc3d064cb5 Moved template C code to targets dir. Cleaned up some forgotten print.
etisserant
parents:
diff changeset
     7
 **/
910
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
     8
944
52a17be9c4d1 Introduce Beremiz Native POU library. Now LOGGER POU is part of Beremiz' native POU library
Edouard Tisserant
parents: 923
diff changeset
     9
#ifndef LOG_BUFFER_SIZE
910
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    10
#define LOG_BUFFER_SIZE (1<<14) /*16Ko*/
944
52a17be9c4d1 Introduce Beremiz Native POU library. Now LOGGER POU is part of Beremiz' native POU library
Edouard Tisserant
parents: 923
diff changeset
    11
#endif
985
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
    12
#ifndef LOG_BUFFER_ATTRS
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
    13
#define LOG_BUFFER_ATTRS
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
    14
#endif
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
    15
910
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    16
#define LOG_BUFFER_MASK (LOG_BUFFER_SIZE-1)
985
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
    17
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
    18
static char LogBuff[LOG_LEVELS][LOG_BUFFER_SIZE] LOG_BUFFER_ATTRS;
1479
8f41aa88aa46 fix many compilation warnings about static variables used by non-static functions
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 1428
diff changeset
    19
static void inline copy_to_log(uint8_t level, uint32_t buffpos, void* buf, uint32_t size){
910
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    20
    if(buffpos + size < LOG_BUFFER_SIZE){
917
401e44bae7c0 Now logging have 4 levels
Edouard Tisserant
parents: 911
diff changeset
    21
        memcpy(&LogBuff[level][buffpos], buf, size);
910
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    22
    }else{
996
17d9c81b5ed8 Fixed typo in copy_to_log
Edouard Tisserant
parents: 995
diff changeset
    23
        uint32_t remaining = LOG_BUFFER_SIZE - buffpos; 
917
401e44bae7c0 Now logging have 4 levels
Edouard Tisserant
parents: 911
diff changeset
    24
        memcpy(&LogBuff[level][buffpos], buf, remaining);
991
afc4963d8f0c Fixed cast in target code to remove void* arithmetic warning
Edouard Tisserant
parents: 985
diff changeset
    25
        memcpy(LogBuff[level], (char*)buf + remaining, size - remaining);
917
401e44bae7c0 Now logging have 4 levels
Edouard Tisserant
parents: 911
diff changeset
    26
    }
401e44bae7c0 Now logging have 4 levels
Edouard Tisserant
parents: 911
diff changeset
    27
}
1479
8f41aa88aa46 fix many compilation warnings about static variables used by non-static functions
Andrey Skvortsov <andrej.skvortzov@gmail.com>
parents: 1428
diff changeset
    28
static void inline copy_from_log(uint8_t level, uint32_t buffpos, void* buf, uint32_t size){
910
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    29
    if(buffpos + size < LOG_BUFFER_SIZE){
917
401e44bae7c0 Now logging have 4 levels
Edouard Tisserant
parents: 911
diff changeset
    30
        memcpy(buf, &LogBuff[level][buffpos], size);
910
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    31
    }else{
911
ffa24427396a Log redirected to console, dump of all available log to console when connecting to PLC
Edouard Tisserant
parents: 910
diff changeset
    32
        uint32_t remaining = LOG_BUFFER_SIZE - buffpos; 
917
401e44bae7c0 Now logging have 4 levels
Edouard Tisserant
parents: 911
diff changeset
    33
        memcpy(buf, &LogBuff[level][buffpos], remaining);
991
afc4963d8f0c Fixed cast in target code to remove void* arithmetic warning
Edouard Tisserant
parents: 985
diff changeset
    34
        memcpy((char*)buf + remaining, LogBuff[level], size - remaining);
910
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    35
    }
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    36
}
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    37
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    38
/* Log buffer structure
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    39
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    40
 |<-Tail1.msgsize->|<-sizeof(mTail)->|<--Tail2.msgsize-->|<-sizeof(mTail)->|...
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    41
 |  Message1 Body  |      Tail1      |   Message2 Body   |      Tail2      |
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    42
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    43
*/
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    44
typedef struct {
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    45
    uint32_t msgidx;
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    46
    uint32_t msgsize;
921
a8db48ec2c31 Added log messages time-stamping
Edouard Tisserant
parents: 917
diff changeset
    47
    unsigned long tick;
a8db48ec2c31 Added log messages time-stamping
Edouard Tisserant
parents: 917
diff changeset
    48
    IEC_TIME time;
910
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    49
} mTail;
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    50
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    51
/* Log cursor : 64b
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    52
   |63 ... 32|31 ... 0|
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    53
   | Message | Buffer |
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    54
   | counter | Index  | */
995
5dcb361a55ef Extended LogCursor attributes
Edouard Tisserant
parents: 991
diff changeset
    55
static uint64_t LogCursor[LOG_LEVELS] LOG_BUFFER_ATTRS = {0x0,0x0,0x0,0x0};
910
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    56
1093
b5f78cff4459 Added support for resetting log messages
Laurent Bessard
parents: 1012
diff changeset
    57
void ResetLogCount(void) {
b5f78cff4459 Added support for resetting log messages
Laurent Bessard
parents: 1012
diff changeset
    58
	uint8_t level;
b5f78cff4459 Added support for resetting log messages
Laurent Bessard
parents: 1012
diff changeset
    59
	for(level=0;level<LOG_LEVELS;level++){
b5f78cff4459 Added support for resetting log messages
Laurent Bessard
parents: 1012
diff changeset
    60
		LogCursor[level] = 0;
b5f78cff4459 Added support for resetting log messages
Laurent Bessard
parents: 1012
diff changeset
    61
	}
b5f78cff4459 Added support for resetting log messages
Laurent Bessard
parents: 1012
diff changeset
    62
}
b5f78cff4459 Added support for resetting log messages
Laurent Bessard
parents: 1012
diff changeset
    63
910
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    64
/* Store one log message of give size */
1002
15c05ba95df4 LogMessage string pointer now char* instead of uint8_t*
Edouard Tisserant
parents: 1001
diff changeset
    65
int LogMessage(uint8_t level, char* buf, uint32_t size){
910
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    66
    if(size < LOG_BUFFER_SIZE - sizeof(mTail)){
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    67
        uint32_t buffpos;
921
a8db48ec2c31 Added log messages time-stamping
Edouard Tisserant
parents: 917
diff changeset
    68
        uint64_t new_cursor, old_cursor;
a8db48ec2c31 Added log messages time-stamping
Edouard Tisserant
parents: 917
diff changeset
    69
910
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    70
        mTail tail;
921
a8db48ec2c31 Added log messages time-stamping
Edouard Tisserant
parents: 917
diff changeset
    71
        tail.msgsize = size;
a8db48ec2c31 Added log messages time-stamping
Edouard Tisserant
parents: 917
diff changeset
    72
        tail.tick = __tick;
a8db48ec2c31 Added log messages time-stamping
Edouard Tisserant
parents: 917
diff changeset
    73
        PLC_GetTime(&tail.time);
a8db48ec2c31 Added log messages time-stamping
Edouard Tisserant
parents: 917
diff changeset
    74
910
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    75
        /* We cannot increment both msg index and string pointer 
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    76
           in a single atomic operation but we can detect having been interrupted.
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    77
           So we can try with atomic compare and swap in a loop until operation
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    78
           succeeds non interrupted */
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    79
        do{
917
401e44bae7c0 Now logging have 4 levels
Edouard Tisserant
parents: 911
diff changeset
    80
            old_cursor = LogCursor[level];
910
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    81
            buffpos = (uint32_t)old_cursor;
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    82
            tail.msgidx = (old_cursor >> 32); 
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    83
            new_cursor = ((uint64_t)(tail.msgidx + 1)<<32) 
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    84
                         | (uint64_t)((buffpos + size + sizeof(mTail)) & LOG_BUFFER_MASK);
954
ab487d32ce9a Made logging compatible with windows API
Edouard Tisserant
parents: 944
diff changeset
    85
        }while(AtomicCompareExchange64(
ab487d32ce9a Made logging compatible with windows API
Edouard Tisserant
parents: 944
diff changeset
    86
            (long long*)&LogCursor[level],
ab487d32ce9a Made logging compatible with windows API
Edouard Tisserant
parents: 944
diff changeset
    87
            (long long)old_cursor,
991
afc4963d8f0c Fixed cast in target code to remove void* arithmetic warning
Edouard Tisserant
parents: 985
diff changeset
    88
            (long long)new_cursor)!=(long long)old_cursor);
917
401e44bae7c0 Now logging have 4 levels
Edouard Tisserant
parents: 911
diff changeset
    89
401e44bae7c0 Now logging have 4 levels
Edouard Tisserant
parents: 911
diff changeset
    90
        copy_to_log(level, buffpos, buf, size);
401e44bae7c0 Now logging have 4 levels
Edouard Tisserant
parents: 911
diff changeset
    91
        copy_to_log(level, (buffpos + size) & LOG_BUFFER_MASK, &tail, sizeof(mTail));
910
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    92
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    93
        return 1; /* Success */
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    94
    }else{
1012
deb0fcab0c64 Fixed warning with LogMessage
Edouard Tisserant
parents: 1002
diff changeset
    95
    	char mstr[] = "Logging error : message too big";
917
401e44bae7c0 Now logging have 4 levels
Edouard Tisserant
parents: 911
diff changeset
    96
        LogMessage(LOG_CRITICAL, mstr, sizeof(mstr));
910
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    97
    }
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    98
    return 0;
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
    99
}
906
de452d65865c Python runtime now dlopens shared library immediatly after transfer, and release it only immediately before reloading a new one. This is probably going to reveal lot of dirty cleanups during start/stop cycles.
Edouard Tisserant
parents: 649
diff changeset
   100
917
401e44bae7c0 Now logging have 4 levels
Edouard Tisserant
parents: 911
diff changeset
   101
uint32_t GetLogCount(uint8_t level){
401e44bae7c0 Now logging have 4 levels
Edouard Tisserant
parents: 911
diff changeset
   102
    return (uint64_t)LogCursor[level] >> 32;
910
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
   103
}
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
   104
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
   105
/* Return message size and content */
921
a8db48ec2c31 Added log messages time-stamping
Edouard Tisserant
parents: 917
diff changeset
   106
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){
917
401e44bae7c0 Now logging have 4 levels
Edouard Tisserant
parents: 911
diff changeset
   107
    uint64_t cursor = LogCursor[level];
910
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
   108
    if(cursor){
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
   109
        /* seach cursor */
911
ffa24427396a Log redirected to console, dump of all available log to console when connecting to PLC
Edouard Tisserant
parents: 910
diff changeset
   110
        uint32_t stailpos = (uint32_t)cursor; 
910
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
   111
        uint32_t smsgidx;
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
   112
        mTail tail;
911
ffa24427396a Log redirected to console, dump of all available log to console when connecting to PLC
Edouard Tisserant
parents: 910
diff changeset
   113
        tail.msgidx = cursor >> 32;
ffa24427396a Log redirected to console, dump of all available log to console when connecting to PLC
Edouard Tisserant
parents: 910
diff changeset
   114
        tail.msgsize = 0;
910
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
   115
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
   116
        /* Message search loop */
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
   117
        do {
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
   118
            smsgidx = tail.msgidx;
911
ffa24427396a Log redirected to console, dump of all available log to console when connecting to PLC
Edouard Tisserant
parents: 910
diff changeset
   119
            stailpos = (stailpos - sizeof(mTail) - tail.msgsize ) & LOG_BUFFER_MASK;
917
401e44bae7c0 Now logging have 4 levels
Edouard Tisserant
parents: 911
diff changeset
   120
            copy_from_log(level, stailpos, &tail, sizeof(mTail));
911
ffa24427396a Log redirected to console, dump of all available log to console when connecting to PLC
Edouard Tisserant
parents: 910
diff changeset
   121
        }while((tail.msgidx == smsgidx - 1) && (tail.msgidx > msgidx));
910
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
   122
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
   123
        if(tail.msgidx == msgidx){
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
   124
            uint32_t sbuffpos = (stailpos - tail.msgsize ) & LOG_BUFFER_MASK; 
921
a8db48ec2c31 Added log messages time-stamping
Edouard Tisserant
parents: 917
diff changeset
   125
            uint32_t totalsize = tail.msgsize;
a8db48ec2c31 Added log messages time-stamping
Edouard Tisserant
parents: 917
diff changeset
   126
            *tick = tail.tick; 
a8db48ec2c31 Added log messages time-stamping
Edouard Tisserant
parents: 917
diff changeset
   127
            *tv_sec = tail.time.tv_sec; 
a8db48ec2c31 Added log messages time-stamping
Edouard Tisserant
parents: 917
diff changeset
   128
            *tv_nsec = tail.time.tv_nsec; 
a8db48ec2c31 Added log messages time-stamping
Edouard Tisserant
parents: 917
diff changeset
   129
            copy_from_log(level, sbuffpos, buf, 
a8db48ec2c31 Added log messages time-stamping
Edouard Tisserant
parents: 917
diff changeset
   130
                          totalsize > max_size ? max_size : totalsize);
910
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
   131
            return totalsize;
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
   132
        }
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
   133
    }
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
   134
    return 0;
f6d06bdd31e8 Experimental logging feature in PLC debug
Edouard Tisserant
parents: 906
diff changeset
   135
}
985
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   136
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   137
#define CALIBRATED -2
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   138
#define NOT_CALIBRATED -1
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   139
static int calibration_count = NOT_CALIBRATED;
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   140
static IEC_TIME cal_begin;
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   141
static long long Tsync = 0;
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   142
static long long FreqCorr = 0;
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   143
static int Nticks = 0;
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   144
static unsigned long last_tick = 0;
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   145
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   146
/*
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   147
 * Called on each external periodic sync event
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   148
 * make PLC tick synchronous with external sync
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   149
 * ratio defines when PLC tick occurs between two external sync
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   150
 * @param sync_align_ratio 
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   151
 *          0->100 : align ratio
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   152
 *          < 0 : no align, calibrate period
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   153
 **/
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   154
void align_tick(int sync_align_ratio)
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   155
{
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   156
	/*
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   157
	printf("align_tick(%d)\n", calibrate);
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   158
	*/
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   159
	if(sync_align_ratio < 0){ /* Calibration */
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   160
		if(calibration_count == CALIBRATED)
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   161
			/* Re-calibration*/
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   162
			calibration_count = NOT_CALIBRATED;
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   163
		if(calibration_count == NOT_CALIBRATED)
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   164
			/* Calibration start, get time*/
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   165
			PLC_GetTime(&cal_begin);
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   166
		calibration_count++;
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   167
	}else{ /* do alignment (if possible) */
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   168
		if(calibration_count >= 0){
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   169
			/* End of calibration */
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   170
			/* Get final time */
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   171
			IEC_TIME cal_end;
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   172
			PLC_GetTime(&cal_end);
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   173
			/*adjust calibration_count*/
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   174
			calibration_count++;
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   175
			/* compute mean of Tsync, over calibration period */
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   176
			Tsync = ((long long)(cal_end.tv_sec - cal_begin.tv_sec) * (long long)1000000000 +
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   177
					(cal_end.tv_nsec - cal_begin.tv_nsec)) / calibration_count;
1428
e14003eb4d42 Simplified use of runtime's global variable __common_ticktime accross extensions.
Edouard Tisserant
parents: 1093
diff changeset
   178
			if( (Nticks = (Tsync / common_ticktime__)) > 0){
e14003eb4d42 Simplified use of runtime's global variable __common_ticktime accross extensions.
Edouard Tisserant
parents: 1093
diff changeset
   179
				FreqCorr = (Tsync % common_ticktime__); /* to be divided by Nticks */
985
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   180
			}else{
1428
e14003eb4d42 Simplified use of runtime's global variable __common_ticktime accross extensions.
Edouard Tisserant
parents: 1093
diff changeset
   181
				FreqCorr = Tsync - (common_ticktime__ % Tsync);
985
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   182
			}
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   183
			/*
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   184
			printf("Tsync = %ld\n", Tsync);
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   185
			printf("calibration_count = %d\n", calibration_count);
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   186
			printf("Nticks = %d\n", Nticks);
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   187
			*/
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   188
			calibration_count = CALIBRATED;
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   189
		}
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   190
		if(calibration_count == CALIBRATED){
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   191
			/* Get Elapsed time since last PLC tick (__CURRENT_TIME) */
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   192
			IEC_TIME now;
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   193
			long long elapsed;
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   194
			long long Tcorr;
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   195
			long long PhaseCorr;
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   196
			long long PeriodicTcorr;
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   197
			PLC_GetTime(&now);
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   198
			elapsed = (now.tv_sec - __CURRENT_TIME.tv_sec) * 1000000000 + now.tv_nsec - __CURRENT_TIME.tv_nsec;
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   199
			if(Nticks > 0){
1428
e14003eb4d42 Simplified use of runtime's global variable __common_ticktime accross extensions.
Edouard Tisserant
parents: 1093
diff changeset
   200
				PhaseCorr = elapsed - (common_ticktime__ + FreqCorr/Nticks)*sync_align_ratio/100; /* to be divided by Nticks */
e14003eb4d42 Simplified use of runtime's global variable __common_ticktime accross extensions.
Edouard Tisserant
parents: 1093
diff changeset
   201
				Tcorr = common_ticktime__ + (PhaseCorr + FreqCorr) / Nticks;
985
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   202
				if(Nticks < 2){
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   203
					/* When Sync source period is near Tick time */
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   204
					/* PhaseCorr may not be applied to Periodic time given to timer */
1428
e14003eb4d42 Simplified use of runtime's global variable __common_ticktime accross extensions.
Edouard Tisserant
parents: 1093
diff changeset
   205
					PeriodicTcorr = common_ticktime__ + FreqCorr / Nticks;
985
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   206
				}else{
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   207
					PeriodicTcorr = Tcorr;
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   208
				}
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   209
			}else if(__tick > last_tick){
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   210
				last_tick = __tick;
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   211
				PhaseCorr = elapsed - (Tsync*sync_align_ratio/100);
1428
e14003eb4d42 Simplified use of runtime's global variable __common_ticktime accross extensions.
Edouard Tisserant
parents: 1093
diff changeset
   212
				PeriodicTcorr = Tcorr = common_ticktime__ + PhaseCorr + FreqCorr;
985
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   213
			}else{
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   214
				/*PLC did not run meanwhile. Nothing to do*/
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   215
				return;
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   216
			}
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   217
			/* DO ALIGNEMENT */
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   218
			PLC_SetTimer(Tcorr - elapsed, PeriodicTcorr);
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   219
		}
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   220
	}
cd8dadcef426 Re-organized C code templates for plc_main. Moved logging out of plc_debug. Factorized redundant _common_ticktime external declaration
Edouard Tisserant
parents: 969
diff changeset
   221
}