etisserant@178: /*
etisserant@178:  * Prototypes for function provided by arch-specific code (main)
etisserant@178:  * concatained after this template
etisserant@178:  ** /
etisserant@178: 
etisserant@178: 
etisserant@49: /*
etisserant@49:  * Functions and variables provied by generated C softPLC
etisserant@49:  **/ 
etisserant@49: extern int common_ticktime__;
etisserant@49: 
etisserant@49: /*
etisserant@49:  * Functions and variables provied by plc.c
etisserant@49:  **/ 
etisserant@49: void run(long int tv_sec, long int tv_nsec);
etisserant@49: 
etisserant@49: #define maxval(a,b) ((a>b)?a:b)
etisserant@49: 
etisserant@49: #include "iec_types.h"
etisserant@203: /*#include "stdio.h" /* For debug */
etisserant@49: 
etisserant@49: /*
etisserant@49:  * Functions and variables provied by generated C softPLC
etisserant@49:  **/ 
etisserant@49: void config_run__(int tick);
etisserant@49: void config_init__(void);
etisserant@49: 
etisserant@49: /*
etisserant@203:  *  Functions and variables to export to generated C softPLC and plugins
etisserant@49:  **/
etisserant@49:  
etisserant@49: IEC_TIME __CURRENT_TIME;
etisserant@203: int __tick = 0;
etisserant@49: 
etisserant@203: static int init_level = 0;
etisserant@203: static int Debugging = 1;
etisserant@49: 
etisserant@54: /*
lbessard@137:  * Prototypes of functions exported by plugins 
etisserant@54:  **/
etisserant@49: %(calls_prototypes)s
etisserant@49: 
etisserant@54: /*
lbessard@137:  * Retrieve input variables, run PLC and publish output variables 
etisserant@54:  **/
etisserant@49: void __run()
etisserant@49: {
lbessard@137:     %(retrieve_calls)s
etisserant@203: 
etisserant@203:     if(Debugging) __retrieve_debug();
etisserant@54:     
etisserant@203:     config_run__(__tick);
etisserant@203: 
etisserant@203:     if(Debugging) __publish_debug();
etisserant@54:     
etisserant@49:     %(publish_calls)s
etisserant@203: 
etisserant@203:     __tick++;
etisserant@49: }
etisserant@49: 
etisserant@54: /*
etisserant@54:  * Initialize variables according to PLC's defalut values,
etisserant@54:  * and then init plugins with that values  
etisserant@54:  **/
etisserant@57: int __init(int argc,char **argv)
etisserant@49: {
etisserant@57:     int res;
etisserant@49:     config_init__();
etisserant@49:     %(init_calls)s
etisserant@57:     return 0;
etisserant@49: }
etisserant@54: /*
etisserant@54:  * Calls plugin cleanup proc.
etisserant@54:  **/
etisserant@49: void __cleanup()
etisserant@49: {
etisserant@49:     %(cleanup_calls)s
etisserant@49: }
etisserant@49: 
etisserant@178: 
etisserant@178: void PLC_GetTime(IEC_TIME *CURRENT_TIME);
etisserant@178: void PLC_SetTimer(long long next, long long period);
etisserant@178: 
etisserant@178: #define CALIBRATED -2
etisserant@178: #define NOT_CALIBRATED -1
etisserant@178: static int calibration_count = NOT_CALIBRATED;
etisserant@178: static IEC_TIME cal_begin;
etisserant@178: static long long Tsync = 0;
etisserant@178: static long long FreqCorr = 0;
etisserant@178: static int Nticks = 0;
etisserant@178: static int  last_tick = 0;
etisserant@178: static long long Ttick = 0;
etisserant@178: #define mod %%
etisserant@178: /*
etisserant@178:  * Call this on each external sync, 
etisserant@203:  * @param sync_align_ratio 0->100 : align ratio, < 0 : no align, calibrate period 
etisserant@178:  **/
etisserant@203: void align_tick(int sync_align_ratio)
etisserant@178: {
etisserant@178: 	/*
etisserant@178: 	printf("align_tick(%%d)\n", calibrate);
etisserant@178: 	*/
etisserant@203: 	if(sync_align_ratio < 0){ /* Calibration */
etisserant@178: 		if(calibration_count == CALIBRATED)
etisserant@178: 			/* Re-calibration*/
etisserant@178: 			calibration_count = NOT_CALIBRATED;
etisserant@178: 		if(calibration_count == NOT_CALIBRATED)
etisserant@178: 			/* Calibration start, get time*/
etisserant@178: 			PLC_GetTime(&cal_begin);
etisserant@178: 		calibration_count++;
etisserant@203: 	}else{ /* do alignment (if possible) */
etisserant@178: 		if(calibration_count >= 0){
etisserant@178: 			/* End of calibration */
etisserant@178: 			/* Get final time */
etisserant@178: 			IEC_TIME cal_end;
etisserant@178: 			PLC_GetTime(&cal_end);
etisserant@178: 			/*adjust calibration_count*/
etisserant@178: 			calibration_count++;
etisserant@178: 			/* compute mean of Tsync, over calibration period */	
etisserant@178: 			Tsync = ((long long)(cal_end.tv_sec - cal_begin.tv_sec) * (long long)1000000000 +
etisserant@178: 					(cal_end.tv_nsec - cal_begin.tv_nsec)) / calibration_count;
etisserant@178: 			if( (Nticks = (Tsync / Ttick)) > 0){
etisserant@178: 				FreqCorr = (Tsync mod Ttick); /* to be divided by Nticks */
etisserant@178: 			}else{
etisserant@178: 				FreqCorr = Tsync - (Ttick mod Tsync);
etisserant@178: 			}
etisserant@178: 			/*
etisserant@178: 			printf("Tsync = %%ld\n", Tsync);
etisserant@178: 			printf("calibration_count = %%d\n", calibration_count);
etisserant@178: 			printf("Nticks = %%d\n", Nticks);
etisserant@178: 			*/
etisserant@178: 			calibration_count = CALIBRATED;
etisserant@178: 		}
etisserant@178: 		if(calibration_count == CALIBRATED){
etisserant@178: 			/* Get Elapsed time since last PLC tick (__CURRENT_TIME) */
etisserant@178: 			IEC_TIME now;
etisserant@178: 			long long elapsed;
etisserant@178: 			long long Tcorr;
etisserant@178: 			long long PhaseCorr;
etisserant@178: 			long long PeriodicTcorr;
etisserant@178: 			PLC_GetTime(&now);
etisserant@178: 			elapsed = (now.tv_sec - __CURRENT_TIME.tv_sec) * 1000000000 + now.tv_nsec - __CURRENT_TIME.tv_nsec;
etisserant@178: 			if(Nticks > 0){
etisserant@203: 				PhaseCorr = elapsed - (Ttick + FreqCorr/Nticks)*sync_align_ratio/100; /* to be divided by Nticks */
etisserant@178: 				Tcorr = Ttick + (PhaseCorr + FreqCorr) / Nticks;
etisserant@178: 				if(Nticks < 2){
etisserant@178: 					/* When Sync source period is near Tick time */
etisserant@178: 					/* PhaseCorr may not be applied to Periodic time given to timer */
etisserant@178: 					PeriodicTcorr = Ttick + FreqCorr / Nticks;
etisserant@178: 				}else{
etisserant@178: 					PeriodicTcorr = Tcorr; 
etisserant@178: 				}
etisserant@203: 			}else if(__tick > last_tick){
etisserant@203: 				last_tick = __tick;
etisserant@203: 				PhaseCorr = elapsed - (Tsync*sync_align_ratio/100);
etisserant@178: 				PeriodicTcorr = Tcorr = Ttick + PhaseCorr + FreqCorr;
etisserant@178: 			}else{
etisserant@178: 				/*PLC did not run meanwhile. Nothing to do*/
etisserant@178: 				return;
etisserant@178: 			}
etisserant@178: 			/* DO ALIGNEMENT */
etisserant@178: 			PLC_SetTimer(Tcorr - elapsed, PeriodicTcorr);
etisserant@178: 		}
etisserant@178: 	}
etisserant@178: }
etisserant@203: 
etisserant@203: int suspendDebug()
etisserant@203: {
etisserant@203:     /* Prevent PLC to enter debug code */
etisserant@203:     Debugging = 0;
etisserant@203: }
etisserant@203: 
etisserant@203: int resumeDebug()
etisserant@203: {
etisserant@203:     /* Let PLC enter debug code */
etisserant@203:     Debugging = 1;
etisserant@203: }