runtime/plc_common_main.c
changeset 209 08dc3d064cb5
parent 208 dd630979f628
child 210 ca3bfcf61192
equal deleted inserted replaced
208:dd630979f628 209:08dc3d064cb5
     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 
       
    28 /*
       
    29  *  Functions and variables to export to generated C softPLC and plugins
       
    30  **/
       
    31  
       
    32 IEC_TIME __CURRENT_TIME;
       
    33 int __tick = 0;
       
    34 
       
    35 static int init_level = 0;
       
    36 static int Debugging = 1;
       
    37 
       
    38 /*
       
    39  * Prototypes of functions exported by plugins 
       
    40  **/
       
    41 %(calls_prototypes)s
       
    42 
       
    43 /*
       
    44  * Retrieve input variables, run PLC and publish output variables 
       
    45  **/
       
    46 void __run()
       
    47 {
       
    48     %(retrieve_calls)s
       
    49 
       
    50     if(Debugging) __retrieve_debug();
       
    51     
       
    52     config_run__(__tick);
       
    53 
       
    54     if(Debugging) __publish_debug();
       
    55     
       
    56     %(publish_calls)s
       
    57 
       
    58     __tick++;
       
    59 }
       
    60 
       
    61 /*
       
    62  * Initialize variables according to PLC's defalut values,
       
    63  * and then init plugins with that values  
       
    64  **/
       
    65 int __init(int argc,char **argv)
       
    66 {
       
    67     int res;
       
    68     config_init__();
       
    69     %(init_calls)s
       
    70     return 0;
       
    71 }
       
    72 /*
       
    73  * Calls plugin cleanup proc.
       
    74  **/
       
    75 void __cleanup()
       
    76 {
       
    77     %(cleanup_calls)s
       
    78 }
       
    79 
       
    80 
       
    81 void PLC_GetTime(IEC_TIME *CURRENT_TIME);
       
    82 void PLC_SetTimer(long long next, long long period);
       
    83 
       
    84 #define CALIBRATED -2
       
    85 #define NOT_CALIBRATED -1
       
    86 static int calibration_count = NOT_CALIBRATED;
       
    87 static IEC_TIME cal_begin;
       
    88 static long long Tsync = 0;
       
    89 static long long FreqCorr = 0;
       
    90 static int Nticks = 0;
       
    91 static int  last_tick = 0;
       
    92 static long long Ttick = 0;
       
    93 #define mod %%
       
    94 /*
       
    95  * Call this on each external sync, 
       
    96  * @param sync_align_ratio 0->100 : align ratio, < 0 : no align, calibrate period 
       
    97  **/
       
    98 void align_tick(int sync_align_ratio)
       
    99 {
       
   100 	/*
       
   101 	printf("align_tick(%%d)\n", calibrate);
       
   102 	*/
       
   103 	if(sync_align_ratio < 0){ /* Calibration */
       
   104 		if(calibration_count == CALIBRATED)
       
   105 			/* Re-calibration*/
       
   106 			calibration_count = NOT_CALIBRATED;
       
   107 		if(calibration_count == NOT_CALIBRATED)
       
   108 			/* Calibration start, get time*/
       
   109 			PLC_GetTime(&cal_begin);
       
   110 		calibration_count++;
       
   111 	}else{ /* do alignment (if possible) */
       
   112 		if(calibration_count >= 0){
       
   113 			/* End of calibration */
       
   114 			/* Get final time */
       
   115 			IEC_TIME cal_end;
       
   116 			PLC_GetTime(&cal_end);
       
   117 			/*adjust calibration_count*/
       
   118 			calibration_count++;
       
   119 			/* compute mean of Tsync, over calibration period */	
       
   120 			Tsync = ((long long)(cal_end.tv_sec - cal_begin.tv_sec) * (long long)1000000000 +
       
   121 					(cal_end.tv_nsec - cal_begin.tv_nsec)) / calibration_count;
       
   122 			if( (Nticks = (Tsync / Ttick)) > 0){
       
   123 				FreqCorr = (Tsync mod Ttick); /* to be divided by Nticks */
       
   124 			}else{
       
   125 				FreqCorr = Tsync - (Ttick mod Tsync);
       
   126 			}
       
   127 			/*
       
   128 			printf("Tsync = %%ld\n", Tsync);
       
   129 			printf("calibration_count = %%d\n", calibration_count);
       
   130 			printf("Nticks = %%d\n", Nticks);
       
   131 			*/
       
   132 			calibration_count = CALIBRATED;
       
   133 		}
       
   134 		if(calibration_count == CALIBRATED){
       
   135 			/* Get Elapsed time since last PLC tick (__CURRENT_TIME) */
       
   136 			IEC_TIME now;
       
   137 			long long elapsed;
       
   138 			long long Tcorr;
       
   139 			long long PhaseCorr;
       
   140 			long long PeriodicTcorr;
       
   141 			PLC_GetTime(&now);
       
   142 			elapsed = (now.tv_sec - __CURRENT_TIME.tv_sec) * 1000000000 + now.tv_nsec - __CURRENT_TIME.tv_nsec;
       
   143 			if(Nticks > 0){
       
   144 				PhaseCorr = elapsed - (Ttick + FreqCorr/Nticks)*sync_align_ratio/100; /* to be divided by Nticks */
       
   145 				Tcorr = Ttick + (PhaseCorr + FreqCorr) / Nticks;
       
   146 				if(Nticks < 2){
       
   147 					/* When Sync source period is near Tick time */
       
   148 					/* PhaseCorr may not be applied to Periodic time given to timer */
       
   149 					PeriodicTcorr = Ttick + FreqCorr / Nticks;
       
   150 				}else{
       
   151 					PeriodicTcorr = Tcorr; 
       
   152 				}
       
   153 			}else if(__tick > last_tick){
       
   154 				last_tick = __tick;
       
   155 				PhaseCorr = elapsed - (Tsync*sync_align_ratio/100);
       
   156 				PeriodicTcorr = Tcorr = Ttick + PhaseCorr + FreqCorr;
       
   157 			}else{
       
   158 				/*PLC did not run meanwhile. Nothing to do*/
       
   159 				return;
       
   160 			}
       
   161 			/* DO ALIGNEMENT */
       
   162 			PLC_SetTimer(Tcorr - elapsed, PeriodicTcorr);
       
   163 		}
       
   164 	}
       
   165 }
       
   166 
       
   167 int suspendDebug()
       
   168 {
       
   169     /* Prevent PLC to enter debug code */
       
   170     Debugging = 0;
       
   171 }
       
   172 
       
   173 int resumeDebug()
       
   174 {
       
   175     /* Let PLC enter debug code */
       
   176     Debugging = 1;
       
   177 }