1

/*

     2

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

     3

 * concatained after this template

     4

 ** /

     5

     6

    48

	/*

    49

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

    50

	*/

    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

}