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

/*#include "stdio.h" /* For debug */

    21

    22

/*

    23

 * Functions and variables provied by generated C softPLC

    24

 **/ 

    25

void config_run__(int tick);

    26

void config_init__(void);

    27

void __init_debug(void);

    28

void __cleanup_debug(void);

    29

    30

    31

/*

    32

 *  Functions and variables to export to generated C softPLC and plugins

    33

 **/

    34

    35

IEC_TIME __CURRENT_TIME;

    37

    38

static int init_level = 0;

    39

static int Debugging = 0;

    40

static int WasDebugging = 0;

    41

void AbortDebug();

    42

    43

/*

    44

 * Prototypes of functions exported by plugins 

    45

 **/

    46

%(calls_prototypes)s

    47

    48

/*

    49

 * Retrieve input variables, run PLC and publish output variables 

    50

 **/

    51

void __run()

    52

{

    55

    %(retrieve_calls)s

    56

    57

    if(Debugging) __retrieve_debug();

    58

    59

    config_run__(__tick);

    60

    61

    if(Debugging) __publish_debug();

    62

    else if(WasDebugging) AbortDebug();

    63

    WasDebugging = Debugging;

    64

    65

    %(publish_calls)s

    66

    67

}

    68

    69

/*

    70

 * Initialize variables according to PLC's defalut values,

    71

 * and then init plugins with that values  

    72

 **/

    73

int __init(int argc,char **argv)

    74

{

    75

    int res;

    76

    config_init__();

    77

    __init_debug();

    78

    %(init_calls)s

    79

    return 0;

    80

}

    81

/*

    82

 * Calls plugin cleanup proc.

    83

 **/

    84

void __cleanup()

    85

{

    86

    %(cleanup_calls)s

    87

    __cleanup_debug();

    88

}

    89

    90

    91

void PLC_GetTime(IEC_TIME *CURRENT_TIME);

    92

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

    93

    94

#define CALIBRATED -2

    95

#define NOT_CALIBRATED -1

    96

static int calibration_count = NOT_CALIBRATED;

    97

static IEC_TIME cal_begin;

    98

static long long Tsync = 0;

    99

static long long FreqCorr = 0;

   100

static int Nticks = 0;

   101

static int  last_tick = 0;

   102

static long long Ttick = 0;

   103

#define mod %%

   104

/*

   105

 * Call this on each external sync, 

   106

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

   107

 **/

   108

void align_tick(int sync_align_ratio)

   109

{

   110

	/*

   111

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

   112

	*/

   113

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

   114

		if(calibration_count == CALIBRATED)

   115

			/* Re-calibration*/

   116

			calibration_count = NOT_CALIBRATED;

   117

		if(calibration_count == NOT_CALIBRATED)

   118

			/* Calibration start, get time*/

   119

			PLC_GetTime(&cal_begin);

   120

		calibration_count++;

   121

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

   122

		if(calibration_count >= 0){

   123

			/* End of calibration */

   124

			/* Get final time */

   125

			IEC_TIME cal_end;

   126

			PLC_GetTime(&cal_end);

   127

			/*adjust calibration_count*/

   128

			calibration_count++;

   129

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

   130

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

   131

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

   132

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

   133

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

   134

			}else{

   135

				FreqCorr = Tsync - (Ttick mod Tsync);

   136

			}

   137

			/*

   138

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

   139

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

   140

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

   141

			*/

   142

			calibration_count = CALIBRATED;

   143

		}

   144

		if(calibration_count == CALIBRATED){

   145

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

   146

			IEC_TIME now;

   147

			long long elapsed;

   148

			long long Tcorr;

   149

			long long PhaseCorr;

   150

			long long PeriodicTcorr;

   151

			PLC_GetTime(&now);

   152

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

   153

			if(Nticks > 0){

   154

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

   155

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

   156

				if(Nticks < 2){

   157

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

   158

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

   159

					PeriodicTcorr = Ttick + FreqCorr / Nticks;

   160

				}else{

   161

					PeriodicTcorr = Tcorr; 

   162

				}

   163

			}else if(__tick > last_tick){

   164

				last_tick = __tick;

   165

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

   166

				PeriodicTcorr = Tcorr = Ttick + PhaseCorr + FreqCorr;

   167

			}else{

   168

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

   169

				return;

   170

			}

   171

			/* DO ALIGNEMENT */

   172

			PLC_SetTimer(Tcorr - elapsed, PeriodicTcorr);

   173

		}

   174

	}

   175

}

   176

   177

extern int WaitDebugData();

   178

void suspendDebug()

   179

{

   180

    /* Prevent PLC to enter debug code */

   181

    Debugging = 0;

   182

    /* wait next tick end to be sure*/

   183

    WaitDebugData();

   184

}

   185

   186

void resumeDebug()

   187

{

   188

    /* Let PLC enter debug code */

   189

    Debugging = 1;

   190

}