4

     6

     7

/*

     9

 **/ 

    10

void config_run__(int tick);

    11

void config_init__(void);

    16

void __init_debug(void);

    17

void __cleanup_debug(void);

    20

    25

    26

/*

    28

 **/

    29

IEC_TIME __CURRENT_TIME;

    30

int __tick = -1;

    31

    33

static int init_level = 0;

    34

    35

/*

    36

 * Prototypes of functions exported by plugins 

    37

 **/

    38

%(calls_prototypes)s

    39

    40

/*

    41

 * Retrieve input variables, run PLC and publish output variables 

    42

 **/

    43

void __run()

    44

{

    45

    __tick++;

    46

    47

    %(retrieve_calls)s

    48

    51

    /*__retrieve_debug();*/

    52

    53

    config_run__(__tick);

    54

    57

    __publish_debug();

    58

    59

    %(publish_calls)s

    60

    61

}

    62

    63

/*

    64

 * Initialize variables according to PLC's defalut values,

    65

 * and then init plugins with that values  

    66

 **/

    67

int __init(int argc,char **argv)

    68

{

    69

    int res;

    70

    config_init__();

    71

    __init_debug();

    72

    %(init_calls)s

    73

    return 0;

    74

}

    75

/*

    76

 * Calls plugin cleanup proc.

    77

 **/

    78

void __cleanup()

    79

{

    80

    %(cleanup_calls)s

    81

    __cleanup_debug();

    82

}

    83

    84

    85

void PLC_GetTime(IEC_TIME *CURRENT_TIME);

    86

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

    87

    88

#define CALIBRATED -2

    89

#define NOT_CALIBRATED -1

    90

static int calibration_count = NOT_CALIBRATED;

    91

static IEC_TIME cal_begin;

    92

static long long Tsync = 0;

    93

static long long FreqCorr = 0;

    94

static int Nticks = 0;

    95

static int  last_tick = 0;

    96

static long long Ttick = 0;

    97

#define mod %%

    98

/*

    99

 * Call this on each external sync, 

   100

 * @param sync_align_ratio 0->100 : align ratio, < 0 : no align, calibrate period 

   101

 **/

   102

void align_tick(int sync_align_ratio)

   103

{

   104

	/*

   105

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

   106

	*/

   107

	if(sync_align_ratio < 0){ /* Calibration */

   108

		if(calibration_count == CALIBRATED)

   109

			/* Re-calibration*/

   110

			calibration_count = NOT_CALIBRATED;

   111

		if(calibration_count == NOT_CALIBRATED)

   112

			/* Calibration start, get time*/

   113

			PLC_GetTime(&cal_begin);

   114

		calibration_count++;

   115

	}else{ /* do alignment (if possible) */

   116

		if(calibration_count >= 0){

   117

			/* End of calibration */

   118

			/* Get final time */

   119

			IEC_TIME cal_end;

   120

			PLC_GetTime(&cal_end);

   121

			/*adjust calibration_count*/

   122

			calibration_count++;

   123

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

   124

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

   125

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

   126

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

   127

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

   128

			}else{

   129

				FreqCorr = Tsync - (Ttick mod Tsync);

   130

			}

   131

			/*

   132

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

   133

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

   134

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

   135

			*/

   136

			calibration_count = CALIBRATED;

   137

		}

   138

		if(calibration_count == CALIBRATED){

   139

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

   140

			IEC_TIME now;

   141

			long long elapsed;

   142

			long long Tcorr;

   143

			long long PhaseCorr;

   144

			long long PeriodicTcorr;

   145

			PLC_GetTime(&now);

   146

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

   147

			if(Nticks > 0){

   148

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

   149

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

   150

				if(Nticks < 2){

   151

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

   152

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

   153

					PeriodicTcorr = Ttick + FreqCorr / Nticks;

   154

				}else{

   155

					PeriodicTcorr = Tcorr; 

   156

				}

   157

			}else if(__tick > last_tick){

   158

				last_tick = __tick;

   159

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

   160

				PeriodicTcorr = Tcorr = Ttick + PhaseCorr + FreqCorr;

   161

			}else{

   162

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

   163

				return;

   164

			}

   165

			/* DO ALIGNEMENT */

   166

			PLC_SetTimer(Tcorr - elapsed, PeriodicTcorr);

   167

		}

   168

	}

   169

}