1

/*

     2

 * Prototypes for function provided by arch-specific code (main)

     3

 * concatained after this template

     4

 ** /

     5

     6

     7

/*

     8

 * Functions and variables provied by generated C softPLC

     9

 **/ 

    10

extern int common_ticktime__;

    11

    12

/*

    13

 * Functions and variables provied by plc.c

    14

 **/ 

    15

void run(long int tv_sec, long int tv_nsec);

    16

    17

#define maxval(a,b) ((a>b)?a:b)

    18

    19

#include "iec_types.h"

    20

    21

/*

    22

 * Functions and variables provied by generated C softPLC

    23

 **/ 

    24

void config_run__(int tick);

    25

void config_init__(void);

    26

    27

/*

    28

 *  Functions and variables to export to generated C softPLC

    29

 **/

    30

    31

IEC_TIME __CURRENT_TIME;

    32

    33

static int tick = 0;

    34

static int init_level=0;

    35

    36

/*

    37

 * Prototypes of functions exported by plugins 

    38

 **/

    39

%(calls_prototypes)s

    40

    41

/*

    42

 * Retrieve input variables, run PLC and publish output variables 

    43

 **/

    44

void __run()

    45

{

    46

    %(retrieve_calls)s

    47

    48

	/*

    49

	printf("run tick = %%d\n", tick + 1);

    50

	*/

    51

    config_run__(tick++);

    52

    53

    %(publish_calls)s

    54

}

    55

    56

/*

    57

 * Initialize variables according to PLC's defalut values,

    58

 * and then init plugins with that values  

    59

 **/

    60

int __init(int argc,char **argv)

    61

{

    62

    int res;

    63

    config_init__();

    64

    %(init_calls)s

    65

    return 0;

    66

}

    67

/*

    68

 * Calls plugin cleanup proc.

    69

 **/

    70

void __cleanup()

    71

{

    72

    %(cleanup_calls)s

    73

}

    74

    75

    76

void PLC_GetTime(IEC_TIME *CURRENT_TIME);

    77

void PLC_SetTimer(long long next, long long period);

    78

    79

#define CALIBRATED -2

    80

#define NOT_CALIBRATED -1

    81

static int calibration_count = NOT_CALIBRATED;

    82

static IEC_TIME cal_begin;

    83

static long long Tsync = 0;

    84

static long long FreqCorr = 0;

    85

static int Nticks = 0;

    86

static int  last_tick = 0;

    87

static long long Ttick = 0;

    88

#define mod %%

    89

/*

    90

 * Call this on each external sync, 

    91

 **/

    92

void align_tick(int calibrate)

    93

{

    94

	/*

    95

	printf("align_tick(%%d)\n", calibrate);

    96

	*/

    97

	if(calibrate){

    98

		if(calibration_count == CALIBRATED)

    99

			/* Re-calibration*/

   100

			calibration_count = NOT_CALIBRATED;

   101

		if(calibration_count == NOT_CALIBRATED)

   102

			/* Calibration start, get time*/

   103

			PLC_GetTime(&cal_begin);

   104

		calibration_count++;

   105

	}else{

   106

		if(calibration_count >= 0){

   107

			/* End of calibration */

   108

			/* Get final time */

   109

			IEC_TIME cal_end;

   110

			PLC_GetTime(&cal_end);

   111

			/*adjust calibration_count*/

   112

			calibration_count++;

   113

			/* compute mean of Tsync, over calibration period */	

   114

			Tsync = ((long long)(cal_end.tv_sec - cal_begin.tv_sec) * (long long)1000000000 +

   115

					(cal_end.tv_nsec - cal_begin.tv_nsec)) / calibration_count;

   116

			if( (Nticks = (Tsync / Ttick)) > 0){

   117

				FreqCorr = (Tsync mod Ttick); /* to be divided by Nticks */

   118

			}else{

   119

				FreqCorr = Tsync - (Ttick mod Tsync);

   120

			}

   121

			/*

   122

			printf("Tsync = %%ld\n", Tsync);

   123

			printf("calibration_count = %%d\n", calibration_count);

   124

			printf("Nticks = %%d\n", Nticks);

   125

			*/

   126

			calibration_count = CALIBRATED;

   127

		}

   128

		if(calibration_count == CALIBRATED){

   129

			/* Get Elapsed time since last PLC tick (__CURRENT_TIME) */

   130

			IEC_TIME now;

   131

			long long elapsed;

   132

			long long Tcorr;

   133

			long long PhaseCorr;

   134

			long long PeriodicTcorr;

   135

			PLC_GetTime(&now);

   136

			elapsed = (now.tv_sec - __CURRENT_TIME.tv_sec) * 1000000000 + now.tv_nsec - __CURRENT_TIME.tv_nsec;

   137

			if(Nticks > 0){

   138

				PhaseCorr = elapsed - (Ttick + FreqCorr/Nticks)*%(sync_align_ratio)d/100; /* to be divided by Nticks */

   139

				Tcorr = Ttick + (PhaseCorr + FreqCorr) / Nticks;

   140

				if(Nticks < 2){

   141

					/* When Sync source period is near Tick time */

   142

					/* PhaseCorr may not be applied to Periodic time given to timer */

   143

					PeriodicTcorr = Ttick + FreqCorr / Nticks;

   144

				}else{

   145

					PeriodicTcorr = Tcorr; 

   146

				}

   147

			}else if(tick > last_tick){

   148

				last_tick = tick;

   149

				PhaseCorr = elapsed - (Tsync*%(sync_align_ratio)d/100);

   150

				PeriodicTcorr = Tcorr = Ttick + PhaseCorr + FreqCorr;

   151

			}else{

   152

				/*PLC did not run meanwhile. Nothing to do*/

   153

				return;

   154

			}

   155

			/* DO ALIGNEMENT */

   156

			PLC_SetTimer(Tcorr - elapsed, PeriodicTcorr);

   157

		}

   158

	}

   159

}