etisserant@280: /** etisserant@280: * Code common to all C targets greg@332: **/ etisserant@209: etisserant@280: #include "iec_types.h" etisserant@209: /* laurent@397: * Prototypes of functions provided by generated C softPLC greg@332: **/ laurent@397: void config_run__(unsigned long tick); etisserant@209: void config_init__(void); etisserant@280: etisserant@280: /* laurent@397: * Prototypes of functions provided by generated target C code etisserant@280: * */ etisserant@235: void __init_debug(void); etisserant@235: void __cleanup_debug(void); etisserant@280: /*void __retrieve_debug(void);*/ etisserant@280: void __publish_debug(void); etisserant@235: etisserant@209: /* etisserant@280: * Variables used by generated C softPLC and plugins etisserant@209: **/ etisserant@209: IEC_TIME __CURRENT_TIME; laurent@397: IEC_BOOL __DEBUG = 0; ed@446: unsigned long __tick = 0; laurent@397: laurent@397: /* laurent@397: * Variable generated by C softPLC and plugins laurent@397: **/ laurent@397: extern unsigned long greatest_tick_count__; etisserant@209: etisserant@280: /* Help to quit cleanly when init fail at a certain level */ etisserant@209: static int init_level = 0; etisserant@209: etisserant@209: /* greg@332: * Prototypes of functions exported by plugins etisserant@209: **/ etisserant@209: %(calls_prototypes)s etisserant@209: etisserant@209: /* greg@332: * Retrieve input variables, run PLC and publish output variables etisserant@209: **/ greg@423: void __run(void) etisserant@209: { etisserant@236: __tick++; laurent@397: if (greatest_tick_count__) laurent@397: __tick %%= greatest_tick_count__; etisserant@236: etisserant@209: %(retrieve_calls)s etisserant@209: etisserant@239: /*__retrieve_debug();*/ greg@332: etisserant@209: config_run__(__tick); etisserant@209: etisserant@239: __publish_debug(); greg@332: etisserant@209: %(publish_calls)s etisserant@209: etisserant@209: } etisserant@209: etisserant@209: /* laurent@397: * Initialize variables according to PLC's default values, greg@332: * and then init plugins with that values etisserant@209: **/ etisserant@209: int __init(int argc,char **argv) etisserant@209: { laurent@386: int res = 0; greg@423: init_level = 0; etisserant@209: config_init__(); etisserant@235: __init_debug(); etisserant@209: %(init_calls)s laurent@386: return res; etisserant@209: } etisserant@209: /* etisserant@209: * Calls plugin cleanup proc. etisserant@209: **/ greg@423: void __cleanup(void) etisserant@209: { etisserant@209: %(cleanup_calls)s etisserant@235: __cleanup_debug(); etisserant@209: } etisserant@209: etisserant@209: etisserant@209: void PLC_GetTime(IEC_TIME *CURRENT_TIME); edouard@518: void PLC_SetTimer(unsigned long long next, unsigned long long period); etisserant@209: etisserant@209: #define CALIBRATED -2 etisserant@209: #define NOT_CALIBRATED -1 etisserant@209: static int calibration_count = NOT_CALIBRATED; etisserant@209: static IEC_TIME cal_begin; etisserant@209: static long long Tsync = 0; etisserant@209: static long long FreqCorr = 0; etisserant@209: static int Nticks = 0; laurent@397: static unsigned long last_tick = 0; etisserant@209: static long long Ttick = 0; etisserant@209: #define mod %% etisserant@209: /* greg@332: * Call this on each external sync, greg@332: * @param sync_align_ratio 0->100 : align ratio, < 0 : no align, calibrate period etisserant@209: **/ etisserant@209: void align_tick(int sync_align_ratio) etisserant@209: { etisserant@209: /* etisserant@209: printf("align_tick(%%d)\n", calibrate); etisserant@209: */ etisserant@209: if(sync_align_ratio < 0){ /* Calibration */ etisserant@209: if(calibration_count == CALIBRATED) etisserant@209: /* Re-calibration*/ etisserant@209: calibration_count = NOT_CALIBRATED; etisserant@209: if(calibration_count == NOT_CALIBRATED) etisserant@209: /* Calibration start, get time*/ etisserant@209: PLC_GetTime(&cal_begin); etisserant@209: calibration_count++; etisserant@209: }else{ /* do alignment (if possible) */ etisserant@209: if(calibration_count >= 0){ etisserant@209: /* End of calibration */ etisserant@209: /* Get final time */ etisserant@209: IEC_TIME cal_end; etisserant@209: PLC_GetTime(&cal_end); etisserant@209: /*adjust calibration_count*/ etisserant@209: calibration_count++; greg@332: /* compute mean of Tsync, over calibration period */ etisserant@209: Tsync = ((long long)(cal_end.tv_sec - cal_begin.tv_sec) * (long long)1000000000 + etisserant@209: (cal_end.tv_nsec - cal_begin.tv_nsec)) / calibration_count; etisserant@209: if( (Nticks = (Tsync / Ttick)) > 0){ etisserant@209: FreqCorr = (Tsync mod Ttick); /* to be divided by Nticks */ etisserant@209: }else{ etisserant@209: FreqCorr = Tsync - (Ttick mod Tsync); etisserant@209: } etisserant@209: /* etisserant@209: printf("Tsync = %%ld\n", Tsync); etisserant@209: printf("calibration_count = %%d\n", calibration_count); etisserant@209: printf("Nticks = %%d\n", Nticks); etisserant@209: */ etisserant@209: calibration_count = CALIBRATED; etisserant@209: } etisserant@209: if(calibration_count == CALIBRATED){ etisserant@209: /* Get Elapsed time since last PLC tick (__CURRENT_TIME) */ etisserant@209: IEC_TIME now; etisserant@209: long long elapsed; etisserant@209: long long Tcorr; etisserant@209: long long PhaseCorr; etisserant@209: long long PeriodicTcorr; etisserant@209: PLC_GetTime(&now); etisserant@209: elapsed = (now.tv_sec - __CURRENT_TIME.tv_sec) * 1000000000 + now.tv_nsec - __CURRENT_TIME.tv_nsec; etisserant@209: if(Nticks > 0){ etisserant@209: PhaseCorr = elapsed - (Ttick + FreqCorr/Nticks)*sync_align_ratio/100; /* to be divided by Nticks */ etisserant@209: Tcorr = Ttick + (PhaseCorr + FreqCorr) / Nticks; etisserant@209: if(Nticks < 2){ etisserant@209: /* When Sync source period is near Tick time */ etisserant@209: /* PhaseCorr may not be applied to Periodic time given to timer */ etisserant@209: PeriodicTcorr = Ttick + FreqCorr / Nticks; etisserant@209: }else{ greg@332: PeriodicTcorr = Tcorr; etisserant@209: } etisserant@209: }else if(__tick > last_tick){ etisserant@209: last_tick = __tick; etisserant@209: PhaseCorr = elapsed - (Tsync*sync_align_ratio/100); etisserant@209: PeriodicTcorr = Tcorr = Ttick + PhaseCorr + FreqCorr; etisserant@209: }else{ etisserant@209: /*PLC did not run meanwhile. Nothing to do*/ etisserant@209: return; etisserant@209: } etisserant@209: /* DO ALIGNEMENT */ etisserant@209: PLC_SetTimer(Tcorr - elapsed, PeriodicTcorr); etisserant@209: } etisserant@209: } etisserant@209: } etisserant@280: etisserant@280: /** etisserant@280: * Prototypes for function provided by arch-specific code (main) etisserant@280: * is concatained hereafter greg@332: **/