1 |
/**************************************************************************** |
2 |
* F i l e D a t a |
3 |
* $Id: WS_WorkingSchedule_INFN.c,v 1.26 2005/03/20 18:27:47 sebastiani Exp $ |
4 |
* |
5 |
**************************************************************************** |
6 |
* S W D e v e l o p m e n t E n v i r o n m e n t |
7 |
* |
8 |
* $Author: sebastiani $ |
9 |
* : |
10 |
**************************************************************************** |
11 |
* |
12 |
*****************************************************************************/ |
13 |
|
14 |
|
15 |
/*============================= Include File ================================*/ |
16 |
|
17 |
#include <src/INFN/LU_SourceFileID_INFN.h> |
18 |
#define __FILEID__ _WS_WorkingSchedule_INFN__c |
19 |
#include <src/INFN/LU_LogUtility_INFN.h> |
20 |
#include <src/INFN/PRH_ParamHandler_INFN_auto.h> |
21 |
LU_DECL_MASK(); |
22 |
|
23 |
#include <src/INFN/PM_PamManager_INFN.h> |
24 |
#include <src/INFN/WS_WorkingSchedule_INFN.h> |
25 |
#include <src/BasicSW/RTEMSInterface/OS_rtems_p.h> |
26 |
#include <src/INFN/SMH_SelectModeHandler_INFN.h> |
27 |
#include <src/INFN/CM_Common_INFN.h> |
28 |
#include <src/INFN/PRH_ParamHandler_INFN_autopri.h> |
29 |
#include <src/INFN/PRH_ParamHandler_INFN.h> |
30 |
|
31 |
/*============================ Global define ================================*/ |
32 |
|
33 |
|
34 |
/*============================== global types ==============================*/ |
35 |
|
36 |
static WS_2_TABLE WS_2_Table[WS_2_N_TABLES]; |
37 |
|
38 |
/* returns the firs and last time value of a table */ |
39 |
#define WS_2_TABLE_FIRST(table) (WS_2_Table[table].basetime) |
40 |
#define WS_2_TABLE_LAST(table) WS_2_GetAbsValueEnd(table) |
41 |
#define WS_2_TABLE_EMPTY(table) (WS_2_Table[table].size == 0) |
42 |
|
43 |
/*****************************************************************************/ |
44 |
/*=========================== Structure define ==============================*/ |
45 |
|
46 |
|
47 |
/*****************************************************************************/ |
48 |
/*============================ Enumerate define =============================*/ |
49 |
|
50 |
/*****************************************************************************/ |
51 |
|
52 |
static WS WS_FavouriteWorkingSchedule; |
53 |
static WS WS_EffectiveWorkingSchedule; |
54 |
|
55 |
static RM_ACQ_SETTING WS_3_NextAcqMode = RM_ACQ_SETTING_B; |
56 |
|
57 |
/*=== WS O P E R A T I O N E N V I R O N M ==*/ |
58 |
|
59 |
/*****************************************************************************/ |
60 |
|
61 |
void WS_ZeroTableWS2() { |
62 |
unsigned short table; |
63 |
for(table=0;table<WS_2_N_TABLES;table++) |
64 |
WS_2_Table[table].size = 0; |
65 |
} |
66 |
|
67 |
status_code WS_Init() { |
68 |
|
69 |
WS_FavouriteWorkingSchedule=PRH_VAR_WS_FAVOURITE_WS; |
70 |
WS_EffectiveWorkingSchedule=WS_0; |
71 |
|
72 |
return SUCCESSFUL; |
73 |
} |
74 |
|
75 |
WS WS_Favourite() { |
76 |
return WS_FavouriteWorkingSchedule; |
77 |
} |
78 |
|
79 |
WS WS_Effective() { |
80 |
return WS_EffectiveWorkingSchedule; |
81 |
} |
82 |
|
83 |
|
84 |
/* return the absolute time of the die time of the table */ |
85 |
static TI_TIME WS_2_GetAbsValueEnd(UINT16 table) { |
86 |
TI_TIME ret_s=WS_2_Table[table].basetime; |
87 |
UINT16 i; |
88 |
for(i=0;i<WS_2_Table[table].size;i++) { |
89 |
ret_s+=WS_2_Table[table].record[i].duration*WS_2_TABLE_DURATION_UNIT_S; |
90 |
} |
91 |
return ret_s; |
92 |
} |
93 |
|
94 |
|
95 |
/* delete tables not needed any more */ |
96 |
status_code WS_2_ClearOldTables(TI_TIME now_s) { |
97 |
status_code status; |
98 |
BYTE table; |
99 |
for(table=0;table<WS_2_N_TABLES;table++) { |
100 |
if(!WS_2_TABLE_EMPTY(table)) |
101 |
if(now_s >= WS_2_TABLE_LAST(table)) { |
102 |
/* the table is not needed any more , clear it */ |
103 |
WS_2_Table[table].size = 0; |
104 |
} |
105 |
} |
106 |
return CM_RC_SUCCESSFUL; |
107 |
} |
108 |
|
109 |
|
110 |
static status_code WS_2_GetNextUsefulTableGap(TI_TIME now_s,TI_TIME *gap_s) { |
111 |
DVOLATILE int i,j; |
112 |
#define WS_2_MAX_TIME 0xFFFFFFFF |
113 |
DVOLATILE TI_TIME min_basetime=WS_2_MAX_TIME; |
114 |
WS_2_ClearOldTables(now_s); |
115 |
for(i=0;i<=WS_2_N_TABLES;i++) |
116 |
{ |
117 |
if(WS_2_Table[i].size!=0 && WS_2_Table[i].basetime<min_basetime) |
118 |
min_basetime=WS_2_Table[i].basetime; |
119 |
} |
120 |
if (min_basetime==WS_2_MAX_TIME) |
121 |
return CM_RC_NOTFOUND; |
122 |
else { |
123 |
*gap_s=(min_basetime-now_s); |
124 |
return CM_RC_SUCCESSFUL; |
125 |
} |
126 |
} |
127 |
|
128 |
|
129 |
|
130 |
/* look for a valid table of a given time. returns the table number in table*/ |
131 |
status_code WS_2_LookForValidTable(TI_TIME now_s,UINT16 *table) { |
132 |
status_code status; |
133 |
BYTE table_count; |
134 |
for(table_count=0;table_count<WS_2_N_TABLES;table_count++) { |
135 |
if( !WS_2_TABLE_EMPTY(table_count) && |
136 |
WS_2_TABLE_FIRST(table_count) <= now_s && |
137 |
now_s <= WS_2_TABLE_LAST(table_count) |
138 |
) { |
139 |
/* this is a valid table */ |
140 |
*table = table_count; |
141 |
return CM_RC_SUCCESSFUL; |
142 |
} |
143 |
} |
144 |
/* no valid table found */ |
145 |
return CM_RC_NOTFOUND; |
146 |
} |
147 |
|
148 |
/* look for a valid record in the given table. returns the acq infos of the record that match |
149 |
the time. Matching time means to be in an appropriate intervalal 'i' such that |
150 |
absolute_starting_time[i] <= now_s < absolute_end_time[i] |
151 |
*/ |
152 |
status_code WS_2_LookForValidInfo(TI_TIME now_s,UINT16 table,UINT16 *idx,RM_ACQ_SETTING *mode,TI_TIME *left_duration_run_ms) { |
153 |
unsigned short i; |
154 |
TI_TIME before_s=WS_2_Table[table].basetime; |
155 |
TI_TIME after_s; |
156 |
for(i=0;i<WS_2_Table[table].size;i++) { |
157 |
after_s=before_s+WS_2_Table[table].record[i].duration*WS_2_TABLE_DURATION_UNIT_S; |
158 |
if(before_s <= now_s && now_s < after_s) { |
159 |
*left_duration_run_ms = (after_s - now_s)*1000; |
160 |
*idx=i; |
161 |
*mode = (RM_ACQ_SETTING)WS_2_Table[table].record[i].acq_setting; |
162 |
return CM_RC_SUCCESSFUL; |
163 |
} |
164 |
before_s=after_s; |
165 |
} |
166 |
return CM_RC_NOTFOUND; |
167 |
} |
168 |
|
169 |
status_code WS_2_WriteWS2Tables() { |
170 |
return PRH_WriteWS2Tables(WS_2_Table); |
171 |
} |
172 |
|
173 |
status_code WS_2_ReadWS2Tables() { |
174 |
return PRH_ReadWS2Tables(WS_2_Table); |
175 |
} |
176 |
|
177 |
|
178 |
status_code WS_2_SetTable(MA_HEADER_MCMD *headerMcmd) { |
179 |
/* |
180 |
MCMD format explanation ---> into WS_2 Table |
181 |
|
182 |
Mi is 1 bit info |
183 |
ti is 8 bit info |
184 |
k is coded into 10 bit |
185 |
T is coded into 32 bit |
186 |
The table into mcmd look like this: |
187 |
|
188 |
------------- |
189 |
T |
190 |
k |
191 |
------------- |
192 |
M0 t0 |
193 |
M1 t1 |
194 |
... ... |
195 |
Mk-1 tk-1 |
196 |
------------- |
197 |
|
198 |
this info is unpacked into the Table[k] as followind mean: |
199 |
|
200 |
|
201 |
Table elem idx Duration field Mode |
202 |
0 t0 M0 |
203 |
1 t1 M1 |
204 |
.... .... ..... |
205 |
k-1 tk-1 Mk-1 |
206 |
|
207 |
*/ |
208 |
DVOLATILE unsigned short *BodyArea, *curPtr; |
209 |
DVOLATILE unsigned char offset = 0; |
210 |
DVOLATILE unsigned short table,len,i,info,old_info,mcmdlen; |
211 |
TI_TIME base_time; |
212 |
BodyArea = curPtr = (unsigned short *) SMH_GETPTR_BODYAREA(headerMcmd->PtrMCMD); |
213 |
/* skip the 6th word */ |
214 |
curPtr++; |
215 |
mcmdlen=(*curPtr & 0x01FF); |
216 |
curPtr++; |
217 |
|
218 |
/* skip the STAMP, already checked: */ |
219 |
curPtr+=2; |
220 |
|
221 |
/* get the time info (32 bit): */ |
222 |
base_time = (*curPtr) << 16; |
223 |
curPtr++; |
224 |
base_time |= *curPtr; |
225 |
curPtr++; |
226 |
|
227 |
/* read the 4 bit, containing the number of the table */ |
228 |
CM_READ_NEXT_BITS_UINT16(curPtr,offset,4,table); |
229 |
|
230 |
if(table >= WS_2_N_TABLES) { |
231 |
/*@LOG WS2: invalid table no*/ |
232 |
LU_INFN_LOG(LU_CRITICAL|LU_HA,LU_MASK(__FILEID__),__FILEID__,__LINE__,table); |
233 |
return CM_RC_MCMD_FORMAT_ERROR; |
234 |
} |
235 |
|
236 |
/* read the 10 bit, containing the number of infos */ |
237 |
CM_READ_NEXT_BITS_UINT16(curPtr,offset,10,len); |
238 |
WS_2_Table[table].size = len; |
239 |
WS_2_Table[table].basetime=base_time; |
240 |
if(len>0) { |
241 |
for(i=0;i<len && i<WS_2_TABLE_MAX_ELEM;i++) { |
242 |
CM_READ_NEXT_BITS_UINT16(curPtr,offset,9,info); |
243 |
WS_2_Table[table].record[i].acq_setting = (info >> 8)&0x1 ? RM_ACQ_SETTING_B : RM_ACQ_SETTING_A; |
244 |
WS_2_Table[table].record[i].duration = ((info & 0x00FF)); |
245 |
} |
246 |
} |
247 |
|
248 |
return CM_RC_SUCCESSFUL; |
249 |
} |
250 |
|
251 |
#ifdef DEBUG |
252 |
|
253 |
status_code WS_2_LogTable(int table,BOOL full) { |
254 |
int i; |
255 |
/*@LOG Table no.*/ |
256 |
LU_INFN_LOG(LU_NORMAL_TRACE,LU_MASK(__FILEID__),__FILEID__,__LINE__,table); |
257 |
/*@LOG Table basetime*/ |
258 |
LU_INFN_LOG(LU_NORMAL_TRACE,LU_MASK(__FILEID__),__FILEID__,__LINE__,WS_2_Table[table].basetime); |
259 |
/*@LOG Table size*/ |
260 |
LU_INFN_LOG(LU_NORMAL_TRACE,LU_MASK(__FILEID__),__FILEID__,__LINE__,WS_2_Table[table].size); |
261 |
if(full) |
262 |
for(i=0;i<WS_2_Table[table].size && i<WS_2_TABLE_MAX_ELEM;i++) { |
263 |
/*@LOG Table: 0xC0000000 | WS_2_Table[table].record[i].acq_setting | i<<16 */ |
264 |
LU_INFN_LOG(LU_NORMAL_TRACE,LU_MASK(__FILEID__),__FILEID__,__LINE__,0xC0000000 | WS_2_Table[table].record[i].acq_setting | i<<16); |
265 |
/*@LOG Table: WS_2_Table[table].record[i].duration */ |
266 |
LU_INFN_LOG(LU_NORMAL_TRACE,LU_MASK(__FILEID__),__FILEID__,__LINE__,WS_2_Table[table].record[i].duration); |
267 |
/*@LOG Table: WS_2_Table[table].record[i].duration*WS_2_TABLE_DURATION_UNIT_S */ |
268 |
LU_INFN_LOG(LU_NORMAL_TRACE,LU_MASK(__FILEID__),__FILEID__,__LINE__,WS_2_Table[table].record[i].duration*WS_2_TABLE_DURATION_UNIT_S); |
269 |
} |
270 |
|
271 |
} |
272 |
|
273 |
status_code WS_2_Log() { |
274 |
DVOLATILE int i; |
275 |
for(i=0;i<WS_2_N_TABLES;i++) { |
276 |
WS_2_LogTable(i,WS_2_FULL); |
277 |
} |
278 |
|
279 |
} |
280 |
#endif |
281 |
|
282 |
|
283 |
status_code WS_1_GetAcqSetting_and_time(TI_TIME OrbitOffset_ms, |
284 |
RM_ACQ_SETTING *acq_mode, |
285 |
TI_TIME *duration_run_ms, |
286 |
WS *effective_ws) { |
287 |
|
288 |
/* |
289 |
|
290 |
this is the orbit: |
291 |
|
292 |
+-------------------------------------------------------+ |
293 |
| A | B | A | B | A | |
294 |
|------|------|------|------|------|------|------|------| |
295 |
| | | | | | | | | |
296 |
| | | | | | | | +-- Ascending Node |
297 |
| | | | | | | | |
298 |
| | | | | | | +--- WS_1_SETTING[3] |
299 |
| | | | | | | |
300 |
| | | | | | +--- South Pole |
301 |
| | | | | | |
302 |
| | | | | +--- WS_1_SETTING[2] |
303 |
| | | | | |
304 |
| | | | +--- Descending Node |
305 |
| | | | |
306 |
| | | +--- WS_1_SETTING[1] |
307 |
| | | |
308 |
| | +--- Nord Pole |
309 |
| | |
310 |
| +--- WS_1_SETTING[0] or WS_1_SETTING[4] |
311 |
| |
312 |
+--Ascending Node |
313 |
|
314 |
|
315 |
|
316 |
*/ |
317 |
|
318 |
int idx=-1; |
319 |
UINT16 i; |
320 |
if(OrbitOffset_ms > PRH_VAR_WS_TIME_ORBIT) { |
321 |
*duration_run_ms=PRH_VAR_RM_TIME_MAX_RUN; |
322 |
*acq_mode =RM_ACQ_SETTING_B; |
323 |
*effective_ws =WS_0; |
324 |
}else{ |
325 |
|
326 |
for(i=0;i<PRH_ARR_N_WS_1_SETTING;i++) |
327 |
if(OrbitOffset_ms >= PRH_ARR_WS_1_SETTING[i]) |
328 |
idx=i; |
329 |
|
330 |
idx++; |
331 |
if(idx >= PRH_ARR_N_WS_1_SETTING) { |
332 |
*duration_run_ms=PRH_VAR_RM_TIME_MAX_RUN; |
333 |
*acq_mode =RM_ACQ_SETTING_B; |
334 |
}else{ |
335 |
// idx now is the next "transition point" |
336 |
*duration_run_ms=PRH_ARR_WS_1_SETTING[idx] - OrbitOffset_ms; |
337 |
*acq_mode = idx % 2 == 0 ? RM_ACQ_SETTING_A : RM_ACQ_SETTING_B; |
338 |
} |
339 |
*effective_ws = WS_1; |
340 |
} |
341 |
return CM_RC_SUCCESSFUL; |
342 |
} |
343 |
|
344 |
/*****************************************************************************/ |
345 |
/* @Function: WS_SetEffectiveWorkingSchedule */ |
346 |
/* @Purpose : */ |
347 |
/* set che effective working schedule based on the favourite working */ |
348 |
/* schedule. tipically done at the Ascending Node */ |
349 |
/* Also returns the acq_setting and the time_run (always) */ |
350 |
/* @@ */ |
351 |
/* @Parameter Name @Message */ |
352 |
/* status_code OUT Return code */ |
353 |
/* @@ */ |
354 |
/*****************************************************************************/ |
355 |
|
356 |
status_code WS_SetEffectiveWorkingSchedule(TI_TIME time_max_run_ms, |
357 |
RM_ACQ_SETTING *acq_mode, |
358 |
TI_TIME *duration_run_ms) { |
359 |
|
360 |
#define WS_CASE_WS0 do { \ |
361 |
WS_EffectiveWorkingSchedule = WS_0; \ |
362 |
result_acq_mode=RM_ACQ_SETTING_B; \ |
363 |
result_duration_run_ms=time_max_run_ms; \ |
364 |
} while(0) |
365 |
|
366 |
#define WS_CASE_WS1 do { \ |
367 |
if(TI_piGetOrbitOffset_s(&OrbitOffset_s) != TI_SUCCESSFUL) { \ |
368 |
WS_CASE_WS0; \ |
369 |
}else{ \ |
370 |
WS_1_GetAcqSetting_and_time(OrbitOffset_s*1000, \ |
371 |
&result_acq_mode, \ |
372 |
&result_duration_run_ms, \ |
373 |
&WS_EffectiveWorkingSchedule); \ |
374 |
} \ |
375 |
} while(0) |
376 |
|
377 |
TI_TIME unused,now_s; |
378 |
TI_TIME OrbitOffset_s; |
379 |
RM_ACQ_SETTING result_acq_mode; |
380 |
TI_TIME result_duration_run_ms,gap_s; |
381 |
BOOL avail_time; |
382 |
UINT16 table,idx; |
383 |
status_code s; |
384 |
switch(WS_FavouriteWorkingSchedule) { |
385 |
case WS_1: |
386 |
/* chech if the timing of the orbit is available */ |
387 |
WS_CASE_WS1; |
388 |
break; |
389 |
case WS_2: |
390 |
/* check if the TimeSync is available */ |
391 |
if(! (TI_piTimeSyncIsAvailable(&avail_time) == TI_SUCCESSFUL && avail_time)) { |
392 |
/* if the timesync is not available, do the same of WS1-case */ |
393 |
WS_CASE_WS1; |
394 |
}else{ |
395 |
if(TI_piGetCurrentMoscowTime_s(&now_s) == TI_SUCCESSFUL) { |
396 |
/* delete old tables: */ |
397 |
WS_2_ClearOldTables(now_s); |
398 |
|
399 |
if(WS_2_LookForValidTable(now_s,&table) == CM_RC_SUCCESSFUL) { |
400 |
if(WS_2_LookForValidInfo(now_s,table,&idx,&result_acq_mode,&result_duration_run_ms) == CM_RC_SUCCESSFUL) { |
401 |
WS_EffectiveWorkingSchedule = WS_2; |
402 |
/*@LOG going to WS_2: use this table number */ |
403 |
LU_INFN_LOG(LU_DEBUG_TRACE,LU_MASK(__FILEID__),__FILEID__,__LINE__,table); |
404 |
/*@LOG going to WS_2: use this entry now... */ |
405 |
LU_INFN_LOG(LU_DEBUG_TRACE,LU_MASK(__FILEID__),__FILEID__,__LINE__,idx); |
406 |
}else |
407 |
WS_CASE_WS1; |
408 |
} |
409 |
else |
410 |
WS_CASE_WS1; |
411 |
|
412 |
/* check if im in a gap between some table: in such a case the duration (if < of the gap) |
413 |
must be forced to the gap itself in order to dont loose some table */ |
414 |
if( WS_EffectiveWorkingSchedule != WS_2 && |
415 |
WS_2_GetNextUsefulTableGap(now_s,&gap_s) == CM_RC_SUCCESSFUL && |
416 |
gap_s <= result_duration_run_ms/1000 ) { |
417 |
result_duration_run_ms=gap_s*1000; |
418 |
} |
419 |
|
420 |
|
421 |
}else{ |
422 |
WS_CASE_WS1; |
423 |
} |
424 |
} |
425 |
break; |
426 |
case WS_3: |
427 |
WS_EffectiveWorkingSchedule = WS_3; |
428 |
#warning "set result_acq_mode for WS_3" |
429 |
result_acq_mode=WS_3_NextAcqMode; |
430 |
result_duration_run_ms=time_max_run_ms; |
431 |
break; |
432 |
default: |
433 |
WS_CASE_WS0; |
434 |
break; |
435 |
} // switch |
436 |
*acq_mode=result_acq_mode; |
437 |
*duration_run_ms=result_duration_run_ms; |
438 |
/*@LOG result_acq_mode (A=1/B=2) */ |
439 |
LU_INFN_LOG(LU_DEBUG_TRACE,LU_MASK(__FILEID__),__FILEID__,__LINE__,result_acq_mode); |
440 |
/*@LOG result_duration_run_ms */ |
441 |
LU_INFN_LOG(LU_DEBUG_TRACE,LU_MASK(__FILEID__),__FILEID__,__LINE__,result_duration_run_ms); |
442 |
return CM_RC_SUCCESSFUL; |
443 |
} |
444 |
|
445 |
|
446 |
|
447 |
|
448 |
|
449 |
|
450 |
/*****************************************************************************/ |
451 |
/* @Function: WS_SetFavouriteWorkingSchedule */ |
452 |
/* @Purpose : */ |
453 |
/* Set the Favourite Working Schedule. */ |
454 |
/* @@ */ |
455 |
/* @Parameter Name @Message */ |
456 |
/* status_code OUT Return code */ |
457 |
/* @@ */ |
458 |
/*****************************************************************************/ |
459 |
|
460 |
|
461 |
status_code WS_SetFavouriteWorkingSchedule(WS newws) { |
462 |
if(WS_FavouriteWorkingSchedule != newws) { |
463 |
WS_FavouriteWorkingSchedule = newws; |
464 |
PRH_VAR_WS_FAVOURITE_WS = newws; |
465 |
//PRH_int_var_write2eeprom(PRH_VAR_WS_FAVOURITE_WS_IDX); |
466 |
/*@LOG SetFavourite Working Schedule - newws */ |
467 |
LU_INFN_LOG(LU_NORMAL_TRACE,LU_MASK(__FILEID__),__FILEID__,__LINE__,newws); |
468 |
} |
469 |
return CM_RC_SUCCESSFUL; |
470 |
} |
471 |
|
472 |
|
473 |
status_code WS_3_SetNextAcqMode(RM_ACQ_SETTING mode) { |
474 |
WS_3_NextAcqMode = mode; |
475 |
/*@LOG WS_3_SetNextAcqMode - WS_3_NextAcqMode (A/B)*/ |
476 |
LU_INFN_LOG(LU_DEBUG_TRACE,LU_MASK(__FILEID__),__FILEID__,__LINE__,WS_3_NextAcqMode); |
477 |
return CM_RC_SUCCESSFUL; |
478 |
} |