##// END OF EJS Templates
HG_REPOSITORIES » LPP » INSTRUMENTATION » USERS » JEANDET » LFR_FSW
RSS

Fork of HG_REPOSITORIES/LPP/INSTRUMENTATION/SOLO_LFR/DEV_PLE

Clone from https://hephaistos.lpp.polytechnique.fr/rhodecode/HG_REPOSITORIES/LPP/INSTRUMENTATION/SOLO_LFR/DEV_PLE

Bug 803 Tr_BoucleSortie...
paul -
r319:9f779b655c52 R3_plus draft
parent child
Show More
Show file before | Show file after | Hide comments
@@ -1,993 +1,997
1 1 /** General usage functions and RTEMS tasks.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 */
7 7
8 8 #include "fsw_misc.h"
9 9
10 10 void timer_configure(unsigned char timer, unsigned int clock_divider,
11 11 unsigned char interrupt_level, rtems_isr (*timer_isr)() )
12 12 {
13 13 /** This function configures a GPTIMER timer instantiated in the VHDL design.
14 14 *
15 15 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
16 16 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
17 17 * @param clock_divider is the divider of the 1 MHz clock that will be configured.
18 18 * @param interrupt_level is the interrupt level that the timer drives.
19 19 * @param timer_isr is the interrupt subroutine that will be attached to the IRQ driven by the timer.
20 20 *
21 21 * Interrupt levels are described in the SPARC documentation sparcv8.pdf p.76
22 22 *
23 23 */
24 24
25 25 rtems_status_code status;
26 26 rtems_isr_entry old_isr_handler;
27 27
28 28 gptimer_regs->timer[timer].ctrl = INIT_CHAR; // reset the control register
29 29
30 30 status = rtems_interrupt_catch( timer_isr, interrupt_level, &old_isr_handler) ; // see sparcv8.pdf p.76 for interrupt levels
31 31 if (status!=RTEMS_SUCCESSFUL)
32 32 {
33 33 PRINTF("in configure_timer *** ERR rtems_interrupt_catch\n")
34 34 }
35 35
36 36 timer_set_clock_divider( timer, clock_divider);
37 37 }
38 38
39 39 void timer_start(unsigned char timer)
40 40 {
41 41 /** This function starts a GPTIMER timer.
42 42 *
43 43 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
44 44 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
45 45 *
46 46 */
47 47
48 48 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_CLEAR_IRQ;
49 49 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_LD;
50 50 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_EN;
51 51 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_RS;
52 52 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_IE;
53 53 }
54 54
55 55 void timer_stop(unsigned char timer)
56 56 {
57 57 /** This function stops a GPTIMER timer.
58 58 *
59 59 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
60 60 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
61 61 *
62 62 */
63 63
64 64 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl & GPTIMER_EN_MASK;
65 65 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl & GPTIMER_IE_MASK;
66 66 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_CLEAR_IRQ;
67 67 }
68 68
69 69 void timer_set_clock_divider(unsigned char timer, unsigned int clock_divider)
70 70 {
71 71 /** This function sets the clock divider of a GPTIMER timer.
72 72 *
73 73 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
74 74 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
75 75 * @param clock_divider is the divider of the 1 MHz clock that will be configured.
76 76 *
77 77 */
78 78
79 79 gptimer_regs->timer[timer].reload = clock_divider; // base clock frequency is 1 MHz
80 80 }
81 81
82 82 // WATCHDOG
83 83
84 84 rtems_isr watchdog_isr( rtems_vector_number vector )
85 85 {
86 86 rtems_status_code status_code;
87 87
88 88 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_12 );
89 89
90 90 PRINTF("watchdog_isr *** this is the end, exit(0)\n");
91 91
92 92 exit(0);
93 93 }
94 94
95 95 void watchdog_configure(void)
96 96 {
97 97 /** This function configure the watchdog.
98 98 *
99 99 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
100 100 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
101 101 *
102 102 * The watchdog is a timer provided by the GPTIMER IP core of the GRLIB.
103 103 *
104 104 */
105 105
106 106 LEON_Mask_interrupt( IRQ_GPTIMER_WATCHDOG ); // mask gptimer/watchdog interrupt during configuration
107 107
108 108 timer_configure( TIMER_WATCHDOG, CLKDIV_WATCHDOG, IRQ_SPARC_GPTIMER_WATCHDOG, watchdog_isr );
109 109
110 110 LEON_Clear_interrupt( IRQ_GPTIMER_WATCHDOG ); // clear gptimer/watchdog interrupt
111 111 }
112 112
113 113 void watchdog_stop(void)
114 114 {
115 115 LEON_Mask_interrupt( IRQ_GPTIMER_WATCHDOG ); // mask gptimer/watchdog interrupt line
116 116 timer_stop( TIMER_WATCHDOG );
117 117 LEON_Clear_interrupt( IRQ_GPTIMER_WATCHDOG ); // clear gptimer/watchdog interrupt
118 118 }
119 119
120 120 void watchdog_reload(void)
121 121 {
122 122 /** This function reloads the watchdog timer counter with the timer reload value.
123 123 *
124 124 * @param void
125 125 *
126 126 * @return void
127 127 *
128 128 */
129 129
130 130 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | GPTIMER_LD;
131 131 }
132 132
133 133 void watchdog_start(void)
134 134 {
135 135 /** This function starts the watchdog timer.
136 136 *
137 137 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
138 138 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
139 139 *
140 140 */
141 141
142 142 LEON_Clear_interrupt( IRQ_GPTIMER_WATCHDOG );
143 143
144 144 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | GPTIMER_CLEAR_IRQ;
145 145 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | GPTIMER_LD;
146 146 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | GPTIMER_EN;
147 147 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | GPTIMER_IE;
148 148
149 149 LEON_Unmask_interrupt( IRQ_GPTIMER_WATCHDOG );
150 150
151 151 }
152 152
153 153 int enable_apbuart_transmitter( void ) // set the bit 1, TE Transmitter Enable to 1 in the APBUART control register
154 154 {
155 155 struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) REGS_ADDR_APBUART;
156 156
157 157 apbuart_regs->ctrl = APBUART_CTRL_REG_MASK_TE;
158 158
159 159 return 0;
160 160 }
161 161
162 162 void set_apbuart_scaler_reload_register(unsigned int regs, unsigned int value)
163 163 {
164 164 /** This function sets the scaler reload register of the apbuart module
165 165 *
166 166 * @param regs is the address of the apbuart registers in memory
167 167 * @param value is the value that will be stored in the scaler register
168 168 *
169 169 * The value shall be set by the software to get data on the serial interface.
170 170 *
171 171 */
172 172
173 173 struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) regs;
174 174
175 175 apbuart_regs->scaler = value;
176 176
177 177 BOOT_PRINTF1("OK *** apbuart port scaler reload register set to 0x%x\n", value)
178 178 }
179 179
180 180 //************
181 181 // RTEMS TASKS
182 182
183 183 rtems_task load_task(rtems_task_argument argument)
184 184 {
185 185 BOOT_PRINTF("in LOAD *** \n")
186 186
187 187 rtems_status_code status;
188 188 unsigned int i;
189 189 unsigned int j;
190 190 rtems_name name_watchdog_rate_monotonic; // name of the watchdog rate monotonic
191 191 rtems_id watchdog_period_id; // id of the watchdog rate monotonic period
192 192
193 193 name_watchdog_rate_monotonic = rtems_build_name( 'L', 'O', 'A', 'D' );
194 194
195 195 status = rtems_rate_monotonic_create( name_watchdog_rate_monotonic, &watchdog_period_id );
196 196 if( status != RTEMS_SUCCESSFUL ) {
197 197 PRINTF1( "in LOAD *** rtems_rate_monotonic_create failed with status of %d\n", status )
198 198 }
199 199
200 200 i = 0;
201 201 j = 0;
202 202
203 203 watchdog_configure();
204 204
205 205 watchdog_start();
206 206
207 207 set_sy_lfr_watchdog_enabled( true );
208 208
209 209 while(1){
210 210 status = rtems_rate_monotonic_period( watchdog_period_id, WATCHDOG_PERIOD );
211 211 watchdog_reload();
212 212 i = i + 1;
213 213 if ( i == WATCHDOG_LOOP_PRINTF )
214 214 {
215 215 i = 0;
216 216 j = j + 1;
217 217 PRINTF1("%d\n", j)
218 218 }
219 219 #ifdef DEBUG_WATCHDOG
220 220 if (j == WATCHDOG_LOOP_DEBUG )
221 221 {
222 222 status = rtems_task_delete(RTEMS_SELF);
223 223 }
224 224 #endif
225 225 }
226 226 }
227 227
228 228 rtems_task hous_task(rtems_task_argument argument)
229 229 {
230 230 rtems_status_code status;
231 231 rtems_status_code spare_status;
232 232 rtems_id queue_id;
233 233 rtems_rate_monotonic_period_status period_status;
234 bool isSynchronized;
235
236 isSynchronized = false;
234 237
235 238 status = get_message_queue_id_send( &queue_id );
236 239 if (status != RTEMS_SUCCESSFUL)
237 240 {
238 241 PRINTF1("in HOUS *** ERR get_message_queue_id_send %d\n", status)
239 242 }
240 243
241 244 BOOT_PRINTF("in HOUS ***\n");
242 245
243 246 if (rtems_rate_monotonic_ident( name_hk_rate_monotonic, &HK_id) != RTEMS_SUCCESSFUL) {
244 247 status = rtems_rate_monotonic_create( name_hk_rate_monotonic, &HK_id );
245 248 if( status != RTEMS_SUCCESSFUL ) {
246 249 PRINTF1( "rtems_rate_monotonic_create failed with status of %d\n", status );
247 250 }
248 251 }
249 252
250 253 status = rtems_rate_monotonic_cancel(HK_id);
251 254 if( status != RTEMS_SUCCESSFUL ) {
252 255 PRINTF1( "ERR *** in HOUS *** rtems_rate_monotonic_cancel(HK_id) ***code: %d\n", status );
253 256 }
254 257 else {
255 258 DEBUG_PRINTF("OK *** in HOUS *** rtems_rate_monotonic_cancel(HK_id)\n");
256 259 }
257 260
258 261 // startup phase
259 262 status = rtems_rate_monotonic_period( HK_id, SY_LFR_TIME_SYN_TIMEOUT_in_ticks );
260 263 status = rtems_rate_monotonic_get_status( HK_id, &period_status );
261 264 DEBUG_PRINTF1("startup HK, HK_id status = %d\n", period_status.state)
262 while(period_status.state != RATE_MONOTONIC_EXPIRED ) // after SY_LFR_TIME_SYN_TIMEOUT ms, starts HK anyway
265 while( (period_status.state != RATE_MONOTONIC_EXPIRED)
266 && (isSynchronized == false) ) // after SY_LFR_TIME_SYN_TIMEOUT ms, starts HK anyway
263 267 {
264 268 if ((time_management_regs->coarse_time & VAL_LFR_SYNCHRONIZED) == INT32_ALL_0) // check time synchronization
265 269 {
266 break; // break if LFR is synchronized
270 isSynchronized = true;
267 271 }
268 272 else
269 273 {
270 274 status = rtems_rate_monotonic_get_status( HK_id, &period_status );
271 275
272 276 status = rtems_task_wake_after( HK_SYNC_WAIT ); // wait HK_SYNCH_WAIT 100 ms = 10 * 10 ms
273 277 }
274 278 }
275 279 status = rtems_rate_monotonic_cancel(HK_id);
276 280 DEBUG_PRINTF1("startup HK, HK_id status = %d\n", period_status.state)
277 281
278 282 set_hk_lfr_reset_cause( POWER_ON );
279 283
280 284 while(1){ // launch the rate monotonic task
281 285 status = rtems_rate_monotonic_period( HK_id, HK_PERIOD );
282 286 if ( status != RTEMS_SUCCESSFUL ) {
283 287 PRINTF1( "in HOUS *** ERR period: %d\n", status);
284 288 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_6 );
285 289 }
286 290 else {
287 291 housekeeping_packet.packetSequenceControl[BYTE_0] = (unsigned char) (sequenceCounterHK >> SHIFT_1_BYTE);
288 292 housekeeping_packet.packetSequenceControl[BYTE_1] = (unsigned char) (sequenceCounterHK );
289 293 increment_seq_counter( &sequenceCounterHK );
290 294
291 295 housekeeping_packet.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
292 296 housekeeping_packet.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
293 297 housekeeping_packet.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
294 298 housekeeping_packet.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
295 299 housekeeping_packet.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
296 300 housekeeping_packet.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
297 301
298 302 spacewire_update_hk_lfr_link_state( &housekeeping_packet.lfr_status_word[0] );
299 303
300 304 spacewire_read_statistics();
301 305
302 306 update_hk_with_grspw_stats();
303 307
304 308 set_hk_lfr_time_not_synchro();
305 309
306 310 housekeeping_packet.hk_lfr_q_sd_fifo_size_max = hk_lfr_q_sd_fifo_size_max;
307 311 housekeeping_packet.hk_lfr_q_rv_fifo_size_max = hk_lfr_q_rv_fifo_size_max;
308 312 housekeeping_packet.hk_lfr_q_p0_fifo_size_max = hk_lfr_q_p0_fifo_size_max;
309 313 housekeeping_packet.hk_lfr_q_p1_fifo_size_max = hk_lfr_q_p1_fifo_size_max;
310 314 housekeeping_packet.hk_lfr_q_p2_fifo_size_max = hk_lfr_q_p2_fifo_size_max;
311 315
312 316 housekeeping_packet.sy_lfr_common_parameters_spare = parameter_dump_packet.sy_lfr_common_parameters_spare;
313 317 housekeeping_packet.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
314 318 get_temperatures( housekeeping_packet.hk_lfr_temp_scm );
315 319 get_v_e1_e2_f3( housekeeping_packet.hk_lfr_sc_v_f3 );
316 320 get_cpu_load( (unsigned char *) &housekeeping_packet.hk_lfr_cpu_load );
317 321
318 322 hk_lfr_le_me_he_update();
319 323
320 324 housekeeping_packet.hk_lfr_sc_rw_f_flags = cp_rpw_sc_rw_f_flags;
321 325
322 326 // SEND PACKET
323 327 status = rtems_message_queue_send( queue_id, &housekeeping_packet,
324 328 PACKET_LENGTH_HK + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
325 329 if (status != RTEMS_SUCCESSFUL) {
326 330 PRINTF1("in HOUS *** ERR send: %d\n", status)
327 331 }
328 332 }
329 333 }
330 334
331 335 PRINTF("in HOUS *** deleting task\n")
332 336
333 337 status = rtems_task_delete( RTEMS_SELF ); // should not return
334 338
335 339 return;
336 340 }
337 341
338 342 rtems_task avgv_task(rtems_task_argument argument)
339 343 {
340 344 #define MOVING_AVERAGE 16
341 345 rtems_status_code status;
342 346 unsigned int v[MOVING_AVERAGE];
343 347 unsigned int e1[MOVING_AVERAGE];
344 348 unsigned int e2[MOVING_AVERAGE];
345 349 float average_v;
346 350 float average_e1;
347 351 float average_e2;
348 352 unsigned char k;
349 353 unsigned char indexOfOldValue;
350 354
351 355 BOOT_PRINTF("in AVGV ***\n");
352 356
353 357 if (rtems_rate_monotonic_ident( name_avgv_rate_monotonic, &HK_id) != RTEMS_SUCCESSFUL) {
354 358 status = rtems_rate_monotonic_create( name_avgv_rate_monotonic, &AVGV_id );
355 359 if( status != RTEMS_SUCCESSFUL ) {
356 360 PRINTF1( "rtems_rate_monotonic_create failed with status of %d\n", status );
357 361 }
358 362 }
359 363
360 364 status = rtems_rate_monotonic_cancel(AVGV_id);
361 365 if( status != RTEMS_SUCCESSFUL ) {
362 366 PRINTF1( "ERR *** in AVGV *** rtems_rate_monotonic_cancel(AVGV_id) ***code: %d\n", status );
363 367 }
364 368 else {
365 369 DEBUG_PRINTF("OK *** in AVGV *** rtems_rate_monotonic_cancel(AVGV_id)\n");
366 370 }
367 371
368 372 // initialize values
369 373 k = 0;
370 374 indexOfOldValue = MOVING_AVERAGE - 1;
371 375 for (k = 0; k < MOVING_AVERAGE; k++)
372 376 {
373 377 v[k] = 0;
374 378 e1[k] = 0;
375 379 e2[k] = 0;
376 380 average_v = 0.;
377 381 average_e1 = 0.;
378 382 average_e2 = 0.;
379 383 }
380 384
381 385 k = 0;
382 386
383 387 while(1){ // launch the rate monotonic task
384 388 status = rtems_rate_monotonic_period( AVGV_id, AVGV_PERIOD );
385 389 if ( status != RTEMS_SUCCESSFUL ) {
386 390 PRINTF1( "in AVGV *** ERR period: %d\n", status);
387 391 }
388 392 else {
389 393 // store new value in buffer
390 394 v[k] = waveform_picker_regs->v;
391 395 e1[k] = waveform_picker_regs->e1;
392 396 e2[k] = waveform_picker_regs->e2;
393 397 if (k == (MOVING_AVERAGE - 1))
394 398 {
395 399 indexOfOldValue = 0;
396 400 }
397 401 else
398 402 {
399 403 indexOfOldValue = k + 1;
400 404 }
401 405 average_v = average_v + v[k] - v[indexOfOldValue];
402 406 average_e1 = average_e1 + e1[k] - e1[indexOfOldValue];
403 407 average_e2 = average_e2 + e2[k] - e2[indexOfOldValue];
404 408 }
405 409 if (k == (MOVING_AVERAGE-1))
406 410 {
407 411 k = 0;
408 412 printf("tick\n");
409 413 }
410 414 else
411 415 {
412 416 k++;
413 417 }
414 418 }
415 419
416 420 PRINTF("in AVGV *** deleting task\n")
417 421
418 422 status = rtems_task_delete( RTEMS_SELF ); // should not return
419 423
420 424 return;
421 425 }
422 426
423 427 rtems_task dumb_task( rtems_task_argument unused )
424 428 {
425 429 /** This RTEMS taks is used to print messages without affecting the general behaviour of the software.
426 430 *
427 431 * @param unused is the starting argument of the RTEMS task
428 432 *
429 433 * The DUMB taks waits for RTEMS events and print messages depending on the incoming events.
430 434 *
431 435 */
432 436
433 437 unsigned int i;
434 438 unsigned int intEventOut;
435 439 unsigned int coarse_time = 0;
436 440 unsigned int fine_time = 0;
437 441 rtems_event_set event_out;
438 442
439 443 char *DumbMessages[DUMB_MESSAGE_NB] = {DUMB_MESSAGE_0, // RTEMS_EVENT_0
440 444 DUMB_MESSAGE_1, // RTEMS_EVENT_1
441 445 DUMB_MESSAGE_2, // RTEMS_EVENT_2
442 446 DUMB_MESSAGE_3, // RTEMS_EVENT_3
443 447 DUMB_MESSAGE_4, // RTEMS_EVENT_4
444 448 DUMB_MESSAGE_5, // RTEMS_EVENT_5
445 449 DUMB_MESSAGE_6, // RTEMS_EVENT_6
446 450 DUMB_MESSAGE_7, // RTEMS_EVENT_7
447 451 DUMB_MESSAGE_8, // RTEMS_EVENT_8
448 452 DUMB_MESSAGE_9, // RTEMS_EVENT_9
449 453 DUMB_MESSAGE_10, // RTEMS_EVENT_10
450 454 DUMB_MESSAGE_11, // RTEMS_EVENT_11
451 455 DUMB_MESSAGE_12, // RTEMS_EVENT_12
452 456 DUMB_MESSAGE_13, // RTEMS_EVENT_13
453 457 DUMB_MESSAGE_14 // RTEMS_EVENT_14
454 458 };
455 459
456 460 BOOT_PRINTF("in DUMB *** \n")
457 461
458 462 while(1){
459 463 rtems_event_receive(RTEMS_EVENT_0 | RTEMS_EVENT_1 | RTEMS_EVENT_2 | RTEMS_EVENT_3
460 464 | RTEMS_EVENT_4 | RTEMS_EVENT_5 | RTEMS_EVENT_6 | RTEMS_EVENT_7
461 465 | RTEMS_EVENT_8 | RTEMS_EVENT_9 | RTEMS_EVENT_12 | RTEMS_EVENT_13
462 466 | RTEMS_EVENT_14,
463 467 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT
464 468 intEventOut = (unsigned int) event_out;
465 469 for ( i=0; i<NB_RTEMS_EVENTS; i++)
466 470 {
467 471 if ( ((intEventOut >> i) & 1) != 0)
468 472 {
469 473 coarse_time = time_management_regs->coarse_time;
470 474 fine_time = time_management_regs->fine_time;
471 475 if (i==EVENT_12)
472 476 {
473 477 PRINTF1("%s\n", DUMB_MESSAGE_12)
474 478 }
475 479 if (i==EVENT_13)
476 480 {
477 481 PRINTF1("%s\n", DUMB_MESSAGE_13)
478 482 }
479 483 if (i==EVENT_14)
480 484 {
481 485 PRINTF1("%s\n", DUMB_MESSAGE_1)
482 486 }
483 487 }
484 488 }
485 489 }
486 490 }
487 491
488 492 //*****************************
489 493 // init housekeeping parameters
490 494
491 495 void init_housekeeping_parameters( void )
492 496 {
493 497 /** This function initialize the housekeeping_packet global variable with default values.
494 498 *
495 499 */
496 500
497 501 unsigned int i = 0;
498 502 unsigned char *parameters;
499 503 unsigned char sizeOfHK;
500 504
501 505 sizeOfHK = sizeof( Packet_TM_LFR_HK_t );
502 506
503 507 parameters = (unsigned char*) &housekeeping_packet;
504 508
505 509 for(i = 0; i< sizeOfHK; i++)
506 510 {
507 511 parameters[i] = INIT_CHAR;
508 512 }
509 513
510 514 housekeeping_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
511 515 housekeeping_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
512 516 housekeeping_packet.reserved = DEFAULT_RESERVED;
513 517 housekeeping_packet.userApplication = CCSDS_USER_APP;
514 518 housekeeping_packet.packetID[0] = (unsigned char) (APID_TM_HK >> SHIFT_1_BYTE);
515 519 housekeeping_packet.packetID[1] = (unsigned char) (APID_TM_HK);
516 520 housekeeping_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
517 521 housekeeping_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
518 522 housekeeping_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_HK >> SHIFT_1_BYTE);
519 523 housekeeping_packet.packetLength[1] = (unsigned char) (PACKET_LENGTH_HK );
520 524 housekeeping_packet.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
521 525 housekeeping_packet.serviceType = TM_TYPE_HK;
522 526 housekeeping_packet.serviceSubType = TM_SUBTYPE_HK;
523 527 housekeeping_packet.destinationID = TM_DESTINATION_ID_GROUND;
524 528 housekeeping_packet.sid = SID_HK;
525 529
526 530 // init status word
527 531 housekeeping_packet.lfr_status_word[0] = DEFAULT_STATUS_WORD_BYTE0;
528 532 housekeeping_packet.lfr_status_word[1] = DEFAULT_STATUS_WORD_BYTE1;
529 533 // init software version
530 534 housekeeping_packet.lfr_sw_version[0] = SW_VERSION_N1;
531 535 housekeeping_packet.lfr_sw_version[1] = SW_VERSION_N2;
532 536 housekeeping_packet.lfr_sw_version[BYTE_2] = SW_VERSION_N3;
533 537 housekeeping_packet.lfr_sw_version[BYTE_3] = SW_VERSION_N4;
534 538 // init fpga version
535 539 parameters = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
536 540 housekeeping_packet.lfr_fpga_version[BYTE_0] = parameters[BYTE_1]; // n1
537 541 housekeeping_packet.lfr_fpga_version[BYTE_1] = parameters[BYTE_2]; // n2
538 542 housekeeping_packet.lfr_fpga_version[BYTE_2] = parameters[BYTE_3]; // n3
539 543
540 544 housekeeping_packet.hk_lfr_q_sd_fifo_size = MSG_QUEUE_COUNT_SEND;
541 545 housekeeping_packet.hk_lfr_q_rv_fifo_size = MSG_QUEUE_COUNT_RECV;
542 546 housekeeping_packet.hk_lfr_q_p0_fifo_size = MSG_QUEUE_COUNT_PRC0;
543 547 housekeeping_packet.hk_lfr_q_p1_fifo_size = MSG_QUEUE_COUNT_PRC1;
544 548 housekeeping_packet.hk_lfr_q_p2_fifo_size = MSG_QUEUE_COUNT_PRC2;
545 549 }
546 550
547 551 void increment_seq_counter( unsigned short *packetSequenceControl )
548 552 {
549 553 /** This function increment the sequence counter passes in argument.
550 554 *
551 555 * The increment does not affect the grouping flag. In case of an overflow, the counter is reset to 0.
552 556 *
553 557 */
554 558
555 559 unsigned short segmentation_grouping_flag;
556 560 unsigned short sequence_cnt;
557 561
558 562 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << SHIFT_1_BYTE; // keep bits 7 downto 6
559 563 sequence_cnt = (*packetSequenceControl) & SEQ_CNT_MASK; // [0011 1111 1111 1111]
560 564
561 565 if ( sequence_cnt < SEQ_CNT_MAX)
562 566 {
563 567 sequence_cnt = sequence_cnt + 1;
564 568 }
565 569 else
566 570 {
567 571 sequence_cnt = 0;
568 572 }
569 573
570 574 *packetSequenceControl = segmentation_grouping_flag | sequence_cnt ;
571 575 }
572 576
573 577 void getTime( unsigned char *time)
574 578 {
575 579 /** This function write the current local time in the time buffer passed in argument.
576 580 *
577 581 */
578 582
579 583 time[0] = (unsigned char) (time_management_regs->coarse_time>>SHIFT_3_BYTES);
580 584 time[1] = (unsigned char) (time_management_regs->coarse_time>>SHIFT_2_BYTES);
581 585 time[2] = (unsigned char) (time_management_regs->coarse_time>>SHIFT_1_BYTE);
582 586 time[3] = (unsigned char) (time_management_regs->coarse_time);
583 587 time[4] = (unsigned char) (time_management_regs->fine_time>>SHIFT_1_BYTE);
584 588 time[5] = (unsigned char) (time_management_regs->fine_time);
585 589 }
586 590
587 591 unsigned long long int getTimeAsUnsignedLongLongInt( )
588 592 {
589 593 /** This function write the current local time in the time buffer passed in argument.
590 594 *
591 595 */
592 596 unsigned long long int time;
593 597
594 598 time = ( (unsigned long long int) (time_management_regs->coarse_time & COARSE_TIME_MASK) << SHIFT_2_BYTES )
595 599 + time_management_regs->fine_time;
596 600
597 601 return time;
598 602 }
599 603
600 604 void send_dumb_hk( void )
601 605 {
602 606 Packet_TM_LFR_HK_t dummy_hk_packet;
603 607 unsigned char *parameters;
604 608 unsigned int i;
605 609 rtems_id queue_id;
606 610
607 611 dummy_hk_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
608 612 dummy_hk_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
609 613 dummy_hk_packet.reserved = DEFAULT_RESERVED;
610 614 dummy_hk_packet.userApplication = CCSDS_USER_APP;
611 615 dummy_hk_packet.packetID[0] = (unsigned char) (APID_TM_HK >> SHIFT_1_BYTE);
612 616 dummy_hk_packet.packetID[1] = (unsigned char) (APID_TM_HK);
613 617 dummy_hk_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
614 618 dummy_hk_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
615 619 dummy_hk_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_HK >> SHIFT_1_BYTE);
616 620 dummy_hk_packet.packetLength[1] = (unsigned char) (PACKET_LENGTH_HK );
617 621 dummy_hk_packet.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
618 622 dummy_hk_packet.serviceType = TM_TYPE_HK;
619 623 dummy_hk_packet.serviceSubType = TM_SUBTYPE_HK;
620 624 dummy_hk_packet.destinationID = TM_DESTINATION_ID_GROUND;
621 625 dummy_hk_packet.time[0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
622 626 dummy_hk_packet.time[1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
623 627 dummy_hk_packet.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
624 628 dummy_hk_packet.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
625 629 dummy_hk_packet.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
626 630 dummy_hk_packet.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
627 631 dummy_hk_packet.sid = SID_HK;
628 632
629 633 // init status word
630 634 dummy_hk_packet.lfr_status_word[0] = INT8_ALL_F;
631 635 dummy_hk_packet.lfr_status_word[1] = INT8_ALL_F;
632 636 // init software version
633 637 dummy_hk_packet.lfr_sw_version[0] = SW_VERSION_N1;
634 638 dummy_hk_packet.lfr_sw_version[1] = SW_VERSION_N2;
635 639 dummy_hk_packet.lfr_sw_version[BYTE_2] = SW_VERSION_N3;
636 640 dummy_hk_packet.lfr_sw_version[BYTE_3] = SW_VERSION_N4;
637 641 // init fpga version
638 642 parameters = (unsigned char *) (REGS_ADDR_WAVEFORM_PICKER + APB_OFFSET_VHDL_REV);
639 643 dummy_hk_packet.lfr_fpga_version[BYTE_0] = parameters[BYTE_1]; // n1
640 644 dummy_hk_packet.lfr_fpga_version[BYTE_1] = parameters[BYTE_2]; // n2
641 645 dummy_hk_packet.lfr_fpga_version[BYTE_2] = parameters[BYTE_3]; // n3
642 646
643 647 parameters = (unsigned char *) &dummy_hk_packet.hk_lfr_cpu_load;
644 648
645 649 for (i=0; i<(BYTE_POS_HK_REACTION_WHEELS_FREQUENCY - BYTE_POS_HK_LFR_CPU_LOAD); i++)
646 650 {
647 651 parameters[i] = INT8_ALL_F;
648 652 }
649 653
650 654 get_message_queue_id_send( &queue_id );
651 655
652 656 rtems_message_queue_send( queue_id, &dummy_hk_packet,
653 657 PACKET_LENGTH_HK + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
654 658 }
655 659
656 660 void get_temperatures( unsigned char *temperatures )
657 661 {
658 662 unsigned char* temp_scm_ptr;
659 663 unsigned char* temp_pcb_ptr;
660 664 unsigned char* temp_fpga_ptr;
661 665
662 666 // SEL1 SEL0
663 667 // 0 0 => PCB
664 668 // 0 1 => FPGA
665 669 // 1 0 => SCM
666 670
667 671 temp_scm_ptr = (unsigned char *) &time_management_regs->temp_scm;
668 672 temp_pcb_ptr = (unsigned char *) &time_management_regs->temp_pcb;
669 673 temp_fpga_ptr = (unsigned char *) &time_management_regs->temp_fpga;
670 674
671 675 temperatures[ BYTE_0 ] = temp_scm_ptr[ BYTE_2 ];
672 676 temperatures[ BYTE_1 ] = temp_scm_ptr[ BYTE_3 ];
673 677 temperatures[ BYTE_2 ] = temp_pcb_ptr[ BYTE_2 ];
674 678 temperatures[ BYTE_3 ] = temp_pcb_ptr[ BYTE_3 ];
675 679 temperatures[ BYTE_4 ] = temp_fpga_ptr[ BYTE_2 ];
676 680 temperatures[ BYTE_5 ] = temp_fpga_ptr[ BYTE_3 ];
677 681 }
678 682
679 683 void get_v_e1_e2_f3( unsigned char *spacecraft_potential )
680 684 {
681 685 unsigned char* v_ptr;
682 686 unsigned char* e1_ptr;
683 687 unsigned char* e2_ptr;
684 688
685 689 v_ptr = (unsigned char *) &waveform_picker_regs->v;
686 690 e1_ptr = (unsigned char *) &waveform_picker_regs->e1;
687 691 e2_ptr = (unsigned char *) &waveform_picker_regs->e2;
688 692
689 693 spacecraft_potential[ BYTE_0 ] = v_ptr[ BYTE_2 ];
690 694 spacecraft_potential[ BYTE_1 ] = v_ptr[ BYTE_3 ];
691 695 spacecraft_potential[ BYTE_2 ] = e1_ptr[ BYTE_2 ];
692 696 spacecraft_potential[ BYTE_3 ] = e1_ptr[ BYTE_3 ];
693 697 spacecraft_potential[ BYTE_4 ] = e2_ptr[ BYTE_2 ];
694 698 spacecraft_potential[ BYTE_5 ] = e2_ptr[ BYTE_3 ];
695 699 }
696 700
697 701 void get_cpu_load( unsigned char *resource_statistics )
698 702 {
699 703 unsigned char cpu_load;
700 704
701 705 cpu_load = lfr_rtems_cpu_usage_report();
702 706
703 707 // HK_LFR_CPU_LOAD
704 708 resource_statistics[0] = cpu_load;
705 709
706 710 // HK_LFR_CPU_LOAD_MAX
707 711 if (cpu_load > resource_statistics[1])
708 712 {
709 713 resource_statistics[1] = cpu_load;
710 714 }
711 715
712 716 // CPU_LOAD_AVE
713 717 resource_statistics[BYTE_2] = 0;
714 718
715 719 #ifndef PRINT_TASK_STATISTICS
716 720 rtems_cpu_usage_reset();
717 721 #endif
718 722
719 723 }
720 724
721 725 void set_hk_lfr_sc_potential_flag( bool state )
722 726 {
723 727 if (state == true)
724 728 {
725 729 housekeeping_packet.lfr_status_word[1] =
726 730 housekeeping_packet.lfr_status_word[1] | STATUS_WORD_SC_POTENTIAL_FLAG_BIT; // [0100 0000]
727 731 }
728 732 else
729 733 {
730 734 housekeeping_packet.lfr_status_word[1] =
731 735 housekeeping_packet.lfr_status_word[1] & STATUS_WORD_SC_POTENTIAL_FLAG_MASK; // [1011 1111]
732 736 }
733 737 }
734 738
735 739 void set_sy_lfr_pas_filter_enabled( bool state )
736 740 {
737 741 if (state == true)
738 742 {
739 743 housekeeping_packet.lfr_status_word[1] =
740 744 housekeeping_packet.lfr_status_word[1] | STATUS_WORD_SC_POTENTIAL_FLAG_BIT; // [0010 0000]
741 745 }
742 746 else
743 747 {
744 748 housekeeping_packet.lfr_status_word[1] =
745 749 housekeeping_packet.lfr_status_word[1] & STATUS_WORD_SC_POTENTIAL_FLAG_MASK; // [1101 1111]
746 750 }
747 751 }
748 752
749 753 void set_sy_lfr_watchdog_enabled( bool state )
750 754 {
751 755 if (state == true)
752 756 {
753 757 housekeeping_packet.lfr_status_word[1] =
754 758 housekeeping_packet.lfr_status_word[1] | STATUS_WORD_WATCHDOG_BIT; // [0001 0000]
755 759 }
756 760 else
757 761 {
758 762 housekeeping_packet.lfr_status_word[1] =
759 763 housekeeping_packet.lfr_status_word[1] & STATUS_WORD_WATCHDOG_MASK; // [1110 1111]
760 764 }
761 765 }
762 766
763 767 void set_hk_lfr_calib_enable( bool state )
764 768 {
765 769 if (state == true)
766 770 {
767 771 housekeeping_packet.lfr_status_word[1] =
768 772 housekeeping_packet.lfr_status_word[1] | STATUS_WORD_CALIB_BIT; // [0000 1000]
769 773 }
770 774 else
771 775 {
772 776 housekeeping_packet.lfr_status_word[1] =
773 777 housekeeping_packet.lfr_status_word[1] & STATUS_WORD_CALIB_MASK; // [1111 0111]
774 778 }
775 779 }
776 780
777 781 void set_hk_lfr_reset_cause( enum lfr_reset_cause_t lfr_reset_cause )
778 782 {
779 783 housekeeping_packet.lfr_status_word[1] =
780 784 housekeeping_packet.lfr_status_word[1] & STATUS_WORD_RESET_CAUSE_MASK; // [1111 1000]
781 785
782 786 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1]
783 787 | (lfr_reset_cause & STATUS_WORD_RESET_CAUSE_BITS ); // [0000 0111]
784 788
785 789 }
786 790
787 791 void increment_hk_counter( unsigned char newValue, unsigned char oldValue, unsigned int *counter )
788 792 {
789 793 int delta;
790 794
791 795 delta = 0;
792 796
793 797 if (newValue >= oldValue)
794 798 {
795 799 delta = newValue - oldValue;
796 800 }
797 801 else
798 802 {
799 803 delta = 255 - oldValue + newValue;
800 804 }
801 805
802 806 *counter = *counter + delta;
803 807 }
804 808
805 809 void hk_lfr_le_update( void )
806 810 {
807 811 static hk_lfr_le_t old_hk_lfr_le = {0};
808 812 hk_lfr_le_t new_hk_lfr_le;
809 813 unsigned int counter;
810 814
811 815 counter = (((unsigned int) housekeeping_packet.hk_lfr_le_cnt[0]) * 256) + housekeeping_packet.hk_lfr_le_cnt[1];
812 816
813 817 // DPU
814 818 new_hk_lfr_le.dpu_spw_parity = housekeeping_packet.hk_lfr_dpu_spw_parity;
815 819 new_hk_lfr_le.dpu_spw_disconnect= housekeeping_packet.hk_lfr_dpu_spw_disconnect;
816 820 new_hk_lfr_le.dpu_spw_escape = housekeeping_packet.hk_lfr_dpu_spw_escape;
817 821 new_hk_lfr_le.dpu_spw_credit = housekeeping_packet.hk_lfr_dpu_spw_credit;
818 822 new_hk_lfr_le.dpu_spw_write_sync= housekeeping_packet.hk_lfr_dpu_spw_write_sync;
819 823 // TIMECODE
820 824 new_hk_lfr_le.timecode_erroneous= housekeeping_packet.hk_lfr_timecode_erroneous;
821 825 new_hk_lfr_le.timecode_missing = housekeeping_packet.hk_lfr_timecode_missing;
822 826 new_hk_lfr_le.timecode_invalid = housekeeping_packet.hk_lfr_timecode_invalid;
823 827 // TIME
824 828 new_hk_lfr_le.time_timecode_it = housekeeping_packet.hk_lfr_time_timecode_it;
825 829 new_hk_lfr_le.time_not_synchro = housekeeping_packet.hk_lfr_time_not_synchro;
826 830 new_hk_lfr_le.time_timecode_ctr = housekeeping_packet.hk_lfr_time_timecode_ctr;
827 831 //AHB
828 832 new_hk_lfr_le.ahb_correctable = housekeeping_packet.hk_lfr_ahb_correctable;
829 833 // housekeeping_packet.hk_lfr_dpu_spw_rx_ahb => not handled by the grspw driver
830 834 // housekeeping_packet.hk_lfr_dpu_spw_tx_ahb => not handled by the grspw driver
831 835
832 836 // update the le counter
833 837 // DPU
834 838 increment_hk_counter( new_hk_lfr_le.dpu_spw_parity, old_hk_lfr_le.dpu_spw_parity, &counter );
835 839 increment_hk_counter( new_hk_lfr_le.dpu_spw_disconnect,old_hk_lfr_le.dpu_spw_disconnect, &counter );
836 840 increment_hk_counter( new_hk_lfr_le.dpu_spw_escape, old_hk_lfr_le.dpu_spw_escape, &counter );
837 841 increment_hk_counter( new_hk_lfr_le.dpu_spw_credit, old_hk_lfr_le.dpu_spw_credit, &counter );
838 842 increment_hk_counter( new_hk_lfr_le.dpu_spw_write_sync,old_hk_lfr_le.dpu_spw_write_sync, &counter );
839 843 // TIMECODE
840 844 increment_hk_counter( new_hk_lfr_le.timecode_erroneous,old_hk_lfr_le.timecode_erroneous, &counter );
841 845 increment_hk_counter( new_hk_lfr_le.timecode_missing, old_hk_lfr_le.timecode_missing, &counter );
842 846 increment_hk_counter( new_hk_lfr_le.timecode_invalid, old_hk_lfr_le.timecode_invalid, &counter );
843 847 // TIME
844 848 increment_hk_counter( new_hk_lfr_le.time_timecode_it, old_hk_lfr_le.time_timecode_it, &counter );
845 849 increment_hk_counter( new_hk_lfr_le.time_not_synchro, old_hk_lfr_le.time_not_synchro, &counter );
846 850 increment_hk_counter( new_hk_lfr_le.time_timecode_ctr, old_hk_lfr_le.time_timecode_ctr, &counter );
847 851 // AHB
848 852 increment_hk_counter( new_hk_lfr_le.ahb_correctable, old_hk_lfr_le.ahb_correctable, &counter );
849 853
850 854 // DPU
851 855 old_hk_lfr_le.dpu_spw_parity = new_hk_lfr_le.dpu_spw_parity;
852 856 old_hk_lfr_le.dpu_spw_disconnect= new_hk_lfr_le.dpu_spw_disconnect;
853 857 old_hk_lfr_le.dpu_spw_escape = new_hk_lfr_le.dpu_spw_escape;
854 858 old_hk_lfr_le.dpu_spw_credit = new_hk_lfr_le.dpu_spw_credit;
855 859 old_hk_lfr_le.dpu_spw_write_sync= new_hk_lfr_le.dpu_spw_write_sync;
856 860 // TIMECODE
857 861 old_hk_lfr_le.timecode_erroneous= new_hk_lfr_le.timecode_erroneous;
858 862 old_hk_lfr_le.timecode_missing = new_hk_lfr_le.timecode_missing;
859 863 old_hk_lfr_le.timecode_invalid = new_hk_lfr_le.timecode_invalid;
860 864 // TIME
861 865 old_hk_lfr_le.time_timecode_it = new_hk_lfr_le.time_timecode_it;
862 866 old_hk_lfr_le.time_not_synchro = new_hk_lfr_le.time_not_synchro;
863 867 old_hk_lfr_le.time_timecode_ctr = new_hk_lfr_le.time_timecode_ctr;
864 868 //AHB
865 869 old_hk_lfr_le.ahb_correctable = new_hk_lfr_le.ahb_correctable;
866 870 // housekeeping_packet.hk_lfr_dpu_spw_rx_ahb => not handled by the grspw driver
867 871 // housekeeping_packet.hk_lfr_dpu_spw_tx_ahb => not handled by the grspw driver
868 872
869 873 // update housekeeping packet counters, convert unsigned int numbers in 2 bytes numbers
870 874 // LE
871 875 housekeeping_packet.hk_lfr_le_cnt[0] = (unsigned char) ((counter & BYTE0_MASK) >> SHIFT_1_BYTE);
872 876 housekeeping_packet.hk_lfr_le_cnt[1] = (unsigned char) (counter & BYTE1_MASK);
873 877 }
874 878
875 879 void hk_lfr_me_update( void )
876 880 {
877 881 static hk_lfr_me_t old_hk_lfr_me = {0};
878 882 hk_lfr_me_t new_hk_lfr_me;
879 883 unsigned int counter;
880 884
881 885 counter = (((unsigned int) housekeeping_packet.hk_lfr_me_cnt[0]) * 256) + housekeeping_packet.hk_lfr_me_cnt[1];
882 886
883 887 // get the current values
884 888 new_hk_lfr_me.dpu_spw_early_eop = housekeeping_packet.hk_lfr_dpu_spw_early_eop;
885 889 new_hk_lfr_me.dpu_spw_invalid_addr = housekeeping_packet.hk_lfr_dpu_spw_invalid_addr;
886 890 new_hk_lfr_me.dpu_spw_eep = housekeeping_packet.hk_lfr_dpu_spw_eep;
887 891 new_hk_lfr_me.dpu_spw_rx_too_big = housekeeping_packet.hk_lfr_dpu_spw_rx_too_big;
888 892
889 893 // update the me counter
890 894 increment_hk_counter( new_hk_lfr_me.dpu_spw_early_eop, old_hk_lfr_me.dpu_spw_early_eop, &counter );
891 895 increment_hk_counter( new_hk_lfr_me.dpu_spw_invalid_addr, old_hk_lfr_me.dpu_spw_invalid_addr, &counter );
892 896 increment_hk_counter( new_hk_lfr_me.dpu_spw_eep, old_hk_lfr_me.dpu_spw_eep, &counter );
893 897 increment_hk_counter( new_hk_lfr_me.dpu_spw_rx_too_big, old_hk_lfr_me.dpu_spw_rx_too_big, &counter );
894 898
895 899 // store the counters for the next time
896 900 old_hk_lfr_me.dpu_spw_early_eop = new_hk_lfr_me.dpu_spw_early_eop;
897 901 old_hk_lfr_me.dpu_spw_invalid_addr = new_hk_lfr_me.dpu_spw_invalid_addr;
898 902 old_hk_lfr_me.dpu_spw_eep = new_hk_lfr_me.dpu_spw_eep;
899 903 old_hk_lfr_me.dpu_spw_rx_too_big = new_hk_lfr_me.dpu_spw_rx_too_big;
900 904
901 905 // update housekeeping packet counters, convert unsigned int numbers in 2 bytes numbers
902 906 // ME
903 907 housekeeping_packet.hk_lfr_me_cnt[0] = (unsigned char) ((counter & BYTE0_MASK) >> SHIFT_1_BYTE);
904 908 housekeeping_packet.hk_lfr_me_cnt[1] = (unsigned char) (counter & BYTE1_MASK);
905 909 }
906 910
907 911 void hk_lfr_le_me_he_update()
908 912 {
909 913
910 914 unsigned int hk_lfr_he_cnt;
911 915
912 916 hk_lfr_he_cnt = ((unsigned int) housekeeping_packet.hk_lfr_he_cnt[0]) * 256 + housekeeping_packet.hk_lfr_he_cnt[1];
913 917
914 918 //update the low severity error counter
915 919 hk_lfr_le_update( );
916 920
917 921 //update the medium severity error counter
918 922 hk_lfr_me_update();
919 923
920 924 //update the high severity error counter
921 925 hk_lfr_he_cnt = 0;
922 926
923 927 // update housekeeping packet counters, convert unsigned int numbers in 2 bytes numbers
924 928 // HE
925 929 housekeeping_packet.hk_lfr_he_cnt[0] = (unsigned char) ((hk_lfr_he_cnt & BYTE0_MASK) >> SHIFT_1_BYTE);
926 930 housekeeping_packet.hk_lfr_he_cnt[1] = (unsigned char) (hk_lfr_he_cnt & BYTE1_MASK);
927 931
928 932 }
929 933
930 934 void set_hk_lfr_time_not_synchro()
931 935 {
932 936 static unsigned char synchroLost = 1;
933 937 int synchronizationBit;
934 938
935 939 // get the synchronization bit
936 940 synchronizationBit =
937 941 (time_management_regs->coarse_time & VAL_LFR_SYNCHRONIZED) >> BIT_SYNCHRONIZATION; // 1000 0000 0000 0000
938 942
939 943 switch (synchronizationBit)
940 944 {
941 945 case 0:
942 946 if (synchroLost == 1)
943 947 {
944 948 synchroLost = 0;
945 949 }
946 950 break;
947 951 case 1:
948 952 if (synchroLost == 0 )
949 953 {
950 954 synchroLost = 1;
951 955 increase_unsigned_char_counter(&housekeeping_packet.hk_lfr_time_not_synchro);
952 956 update_hk_lfr_last_er_fields( RID_LE_LFR_TIME, CODE_NOT_SYNCHRO );
953 957 }
954 958 break;
955 959 default:
956 960 PRINTF1("in hk_lfr_time_not_synchro *** unexpected value for synchronizationBit = %d\n", synchronizationBit);
957 961 break;
958 962 }
959 963
960 964 }
961 965
962 966 void set_hk_lfr_ahb_correctable() // CRITICITY L
963 967 {
964 968 /** This function builds the error counter hk_lfr_ahb_correctable using the statistics provided
965 969 * by the Cache Control Register (ASI 2, offset 0) and in the Register Protection Control Register (ASR16) on the
966 970 * detected errors in the cache, in the integer unit and in the floating point unit.
967 971 *
968 972 * @param void
969 973 *
970 974 * @return void
971 975 *
972 976 * All errors are summed to set the value of the hk_lfr_ahb_correctable counter.
973 977 *
974 978 */
975 979
976 980 unsigned int ahb_correctable;
977 981 unsigned int instructionErrorCounter;
978 982 unsigned int dataErrorCounter;
979 983 unsigned int fprfErrorCounter;
980 984 unsigned int iurfErrorCounter;
981 985
982 986 CCR_getInstructionAndDataErrorCounters( &instructionErrorCounter, &dataErrorCounter);
983 987 ASR16_get_FPRF_IURF_ErrorCounters( &fprfErrorCounter, &iurfErrorCounter);
984 988
985 989 ahb_correctable = instructionErrorCounter
986 990 + dataErrorCounter
987 991 + fprfErrorCounter
988 992 + iurfErrorCounter
989 993 + housekeeping_packet.hk_lfr_ahb_correctable;
990 994
991 995 housekeeping_packet.hk_lfr_ahb_correctable = (unsigned char) (ahb_correctable & INT8_ALL_F); // [1111 1111]
992 996
993 997 }
Show file before | Show file after | Hide comments
@@ -1,1610 +1,1611
1 1 /** Functions related to the SpaceWire interface.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle SpaceWire transmissions:
7 7 * - configuration of the SpaceWire link
8 8 * - SpaceWire related interruption requests processing
9 9 * - transmission of TeleMetry packets by a dedicated RTEMS task
10 10 * - reception of TeleCommands by a dedicated RTEMS task
11 11 *
12 12 */
13 13
14 14 #include "fsw_spacewire.h"
15 15
16 16 rtems_name semq_name;
17 17 rtems_id semq_id;
18 18
19 19 //*****************
20 20 // waveform headers
21 21 Header_TM_LFR_SCIENCE_CWF_t headerCWF;
22 22 Header_TM_LFR_SCIENCE_SWF_t headerSWF;
23 23 Header_TM_LFR_SCIENCE_ASM_t headerASM;
24 24
25 25 unsigned char previousTimecodeCtr = 0;
26 26 unsigned int *grspwPtr = (unsigned int *) (REGS_ADDR_GRSPW + APB_OFFSET_GRSPW_TIME_REGISTER);
27 27
28 28 //***********
29 29 // RTEMS TASK
30 30 rtems_task spiq_task(rtems_task_argument unused)
31 31 {
32 32 /** This RTEMS task is awaken by an rtems_event sent by the interruption subroutine of the SpaceWire driver.
33 33 *
34 34 * @param unused is the starting argument of the RTEMS task
35 35 *
36 36 */
37 37
38 38 rtems_event_set event_out;
39 39 rtems_status_code status;
40 40 int linkStatus;
41 41
42 42 BOOT_PRINTF("in SPIQ *** \n")
43 43
44 44 while(true){
45 45 rtems_event_receive(SPW_LINKERR_EVENT, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an SPW_LINKERR_EVENT
46 46 PRINTF("in SPIQ *** got SPW_LINKERR_EVENT\n")
47 47
48 48 // [0] SUSPEND RECV AND SEND TASKS
49 49 status = rtems_task_suspend( Task_id[ TASKID_RECV ] );
50 50 if ( status != RTEMS_SUCCESSFUL ) {
51 51 PRINTF("in SPIQ *** ERR suspending RECV Task\n")
52 52 }
53 53 status = rtems_task_suspend( Task_id[ TASKID_SEND ] );
54 54 if ( status != RTEMS_SUCCESSFUL ) {
55 55 PRINTF("in SPIQ *** ERR suspending SEND Task\n")
56 56 }
57 57
58 58 // [1] CHECK THE LINK
59 59 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (1)
60 60 if ( linkStatus != SPW_LINK_OK) {
61 61 PRINTF1("in SPIQ *** linkStatus %d, wait...\n", linkStatus)
62 62 status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms
63 63 }
64 64
65 65 // [2] RECHECK THE LINK AFTER SY_LFR_DPU_CONNECT_TIMEOUT
66 66 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (2)
67 67 if ( linkStatus != SPW_LINK_OK ) // [2.a] not in run state, reset the link
68 68 {
69 69 spacewire_read_statistics();
70 70 status = spacewire_several_connect_attemps( );
71 71 }
72 72 else // [2.b] in run state, start the link
73 73 {
74 74 status = spacewire_stop_and_start_link( fdSPW ); // start the link
75 75 if ( status != RTEMS_SUCCESSFUL)
76 76 {
77 77 PRINTF1("in SPIQ *** ERR spacewire_stop_and_start_link %d\n", status)
78 78 }
79 79 }
80 80
81 81 // [3] COMPLETE RECOVERY ACTION AFTER SY_LFR_DPU_CONNECT_ATTEMPTS
82 82 if ( status == RTEMS_SUCCESSFUL ) // [3.a] the link is in run state and has been started successfully
83 83 {
84 84 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
85 85 if ( status != RTEMS_SUCCESSFUL ) {
86 86 PRINTF("in SPIQ *** ERR resuming SEND Task\n")
87 87 }
88 88 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
89 89 if ( status != RTEMS_SUCCESSFUL ) {
90 90 PRINTF("in SPIQ *** ERR resuming RECV Task\n")
91 91 }
92 92 }
93 93 else // [3.b] the link is not in run state, go in STANDBY mode
94 94 {
95 95 status = enter_mode_standby();
96 96 if ( status != RTEMS_SUCCESSFUL )
97 97 {
98 98 PRINTF1("in SPIQ *** ERR enter_standby_mode *** code %d\n", status)
99 99 }
100 100 {
101 101 updateLFRCurrentMode( LFR_MODE_STANDBY );
102 102 }
103 103 // wake the LINK task up to wait for the link recovery
104 104 status = rtems_event_send ( Task_id[TASKID_LINK], RTEMS_EVENT_0 );
105 105 status = rtems_task_suspend( RTEMS_SELF );
106 106 }
107 107 }
108 108 }
109 109
110 110 rtems_task recv_task( rtems_task_argument unused )
111 111 {
112 112 /** This RTEMS task is dedicated to the reception of incoming TeleCommands.
113 113 *
114 114 * @param unused is the starting argument of the RTEMS task
115 115 *
116 116 * The RECV task blocks on a call to the read system call, waiting for incoming SpaceWire data. When unblocked:
117 117 * 1. It reads the incoming data.
118 118 * 2. Launches the acceptance procedure.
119 119 * 3. If the Telecommand is valid, sends it to a dedicated RTEMS message queue.
120 120 *
121 121 */
122 122
123 123 int len;
124 124 ccsdsTelecommandPacket_t currentTC;
125 125 unsigned char computed_CRC[ BYTES_PER_CRC ];
126 126 unsigned char currentTC_LEN_RCV[ BYTES_PER_PKT_LEN ];
127 127 unsigned char destinationID;
128 128 unsigned int estimatedPacketLength;
129 129 unsigned int parserCode;
130 130 rtems_status_code status;
131 131 rtems_id queue_recv_id;
132 132 rtems_id queue_send_id;
133 133
134 134 initLookUpTableForCRC(); // the table is used to compute Cyclic Redundancy Codes
135 135
136 136 status = get_message_queue_id_recv( &queue_recv_id );
137 137 if (status != RTEMS_SUCCESSFUL)
138 138 {
139 139 PRINTF1("in RECV *** ERR get_message_queue_id_recv %d\n", status)
140 140 }
141 141
142 142 status = get_message_queue_id_send( &queue_send_id );
143 143 if (status != RTEMS_SUCCESSFUL)
144 144 {
145 145 PRINTF1("in RECV *** ERR get_message_queue_id_send %d\n", status)
146 146 }
147 147
148 148 BOOT_PRINTF("in RECV *** \n")
149 149
150 150 while(1)
151 151 {
152 152 len = read( fdSPW, (char*) &currentTC, CCSDS_TC_PKT_MAX_SIZE ); // the call to read is blocking
153 153 if (len == -1){ // error during the read call
154 154 PRINTF1("in RECV *** last read call returned -1, ERRNO %d\n", errno)
155 155 }
156 156 else {
157 157 if ( (len+1) < CCSDS_TC_PKT_MIN_SIZE ) {
158 158 PRINTF("in RECV *** packet lenght too short\n")
159 159 }
160 160 else {
161 161 estimatedPacketLength = (unsigned int) (len - CCSDS_TC_TM_PACKET_OFFSET - PROTID_RES_APP); // => -3 is for Prot ID, Reserved and User App bytes
162 162 //PRINTF1("incoming TC with Length (byte): %d\n", len - 3);
163 163 currentTC_LEN_RCV[ 0 ] = (unsigned char) (estimatedPacketLength >> SHIFT_1_BYTE);
164 164 currentTC_LEN_RCV[ 1 ] = (unsigned char) (estimatedPacketLength );
165 165 // CHECK THE TC
166 166 parserCode = tc_parser( &currentTC, estimatedPacketLength, computed_CRC ) ;
167 167 if ( (parserCode == ILLEGAL_APID) || (parserCode == WRONG_LEN_PKT)
168 168 || (parserCode == INCOR_CHECKSUM) || (parserCode == ILL_TYPE)
169 169 || (parserCode == ILL_SUBTYPE) || (parserCode == WRONG_APP_DATA)
170 170 || (parserCode == WRONG_SRC_ID) )
171 171 { // send TM_LFR_TC_EXE_CORRUPTED
172 172 PRINTF1("TC corrupted received, with code: %d\n", parserCode);
173 173 if ( !( (currentTC.serviceType==TC_TYPE_TIME) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_TIME) )
174 174 &&
175 175 !( (currentTC.serviceType==TC_TYPE_GEN) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_INFO))
176 176 )
177 177 {
178 178 if ( parserCode == WRONG_SRC_ID )
179 179 {
180 180 destinationID = SID_TC_GROUND;
181 181 }
182 182 else
183 183 {
184 184 destinationID = currentTC.sourceID;
185 185 }
186 186 send_tm_lfr_tc_exe_corrupted( &currentTC, queue_send_id,
187 187 computed_CRC, currentTC_LEN_RCV,
188 188 destinationID );
189 189 }
190 190 }
191 191 else
192 192 { // send valid TC to the action launcher
193 193 status = rtems_message_queue_send( queue_recv_id, &currentTC,
194 194 estimatedPacketLength + CCSDS_TC_TM_PACKET_OFFSET + PROTID_RES_APP);
195 195 }
196 196 }
197 197 }
198 198
199 199 update_queue_max_count( queue_recv_id, &hk_lfr_q_rv_fifo_size_max );
200 200
201 201 }
202 202 }
203 203
204 204 rtems_task send_task( rtems_task_argument argument)
205 205 {
206 206 /** This RTEMS task is dedicated to the transmission of TeleMetry packets.
207 207 *
208 208 * @param unused is the starting argument of the RTEMS task
209 209 *
210 210 * The SEND task waits for a message to become available in the dedicated RTEMS queue. When a message arrives:
211 211 * - if the first byte is equal to CCSDS_DESTINATION_ID, the message is sent as is using the write system call.
212 212 * - if the first byte is not equal to CCSDS_DESTINATION_ID, the message is handled as a spw_ioctl_pkt_send. After
213 213 * analyzis, the packet is sent either using the write system call or using the ioctl call SPACEWIRE_IOCTRL_SEND, depending on the
214 214 * data it contains.
215 215 *
216 216 */
217 217
218 218 rtems_status_code status; // RTEMS status code
219 219 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
220 220 ring_node *incomingRingNodePtr;
221 221 int ring_node_address;
222 222 char *charPtr;
223 223 spw_ioctl_pkt_send *spw_ioctl_send;
224 224 size_t size; // size of the incoming TC packet
225 225 rtems_id queue_send_id;
226 226 unsigned int sid;
227 227 unsigned char sidAsUnsignedChar;
228 228 unsigned char type;
229 229
230 230 incomingRingNodePtr = NULL;
231 231 ring_node_address = 0;
232 232 charPtr = (char *) &ring_node_address;
233 233 sid = 0;
234 234 sidAsUnsignedChar = 0;
235 235
236 236 init_header_cwf( &headerCWF );
237 237 init_header_swf( &headerSWF );
238 238 init_header_asm( &headerASM );
239 239
240 240 status = get_message_queue_id_send( &queue_send_id );
241 241 if (status != RTEMS_SUCCESSFUL)
242 242 {
243 243 PRINTF1("in HOUS *** ERR get_message_queue_id_send %d\n", status)
244 244 }
245 245
246 246 BOOT_PRINTF("in SEND *** \n")
247 247
248 248 while(1)
249 249 {
250 250 status = rtems_message_queue_receive( queue_send_id, incomingData, &size,
251 251 RTEMS_WAIT, RTEMS_NO_TIMEOUT );
252 252
253 253 if (status!=RTEMS_SUCCESSFUL)
254 254 {
255 255 PRINTF1("in SEND *** (1) ERR = %d\n", status)
256 256 }
257 257 else
258 258 {
259 259 if ( size == sizeof(ring_node*) )
260 260 {
261 261 charPtr[0] = incomingData[0];
262 262 charPtr[1] = incomingData[1];
263 263 charPtr[BYTE_2] = incomingData[BYTE_2];
264 264 charPtr[BYTE_3] = incomingData[BYTE_3];
265 265 incomingRingNodePtr = (ring_node*) ring_node_address;
266 266 sid = incomingRingNodePtr->sid;
267 267 if ( (sid==SID_NORM_CWF_LONG_F3)
268 268 || (sid==SID_BURST_CWF_F2 )
269 269 || (sid==SID_SBM1_CWF_F1 )
270 270 || (sid==SID_SBM2_CWF_F2 ))
271 271 {
272 272 spw_send_waveform_CWF( incomingRingNodePtr, &headerCWF );
273 273 }
274 274 else if ( (sid==SID_NORM_SWF_F0) || (sid== SID_NORM_SWF_F1) || (sid==SID_NORM_SWF_F2) )
275 275 {
276 276 spw_send_waveform_SWF( incomingRingNodePtr, &headerSWF );
277 277 }
278 278 else if ( (sid==SID_NORM_CWF_F3) )
279 279 {
280 280 spw_send_waveform_CWF3_light( incomingRingNodePtr, &headerCWF );
281 281 }
282 282 else if (sid==SID_NORM_ASM_F0)
283 283 {
284 284 spw_send_asm_f0( incomingRingNodePtr, &headerASM );
285 285 }
286 286 else if (sid==SID_NORM_ASM_F1)
287 287 {
288 288 spw_send_asm_f1( incomingRingNodePtr, &headerASM );
289 289 }
290 290 else if (sid==SID_NORM_ASM_F2)
291 291 {
292 292 spw_send_asm_f2( incomingRingNodePtr, &headerASM );
293 293 }
294 294 else if ( sid==TM_CODE_K_DUMP )
295 295 {
296 296 spw_send_k_dump( incomingRingNodePtr );
297 297 }
298 298 else
299 299 {
300 300 PRINTF1("unexpected sid = %d\n", sid);
301 301 }
302 302 }
303 303 else if ( incomingData[0] == CCSDS_DESTINATION_ID ) // the incoming message is a ccsds packet
304 304 {
305 305 sidAsUnsignedChar = (unsigned char) incomingData[ PACKET_POS_PA_LFR_SID_PKT ];
306 306 sid = sidAsUnsignedChar;
307 307 type = (unsigned char) incomingData[ PACKET_POS_SERVICE_TYPE ];
308 308 if (type == TM_TYPE_LFR_SCIENCE) // this is a BP packet, all other types are handled differently
309 309 // SET THE SEQUENCE_CNT PARAMETER IN CASE OF BP0 OR BP1 PACKETS
310 310 {
311 311 increment_seq_counter_source_id( (unsigned char*) &incomingData[ PACKET_POS_SEQUENCE_CNT ], sid );
312 312 }
313 313
314 314 status = write( fdSPW, incomingData, size );
315 315 if (status == -1){
316 316 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
317 317 }
318 318 }
319 319 else // the incoming message is a spw_ioctl_pkt_send structure
320 320 {
321 321 spw_ioctl_send = (spw_ioctl_pkt_send*) incomingData;
322 322 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, spw_ioctl_send );
323 323 if (status == -1){
324 324 PRINTF2("in SEND *** (2.b) ERRNO = %d, RTEMS = %d\n", errno, status)
325 325 }
326 326 }
327 327 }
328 328
329 329 update_queue_max_count( queue_send_id, &hk_lfr_q_sd_fifo_size_max );
330 330
331 331 }
332 332 }
333 333
334 334 rtems_task link_task( rtems_task_argument argument )
335 335 {
336 336 rtems_event_set event_out;
337 337 rtems_status_code status;
338 338 int linkStatus;
339 339
340 340 BOOT_PRINTF("in LINK ***\n")
341 341
342 342 while(1)
343 343 {
344 344 // wait for an RTEMS_EVENT
345 345 rtems_event_receive( RTEMS_EVENT_0,
346 346 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
347 347 PRINTF("in LINK *** wait for the link\n")
348 348 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
349 349 while( linkStatus != SPW_LINK_OK) // wait for the link
350 350 {
351 351 status = rtems_task_wake_after( SPW_LINK_WAIT ); // monitor the link each 100ms
352 352 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
353 353 watchdog_reload();
354 354 }
355 355
356 356 spacewire_read_statistics();
357 357 status = spacewire_stop_and_start_link( fdSPW );
358 358
359 359 if (status != RTEMS_SUCCESSFUL)
360 360 {
361 361 PRINTF1("in LINK *** ERR link not started %d\n", status)
362 362 }
363 363 else
364 364 {
365 365 PRINTF("in LINK *** OK link started\n")
366 366 }
367 367
368 368 // restart the SPIQ task
369 369 status = rtems_task_restart( Task_id[TASKID_SPIQ], 1 );
370 370 if ( status != RTEMS_SUCCESSFUL ) {
371 371 PRINTF("in SPIQ *** ERR restarting SPIQ Task\n")
372 372 }
373 373
374 374 // restart RECV and SEND
375 375 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
376 376 if ( status != RTEMS_SUCCESSFUL ) {
377 377 PRINTF("in SPIQ *** ERR restarting SEND Task\n")
378 378 }
379 379 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
380 380 if ( status != RTEMS_SUCCESSFUL ) {
381 381 PRINTF("in SPIQ *** ERR restarting RECV Task\n")
382 382 }
383 383 }
384 384 }
385 385
386 386 //****************
387 387 // OTHER FUNCTIONS
388 388 int spacewire_open_link( void ) // by default, the driver resets the core: [SPW_CTRL_WRITE(pDev, SPW_CTRL_RESET);]
389 389 {
390 390 /** This function opens the SpaceWire link.
391 391 *
392 392 * @return a valid file descriptor in case of success, -1 in case of a failure
393 393 *
394 394 */
395 395 rtems_status_code status;
396 396
397 397 fdSPW = open(GRSPW_DEVICE_NAME, O_RDWR); // open the device. the open call resets the hardware
398 398 if ( fdSPW < 0 ) {
399 399 PRINTF1("ERR *** in configure_spw_link *** error opening "GRSPW_DEVICE_NAME" with ERR %d\n", errno)
400 400 }
401 401 else
402 402 {
403 403 status = RTEMS_SUCCESSFUL;
404 404 }
405 405
406 406 return status;
407 407 }
408 408
409 409 int spacewire_start_link( int fd )
410 410 {
411 411 rtems_status_code status;
412 412
413 413 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
414 414 // -1 default hardcoded driver timeout
415 415
416 416 return status;
417 417 }
418 418
419 419 int spacewire_stop_and_start_link( int fd )
420 420 {
421 421 rtems_status_code status;
422 422
423 423 status = ioctl( fd, SPACEWIRE_IOCTRL_STOP); // start fails if link pDev->running != 0
424 424 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
425 425 // -1 default hardcoded driver timeout
426 426
427 427 return status;
428 428 }
429 429
430 430 int spacewire_configure_link( int fd )
431 431 {
432 432 /** This function configures the SpaceWire link.
433 433 *
434 434 * @return GR-RTEMS-DRIVER directive status codes:
435 435 * - 22 EINVAL - Null pointer or an out of range value was given as the argument.
436 436 * - 16 EBUSY - Only used for SEND. Returned when no descriptors are avialble in non-blocking mode.
437 437 * - 88 ENOSYS - Returned for SET_DESTKEY if RMAP command handler is not available or if a non-implemented call is used.
438 438 * - 116 ETIMEDOUT - REturned for SET_PACKET_SIZE and START if the link could not be brought up.
439 439 * - 12 ENOMEM - Returned for SET_PACKETSIZE if it was unable to allocate the new buffers.
440 440 * - 5 EIO - Error when writing to grswp hardware registers.
441 441 * - 2 ENOENT - No such file or directory
442 442 */
443 443
444 444 rtems_status_code status;
445 445
446 446 spacewire_set_NP(1, REGS_ADDR_GRSPW); // [N]o [P]ort force
447 447 spacewire_set_RE(1, REGS_ADDR_GRSPW); // [R]MAP [E]nable, the dedicated call seems to break the no port force configuration
448 448 spw_ioctl_packetsize packetsize;
449 449
450 450 packetsize.rxsize = SPW_RXSIZE;
451 451 packetsize.txdsize = SPW_TXDSIZE;
452 452 packetsize.txhsize = SPW_TXHSIZE;
453 453
454 454 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_RXBLOCK, 1); // sets the blocking mode for reception
455 455 if (status!=RTEMS_SUCCESSFUL) {
456 456 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_RXBLOCK\n")
457 457 }
458 458 //
459 459 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_EVENT_ID, Task_id[TASKID_SPIQ]); // sets the task ID to which an event is sent when a
460 460 if (status!=RTEMS_SUCCESSFUL) {
461 461 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_EVENT_ID\n") // link-error interrupt occurs
462 462 }
463 463 //
464 464 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_DISABLE_ERR, 0); // automatic link-disabling due to link-error interrupts
465 465 if (status!=RTEMS_SUCCESSFUL) {
466 466 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_DISABLE_ERR\n")
467 467 }
468 468 //
469 469 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ, 1); // sets the link-error interrupt bit
470 470 if (status!=RTEMS_SUCCESSFUL) {
471 471 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ\n")
472 472 }
473 473 //
474 474 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK, 1); // transmission blocks
475 475 if (status!=RTEMS_SUCCESSFUL) {
476 476 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK\n")
477 477 }
478 478 //
479 479 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL, 1); // transmission blocks when no transmission descriptor is available
480 480 if (status!=RTEMS_SUCCESSFUL) {
481 481 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL\n")
482 482 }
483 483 //
484 484 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TCODE_CTRL, CONF_TCODE_CTRL); // [Time Rx : Time Tx : Link error : Tick-out IRQ]
485 485 if (status!=RTEMS_SUCCESSFUL) {
486 486 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TCODE_CTRL,\n")
487 487 }
488 488 //
489 489 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_PACKETSIZE, packetsize); // set rxsize, txdsize and txhsize
490 490 if (status!=RTEMS_SUCCESSFUL) {
491 491 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_PACKETSIZE,\n")
492 492 }
493 493
494 494 return status;
495 495 }
496 496
497 497 int spacewire_several_connect_attemps( void )
498 498 {
499 499 /** This function is executed by the SPIQ rtems_task wehn it has been awaken by an interruption raised by the SpaceWire driver.
500 500 *
501 501 * @return RTEMS directive status code:
502 502 * - RTEMS_UNSATISFIED is returned is the link is not in the running state after 10 s.
503 503 * - RTEMS_SUCCESSFUL is returned if the link is up before the timeout.
504 504 *
505 505 */
506 506
507 507 rtems_status_code status_spw;
508 508 rtems_status_code status;
509 509 int i;
510 510
511 for ( i=0; i<SY_LFR_DPU_CONNECT_ATTEMPT; i++ )
511 i = 0;
512 while (i < SY_LFR_DPU_CONNECT_ATTEMPT)
512 513 {
513 514 PRINTF1("in spacewire_reset_link *** link recovery, try %d\n", i);
514 515
515 516 // CLOSING THE DRIVER AT THIS POINT WILL MAKE THE SEND TASK BLOCK THE SYSTEM
516 517
517 518 status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms
518 519
519 520 status_spw = spacewire_stop_and_start_link( fdSPW );
520 521
521 522 if ( status_spw != RTEMS_SUCCESSFUL )
522 523 {
523 PRINTF1("in spacewire_reset_link *** ERR spacewire_start_link code %d\n", status_spw)
524 i = i + 1;
525 PRINTF1("in spacewire_reset_link *** ERR spacewire_start_link code %d\n", status_spw);
524 526 }
525
526 if ( status_spw == RTEMS_SUCCESSFUL)
527 else
527 528 {
528 break;
529 i = SY_LFR_DPU_CONNECT_ATTEMPT;
529 530 }
530 531 }
531 532
532 533 return status_spw;
533 534 }
534 535
535 536 void spacewire_set_NP( unsigned char val, unsigned int regAddr ) // [N]o [P]ort force
536 537 {
537 538 /** This function sets the [N]o [P]ort force bit of the GRSPW control register.
538 539 *
539 540 * @param val is the value, 0 or 1, used to set the value of the NP bit.
540 541 * @param regAddr is the address of the GRSPW control register.
541 542 *
542 543 * NP is the bit 20 of the GRSPW control register.
543 544 *
544 545 */
545 546
546 547 unsigned int *spwptr = (unsigned int*) regAddr;
547 548
548 549 if (val == 1) {
549 550 *spwptr = *spwptr | SPW_BIT_NP; // [NP] set the No port force bit
550 551 }
551 552 if (val== 0) {
552 553 *spwptr = *spwptr & SPW_BIT_NP_MASK;
553 554 }
554 555 }
555 556
556 557 void spacewire_set_RE( unsigned char val, unsigned int regAddr ) // [R]MAP [E]nable
557 558 {
558 559 /** This function sets the [R]MAP [E]nable bit of the GRSPW control register.
559 560 *
560 561 * @param val is the value, 0 or 1, used to set the value of the RE bit.
561 562 * @param regAddr is the address of the GRSPW control register.
562 563 *
563 564 * RE is the bit 16 of the GRSPW control register.
564 565 *
565 566 */
566 567
567 568 unsigned int *spwptr = (unsigned int*) regAddr;
568 569
569 570 if (val == 1)
570 571 {
571 572 *spwptr = *spwptr | SPW_BIT_RE; // [RE] set the RMAP Enable bit
572 573 }
573 574 if (val== 0)
574 575 {
575 576 *spwptr = *spwptr & SPW_BIT_RE_MASK;
576 577 }
577 578 }
578 579
579 580 void spacewire_read_statistics( void )
580 581 {
581 582 /** This function reads the SpaceWire statistics from the grspw RTEMS driver.
582 583 *
583 584 * @param void
584 585 *
585 586 * @return void
586 587 *
587 588 * Once they are read, the counters are stored in a global variable used during the building of the
588 589 * HK packets.
589 590 *
590 591 */
591 592
592 593 rtems_status_code status;
593 594 spw_stats current;
594 595
595 596 spacewire_get_last_error();
596 597
597 598 // read the current statistics
598 599 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &current );
599 600
600 601 // clear the counters
601 602 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_CLR_STATISTICS );
602 603
603 604 // typedef struct {
604 605 // unsigned int tx_link_err; // NOT IN HK
605 606 // unsigned int rx_rmap_header_crc_err; // NOT IN HK
606 607 // unsigned int rx_rmap_data_crc_err; // NOT IN HK
607 608 // unsigned int rx_eep_err;
608 609 // unsigned int rx_truncated;
609 610 // unsigned int parity_err;
610 611 // unsigned int escape_err;
611 612 // unsigned int credit_err;
612 613 // unsigned int write_sync_err;
613 614 // unsigned int disconnect_err;
614 615 // unsigned int early_ep;
615 616 // unsigned int invalid_address;
616 617 // unsigned int packets_sent;
617 618 // unsigned int packets_received;
618 619 // } spw_stats;
619 620
620 621 // rx_eep_err
621 622 grspw_stats.rx_eep_err = grspw_stats.rx_eep_err + current.rx_eep_err;
622 623 // rx_truncated
623 624 grspw_stats.rx_truncated = grspw_stats.rx_truncated + current.rx_truncated;
624 625 // parity_err
625 626 grspw_stats.parity_err = grspw_stats.parity_err + current.parity_err;
626 627 // escape_err
627 628 grspw_stats.escape_err = grspw_stats.escape_err + current.escape_err;
628 629 // credit_err
629 630 grspw_stats.credit_err = grspw_stats.credit_err + current.credit_err;
630 631 // write_sync_err
631 632 grspw_stats.write_sync_err = grspw_stats.write_sync_err + current.write_sync_err;
632 633 // disconnect_err
633 634 grspw_stats.disconnect_err = grspw_stats.disconnect_err + current.disconnect_err;
634 635 // early_ep
635 636 grspw_stats.early_ep = grspw_stats.early_ep + current.early_ep;
636 637 // invalid_address
637 638 grspw_stats.invalid_address = grspw_stats.invalid_address + current.invalid_address;
638 639 // packets_sent
639 640 grspw_stats.packets_sent = grspw_stats.packets_sent + current.packets_sent;
640 641 // packets_received
641 642 grspw_stats.packets_received= grspw_stats.packets_received + current.packets_received;
642 643
643 644 }
644 645
645 646 void spacewire_get_last_error( void )
646 647 {
647 648 static spw_stats previous;
648 649 spw_stats current;
649 650 rtems_status_code status;
650 651
651 652 unsigned int hk_lfr_last_er_rid;
652 653 unsigned char hk_lfr_last_er_code;
653 654 int coarseTime;
654 655 int fineTime;
655 656 unsigned char update_hk_lfr_last_er;
656 657
657 658 update_hk_lfr_last_er = 0;
658 659
659 660 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &current );
660 661
661 662 // get current time
662 663 coarseTime = time_management_regs->coarse_time;
663 664 fineTime = time_management_regs->fine_time;
664 665
665 666 // typedef struct {
666 667 // unsigned int tx_link_err; // NOT IN HK
667 668 // unsigned int rx_rmap_header_crc_err; // NOT IN HK
668 669 // unsigned int rx_rmap_data_crc_err; // NOT IN HK
669 670 // unsigned int rx_eep_err;
670 671 // unsigned int rx_truncated;
671 672 // unsigned int parity_err;
672 673 // unsigned int escape_err;
673 674 // unsigned int credit_err;
674 675 // unsigned int write_sync_err;
675 676 // unsigned int disconnect_err;
676 677 // unsigned int early_ep;
677 678 // unsigned int invalid_address;
678 679 // unsigned int packets_sent;
679 680 // unsigned int packets_received;
680 681 // } spw_stats;
681 682
682 683 // tx_link_err *** no code associated to this field
683 684 // rx_rmap_header_crc_err *** LE *** in HK
684 685 if (previous.rx_rmap_header_crc_err != current.rx_rmap_header_crc_err)
685 686 {
686 687 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
687 688 hk_lfr_last_er_code = CODE_HEADER_CRC;
688 689 update_hk_lfr_last_er = 1;
689 690 }
690 691 // rx_rmap_data_crc_err *** LE *** NOT IN HK
691 692 if (previous.rx_rmap_data_crc_err != current.rx_rmap_data_crc_err)
692 693 {
693 694 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
694 695 hk_lfr_last_er_code = CODE_DATA_CRC;
695 696 update_hk_lfr_last_er = 1;
696 697 }
697 698 // rx_eep_err
698 699 if (previous.rx_eep_err != current.rx_eep_err)
699 700 {
700 701 hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW;
701 702 hk_lfr_last_er_code = CODE_EEP;
702 703 update_hk_lfr_last_er = 1;
703 704 }
704 705 // rx_truncated
705 706 if (previous.rx_truncated != current.rx_truncated)
706 707 {
707 708 hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW;
708 709 hk_lfr_last_er_code = CODE_RX_TOO_BIG;
709 710 update_hk_lfr_last_er = 1;
710 711 }
711 712 // parity_err
712 713 if (previous.parity_err != current.parity_err)
713 714 {
714 715 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
715 716 hk_lfr_last_er_code = CODE_PARITY;
716 717 update_hk_lfr_last_er = 1;
717 718 }
718 719 // escape_err
719 720 if (previous.parity_err != current.parity_err)
720 721 {
721 722 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
722 723 hk_lfr_last_er_code = CODE_ESCAPE;
723 724 update_hk_lfr_last_er = 1;
724 725 }
725 726 // credit_err
726 727 if (previous.credit_err != current.credit_err)
727 728 {
728 729 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
729 730 hk_lfr_last_er_code = CODE_CREDIT;
730 731 update_hk_lfr_last_er = 1;
731 732 }
732 733 // write_sync_err
733 734 if (previous.write_sync_err != current.write_sync_err)
734 735 {
735 736 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
736 737 hk_lfr_last_er_code = CODE_WRITE_SYNC;
737 738 update_hk_lfr_last_er = 1;
738 739 }
739 740 // disconnect_err
740 741 if (previous.disconnect_err != current.disconnect_err)
741 742 {
742 743 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
743 744 hk_lfr_last_er_code = CODE_DISCONNECT;
744 745 update_hk_lfr_last_er = 1;
745 746 }
746 747 // early_ep
747 748 if (previous.early_ep != current.early_ep)
748 749 {
749 750 hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW;
750 751 hk_lfr_last_er_code = CODE_EARLY_EOP_EEP;
751 752 update_hk_lfr_last_er = 1;
752 753 }
753 754 // invalid_address
754 755 if (previous.invalid_address != current.invalid_address)
755 756 {
756 757 hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW;
757 758 hk_lfr_last_er_code = CODE_INVALID_ADDRESS;
758 759 update_hk_lfr_last_er = 1;
759 760 }
760 761
761 762 // if a field has changed, update the hk_last_er fields
762 763 if (update_hk_lfr_last_er == 1)
763 764 {
764 765 update_hk_lfr_last_er_fields( hk_lfr_last_er_rid, hk_lfr_last_er_code );
765 766 }
766 767
767 768 previous = current;
768 769 }
769 770
770 771 void update_hk_lfr_last_er_fields(unsigned int rid, unsigned char code)
771 772 {
772 773 unsigned char *coarseTimePtr;
773 774 unsigned char *fineTimePtr;
774 775
775 776 coarseTimePtr = (unsigned char*) &time_management_regs->coarse_time;
776 777 fineTimePtr = (unsigned char*) &time_management_regs->fine_time;
777 778
778 779 housekeeping_packet.hk_lfr_last_er_rid[0] = (unsigned char) ((rid & BYTE0_MASK) >> SHIFT_1_BYTE );
779 780 housekeeping_packet.hk_lfr_last_er_rid[1] = (unsigned char) (rid & BYTE1_MASK);
780 781 housekeeping_packet.hk_lfr_last_er_code = code;
781 782 housekeeping_packet.hk_lfr_last_er_time[0] = coarseTimePtr[0];
782 783 housekeeping_packet.hk_lfr_last_er_time[1] = coarseTimePtr[1];
783 784 housekeeping_packet.hk_lfr_last_er_time[BYTE_2] = coarseTimePtr[BYTE_2];
784 785 housekeeping_packet.hk_lfr_last_er_time[BYTE_3] = coarseTimePtr[BYTE_3];
785 786 housekeeping_packet.hk_lfr_last_er_time[BYTE_4] = fineTimePtr[BYTE_2];
786 787 housekeeping_packet.hk_lfr_last_er_time[BYTE_5] = fineTimePtr[BYTE_3];
787 788 }
788 789
789 790 void update_hk_with_grspw_stats( void )
790 791 {
791 792 //****************************
792 793 // DPU_SPACEWIRE_IF_STATISTICS
793 794 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[0] = (unsigned char) (grspw_stats.packets_received >> SHIFT_1_BYTE);
794 795 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[1] = (unsigned char) (grspw_stats.packets_received);
795 796 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[0] = (unsigned char) (grspw_stats.packets_sent >> SHIFT_1_BYTE);
796 797 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[1] = (unsigned char) (grspw_stats.packets_sent);
797 798
798 799 //******************************************
799 800 // ERROR COUNTERS / SPACEWIRE / LOW SEVERITY
800 801 housekeeping_packet.hk_lfr_dpu_spw_parity = (unsigned char) grspw_stats.parity_err;
801 802 housekeeping_packet.hk_lfr_dpu_spw_disconnect = (unsigned char) grspw_stats.disconnect_err;
802 803 housekeeping_packet.hk_lfr_dpu_spw_escape = (unsigned char) grspw_stats.escape_err;
803 804 housekeeping_packet.hk_lfr_dpu_spw_credit = (unsigned char) grspw_stats.credit_err;
804 805 housekeeping_packet.hk_lfr_dpu_spw_write_sync = (unsigned char) grspw_stats.write_sync_err;
805 806
806 807 //*********************************************
807 808 // ERROR COUNTERS / SPACEWIRE / MEDIUM SEVERITY
808 809 housekeeping_packet.hk_lfr_dpu_spw_early_eop = (unsigned char) grspw_stats.early_ep;
809 810 housekeeping_packet.hk_lfr_dpu_spw_invalid_addr = (unsigned char) grspw_stats.invalid_address;
810 811 housekeeping_packet.hk_lfr_dpu_spw_eep = (unsigned char) grspw_stats.rx_eep_err;
811 812 housekeeping_packet.hk_lfr_dpu_spw_rx_too_big = (unsigned char) grspw_stats.rx_truncated;
812 813 }
813 814
814 815 void spacewire_update_hk_lfr_link_state( unsigned char *hk_lfr_status_word_0 )
815 816 {
816 817 unsigned int *statusRegisterPtr;
817 818 unsigned char linkState;
818 819
819 820 statusRegisterPtr = (unsigned int *) (REGS_ADDR_GRSPW + APB_OFFSET_GRSPW_STATUS_REGISTER);
820 821 linkState =
821 822 (unsigned char) ( ( (*statusRegisterPtr) >> SPW_LINK_STAT_POS) & STATUS_WORD_LINK_STATE_BITS); // [0000 0111]
822 823
823 824 *hk_lfr_status_word_0 = *hk_lfr_status_word_0 & STATUS_WORD_LINK_STATE_MASK; // [1111 1000] set link state to 0
824 825
825 826 *hk_lfr_status_word_0 = *hk_lfr_status_word_0 | linkState; // update hk_lfr_dpu_spw_link_state
826 827 }
827 828
828 829 void increase_unsigned_char_counter( unsigned char *counter )
829 830 {
830 831 // update the number of valid timecodes that have been received
831 832 if (*counter == UINT8_MAX)
832 833 {
833 834 *counter = 0;
834 835 }
835 836 else
836 837 {
837 838 *counter = *counter + 1;
838 839 }
839 840 }
840 841
841 842 unsigned int check_timecode_and_previous_timecode_coherency(unsigned char currentTimecodeCtr)
842 843 {
843 844 /** This function checks the coherency between the incoming timecode and the last valid timecode.
844 845 *
845 846 * @param currentTimecodeCtr is the incoming timecode
846 847 *
847 848 * @return returned codes::
848 849 * - LFR_DEFAULT
849 850 * - LFR_SUCCESSFUL
850 851 *
851 852 */
852 853
853 854 static unsigned char firstTickout = 1;
854 855 unsigned char ret;
855 856
856 857 ret = LFR_DEFAULT;
857 858
858 859 if (firstTickout == 0)
859 860 {
860 861 if (currentTimecodeCtr == 0)
861 862 {
862 863 if (previousTimecodeCtr == SPW_TIMECODE_MAX)
863 864 {
864 865 ret = LFR_SUCCESSFUL;
865 866 }
866 867 else
867 868 {
868 869 ret = LFR_DEFAULT;
869 870 }
870 871 }
871 872 else
872 873 {
873 874 if (currentTimecodeCtr == (previousTimecodeCtr +1))
874 875 {
875 876 ret = LFR_SUCCESSFUL;
876 877 }
877 878 else
878 879 {
879 880 ret = LFR_DEFAULT;
880 881 }
881 882 }
882 883 }
883 884 else
884 885 {
885 886 firstTickout = 0;
886 887 ret = LFR_SUCCESSFUL;
887 888 }
888 889
889 890 return ret;
890 891 }
891 892
892 893 unsigned int check_timecode_and_internal_time_coherency(unsigned char timecode, unsigned char internalTime)
893 894 {
894 895 unsigned int ret;
895 896
896 897 ret = LFR_DEFAULT;
897 898
898 899 if (timecode == internalTime)
899 900 {
900 901 ret = LFR_SUCCESSFUL;
901 902 }
902 903 else
903 904 {
904 905 ret = LFR_DEFAULT;
905 906 }
906 907
907 908 return ret;
908 909 }
909 910
910 911 void timecode_irq_handler( void *pDev, void *regs, int minor, unsigned int tc )
911 912 {
912 913 // a tickout has been emitted, perform actions on the incoming timecode
913 914
914 915 unsigned char incomingTimecode;
915 916 unsigned char updateTime;
916 917 unsigned char internalTime;
917 918 rtems_status_code status;
918 919
919 920 incomingTimecode = (unsigned char) (grspwPtr[0] & TIMECODE_MASK);
920 921 updateTime = time_management_regs->coarse_time_load & TIMECODE_MASK;
921 922 internalTime = time_management_regs->coarse_time & TIMECODE_MASK;
922 923
923 924 housekeeping_packet.hk_lfr_dpu_spw_last_timc = incomingTimecode;
924 925
925 926 // update the number of tickout that have been generated
926 927 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt );
927 928
928 929 //**************************
929 930 // HK_LFR_TIMECODE_ERRONEOUS
930 931 // MISSING and INVALID are handled by the timecode_timer_routine service routine
931 932 if (check_timecode_and_previous_timecode_coherency( incomingTimecode ) == LFR_DEFAULT)
932 933 {
933 934 // this is unexpected but a tickout could have been raised despite of the timecode being erroneous
934 935 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_erroneous );
935 936 update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_ERRONEOUS );
936 937 }
937 938
938 939 //************************
939 940 // HK_LFR_TIME_TIMECODE_IT
940 941 // check the coherency between the SpaceWire timecode and the Internal Time
941 942 if (check_timecode_and_internal_time_coherency( incomingTimecode, internalTime ) == LFR_DEFAULT)
942 943 {
943 944 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_time_timecode_it );
944 945 update_hk_lfr_last_er_fields( RID_LE_LFR_TIME, CODE_TIMECODE_IT );
945 946 }
946 947
947 948 //********************
948 949 // HK_LFR_TIMECODE_CTR
949 950 // check the value of the timecode with respect to the last TC_LFR_UPDATE_TIME => SSS-CP-FS-370
950 951 if (oneTcLfrUpdateTimeReceived == 1)
951 952 {
952 953 if ( incomingTimecode != updateTime )
953 954 {
954 955 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_time_timecode_ctr );
955 956 update_hk_lfr_last_er_fields( RID_LE_LFR_TIME, CODE_TIMECODE_CTR );
956 957 }
957 958 }
958 959
959 960 // launch the timecode timer to detect missing or invalid timecodes
960 961 previousTimecodeCtr = incomingTimecode; // update the previousTimecodeCtr value
961 962 status = rtems_timer_fire_after( timecode_timer_id, TIMECODE_TIMER_TIMEOUT, timecode_timer_routine, NULL );
962 963 if (status != RTEMS_SUCCESSFUL)
963 964 {
964 965 rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_14 );
965 966 }
966 967 }
967 968
968 969 rtems_timer_service_routine timecode_timer_routine( rtems_id timer_id, void *user_data )
969 970 {
970 971 static unsigned char initStep = 1;
971 972
972 973 unsigned char currentTimecodeCtr;
973 974
974 975 currentTimecodeCtr = (unsigned char) (grspwPtr[0] & TIMECODE_MASK);
975 976
976 977 if (initStep == 1)
977 978 {
978 979 if (currentTimecodeCtr == previousTimecodeCtr)
979 980 {
980 981 //************************
981 982 // HK_LFR_TIMECODE_MISSING
982 983 // the timecode value has not changed, no valid timecode has been received, the timecode is MISSING
983 984 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_missing );
984 985 update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_MISSING );
985 986 }
986 987 else if (currentTimecodeCtr == (previousTimecodeCtr+1))
987 988 {
988 989 // the timecode value has changed and the value is valid, this is unexpected because
989 990 // the timer should not have fired, the timecode_irq_handler should have been raised
990 991 }
991 992 else
992 993 {
993 994 //************************
994 995 // HK_LFR_TIMECODE_INVALID
995 996 // the timecode value has changed and the value is not valid, no tickout has been generated
996 997 // this is why the timer has fired
997 998 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_invalid );
998 999 update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_INVALID );
999 1000 }
1000 1001 }
1001 1002 else
1002 1003 {
1003 1004 initStep = 1;
1004 1005 //************************
1005 1006 // HK_LFR_TIMECODE_MISSING
1006 1007 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_missing );
1007 1008 update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_MISSING );
1008 1009 }
1009 1010
1010 1011 rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_13 );
1011 1012 }
1012 1013
1013 1014 void init_header_cwf( Header_TM_LFR_SCIENCE_CWF_t *header )
1014 1015 {
1015 1016 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
1016 1017 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1017 1018 header->reserved = DEFAULT_RESERVED;
1018 1019 header->userApplication = CCSDS_USER_APP;
1019 1020 header->packetSequenceControl[0]= TM_PACKET_SEQ_CTRL_STANDALONE;
1020 1021 header->packetSequenceControl[1]= TM_PACKET_SEQ_CNT_DEFAULT;
1021 1022 header->packetLength[0] = INIT_CHAR;
1022 1023 header->packetLength[1] = INIT_CHAR;
1023 1024 // DATA FIELD HEADER
1024 1025 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
1025 1026 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
1026 1027 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; // service subtype
1027 1028 header->destinationID = TM_DESTINATION_ID_GROUND;
1028 1029 header->time[BYTE_0] = INIT_CHAR;
1029 1030 header->time[BYTE_1] = INIT_CHAR;
1030 1031 header->time[BYTE_2] = INIT_CHAR;
1031 1032 header->time[BYTE_3] = INIT_CHAR;
1032 1033 header->time[BYTE_4] = INIT_CHAR;
1033 1034 header->time[BYTE_5] = INIT_CHAR;
1034 1035 // AUXILIARY DATA HEADER
1035 1036 header->sid = INIT_CHAR;
1036 1037 header->pa_bia_status_info = DEFAULT_HKBIA;
1037 1038 header->blkNr[0] = INIT_CHAR;
1038 1039 header->blkNr[1] = INIT_CHAR;
1039 1040 }
1040 1041
1041 1042 void init_header_swf( Header_TM_LFR_SCIENCE_SWF_t *header )
1042 1043 {
1043 1044 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
1044 1045 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1045 1046 header->reserved = DEFAULT_RESERVED;
1046 1047 header->userApplication = CCSDS_USER_APP;
1047 1048 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> SHIFT_1_BYTE);
1048 1049 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1049 1050 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1050 1051 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1051 1052 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> SHIFT_1_BYTE);
1052 1053 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
1053 1054 // DATA FIELD HEADER
1054 1055 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
1055 1056 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
1056 1057 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; // service subtype
1057 1058 header->destinationID = TM_DESTINATION_ID_GROUND;
1058 1059 header->time[BYTE_0] = INIT_CHAR;
1059 1060 header->time[BYTE_1] = INIT_CHAR;
1060 1061 header->time[BYTE_2] = INIT_CHAR;
1061 1062 header->time[BYTE_3] = INIT_CHAR;
1062 1063 header->time[BYTE_4] = INIT_CHAR;
1063 1064 header->time[BYTE_5] = INIT_CHAR;
1064 1065 // AUXILIARY DATA HEADER
1065 1066 header->sid = INIT_CHAR;
1066 1067 header->pa_bia_status_info = DEFAULT_HKBIA;
1067 1068 header->pktCnt = PKTCNT_SWF; // PKT_CNT
1068 1069 header->pktNr = INIT_CHAR;
1069 1070 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> SHIFT_1_BYTE);
1070 1071 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
1071 1072 }
1072 1073
1073 1074 void init_header_asm( Header_TM_LFR_SCIENCE_ASM_t *header )
1074 1075 {
1075 1076 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
1076 1077 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1077 1078 header->reserved = DEFAULT_RESERVED;
1078 1079 header->userApplication = CCSDS_USER_APP;
1079 1080 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> SHIFT_1_BYTE);
1080 1081 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1081 1082 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1082 1083 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1083 1084 header->packetLength[0] = INIT_CHAR;
1084 1085 header->packetLength[1] = INIT_CHAR;
1085 1086 // DATA FIELD HEADER
1086 1087 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
1087 1088 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
1088 1089 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
1089 1090 header->destinationID = TM_DESTINATION_ID_GROUND;
1090 1091 header->time[BYTE_0] = INIT_CHAR;
1091 1092 header->time[BYTE_1] = INIT_CHAR;
1092 1093 header->time[BYTE_2] = INIT_CHAR;
1093 1094 header->time[BYTE_3] = INIT_CHAR;
1094 1095 header->time[BYTE_4] = INIT_CHAR;
1095 1096 header->time[BYTE_5] = INIT_CHAR;
1096 1097 // AUXILIARY DATA HEADER
1097 1098 header->sid = INIT_CHAR;
1098 1099 header->pa_bia_status_info = INIT_CHAR;
1099 1100 header->pa_lfr_pkt_cnt_asm = INIT_CHAR;
1100 1101 header->pa_lfr_pkt_nr_asm = INIT_CHAR;
1101 1102 header->pa_lfr_asm_blk_nr[0] = INIT_CHAR;
1102 1103 header->pa_lfr_asm_blk_nr[1] = INIT_CHAR;
1103 1104 }
1104 1105
1105 1106 int spw_send_waveform_CWF( ring_node *ring_node_to_send,
1106 1107 Header_TM_LFR_SCIENCE_CWF_t *header )
1107 1108 {
1108 1109 /** This function sends CWF CCSDS packets (F2, F1 or F0).
1109 1110 *
1110 1111 * @param waveform points to the buffer containing the data that will be send.
1111 1112 * @param sid is the source identifier of the data that will be sent.
1112 1113 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
1113 1114 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
1114 1115 * contain information to setup the transmission of the data packets.
1115 1116 *
1116 1117 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
1117 1118 *
1118 1119 */
1119 1120
1120 1121 unsigned int i;
1121 1122 int ret;
1122 1123 unsigned int coarseTime;
1123 1124 unsigned int fineTime;
1124 1125 rtems_status_code status;
1125 1126 spw_ioctl_pkt_send spw_ioctl_send_CWF;
1126 1127 int *dataPtr;
1127 1128 unsigned char sid;
1128 1129
1129 1130 spw_ioctl_send_CWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_CWF;
1130 1131 spw_ioctl_send_CWF.options = 0;
1131 1132
1132 1133 ret = LFR_DEFAULT;
1133 1134 sid = (unsigned char) ring_node_to_send->sid;
1134 1135
1135 1136 coarseTime = ring_node_to_send->coarseTime;
1136 1137 fineTime = ring_node_to_send->fineTime;
1137 1138 dataPtr = (int*) ring_node_to_send->buffer_address;
1138 1139
1139 1140 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> SHIFT_1_BYTE);
1140 1141 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
1141 1142 header->pa_bia_status_info = pa_bia_status_info;
1142 1143 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1143 1144 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> SHIFT_1_BYTE);
1144 1145 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
1145 1146
1146 1147 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF; i++) // send waveform
1147 1148 {
1148 1149 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF * NB_WORDS_SWF_BLK) ];
1149 1150 spw_ioctl_send_CWF.hdr = (char*) header;
1150 1151 // BUILD THE DATA
1151 1152 spw_ioctl_send_CWF.dlen = BLK_NR_CWF * NB_BYTES_SWF_BLK;
1152 1153
1153 1154 // SET PACKET SEQUENCE CONTROL
1154 1155 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1155 1156
1156 1157 // SET SID
1157 1158 header->sid = sid;
1158 1159
1159 1160 // SET PACKET TIME
1160 1161 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime);
1161 1162 //
1162 1163 header->time[0] = header->acquisitionTime[0];
1163 1164 header->time[1] = header->acquisitionTime[1];
1164 1165 header->time[BYTE_2] = header->acquisitionTime[BYTE_2];
1165 1166 header->time[BYTE_3] = header->acquisitionTime[BYTE_3];
1166 1167 header->time[BYTE_4] = header->acquisitionTime[BYTE_4];
1167 1168 header->time[BYTE_5] = header->acquisitionTime[BYTE_5];
1168 1169
1169 1170 // SET PACKET ID
1170 1171 if ( (sid == SID_SBM1_CWF_F1) || (sid == SID_SBM2_CWF_F2) )
1171 1172 {
1172 1173 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2 >> SHIFT_1_BYTE);
1173 1174 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2);
1174 1175 }
1175 1176 else
1176 1177 {
1177 1178 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> SHIFT_1_BYTE);
1178 1179 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1179 1180 }
1180 1181
1181 1182 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
1182 1183 if (status != RTEMS_SUCCESSFUL) {
1183 1184 ret = LFR_DEFAULT;
1184 1185 }
1185 1186 }
1186 1187
1187 1188 return ret;
1188 1189 }
1189 1190
1190 1191 int spw_send_waveform_SWF( ring_node *ring_node_to_send,
1191 1192 Header_TM_LFR_SCIENCE_SWF_t *header )
1192 1193 {
1193 1194 /** This function sends SWF CCSDS packets (F2, F1 or F0).
1194 1195 *
1195 1196 * @param waveform points to the buffer containing the data that will be send.
1196 1197 * @param sid is the source identifier of the data that will be sent.
1197 1198 * @param headerSWF points to a table of headers that have been prepared for the data transmission.
1198 1199 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
1199 1200 * contain information to setup the transmission of the data packets.
1200 1201 *
1201 1202 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
1202 1203 *
1203 1204 */
1204 1205
1205 1206 unsigned int i;
1206 1207 int ret;
1207 1208 unsigned int coarseTime;
1208 1209 unsigned int fineTime;
1209 1210 rtems_status_code status;
1210 1211 spw_ioctl_pkt_send spw_ioctl_send_SWF;
1211 1212 int *dataPtr;
1212 1213 unsigned char sid;
1213 1214
1214 1215 spw_ioctl_send_SWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_SWF;
1215 1216 spw_ioctl_send_SWF.options = 0;
1216 1217
1217 1218 ret = LFR_DEFAULT;
1218 1219
1219 1220 coarseTime = ring_node_to_send->coarseTime;
1220 1221 fineTime = ring_node_to_send->fineTime;
1221 1222 dataPtr = (int*) ring_node_to_send->buffer_address;
1222 1223 sid = ring_node_to_send->sid;
1223 1224
1224 1225 header->pa_bia_status_info = pa_bia_status_info;
1225 1226 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1226 1227
1227 1228 for (i=0; i<PKTCNT_SWF; i++) // send waveform
1228 1229 {
1229 1230 spw_ioctl_send_SWF.data = (char*) &dataPtr[ (i * BLK_NR_304 * NB_WORDS_SWF_BLK) ];
1230 1231 spw_ioctl_send_SWF.hdr = (char*) header;
1231 1232
1232 1233 // SET PACKET SEQUENCE CONTROL
1233 1234 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1234 1235
1235 1236 // SET PACKET LENGTH AND BLKNR
1236 1237 if (i == (PKTCNT_SWF-1))
1237 1238 {
1238 1239 spw_ioctl_send_SWF.dlen = BLK_NR_224 * NB_BYTES_SWF_BLK;
1239 1240 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_224 >> SHIFT_1_BYTE);
1240 1241 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_224 );
1241 1242 header->blkNr[0] = (unsigned char) (BLK_NR_224 >> SHIFT_1_BYTE);
1242 1243 header->blkNr[1] = (unsigned char) (BLK_NR_224 );
1243 1244 }
1244 1245 else
1245 1246 {
1246 1247 spw_ioctl_send_SWF.dlen = BLK_NR_304 * NB_BYTES_SWF_BLK;
1247 1248 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_304 >> SHIFT_1_BYTE);
1248 1249 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_304 );
1249 1250 header->blkNr[0] = (unsigned char) (BLK_NR_304 >> SHIFT_1_BYTE);
1250 1251 header->blkNr[1] = (unsigned char) (BLK_NR_304 );
1251 1252 }
1252 1253
1253 1254 // SET PACKET TIME
1254 1255 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime );
1255 1256 //
1256 1257 header->time[BYTE_0] = header->acquisitionTime[BYTE_0];
1257 1258 header->time[BYTE_1] = header->acquisitionTime[BYTE_1];
1258 1259 header->time[BYTE_2] = header->acquisitionTime[BYTE_2];
1259 1260 header->time[BYTE_3] = header->acquisitionTime[BYTE_3];
1260 1261 header->time[BYTE_4] = header->acquisitionTime[BYTE_4];
1261 1262 header->time[BYTE_5] = header->acquisitionTime[BYTE_5];
1262 1263
1263 1264 // SET SID
1264 1265 header->sid = sid;
1265 1266
1266 1267 // SET PKTNR
1267 1268 header->pktNr = i+1; // PKT_NR
1268 1269
1269 1270 // SEND PACKET
1270 1271 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_SWF );
1271 1272 if (status != RTEMS_SUCCESSFUL) {
1272 1273 ret = LFR_DEFAULT;
1273 1274 }
1274 1275 }
1275 1276
1276 1277 return ret;
1277 1278 }
1278 1279
1279 1280 int spw_send_waveform_CWF3_light( ring_node *ring_node_to_send,
1280 1281 Header_TM_LFR_SCIENCE_CWF_t *header )
1281 1282 {
1282 1283 /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data.
1283 1284 *
1284 1285 * @param waveform points to the buffer containing the data that will be send.
1285 1286 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
1286 1287 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
1287 1288 * contain information to setup the transmission of the data packets.
1288 1289 *
1289 1290 * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer
1290 1291 * from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks.
1291 1292 *
1292 1293 */
1293 1294
1294 1295 unsigned int i;
1295 1296 int ret;
1296 1297 unsigned int coarseTime;
1297 1298 unsigned int fineTime;
1298 1299 rtems_status_code status;
1299 1300 spw_ioctl_pkt_send spw_ioctl_send_CWF;
1300 1301 char *dataPtr;
1301 1302 unsigned char sid;
1302 1303
1303 1304 spw_ioctl_send_CWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_CWF;
1304 1305 spw_ioctl_send_CWF.options = 0;
1305 1306
1306 1307 ret = LFR_DEFAULT;
1307 1308 sid = ring_node_to_send->sid;
1308 1309
1309 1310 coarseTime = ring_node_to_send->coarseTime;
1310 1311 fineTime = ring_node_to_send->fineTime;
1311 1312 dataPtr = (char*) ring_node_to_send->buffer_address;
1312 1313
1313 1314 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_672 >> SHIFT_1_BYTE);
1314 1315 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_672 );
1315 1316 header->pa_bia_status_info = pa_bia_status_info;
1316 1317 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1317 1318 header->blkNr[0] = (unsigned char) (BLK_NR_CWF_SHORT_F3 >> SHIFT_1_BYTE);
1318 1319 header->blkNr[1] = (unsigned char) (BLK_NR_CWF_SHORT_F3 );
1319 1320
1320 1321 //*********************
1321 1322 // SEND CWF3_light DATA
1322 1323 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF_LIGHT; i++) // send waveform
1323 1324 {
1324 1325 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK) ];
1325 1326 spw_ioctl_send_CWF.hdr = (char*) header;
1326 1327 // BUILD THE DATA
1327 1328 spw_ioctl_send_CWF.dlen = BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK;
1328 1329
1329 1330 // SET PACKET SEQUENCE COUNTER
1330 1331 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1331 1332
1332 1333 // SET SID
1333 1334 header->sid = sid;
1334 1335
1335 1336 // SET PACKET TIME
1336 1337 compute_acquisition_time( coarseTime, fineTime, SID_NORM_CWF_F3, i, header->acquisitionTime );
1337 1338 //
1338 1339 header->time[BYTE_0] = header->acquisitionTime[BYTE_0];
1339 1340 header->time[BYTE_1] = header->acquisitionTime[BYTE_1];
1340 1341 header->time[BYTE_2] = header->acquisitionTime[BYTE_2];
1341 1342 header->time[BYTE_3] = header->acquisitionTime[BYTE_3];
1342 1343 header->time[BYTE_4] = header->acquisitionTime[BYTE_4];
1343 1344 header->time[BYTE_5] = header->acquisitionTime[BYTE_5];
1344 1345
1345 1346 // SET PACKET ID
1346 1347 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> SHIFT_1_BYTE);
1347 1348 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1348 1349
1349 1350 // SEND PACKET
1350 1351 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
1351 1352 if (status != RTEMS_SUCCESSFUL) {
1352 1353 ret = LFR_DEFAULT;
1353 1354 }
1354 1355 }
1355 1356
1356 1357 return ret;
1357 1358 }
1358 1359
1359 1360 void spw_send_asm_f0( ring_node *ring_node_to_send,
1360 1361 Header_TM_LFR_SCIENCE_ASM_t *header )
1361 1362 {
1362 1363 unsigned int i;
1363 1364 unsigned int length = 0;
1364 1365 rtems_status_code status;
1365 1366 unsigned int sid;
1366 1367 float *spectral_matrix;
1367 1368 int coarseTime;
1368 1369 int fineTime;
1369 1370 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1370 1371
1371 1372 sid = ring_node_to_send->sid;
1372 1373 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1373 1374 coarseTime = ring_node_to_send->coarseTime;
1374 1375 fineTime = ring_node_to_send->fineTime;
1375 1376
1376 1377 header->pa_bia_status_info = pa_bia_status_info;
1377 1378 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1378 1379
1379 1380 for (i=0; i<PKTCNT_ASM; i++)
1380 1381 {
1381 1382 if ((i==0) || (i==1))
1382 1383 {
1383 1384 spw_ioctl_send_ASM.dlen = DLEN_ASM_F0_PKT_1;
1384 1385 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1385 1386 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0_1) ) * NB_VALUES_PER_SM )
1386 1387 ];
1387 1388 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0_1;
1388 1389 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1389 1390 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_1) >> SHIFT_1_BYTE ); // BLK_NR MSB
1390 1391 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0_1); // BLK_NR LSB
1391 1392 }
1392 1393 else
1393 1394 {
1394 1395 spw_ioctl_send_ASM.dlen = DLEN_ASM_F0_PKT_2;
1395 1396 spw_ioctl_send_ASM.data = (char*) &spectral_matrix[
1396 1397 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0_1) ) * NB_VALUES_PER_SM )
1397 1398 ];
1398 1399 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0_2;
1399 1400 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1400 1401 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_2) >> SHIFT_1_BYTE ); // BLK_NR MSB
1401 1402 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0_2); // BLK_NR LSB
1402 1403 }
1403 1404
1404 1405 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1405 1406 spw_ioctl_send_ASM.hdr = (char *) header;
1406 1407 spw_ioctl_send_ASM.options = 0;
1407 1408
1408 1409 // (2) BUILD THE HEADER
1409 1410 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1410 1411 header->packetLength[0] = (unsigned char) (length >> SHIFT_1_BYTE);
1411 1412 header->packetLength[1] = (unsigned char) (length);
1412 1413 header->sid = (unsigned char) sid; // SID
1413 1414 header->pa_lfr_pkt_cnt_asm = PKTCNT_ASM;
1414 1415 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1415 1416
1416 1417 // (3) SET PACKET TIME
1417 1418 header->time[BYTE_0] = (unsigned char) (coarseTime >> SHIFT_3_BYTES);
1418 1419 header->time[BYTE_1] = (unsigned char) (coarseTime >> SHIFT_2_BYTES);
1419 1420 header->time[BYTE_2] = (unsigned char) (coarseTime >> SHIFT_1_BYTE);
1420 1421 header->time[BYTE_3] = (unsigned char) (coarseTime);
1421 1422 header->time[BYTE_4] = (unsigned char) (fineTime >> SHIFT_1_BYTE);
1422 1423 header->time[BYTE_5] = (unsigned char) (fineTime);
1423 1424 //
1424 1425 header->acquisitionTime[BYTE_0] = header->time[BYTE_0];
1425 1426 header->acquisitionTime[BYTE_1] = header->time[BYTE_1];
1426 1427 header->acquisitionTime[BYTE_2] = header->time[BYTE_2];
1427 1428 header->acquisitionTime[BYTE_3] = header->time[BYTE_3];
1428 1429 header->acquisitionTime[BYTE_4] = header->time[BYTE_4];
1429 1430 header->acquisitionTime[BYTE_5] = header->time[BYTE_5];
1430 1431
1431 1432 // (4) SEND PACKET
1432 1433 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1433 1434 if (status != RTEMS_SUCCESSFUL) {
1434 1435 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1435 1436 }
1436 1437 }
1437 1438 }
1438 1439
1439 1440 void spw_send_asm_f1( ring_node *ring_node_to_send,
1440 1441 Header_TM_LFR_SCIENCE_ASM_t *header )
1441 1442 {
1442 1443 unsigned int i;
1443 1444 unsigned int length = 0;
1444 1445 rtems_status_code status;
1445 1446 unsigned int sid;
1446 1447 float *spectral_matrix;
1447 1448 int coarseTime;
1448 1449 int fineTime;
1449 1450 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1450 1451
1451 1452 sid = ring_node_to_send->sid;
1452 1453 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1453 1454 coarseTime = ring_node_to_send->coarseTime;
1454 1455 fineTime = ring_node_to_send->fineTime;
1455 1456
1456 1457 header->pa_bia_status_info = pa_bia_status_info;
1457 1458 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1458 1459
1459 1460 for (i=0; i<PKTCNT_ASM; i++)
1460 1461 {
1461 1462 if ((i==0) || (i==1))
1462 1463 {
1463 1464 spw_ioctl_send_ASM.dlen = DLEN_ASM_F1_PKT_1;
1464 1465 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1465 1466 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1_1) ) * NB_VALUES_PER_SM )
1466 1467 ];
1467 1468 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1_1;
1468 1469 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1469 1470 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_1) >> SHIFT_1_BYTE ); // BLK_NR MSB
1470 1471 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1_1); // BLK_NR LSB
1471 1472 }
1472 1473 else
1473 1474 {
1474 1475 spw_ioctl_send_ASM.dlen = DLEN_ASM_F1_PKT_2;
1475 1476 spw_ioctl_send_ASM.data = (char*) &spectral_matrix[
1476 1477 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1_1) ) * NB_VALUES_PER_SM )
1477 1478 ];
1478 1479 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1_2;
1479 1480 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1480 1481 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_2) >> SHIFT_1_BYTE ); // BLK_NR MSB
1481 1482 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1_2); // BLK_NR LSB
1482 1483 }
1483 1484
1484 1485 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1485 1486 spw_ioctl_send_ASM.hdr = (char *) header;
1486 1487 spw_ioctl_send_ASM.options = 0;
1487 1488
1488 1489 // (2) BUILD THE HEADER
1489 1490 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1490 1491 header->packetLength[0] = (unsigned char) (length >> SHIFT_1_BYTE);
1491 1492 header->packetLength[1] = (unsigned char) (length);
1492 1493 header->sid = (unsigned char) sid; // SID
1493 1494 header->pa_lfr_pkt_cnt_asm = PKTCNT_ASM;
1494 1495 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1495 1496
1496 1497 // (3) SET PACKET TIME
1497 1498 header->time[BYTE_0] = (unsigned char) (coarseTime >> SHIFT_3_BYTES);
1498 1499 header->time[BYTE_1] = (unsigned char) (coarseTime >> SHIFT_2_BYTES);
1499 1500 header->time[BYTE_2] = (unsigned char) (coarseTime >> SHIFT_1_BYTE);
1500 1501 header->time[BYTE_3] = (unsigned char) (coarseTime);
1501 1502 header->time[BYTE_4] = (unsigned char) (fineTime >> SHIFT_1_BYTE);
1502 1503 header->time[BYTE_5] = (unsigned char) (fineTime);
1503 1504 //
1504 1505 header->acquisitionTime[BYTE_0] = header->time[BYTE_0];
1505 1506 header->acquisitionTime[BYTE_1] = header->time[BYTE_1];
1506 1507 header->acquisitionTime[BYTE_2] = header->time[BYTE_2];
1507 1508 header->acquisitionTime[BYTE_3] = header->time[BYTE_3];
1508 1509 header->acquisitionTime[BYTE_4] = header->time[BYTE_4];
1509 1510 header->acquisitionTime[BYTE_5] = header->time[BYTE_5];
1510 1511
1511 1512 // (4) SEND PACKET
1512 1513 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1513 1514 if (status != RTEMS_SUCCESSFUL) {
1514 1515 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1515 1516 }
1516 1517 }
1517 1518 }
1518 1519
1519 1520 void spw_send_asm_f2( ring_node *ring_node_to_send,
1520 1521 Header_TM_LFR_SCIENCE_ASM_t *header )
1521 1522 {
1522 1523 unsigned int i;
1523 1524 unsigned int length = 0;
1524 1525 rtems_status_code status;
1525 1526 unsigned int sid;
1526 1527 float *spectral_matrix;
1527 1528 int coarseTime;
1528 1529 int fineTime;
1529 1530 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1530 1531
1531 1532 sid = ring_node_to_send->sid;
1532 1533 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1533 1534 coarseTime = ring_node_to_send->coarseTime;
1534 1535 fineTime = ring_node_to_send->fineTime;
1535 1536
1536 1537 header->pa_bia_status_info = pa_bia_status_info;
1537 1538 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1538 1539
1539 1540 for (i=0; i<PKTCNT_ASM; i++)
1540 1541 {
1541 1542
1542 1543 spw_ioctl_send_ASM.dlen = DLEN_ASM_F2_PKT;
1543 1544 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1544 1545 ( (ASM_F2_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F2) ) * NB_VALUES_PER_SM )
1545 1546 ];
1546 1547 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F2;
1547 1548 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3;
1548 1549 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F2) >> SHIFT_1_BYTE ); // BLK_NR MSB
1549 1550 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F2); // BLK_NR LSB
1550 1551
1551 1552 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1552 1553 spw_ioctl_send_ASM.hdr = (char *) header;
1553 1554 spw_ioctl_send_ASM.options = 0;
1554 1555
1555 1556 // (2) BUILD THE HEADER
1556 1557 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1557 1558 header->packetLength[0] = (unsigned char) (length >> SHIFT_1_BYTE);
1558 1559 header->packetLength[1] = (unsigned char) (length);
1559 1560 header->sid = (unsigned char) sid; // SID
1560 1561 header->pa_lfr_pkt_cnt_asm = PKTCNT_ASM;
1561 1562 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1562 1563
1563 1564 // (3) SET PACKET TIME
1564 1565 header->time[BYTE_0] = (unsigned char) (coarseTime >> SHIFT_3_BYTES);
1565 1566 header->time[BYTE_1] = (unsigned char) (coarseTime >> SHIFT_2_BYTES);
1566 1567 header->time[BYTE_2] = (unsigned char) (coarseTime >> SHIFT_1_BYTE);
1567 1568 header->time[BYTE_3] = (unsigned char) (coarseTime);
1568 1569 header->time[BYTE_4] = (unsigned char) (fineTime >> SHIFT_1_BYTE);
1569 1570 header->time[BYTE_5] = (unsigned char) (fineTime);
1570 1571 //
1571 1572 header->acquisitionTime[BYTE_0] = header->time[BYTE_0];
1572 1573 header->acquisitionTime[BYTE_1] = header->time[BYTE_1];
1573 1574 header->acquisitionTime[BYTE_2] = header->time[BYTE_2];
1574 1575 header->acquisitionTime[BYTE_3] = header->time[BYTE_3];
1575 1576 header->acquisitionTime[BYTE_4] = header->time[BYTE_4];
1576 1577 header->acquisitionTime[BYTE_5] = header->time[BYTE_5];
1577 1578
1578 1579 // (4) SEND PACKET
1579 1580 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1580 1581 if (status != RTEMS_SUCCESSFUL) {
1581 1582 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1582 1583 }
1583 1584 }
1584 1585 }
1585 1586
1586 1587 void spw_send_k_dump( ring_node *ring_node_to_send )
1587 1588 {
1588 1589 rtems_status_code status;
1589 1590 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump;
1590 1591 unsigned int packetLength;
1591 1592 unsigned int size;
1592 1593
1593 1594 PRINTF("spw_send_k_dump\n")
1594 1595
1595 1596 kcoefficients_dump = (Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *) ring_node_to_send->buffer_address;
1596 1597
1597 1598 packetLength = (kcoefficients_dump->packetLength[0] * CONST_256) + kcoefficients_dump->packetLength[1];
1598 1599
1599 1600 size = packetLength + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
1600 1601
1601 1602 PRINTF2("packetLength %d, size %d\n", packetLength, size )
1602 1603
1603 1604 status = write( fdSPW, (char *) ring_node_to_send->buffer_address, size );
1604 1605
1605 1606 if (status == -1){
1606 1607 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
1607 1608 }
1608 1609
1609 1610 ring_node_to_send->status = INIT_CHAR;
1610 1611 }
Show file before | Show file after | Hide comments
@@ -1,794 +1,800
1 1 /** Functions related to data processing.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 7 *
8 8 */
9 9
10 10 #include "fsw_processing.h"
11 11 #include "fsw_processing_globals.c"
12 12 #include "fsw_init.h"
13 13
14 14 unsigned int nb_sm_f0;
15 15 unsigned int nb_sm_f0_aux_f1;
16 16 unsigned int nb_sm_f1;
17 17 unsigned int nb_sm_f0_aux_f2;
18 18
19 19 typedef enum restartState_t
20 20 {
21 21 WAIT_FOR_F2,
22 22 WAIT_FOR_F1,
23 23 WAIT_FOR_F0
24 24 } restartState;
25 25
26 26 //************************
27 27 // spectral matrices rings
28 28 ring_node sm_ring_f0[ NB_RING_NODES_SM_F0 ];
29 29 ring_node sm_ring_f1[ NB_RING_NODES_SM_F1 ];
30 30 ring_node sm_ring_f2[ NB_RING_NODES_SM_F2 ];
31 31 ring_node *current_ring_node_sm_f0;
32 32 ring_node *current_ring_node_sm_f1;
33 33 ring_node *current_ring_node_sm_f2;
34 34 ring_node *ring_node_for_averaging_sm_f0;
35 35 ring_node *ring_node_for_averaging_sm_f1;
36 36 ring_node *ring_node_for_averaging_sm_f2;
37 37
38 38 //
39 39 ring_node * getRingNodeForAveraging( unsigned char frequencyChannel)
40 40 {
41 41 ring_node *node;
42 42
43 43 node = NULL;
44 44 switch ( frequencyChannel ) {
45 45 case CHANNELF0:
46 46 node = ring_node_for_averaging_sm_f0;
47 47 break;
48 48 case CHANNELF1:
49 49 node = ring_node_for_averaging_sm_f1;
50 50 break;
51 51 case CHANNELF2:
52 52 node = ring_node_for_averaging_sm_f2;
53 53 break;
54 54 default:
55 55 break;
56 56 }
57 57
58 58 return node;
59 59 }
60 60
61 61 //***********************************************************
62 62 // Interrupt Service Routine for spectral matrices processing
63 63
64 64 void spectral_matrices_isr_f0( int statusReg )
65 65 {
66 66 unsigned char status;
67 67 rtems_status_code status_code;
68 68 ring_node *full_ring_node;
69 69
70 70 status = (unsigned char) (statusReg & BITS_STATUS_F0); // [0011] get the status_ready_matrix_f0_x bits
71 71
72 72 switch(status)
73 73 {
74 74 case 0:
75 75 break;
76 76 case BIT_READY_0_1:
77 77 // UNEXPECTED VALUE
78 78 spectral_matrix_regs->status = BIT_READY_0_1; // [0011]
79 79 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
80 80 break;
81 81 case BIT_READY_0:
82 82 full_ring_node = current_ring_node_sm_f0->previous;
83 83 full_ring_node->coarseTime = spectral_matrix_regs->f0_0_coarse_time;
84 84 full_ring_node->fineTime = spectral_matrix_regs->f0_0_fine_time;
85 85 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
86 86 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->buffer_address;
87 87 // if there are enough ring nodes ready, wake up an AVFx task
88 88 nb_sm_f0 = nb_sm_f0 + 1;
89 89 if (nb_sm_f0 == NB_SM_BEFORE_AVF0_F1)
90 90 {
91 91 ring_node_for_averaging_sm_f0 = full_ring_node;
92 92 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
93 93 {
94 94 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
95 95 }
96 96 nb_sm_f0 = 0;
97 97 }
98 98 spectral_matrix_regs->status = BIT_READY_0; // [0000 0001]
99 99 break;
100 100 case BIT_READY_1:
101 101 full_ring_node = current_ring_node_sm_f0->previous;
102 102 full_ring_node->coarseTime = spectral_matrix_regs->f0_1_coarse_time;
103 103 full_ring_node->fineTime = spectral_matrix_regs->f0_1_fine_time;
104 104 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
105 105 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
106 106 // if there are enough ring nodes ready, wake up an AVFx task
107 107 nb_sm_f0 = nb_sm_f0 + 1;
108 108 if (nb_sm_f0 == NB_SM_BEFORE_AVF0_F1)
109 109 {
110 110 ring_node_for_averaging_sm_f0 = full_ring_node;
111 111 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
112 112 {
113 113 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
114 114 }
115 115 nb_sm_f0 = 0;
116 116 }
117 117 spectral_matrix_regs->status = BIT_READY_1; // [0000 0010]
118 118 break;
119 default:
120 break;
119 121 }
120 122 }
121 123
122 124 void spectral_matrices_isr_f1( int statusReg )
123 125 {
124 126 rtems_status_code status_code;
125 127 unsigned char status;
126 128 ring_node *full_ring_node;
127 129
128 130 status = (unsigned char) ((statusReg & BITS_STATUS_F1) >> SHIFT_2_BITS); // [1100] get the status_ready_matrix_f1_x bits
129 131
130 132 switch(status)
131 133 {
132 134 case 0:
133 135 break;
134 136 case BIT_READY_0_1:
135 137 // UNEXPECTED VALUE
136 138 spectral_matrix_regs->status = BITS_STATUS_F1; // [1100]
137 139 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
138 140 break;
139 141 case BIT_READY_0:
140 142 full_ring_node = current_ring_node_sm_f1->previous;
141 143 full_ring_node->coarseTime = spectral_matrix_regs->f1_0_coarse_time;
142 144 full_ring_node->fineTime = spectral_matrix_regs->f1_0_fine_time;
143 145 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
144 146 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->buffer_address;
145 147 // if there are enough ring nodes ready, wake up an AVFx task
146 148 nb_sm_f1 = nb_sm_f1 + 1;
147 149 if (nb_sm_f1 == NB_SM_BEFORE_AVF0_F1)
148 150 {
149 151 ring_node_for_averaging_sm_f1 = full_ring_node;
150 152 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
151 153 {
152 154 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
153 155 }
154 156 nb_sm_f1 = 0;
155 157 }
156 158 spectral_matrix_regs->status = BIT_STATUS_F1_0; // [0000 0100]
157 159 break;
158 160 case BIT_READY_1:
159 161 full_ring_node = current_ring_node_sm_f1->previous;
160 162 full_ring_node->coarseTime = spectral_matrix_regs->f1_1_coarse_time;
161 163 full_ring_node->fineTime = spectral_matrix_regs->f1_1_fine_time;
162 164 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
163 165 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
164 166 // if there are enough ring nodes ready, wake up an AVFx task
165 167 nb_sm_f1 = nb_sm_f1 + 1;
166 168 if (nb_sm_f1 == NB_SM_BEFORE_AVF0_F1)
167 169 {
168 170 ring_node_for_averaging_sm_f1 = full_ring_node;
169 171 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
170 172 {
171 173 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
172 174 }
173 175 nb_sm_f1 = 0;
174 176 }
175 177 spectral_matrix_regs->status = BIT_STATUS_F1_1; // [1000 0000]
176 178 break;
179 default:
180 break;
177 181 }
178 182 }
179 183
180 184 void spectral_matrices_isr_f2( int statusReg )
181 185 {
182 186 unsigned char status;
183 187 rtems_status_code status_code;
184 188
185 189 status = (unsigned char) ((statusReg & BITS_STATUS_F2) >> SHIFT_4_BITS); // [0011 0000] get the status_ready_matrix_f2_x bits
186 190
187 191 switch(status)
188 192 {
189 193 case 0:
190 194 break;
191 195 case BIT_READY_0_1:
192 196 // UNEXPECTED VALUE
193 197 spectral_matrix_regs->status = BITS_STATUS_F2; // [0011 0000]
194 198 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
195 199 break;
196 200 case BIT_READY_0:
197 201 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
198 202 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
199 203 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_0_coarse_time;
200 204 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_0_fine_time;
201 205 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->buffer_address;
202 206 spectral_matrix_regs->status = BIT_STATUS_F2_0; // [0001 0000]
203 207 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
204 208 {
205 209 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
206 210 }
207 211 break;
208 212 case BIT_READY_1:
209 213 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
210 214 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
211 215 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_1_coarse_time;
212 216 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_1_fine_time;
213 217 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
214 218 spectral_matrix_regs->status = BIT_STATUS_F2_1; // [0010 0000]
215 219 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
216 220 {
217 221 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
218 222 }
219 223 break;
224 default:
225 break;
220 226 }
221 227 }
222 228
223 229 void spectral_matrix_isr_error_handler( int statusReg )
224 230 {
225 231 // STATUS REGISTER
226 232 // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
227 233 // 10 9 8
228 234 // buffer_full ** [bad_component_err] ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
229 235 // 7 6 5 4 3 2 1 0
230 236 // [bad_component_err] not defined in the last version of the VHDL code
231 237
232 238 rtems_status_code status_code;
233 239
234 240 //***************************************************
235 241 // the ASM status register is copied in the HK packet
236 242 housekeeping_packet.hk_lfr_vhdl_aa_sm = (unsigned char) ((statusReg & BITS_HK_AA_SM) >> SHIFT_7_BITS); // [0111 1000 0000]
237 243
238 244 if (statusReg & BITS_SM_ERR) // [0111 1100 0000]
239 245 {
240 246 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_8 );
241 247 }
242 248
243 249 spectral_matrix_regs->status = spectral_matrix_regs->status & BITS_SM_ERR;
244 250
245 251 }
246 252
247 253 rtems_isr spectral_matrices_isr( rtems_vector_number vector )
248 254 {
249 255 // STATUS REGISTER
250 256 // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
251 257 // 10 9 8
252 258 // buffer_full ** bad_component_err ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
253 259 // 7 6 5 4 3 2 1 0
254 260
255 261 int statusReg;
256 262
257 263 static restartState state = WAIT_FOR_F2;
258 264
259 265 statusReg = spectral_matrix_regs->status;
260 266
261 267 if (thisIsAnASMRestart == 0)
262 268 { // this is not a restart sequence, process incoming matrices normally
263 269 spectral_matrices_isr_f0( statusReg );
264 270
265 271 spectral_matrices_isr_f1( statusReg );
266 272
267 273 spectral_matrices_isr_f2( statusReg );
268 274 }
269 275 else
270 276 { // a restart sequence has to be launched
271 277 switch (state) {
272 278 case WAIT_FOR_F2:
273 279 if ((statusReg & BITS_STATUS_F2) != INIT_CHAR) // [0011 0000] check the status_ready_matrix_f2_x bits
274 280 {
275 281 state = WAIT_FOR_F1;
276 282 }
277 283 break;
278 284 case WAIT_FOR_F1:
279 285 if ((statusReg & BITS_STATUS_F1) != INIT_CHAR) // [0000 1100] check the status_ready_matrix_f1_x bits
280 286 {
281 287 state = WAIT_FOR_F0;
282 288 }
283 289 break;
284 290 case WAIT_FOR_F0:
285 291 if ((statusReg & BITS_STATUS_F0) != INIT_CHAR) // [0000 0011] check the status_ready_matrix_f0_x bits
286 292 {
287 293 state = WAIT_FOR_F2;
288 294 thisIsAnASMRestart = 0;
289 295 }
290 296 break;
291 297 default:
292 298 break;
293 299 }
294 300 reset_sm_status();
295 301 }
296 302
297 303 spectral_matrix_isr_error_handler( statusReg );
298 304
299 305 }
300 306
301 307 //******************
302 308 // Spectral Matrices
303 309
304 310 void reset_nb_sm( void )
305 311 {
306 312 nb_sm_f0 = 0;
307 313 nb_sm_f0_aux_f1 = 0;
308 314 nb_sm_f0_aux_f2 = 0;
309 315
310 316 nb_sm_f1 = 0;
311 317 }
312 318
313 319 void SM_init_rings( void )
314 320 {
315 321 init_ring( sm_ring_f0, NB_RING_NODES_SM_F0, sm_f0, TOTAL_SIZE_SM );
316 322 init_ring( sm_ring_f1, NB_RING_NODES_SM_F1, sm_f1, TOTAL_SIZE_SM );
317 323 init_ring( sm_ring_f2, NB_RING_NODES_SM_F2, sm_f2, TOTAL_SIZE_SM );
318 324
319 325 DEBUG_PRINTF1("sm_ring_f0 @%x\n", (unsigned int) sm_ring_f0)
320 326 DEBUG_PRINTF1("sm_ring_f1 @%x\n", (unsigned int) sm_ring_f1)
321 327 DEBUG_PRINTF1("sm_ring_f2 @%x\n", (unsigned int) sm_ring_f2)
322 328 DEBUG_PRINTF1("sm_f0 @%x\n", (unsigned int) sm_f0)
323 329 DEBUG_PRINTF1("sm_f1 @%x\n", (unsigned int) sm_f1)
324 330 DEBUG_PRINTF1("sm_f2 @%x\n", (unsigned int) sm_f2)
325 331 }
326 332
327 333 void ASM_generic_init_ring( ring_node_asm *ring, unsigned char nbNodes )
328 334 {
329 335 unsigned char i;
330 336
331 337 ring[ nbNodes - 1 ].next
332 338 = (ring_node_asm*) &ring[ 0 ];
333 339
334 340 for(i=0; i<nbNodes-1; i++)
335 341 {
336 342 ring[ i ].next = (ring_node_asm*) &ring[ i + 1 ];
337 343 }
338 344 }
339 345
340 346 void SM_reset_current_ring_nodes( void )
341 347 {
342 348 current_ring_node_sm_f0 = sm_ring_f0[0].next;
343 349 current_ring_node_sm_f1 = sm_ring_f1[0].next;
344 350 current_ring_node_sm_f2 = sm_ring_f2[0].next;
345 351
346 352 ring_node_for_averaging_sm_f0 = NULL;
347 353 ring_node_for_averaging_sm_f1 = NULL;
348 354 ring_node_for_averaging_sm_f2 = NULL;
349 355 }
350 356
351 357 //*****************
352 358 // Basic Parameters
353 359
354 360 void BP_init_header( bp_packet *packet,
355 361 unsigned int apid, unsigned char sid,
356 362 unsigned int packetLength, unsigned char blkNr )
357 363 {
358 364 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
359 365 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
360 366 packet->reserved = INIT_CHAR;
361 367 packet->userApplication = CCSDS_USER_APP;
362 368 packet->packetID[0] = (unsigned char) (apid >> SHIFT_1_BYTE);
363 369 packet->packetID[1] = (unsigned char) (apid);
364 370 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
365 371 packet->packetSequenceControl[1] = INIT_CHAR;
366 372 packet->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE);
367 373 packet->packetLength[1] = (unsigned char) (packetLength);
368 374 // DATA FIELD HEADER
369 375 packet->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
370 376 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
371 377 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
372 378 packet->destinationID = TM_DESTINATION_ID_GROUND;
373 379 packet->time[BYTE_0] = INIT_CHAR;
374 380 packet->time[BYTE_1] = INIT_CHAR;
375 381 packet->time[BYTE_2] = INIT_CHAR;
376 382 packet->time[BYTE_3] = INIT_CHAR;
377 383 packet->time[BYTE_4] = INIT_CHAR;
378 384 packet->time[BYTE_5] = INIT_CHAR;
379 385 // AUXILIARY DATA HEADER
380 386 packet->sid = sid;
381 387 packet->pa_bia_status_info = INIT_CHAR;
382 388 packet->sy_lfr_common_parameters_spare = INIT_CHAR;
383 389 packet->sy_lfr_common_parameters = INIT_CHAR;
384 390 packet->acquisitionTime[BYTE_0] = INIT_CHAR;
385 391 packet->acquisitionTime[BYTE_1] = INIT_CHAR;
386 392 packet->acquisitionTime[BYTE_2] = INIT_CHAR;
387 393 packet->acquisitionTime[BYTE_3] = INIT_CHAR;
388 394 packet->acquisitionTime[BYTE_4] = INIT_CHAR;
389 395 packet->acquisitionTime[BYTE_5] = INIT_CHAR;
390 396 packet->pa_lfr_bp_blk_nr[0] = INIT_CHAR; // BLK_NR MSB
391 397 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
392 398 }
393 399
394 400 void BP_init_header_with_spare( bp_packet_with_spare *packet,
395 401 unsigned int apid, unsigned char sid,
396 402 unsigned int packetLength , unsigned char blkNr)
397 403 {
398 404 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
399 405 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
400 406 packet->reserved = INIT_CHAR;
401 407 packet->userApplication = CCSDS_USER_APP;
402 408 packet->packetID[0] = (unsigned char) (apid >> SHIFT_1_BYTE);
403 409 packet->packetID[1] = (unsigned char) (apid);
404 410 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
405 411 packet->packetSequenceControl[1] = INIT_CHAR;
406 412 packet->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE);
407 413 packet->packetLength[1] = (unsigned char) (packetLength);
408 414 // DATA FIELD HEADER
409 415 packet->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
410 416 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
411 417 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
412 418 packet->destinationID = TM_DESTINATION_ID_GROUND;
413 419 // AUXILIARY DATA HEADER
414 420 packet->sid = sid;
415 421 packet->pa_bia_status_info = INIT_CHAR;
416 422 packet->sy_lfr_common_parameters_spare = INIT_CHAR;
417 423 packet->sy_lfr_common_parameters = INIT_CHAR;
418 424 packet->time[BYTE_0] = INIT_CHAR;
419 425 packet->time[BYTE_1] = INIT_CHAR;
420 426 packet->time[BYTE_2] = INIT_CHAR;
421 427 packet->time[BYTE_3] = INIT_CHAR;
422 428 packet->time[BYTE_4] = INIT_CHAR;
423 429 packet->time[BYTE_5] = INIT_CHAR;
424 430 packet->source_data_spare = INIT_CHAR;
425 431 packet->pa_lfr_bp_blk_nr[0] = INIT_CHAR; // BLK_NR MSB
426 432 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
427 433 }
428 434
429 435 void BP_send(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
430 436 {
431 437 rtems_status_code status;
432 438
433 439 // SEND PACKET
434 440 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
435 441 if (status != RTEMS_SUCCESSFUL)
436 442 {
437 443 PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
438 444 }
439 445 }
440 446
441 447 void BP_send_s1_s2(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
442 448 {
443 449 /** This function is used to send the BP paquets when needed.
444 450 *
445 451 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
446 452 *
447 453 * @return void
448 454 *
449 455 * SBM1 and SBM2 paquets are sent depending on the type of the LFR mode transition.
450 456 * BURST paquets are sent everytime.
451 457 *
452 458 */
453 459
454 460 rtems_status_code status;
455 461
456 462 // SEND PACKET
457 463 // before lastValidTransitionDate, the data are drops even if they are ready
458 464 // this guarantees that no SBM packets will be received before the requested enter mode time
459 465 if ( time_management_regs->coarse_time >= lastValidEnterModeTime)
460 466 {
461 467 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
462 468 if (status != RTEMS_SUCCESSFUL)
463 469 {
464 470 PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
465 471 }
466 472 }
467 473 }
468 474
469 475 //******************
470 476 // general functions
471 477
472 478 void reset_sm_status( void )
473 479 {
474 480 // error
475 481 // 10 --------------- 9 ---------------- 8 ---------------- 7 ---------
476 482 // input_fif0_write_2 input_fifo_write_1 input_fifo_write_0 buffer_full
477 483 // ---------- 5 -- 4 -- 3 -- 2 -- 1 -- 0 --
478 484 // ready bits f2_1 f2_0 f1_1 f1_1 f0_1 f0_0
479 485
480 486 spectral_matrix_regs->status = BITS_STATUS_REG; // [0111 1111 1111]
481 487 }
482 488
483 489 void reset_spectral_matrix_regs( void )
484 490 {
485 491 /** This function resets the spectral matrices module registers.
486 492 *
487 493 * The registers affected by this function are located at the following offset addresses:
488 494 *
489 495 * - 0x00 config
490 496 * - 0x04 status
491 497 * - 0x08 matrixF0_Address0
492 498 * - 0x10 matrixFO_Address1
493 499 * - 0x14 matrixF1_Address
494 500 * - 0x18 matrixF2_Address
495 501 *
496 502 */
497 503
498 504 set_sm_irq_onError( 0 );
499 505
500 506 set_sm_irq_onNewMatrix( 0 );
501 507
502 508 reset_sm_status();
503 509
504 510 // F1
505 511 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->previous->buffer_address;
506 512 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
507 513 // F2
508 514 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->previous->buffer_address;
509 515 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
510 516 // F3
511 517 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->previous->buffer_address;
512 518 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
513 519
514 520 spectral_matrix_regs->matrix_length = DEFAULT_MATRIX_LENGTH; // 25 * 128 / 16 = 200 = 0xc8
515 521 }
516 522
517 523 void set_time( unsigned char *time, unsigned char * timeInBuffer )
518 524 {
519 525 time[BYTE_0] = timeInBuffer[BYTE_0];
520 526 time[BYTE_1] = timeInBuffer[BYTE_1];
521 527 time[BYTE_2] = timeInBuffer[BYTE_2];
522 528 time[BYTE_3] = timeInBuffer[BYTE_3];
523 529 time[BYTE_4] = timeInBuffer[BYTE_6];
524 530 time[BYTE_5] = timeInBuffer[BYTE_7];
525 531 }
526 532
527 533 unsigned long long int get_acquisition_time( unsigned char *timePtr )
528 534 {
529 535 unsigned long long int acquisitionTimeAslong;
530 536 acquisitionTimeAslong = INIT_CHAR;
531 537 acquisitionTimeAslong =
532 538 ( (unsigned long long int) (timePtr[BYTE_0] & SYNC_BIT_MASK) << SHIFT_5_BYTES ) // [0111 1111] mask the synchronization bit
533 539 + ( (unsigned long long int) timePtr[BYTE_1] << SHIFT_4_BYTES )
534 540 + ( (unsigned long long int) timePtr[BYTE_2] << SHIFT_3_BYTES )
535 541 + ( (unsigned long long int) timePtr[BYTE_3] << SHIFT_2_BYTES )
536 542 + ( (unsigned long long int) timePtr[BYTE_6] << SHIFT_1_BYTE )
537 543 + ( (unsigned long long int) timePtr[BYTE_7] );
538 544 return acquisitionTimeAslong;
539 545 }
540 546
541 547 unsigned char getSID( rtems_event_set event )
542 548 {
543 549 unsigned char sid;
544 550
545 551 rtems_event_set eventSetBURST;
546 552 rtems_event_set eventSetSBM;
547 553
548 554 //******
549 555 // BURST
550 556 eventSetBURST = RTEMS_EVENT_BURST_BP1_F0
551 557 | RTEMS_EVENT_BURST_BP1_F1
552 558 | RTEMS_EVENT_BURST_BP2_F0
553 559 | RTEMS_EVENT_BURST_BP2_F1;
554 560
555 561 //****
556 562 // SBM
557 563 eventSetSBM = RTEMS_EVENT_SBM_BP1_F0
558 564 | RTEMS_EVENT_SBM_BP1_F1
559 565 | RTEMS_EVENT_SBM_BP2_F0
560 566 | RTEMS_EVENT_SBM_BP2_F1;
561 567
562 568 if (event & eventSetBURST)
563 569 {
564 570 sid = SID_BURST_BP1_F0;
565 571 }
566 572 else if (event & eventSetSBM)
567 573 {
568 574 sid = SID_SBM1_BP1_F0;
569 575 }
570 576 else
571 577 {
572 578 sid = 0;
573 579 }
574 580
575 581 return sid;
576 582 }
577 583
578 584 void extractReImVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
579 585 {
580 586 unsigned int i;
581 587 float re;
582 588 float im;
583 589
584 590 for (i=0; i<NB_BINS_PER_SM; i++){
585 591 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + (i * SM_BYTES_PER_VAL) ];
586 592 im = inputASM[ (asmComponent*NB_BINS_PER_SM) + (i * SM_BYTES_PER_VAL) + 1];
587 593 outputASM[ ( asmComponent *NB_BINS_PER_SM) + i] = re;
588 594 outputASM[ ((asmComponent+1)*NB_BINS_PER_SM) + i] = im;
589 595 }
590 596 }
591 597
592 598 void copyReVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
593 599 {
594 600 unsigned int i;
595 601 float re;
596 602
597 603 for (i=0; i<NB_BINS_PER_SM; i++){
598 604 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i];
599 605 outputASM[ (asmComponent*NB_BINS_PER_SM) + i] = re;
600 606 }
601 607 }
602 608
603 609 void ASM_patch( float *inputASM, float *outputASM )
604 610 {
605 611 extractReImVectors( inputASM, outputASM, ASM_COMP_B1B2); // b1b2
606 612 extractReImVectors( inputASM, outputASM, ASM_COMP_B1B3 ); // b1b3
607 613 extractReImVectors( inputASM, outputASM, ASM_COMP_B1E1 ); // b1e1
608 614 extractReImVectors( inputASM, outputASM, ASM_COMP_B1E2 ); // b1e2
609 615 extractReImVectors( inputASM, outputASM, ASM_COMP_B2B3 ); // b2b3
610 616 extractReImVectors( inputASM, outputASM, ASM_COMP_B2E1 ); // b2e1
611 617 extractReImVectors( inputASM, outputASM, ASM_COMP_B2E2 ); // b2e2
612 618 extractReImVectors( inputASM, outputASM, ASM_COMP_B3E1 ); // b3e1
613 619 extractReImVectors( inputASM, outputASM, ASM_COMP_B3E2 ); // b3e2
614 620 extractReImVectors( inputASM, outputASM, ASM_COMP_E1E2 ); // e1e2
615 621
616 622 copyReVectors(inputASM, outputASM, ASM_COMP_B1B1 ); // b1b1
617 623 copyReVectors(inputASM, outputASM, ASM_COMP_B2B2 ); // b2b2
618 624 copyReVectors(inputASM, outputASM, ASM_COMP_B3B3); // b3b3
619 625 copyReVectors(inputASM, outputASM, ASM_COMP_E1E1); // e1e1
620 626 copyReVectors(inputASM, outputASM, ASM_COMP_E2E2); // e2e2
621 627 }
622 628
623 629 void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
624 630 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage,
625 631 unsigned char ASMIndexStart,
626 632 unsigned char channel )
627 633 {
628 634 //*************
629 635 // input format
630 636 // component0[0 .. 127] component1[0 .. 127] .. component24[0 .. 127]
631 637 //**************
632 638 // output format
633 639 // matr0[0 .. 24] matr1[0 .. 24] .. matr127[0 .. 24]
634 640 //************
635 641 // compression
636 642 // matr0[0 .. 24] matr1[0 .. 24] .. matr11[0 .. 24] => f0 NORM
637 643 // matr0[0 .. 24] matr1[0 .. 24] .. matr22[0 .. 24] => f0 BURST, SBM
638 644
639 645 int frequencyBin;
640 646 int asmComponent;
641 647 int offsetASM;
642 648 int offsetCompressed;
643 649 int offsetFBin;
644 650 int fBinMask;
645 651 int k;
646 652
647 653 // BUILD DATA
648 654 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
649 655 {
650 656 for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
651 657 {
652 658 offsetCompressed = // NO TIME OFFSET
653 659 (frequencyBin * NB_VALUES_PER_SM)
654 660 + asmComponent;
655 661 offsetASM = // NO TIME OFFSET
656 662 (asmComponent * NB_BINS_PER_SM)
657 663 + ASMIndexStart
658 664 + (frequencyBin * nbBinsToAverage);
659 665 offsetFBin = ASMIndexStart
660 666 + (frequencyBin * nbBinsToAverage);
661 667 compressed_spec_mat[ offsetCompressed ] = 0;
662 668 for ( k = 0; k < nbBinsToAverage; k++ )
663 669 {
664 670 fBinMask = getFBinMask( offsetFBin + k, channel );
665 671 compressed_spec_mat[offsetCompressed ] = compressed_spec_mat[ offsetCompressed ]
666 672 + (averaged_spec_mat[ offsetASM + k ] * fBinMask);
667 673 }
668 674 if (divider != 0)
669 675 {
670 676 compressed_spec_mat[ offsetCompressed ] = compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
671 677 }
672 678 else
673 679 {
674 680 compressed_spec_mat[ offsetCompressed ] = INIT_FLOAT;
675 681 }
676 682 }
677 683 }
678 684
679 685 }
680 686
681 687 int getFBinMask( int index, unsigned char channel )
682 688 {
683 689 unsigned int indexInChar;
684 690 unsigned int indexInTheChar;
685 691 int fbin;
686 692 unsigned char *sy_lfr_fbins_fx_word1;
687 693
688 694 sy_lfr_fbins_fx_word1 = parameter_dump_packet.sy_lfr_fbins.fx.f0_word1;
689 695
690 696 switch(channel)
691 697 {
692 698 case CHANNELF0:
693 699 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f0;
694 700 break;
695 701 case CHANNELF1:
696 702 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f1;
697 703 break;
698 704 case CHANNELF2:
699 705 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f2;
700 706 break;
701 707 default:
702 708 PRINTF("ERR *** in getFBinMask, wrong frequency channel")
703 709 }
704 710
705 711 indexInChar = index >> SHIFT_3_BITS;
706 712 indexInTheChar = index - (indexInChar * BITS_PER_BYTE);
707 713
708 714 fbin = (int) ((sy_lfr_fbins_fx_word1[ BYTES_PER_MASK - 1 - indexInChar] >> indexInTheChar) & 1);
709 715
710 716 return fbin;
711 717 }
712 718
713 719 unsigned char acquisitionTimeIsValid( unsigned int coarseTime, unsigned int fineTime, unsigned char channel)
714 720 {
715 721 u_int64_t acquisitionTime;
716 722 u_int64_t timecodeReference;
717 723 u_int64_t offsetInFineTime;
718 724 u_int64_t shiftInFineTime;
719 725 u_int64_t tBadInFineTime;
720 726 u_int64_t acquisitionTimeRangeMin;
721 727 u_int64_t acquisitionTimeRangeMax;
722 728 unsigned char pasFilteringIsEnabled;
723 729 unsigned char ret;
724 730
725 731 pasFilteringIsEnabled = (filterPar.spare_sy_lfr_pas_filter_enabled & 1); // [0000 0001]
726 732 ret = 1;
727 733
728 734 // compute acquisition time from caoarseTime and fineTime
729 735 acquisitionTime = ( ((u_int64_t)coarseTime) << SHIFT_2_BYTES )
730 736 + (u_int64_t) fineTime;
731 737
732 738 // compute the timecode reference
733 739 timecodeReference = (u_int64_t) ( (floor( ((double) coarseTime) / ((double) filterPar.sy_lfr_pas_filter_modulus) )
734 740 * ((double) filterPar.sy_lfr_pas_filter_modulus)) * CONST_65536 );
735 741
736 742 // compute the acquitionTime range
737 743 offsetInFineTime = ((double) filterPar.sy_lfr_pas_filter_offset) * CONST_65536;
738 744 shiftInFineTime = ((double) filterPar.sy_lfr_pas_filter_shift) * CONST_65536;
739 745 tBadInFineTime = ((double) filterPar.sy_lfr_pas_filter_tbad) * CONST_65536;
740 746
741 747 acquisitionTimeRangeMin =
742 748 timecodeReference
743 749 + offsetInFineTime
744 750 + shiftInFineTime
745 751 - acquisitionDurations[channel];
746 752 acquisitionTimeRangeMax =
747 753 timecodeReference
748 754 + offsetInFineTime
749 755 + shiftInFineTime
750 756 + tBadInFineTime;
751 757
752 758 if ( (acquisitionTime >= acquisitionTimeRangeMin)
753 759 && (acquisitionTime <= acquisitionTimeRangeMax)
754 760 && (pasFilteringIsEnabled == 1) )
755 761 {
756 762 ret = 0; // the acquisition time is INSIDE the range, the matrix shall be ignored
757 763 }
758 764 else
759 765 {
760 766 ret = 1; // the acquisition time is OUTSIDE the range, the matrix can be used for the averaging
761 767 }
762 768
763 769 // printf("coarseTime = %x, fineTime = %x\n",
764 770 // coarseTime,
765 771 // fineTime);
766 772
767 773 // printf("[ret = %d] *** acquisitionTime = %f, Reference = %f",
768 774 // ret,
769 775 // acquisitionTime / 65536.,
770 776 // timecodeReference / 65536.);
771 777
772 778 // printf(", Min = %f, Max = %f\n",
773 779 // acquisitionTimeRangeMin / 65536.,
774 780 // acquisitionTimeRangeMax / 65536.);
775 781
776 782 return ret;
777 783 }
778 784
779 785 void init_kcoeff_sbm_from_kcoeff_norm(float *input_kcoeff, float *output_kcoeff, unsigned char nb_bins_norm)
780 786 {
781 787 unsigned char bin;
782 788 unsigned char kcoeff;
783 789
784 790 for (bin=0; bin<nb_bins_norm; bin++)
785 791 {
786 792 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
787 793 {
788 794 output_kcoeff[ ( ( bin * NB_K_COEFF_PER_BIN ) + kcoeff ) * SBM_COEFF_PER_NORM_COEFF ]
789 795 = input_kcoeff[ (bin*NB_K_COEFF_PER_BIN) + kcoeff ];
790 796 output_kcoeff[ ( ( bin * NB_K_COEFF_PER_BIN ) + kcoeff ) * SBM_COEFF_PER_NORM_COEFF + 1 ]
791 797 = input_kcoeff[ (bin*NB_K_COEFF_PER_BIN) + kcoeff ];
792 798 }
793 799 }
794 800 }
Show file before | Show file after | Hide comments
@@ -1,1312 +1,1319
1 1 /** Functions and tasks related to waveform packet generation.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle waveforms, in snapshot or continuous format.\n
7 7 *
8 8 */
9 9
10 10 #include "wf_handler.h"
11 11
12 12 //***************
13 13 // waveform rings
14 14 // F0
15 15 ring_node waveform_ring_f0[NB_RING_NODES_F0];
16 16 ring_node *current_ring_node_f0;
17 17 ring_node *ring_node_to_send_swf_f0;
18 18 // F1
19 19 ring_node waveform_ring_f1[NB_RING_NODES_F1];
20 20 ring_node *current_ring_node_f1;
21 21 ring_node *ring_node_to_send_swf_f1;
22 22 ring_node *ring_node_to_send_cwf_f1;
23 23 // F2
24 24 ring_node waveform_ring_f2[NB_RING_NODES_F2];
25 25 ring_node *current_ring_node_f2;
26 26 ring_node *ring_node_to_send_swf_f2;
27 27 ring_node *ring_node_to_send_cwf_f2;
28 28 // F3
29 29 ring_node waveform_ring_f3[NB_RING_NODES_F3];
30 30 ring_node *current_ring_node_f3;
31 31 ring_node *ring_node_to_send_cwf_f3;
32 32 char wf_cont_f3_light[ (NB_SAMPLES_PER_SNAPSHOT) * NB_BYTES_CWF3_LIGHT_BLK ];
33 33
34 34 bool extractSWF1 = false;
35 35 bool extractSWF2 = false;
36 36 bool swf0_ready_flag_f1 = false;
37 37 bool swf0_ready_flag_f2 = false;
38 38 bool swf1_ready = false;
39 39 bool swf2_ready = false;
40 40
41 41 int swf1_extracted[ (NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK) ];
42 42 int swf2_extracted[ (NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK) ];
43 43 ring_node ring_node_swf1_extracted;
44 44 ring_node ring_node_swf2_extracted;
45 45
46 46 typedef enum resynchro_state_t
47 47 {
48 48 MEASURE,
49 49 CORRECTION
50 50 } resynchro_state;
51 51
52 52 //*********************
53 53 // Interrupt SubRoutine
54 54
55 55 ring_node * getRingNodeToSendCWF( unsigned char frequencyChannel)
56 56 {
57 57 ring_node *node;
58 58
59 59 node = NULL;
60 60 switch ( frequencyChannel ) {
61 61 case CHANNELF1:
62 62 node = ring_node_to_send_cwf_f1;
63 63 break;
64 64 case CHANNELF2:
65 65 node = ring_node_to_send_cwf_f2;
66 66 break;
67 67 case CHANNELF3:
68 68 node = ring_node_to_send_cwf_f3;
69 69 break;
70 70 default:
71 71 break;
72 72 }
73 73
74 74 return node;
75 75 }
76 76
77 77 ring_node * getRingNodeToSendSWF( unsigned char frequencyChannel)
78 78 {
79 79 ring_node *node;
80 80
81 81 node = NULL;
82 82 switch ( frequencyChannel ) {
83 83 case CHANNELF0:
84 84 node = ring_node_to_send_swf_f0;
85 85 break;
86 86 case CHANNELF1:
87 87 node = ring_node_to_send_swf_f1;
88 88 break;
89 89 case CHANNELF2:
90 90 node = ring_node_to_send_swf_f2;
91 91 break;
92 92 default:
93 93 break;
94 94 }
95 95
96 96 return node;
97 97 }
98 98
99 99 void reset_extractSWF( void )
100 100 {
101 101 extractSWF1 = false;
102 102 extractSWF2 = false;
103 103 swf0_ready_flag_f1 = false;
104 104 swf0_ready_flag_f2 = false;
105 105 swf1_ready = false;
106 106 swf2_ready = false;
107 107 }
108 108
109 109 inline void waveforms_isr_f3( void )
110 110 {
111 111 rtems_status_code spare_status;
112 112
113 113 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_BURST) // in BURST the data are used to place v, e1 and e2 in the HK packet
114 114 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
115 115 { // in modes other than STANDBY and BURST, send the CWF_F3 data
116 116 //***
117 117 // F3
118 118 if ( (waveform_picker_regs->status & BITS_WFP_STATUS_F3) != INIT_CHAR ) { // [1100 0000] check the f3 full bits
119 119 ring_node_to_send_cwf_f3 = current_ring_node_f3->previous;
120 120 current_ring_node_f3 = current_ring_node_f3->next;
121 121 if ((waveform_picker_regs->status & BIT_WFP_BUF_F3_0) == BIT_WFP_BUF_F3_0){ // [0100 0000] f3 buffer 0 is full
122 122 ring_node_to_send_cwf_f3->coarseTime = waveform_picker_regs->f3_0_coarse_time;
123 123 ring_node_to_send_cwf_f3->fineTime = waveform_picker_regs->f3_0_fine_time;
124 124 waveform_picker_regs->addr_data_f3_0 = current_ring_node_f3->buffer_address;
125 125 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F3_0; // [1000 1000 0100 0000]
126 126 }
127 127 else if ((waveform_picker_regs->status & BIT_WFP_BUF_F3_1) == BIT_WFP_BUF_F3_1){ // [1000 0000] f3 buffer 1 is full
128 128 ring_node_to_send_cwf_f3->coarseTime = waveform_picker_regs->f3_1_coarse_time;
129 129 ring_node_to_send_cwf_f3->fineTime = waveform_picker_regs->f3_1_fine_time;
130 130 waveform_picker_regs->addr_data_f3_1 = current_ring_node_f3->buffer_address;
131 131 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F3_1; // [1000 1000 1000 0000]
132 132 }
133 133 if (rtems_event_send( Task_id[TASKID_CWF3], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
134 134 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
135 135 }
136 136 }
137 137 }
138 138 }
139 139
140 140 inline void waveforms_isr_burst( void )
141 141 {
142 142 unsigned char status;
143 143 rtems_status_code spare_status;
144 144
145 145 status = (waveform_picker_regs->status & BITS_WFP_STATUS_F2) >> SHIFT_WFP_STATUS_F2; // [0011 0000] get the status bits for f2
146 146
147 147 switch(status)
148 148 {
149 149 case BIT_WFP_BUFFER_0:
150 150 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
151 151 ring_node_to_send_cwf_f2->sid = SID_BURST_CWF_F2;
152 152 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
153 153 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
154 154 current_ring_node_f2 = current_ring_node_f2->next;
155 155 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
156 156 if (rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_BURST ) != RTEMS_SUCCESSFUL) {
157 157 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
158 158 }
159 159 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F2_0; // [0100 0100 0001 0000]
160 160 break;
161 161 case BIT_WFP_BUFFER_1:
162 162 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
163 163 ring_node_to_send_cwf_f2->sid = SID_BURST_CWF_F2;
164 164 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
165 165 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
166 166 current_ring_node_f2 = current_ring_node_f2->next;
167 167 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
168 168 if (rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_BURST ) != RTEMS_SUCCESSFUL) {
169 169 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
170 170 }
171 171 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F2_1; // [0100 0100 0010 0000]
172 172 break;
173 173 default:
174 174 break;
175 175 }
176 176 }
177 177
178 178 inline void waveform_isr_normal_sbm1_sbm2( void )
179 179 {
180 180 rtems_status_code status;
181 181
182 182 //***
183 183 // F0
184 184 if ( (waveform_picker_regs->status & BITS_WFP_STATUS_F0) != INIT_CHAR ) // [0000 0011] check the f0 full bits
185 185 {
186 186 swf0_ready_flag_f1 = true;
187 187 swf0_ready_flag_f2 = true;
188 188 ring_node_to_send_swf_f0 = current_ring_node_f0->previous;
189 189 current_ring_node_f0 = current_ring_node_f0->next;
190 190 if ( (waveform_picker_regs->status & BIT_WFP_BUFFER_0) == BIT_WFP_BUFFER_0)
191 191 {
192 192
193 193 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_0_coarse_time;
194 194 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_0_fine_time;
195 195 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->buffer_address;
196 196 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F0_0; // [0001 0001 0000 0001]
197 197 }
198 198 else if ( (waveform_picker_regs->status & BIT_WFP_BUFFER_1) == BIT_WFP_BUFFER_1)
199 199 {
200 200 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_1_coarse_time;
201 201 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_1_fine_time;
202 202 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address;
203 203 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F0_1; // [0001 0001 0000 0010]
204 204 }
205 205 // send an event to the WFRM task for resynchro activities
206 206 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_SWF_RESYNCH );
207 207 }
208 208
209 209 //***
210 210 // F1
211 211 if ( (waveform_picker_regs->status & 0x0c) != INIT_CHAR ) { // [0000 1100] check the f1 full bits
212 212 // (1) change the receiving buffer for the waveform picker
213 213 ring_node_to_send_cwf_f1 = current_ring_node_f1->previous;
214 214 current_ring_node_f1 = current_ring_node_f1->next;
215 215 if ( (waveform_picker_regs->status & BIT_WFP_BUF_F1_0) == BIT_WFP_BUF_F1_0)
216 216 {
217 217 ring_node_to_send_cwf_f1->coarseTime = waveform_picker_regs->f1_0_coarse_time;
218 218 ring_node_to_send_cwf_f1->fineTime = waveform_picker_regs->f1_0_fine_time;
219 219 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->buffer_address;
220 220 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F1_0; // [0010 0010 0000 0100] f1 bits = 0
221 221 }
222 222 else if ( (waveform_picker_regs->status & BIT_WFP_BUF_F1_1) == BIT_WFP_BUF_F1_1)
223 223 {
224 224 ring_node_to_send_cwf_f1->coarseTime = waveform_picker_regs->f1_1_coarse_time;
225 225 ring_node_to_send_cwf_f1->fineTime = waveform_picker_regs->f1_1_fine_time;
226 226 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address;
227 227 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F1_1; // [0010 0010 0000 1000] f1 bits = 0
228 228 }
229 229 // (2) send an event for the the CWF1 task for transmission (and snapshot extraction if needed)
230 230 status = rtems_event_send( Task_id[TASKID_CWF1], RTEMS_EVENT_MODE_NORM_S1_S2 );
231 231 }
232 232
233 233 //***
234 234 // F2
235 235 if ( (waveform_picker_regs->status & BITS_WFP_STATUS_F2) != INIT_CHAR ) { // [0011 0000] check the f2 full bit
236 236 // (1) change the receiving buffer for the waveform picker
237 237 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
238 238 ring_node_to_send_cwf_f2->sid = SID_SBM2_CWF_F2;
239 239 current_ring_node_f2 = current_ring_node_f2->next;
240 240 if ( (waveform_picker_regs->status & BIT_WFP_BUF_F2_0) == BIT_WFP_BUF_F2_0)
241 241 {
242 242 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
243 243 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
244 244 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
245 245 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F2_0; // [0100 0100 0001 0000]
246 246 }
247 247 else if ( (waveform_picker_regs->status & BIT_WFP_BUF_F2_1) == BIT_WFP_BUF_F2_1)
248 248 {
249 249 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
250 250 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
251 251 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
252 252 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F2_1; // [0100 0100 0010 0000]
253 253 }
254 254 // (2) send an event for the waveforms transmission
255 255 status = rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_NORM_S1_S2 );
256 256 }
257 257 }
258 258
259 259 rtems_isr waveforms_isr( rtems_vector_number vector )
260 260 {
261 261 /** This is the interrupt sub routine called by the waveform picker core.
262 262 *
263 263 * This ISR launch different actions depending mainly on two pieces of information:
264 264 * 1. the values read in the registers of the waveform picker.
265 265 * 2. the current LFR mode.
266 266 *
267 267 */
268 268
269 269 // STATUS
270 270 // new error error buffer full
271 271 // 15 14 13 12 11 10 9 8
272 272 // f3 f2 f1 f0 f3 f2 f1 f0
273 273 //
274 274 // ready buffer
275 275 // 7 6 5 4 3 2 1 0
276 276 // f3_1 f3_0 f2_1 f2_0 f1_1 f1_0 f0_1 f0_0
277 277
278 278 rtems_status_code spare_status;
279 279
280 280 waveforms_isr_f3();
281 281
282 282 //*************************************************
283 283 // copy the status bits in the housekeeping packets
284 284 housekeeping_packet.hk_lfr_vhdl_iir_cal =
285 285 (unsigned char) ((waveform_picker_regs->status & BYTE0_MASK) >> SHIFT_1_BYTE);
286 286
287 287 if ( (waveform_picker_regs->status & BYTE0_MASK) != INIT_CHAR) // [1111 1111 0000 0000] check the error bits
288 288 {
289 289 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_10 );
290 290 }
291 291
292 292 switch(lfrCurrentMode)
293 293 {
294 294 //********
295 295 // STANDBY
296 296 case LFR_MODE_STANDBY:
297 297 break;
298 298 //**************************
299 299 // LFR NORMAL, SBM1 and SBM2
300 300 case LFR_MODE_NORMAL:
301 301 case LFR_MODE_SBM1:
302 302 case LFR_MODE_SBM2:
303 303 waveform_isr_normal_sbm1_sbm2();
304 304 break;
305 305 //******
306 306 // BURST
307 307 case LFR_MODE_BURST:
308 308 waveforms_isr_burst();
309 309 break;
310 310 //********
311 311 // DEFAULT
312 312 default:
313 313 break;
314 314 }
315 315 }
316 316
317 317 //************
318 318 // RTEMS TASKS
319 319
320 320 rtems_task wfrm_task(rtems_task_argument argument) //used with the waveform picker VHDL IP
321 321 {
322 322 /** This RTEMS task is dedicated to the transmission of snapshots of the NORMAL mode.
323 323 *
324 324 * @param unused is the starting argument of the RTEMS task
325 325 *
326 326 * The following data packets are sent by this task:
327 327 * - TM_LFR_SCIENCE_NORMAL_SWF_F0
328 328 * - TM_LFR_SCIENCE_NORMAL_SWF_F1
329 329 * - TM_LFR_SCIENCE_NORMAL_SWF_F2
330 330 *
331 331 */
332 332
333 333 rtems_event_set event_out;
334 334 rtems_id queue_id;
335 335 rtems_status_code status;
336 336 ring_node *ring_node_swf1_extracted_ptr;
337 337 ring_node *ring_node_swf2_extracted_ptr;
338 338
339 339 ring_node_swf1_extracted_ptr = (ring_node *) &ring_node_swf1_extracted;
340 340 ring_node_swf2_extracted_ptr = (ring_node *) &ring_node_swf2_extracted;
341 341
342 342 status = get_message_queue_id_send( &queue_id );
343 343 if (status != RTEMS_SUCCESSFUL)
344 344 {
345 345 PRINTF1("in WFRM *** ERR get_message_queue_id_send %d\n", status);
346 346 }
347 347
348 348 BOOT_PRINTF("in WFRM ***\n");
349 349
350 350 while(1){
351 351 // wait for an RTEMS_EVENT
352 352 rtems_event_receive(RTEMS_EVENT_MODE_NORMAL | RTEMS_EVENT_SWF_RESYNCH,
353 353 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
354 354
355 355 if (event_out == RTEMS_EVENT_MODE_NORMAL)
356 356 {
357 357 DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_SBM2\n");
358 358 ring_node_to_send_swf_f0->sid = SID_NORM_SWF_F0;
359 359 ring_node_swf1_extracted_ptr->sid = SID_NORM_SWF_F1;
360 360 ring_node_swf2_extracted_ptr->sid = SID_NORM_SWF_F2;
361 361 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f0, sizeof( ring_node* ) );
362 362 status = rtems_message_queue_send( queue_id, &ring_node_swf1_extracted_ptr, sizeof( ring_node* ) );
363 363 status = rtems_message_queue_send( queue_id, &ring_node_swf2_extracted_ptr, sizeof( ring_node* ) );
364 364 }
365 365 if (event_out == RTEMS_EVENT_SWF_RESYNCH)
366 366 {
367 367 snapshot_resynchronization( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
368 368 }
369 369 }
370 370 }
371 371
372 372 rtems_task cwf3_task(rtems_task_argument argument) //used with the waveform picker VHDL IP
373 373 {
374 374 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f3.
375 375 *
376 376 * @param unused is the starting argument of the RTEMS task
377 377 *
378 378 * The following data packet is sent by this task:
379 379 * - TM_LFR_SCIENCE_NORMAL_CWF_F3
380 380 *
381 381 */
382 382
383 383 rtems_event_set event_out;
384 384 rtems_id queue_id;
385 385 rtems_status_code status;
386 386 ring_node ring_node_cwf3_light;
387 387 ring_node *ring_node_to_send_cwf;
388 388
389 389 status = get_message_queue_id_send( &queue_id );
390 390 if (status != RTEMS_SUCCESSFUL)
391 391 {
392 392 PRINTF1("in CWF3 *** ERR get_message_queue_id_send %d\n", status)
393 393 }
394 394
395 395 ring_node_to_send_cwf_f3->sid = SID_NORM_CWF_LONG_F3;
396 396
397 397 // init the ring_node_cwf3_light structure
398 398 ring_node_cwf3_light.buffer_address = (int) wf_cont_f3_light;
399 399 ring_node_cwf3_light.coarseTime = INIT_CHAR;
400 400 ring_node_cwf3_light.fineTime = INIT_CHAR;
401 401 ring_node_cwf3_light.next = NULL;
402 402 ring_node_cwf3_light.previous = NULL;
403 403 ring_node_cwf3_light.sid = SID_NORM_CWF_F3;
404 404 ring_node_cwf3_light.status = INIT_CHAR;
405 405
406 406 BOOT_PRINTF("in CWF3 ***\n");
407 407
408 408 while(1){
409 409 // wait for an RTEMS_EVENT
410 410 rtems_event_receive( RTEMS_EVENT_0,
411 411 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
412 412 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
413 413 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode==LFR_MODE_SBM2) )
414 414 {
415 415 ring_node_to_send_cwf = getRingNodeToSendCWF( CHANNELF3 );
416 416 if ( (parameter_dump_packet.sy_lfr_n_cwf_long_f3 & BIT_CWF_LONG_F3) == BIT_CWF_LONG_F3)
417 417 {
418 418 PRINTF("send CWF_LONG_F3\n");
419 419 ring_node_to_send_cwf_f3->sid = SID_NORM_CWF_LONG_F3;
420 420 status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf, sizeof( ring_node* ) );
421 421 }
422 422 else
423 423 {
424 424 PRINTF("send CWF_F3 (light)\n");
425 425 send_waveform_CWF3_light( ring_node_to_send_cwf, &ring_node_cwf3_light, queue_id );
426 426 }
427 427
428 428 }
429 429 else
430 430 {
431 431 PRINTF1("in CWF3 *** lfrCurrentMode is %d, no data will be sent\n", lfrCurrentMode)
432 432 }
433 433 }
434 434 }
435 435
436 436 rtems_task cwf2_task(rtems_task_argument argument) // ONLY USED IN BURST AND SBM2
437 437 {
438 438 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f2.
439 439 *
440 440 * @param unused is the starting argument of the RTEMS task
441 441 *
442 442 * The following data packet is sent by this function:
443 443 * - TM_LFR_SCIENCE_BURST_CWF_F2
444 444 * - TM_LFR_SCIENCE_SBM2_CWF_F2
445 445 *
446 446 */
447 447
448 448 rtems_event_set event_out;
449 449 rtems_id queue_id;
450 450 rtems_status_code status;
451 451 ring_node *ring_node_to_send;
452 452 unsigned long long int acquisitionTimeF0_asLong;
453 453
454 454 acquisitionTimeF0_asLong = INIT_CHAR;
455 455
456 456 status = get_message_queue_id_send( &queue_id );
457 457 if (status != RTEMS_SUCCESSFUL)
458 458 {
459 459 PRINTF1("in CWF2 *** ERR get_message_queue_id_send %d\n", status)
460 460 }
461 461
462 462 BOOT_PRINTF("in CWF2 ***\n");
463 463
464 464 while(1){
465 465 // wait for an RTEMS_EVENT// send the snapshot when built
466 466 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_SBM2 );
467 467 rtems_event_receive( RTEMS_EVENT_MODE_NORM_S1_S2 | RTEMS_EVENT_MODE_BURST,
468 468 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
469 469 ring_node_to_send = getRingNodeToSendCWF( CHANNELF2 );
470 470 if (event_out == RTEMS_EVENT_MODE_BURST)
471 471 { // data are sent whatever the transition time
472 472 status = rtems_message_queue_send( queue_id, &ring_node_to_send, sizeof( ring_node* ) );
473 473 }
474 474 else if (event_out == RTEMS_EVENT_MODE_NORM_S1_S2)
475 475 {
476 476 if ( lfrCurrentMode == LFR_MODE_SBM2 )
477 477 {
478 478 // data are sent depending on the transition time
479 479 if ( time_management_regs->coarse_time >= lastValidEnterModeTime)
480 480 {
481 481 status = rtems_message_queue_send( queue_id, &ring_node_to_send, sizeof( ring_node* ) );
482 482 }
483 483 }
484 484 // launch snapshot extraction if needed
485 485 if (extractSWF2 == true)
486 486 {
487 487 ring_node_to_send_swf_f2 = ring_node_to_send_cwf_f2;
488 488 // extract the snapshot
489 489 build_snapshot_from_ring( ring_node_to_send_swf_f2, CHANNELF2, acquisitionTimeF0_asLong,
490 490 &ring_node_swf2_extracted, swf2_extracted );
491 491 extractSWF2 = false;
492 492 swf2_ready = true; // once the snapshot at f2 is ready the CWF1 task will send an event to WFRM
493 493 }
494 494 if (swf0_ready_flag_f2 == true)
495 495 {
496 496 extractSWF2 = true;
497 497 // record the acquition time of the f0 snapshot to use to build the snapshot at f2
498 498 acquisitionTimeF0_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
499 499 swf0_ready_flag_f2 = false;
500 500 }
501 501 }
502 502 }
503 503 }
504 504
505 505 rtems_task cwf1_task(rtems_task_argument argument) // ONLY USED IN SBM1
506 506 {
507 507 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f1.
508 508 *
509 509 * @param unused is the starting argument of the RTEMS task
510 510 *
511 511 * The following data packet is sent by this function:
512 512 * - TM_LFR_SCIENCE_SBM1_CWF_F1
513 513 *
514 514 */
515 515
516 516 rtems_event_set event_out;
517 517 rtems_id queue_id;
518 518 rtems_status_code status;
519 519
520 520 ring_node *ring_node_to_send_cwf;
521 521
522 522 status = get_message_queue_id_send( &queue_id );
523 523 if (status != RTEMS_SUCCESSFUL)
524 524 {
525 525 PRINTF1("in CWF1 *** ERR get_message_queue_id_send %d\n", status)
526 526 }
527 527
528 528 BOOT_PRINTF("in CWF1 ***\n");
529 529
530 530 while(1){
531 531 // wait for an RTEMS_EVENT
532 532 rtems_event_receive( RTEMS_EVENT_MODE_NORM_S1_S2,
533 533 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
534 534 ring_node_to_send_cwf = getRingNodeToSendCWF( 1 );
535 535 ring_node_to_send_cwf_f1->sid = SID_SBM1_CWF_F1;
536 536 if (lfrCurrentMode == LFR_MODE_SBM1)
537 537 {
538 538 // data are sent depending on the transition time
539 539 if ( time_management_regs->coarse_time >= lastValidEnterModeTime )
540 540 {
541 541 status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf, sizeof( ring_node* ) );
542 542 }
543 543 }
544 544 // launch snapshot extraction if needed
545 545 if (extractSWF1 == true)
546 546 {
547 547 ring_node_to_send_swf_f1 = ring_node_to_send_cwf;
548 548 // launch the snapshot extraction
549 549 status = rtems_event_send( Task_id[TASKID_SWBD], RTEMS_EVENT_MODE_NORM_S1_S2 );
550 550 extractSWF1 = false;
551 551 }
552 552 if (swf0_ready_flag_f1 == true)
553 553 {
554 554 extractSWF1 = true;
555 555 swf0_ready_flag_f1 = false; // this step shall be executed only one time
556 556 }
557 557 if ((swf1_ready == true) && (swf2_ready == true)) // swf_f1 is ready after the extraction
558 558 {
559 559 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_NORMAL );
560 560 swf1_ready = false;
561 561 swf2_ready = false;
562 562 }
563 563 }
564 564 }
565 565
566 566 rtems_task swbd_task(rtems_task_argument argument)
567 567 {
568 568 /** This RTEMS task is dedicated to the building of snapshots from different continuous waveforms buffers.
569 569 *
570 570 * @param unused is the starting argument of the RTEMS task
571 571 *
572 572 */
573 573
574 574 rtems_event_set event_out;
575 575 unsigned long long int acquisitionTimeF0_asLong;
576 576
577 577 acquisitionTimeF0_asLong = INIT_CHAR;
578 578
579 579 BOOT_PRINTF("in SWBD ***\n")
580 580
581 581 while(1){
582 582 // wait for an RTEMS_EVENT
583 583 rtems_event_receive( RTEMS_EVENT_MODE_NORM_S1_S2,
584 584 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
585 585 if (event_out == RTEMS_EVENT_MODE_NORM_S1_S2)
586 586 {
587 587 acquisitionTimeF0_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
588 build_snapshot_from_ring( ring_node_to_send_swf_f1, 1, acquisitionTimeF0_asLong,
588 build_snapshot_from_ring( ring_node_to_send_swf_f1, CHANNELF1, acquisitionTimeF0_asLong,
589 589 &ring_node_swf1_extracted, swf1_extracted );
590 590 swf1_ready = true; // the snapshot has been extracted and is ready to be sent
591 591 }
592 592 else
593 593 {
594 594 PRINTF1("in SWBD *** unexpected rtems event received %x\n", (int) event_out)
595 595 }
596 596 }
597 597 }
598 598
599 599 //******************
600 600 // general functions
601 601
602 602 void WFP_init_rings( void )
603 603 {
604 604 // F0 RING
605 605 init_ring( waveform_ring_f0, NB_RING_NODES_F0, wf_buffer_f0, WFRM_BUFFER );
606 606 // F1 RING
607 607 init_ring( waveform_ring_f1, NB_RING_NODES_F1, wf_buffer_f1, WFRM_BUFFER );
608 608 // F2 RING
609 609 init_ring( waveform_ring_f2, NB_RING_NODES_F2, wf_buffer_f2, WFRM_BUFFER );
610 610 // F3 RING
611 611 init_ring( waveform_ring_f3, NB_RING_NODES_F3, wf_buffer_f3, WFRM_BUFFER );
612 612
613 613 ring_node_swf1_extracted.buffer_address = (int) swf1_extracted;
614 614 ring_node_swf2_extracted.buffer_address = (int) swf2_extracted;
615 615
616 616 DEBUG_PRINTF1("waveform_ring_f0 @%x\n", (unsigned int) waveform_ring_f0)
617 617 DEBUG_PRINTF1("waveform_ring_f1 @%x\n", (unsigned int) waveform_ring_f1)
618 618 DEBUG_PRINTF1("waveform_ring_f2 @%x\n", (unsigned int) waveform_ring_f2)
619 619 DEBUG_PRINTF1("waveform_ring_f3 @%x\n", (unsigned int) waveform_ring_f3)
620 620 DEBUG_PRINTF1("wf_buffer_f0 @%x\n", (unsigned int) wf_buffer_f0)
621 621 DEBUG_PRINTF1("wf_buffer_f1 @%x\n", (unsigned int) wf_buffer_f1)
622 622 DEBUG_PRINTF1("wf_buffer_f2 @%x\n", (unsigned int) wf_buffer_f2)
623 623 DEBUG_PRINTF1("wf_buffer_f3 @%x\n", (unsigned int) wf_buffer_f3)
624 624
625 625 }
626 626
627 627 void WFP_reset_current_ring_nodes( void )
628 628 {
629 629 current_ring_node_f0 = waveform_ring_f0[0].next;
630 630 current_ring_node_f1 = waveform_ring_f1[0].next;
631 631 current_ring_node_f2 = waveform_ring_f2[0].next;
632 632 current_ring_node_f3 = waveform_ring_f3[0].next;
633 633
634 634 ring_node_to_send_swf_f0 = waveform_ring_f0;
635 635 ring_node_to_send_swf_f1 = waveform_ring_f1;
636 636 ring_node_to_send_swf_f2 = waveform_ring_f2;
637 637
638 638 ring_node_to_send_cwf_f1 = waveform_ring_f1;
639 639 ring_node_to_send_cwf_f2 = waveform_ring_f2;
640 640 ring_node_to_send_cwf_f3 = waveform_ring_f3;
641 641 }
642 642
643 643 int send_waveform_CWF3_light( ring_node *ring_node_to_send, ring_node *ring_node_cwf3_light, rtems_id queue_id )
644 644 {
645 645 /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data.
646 646 *
647 647 * @param waveform points to the buffer containing the data that will be send.
648 648 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
649 649 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
650 650 * contain information to setup the transmission of the data packets.
651 651 *
652 652 * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer
653 653 * from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks.
654 654 *
655 655 */
656 656
657 657 unsigned int i;
658 658 unsigned int j;
659 659 int ret;
660 660 rtems_status_code status;
661 661
662 662 char *sample;
663 663 int *dataPtr;
664 664
665 665 ret = LFR_DEFAULT;
666 666
667 667 dataPtr = (int*) ring_node_to_send->buffer_address;
668 668
669 669 ring_node_cwf3_light->coarseTime = ring_node_to_send->coarseTime;
670 670 ring_node_cwf3_light->fineTime = ring_node_to_send->fineTime;
671 671
672 672 //**********************
673 673 // BUILD CWF3_light DATA
674 674 for ( i=0; i< NB_SAMPLES_PER_SNAPSHOT; i++)
675 675 {
676 676 sample = (char*) &dataPtr[ (i * NB_WORDS_SWF_BLK) ];
677 677 for (j=0; j < CWF_BLK_SIZE; j++)
678 678 {
679 679 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + j] = sample[ j ];
680 680 }
681 681 }
682 682
683 683 // SEND PACKET
684 684 status = rtems_message_queue_send( queue_id, &ring_node_cwf3_light, sizeof( ring_node* ) );
685 685 if (status != RTEMS_SUCCESSFUL) {
686 686 ret = LFR_DEFAULT;
687 687 }
688 688
689 689 return ret;
690 690 }
691 691
692 692 void compute_acquisition_time( unsigned int coarseTime, unsigned int fineTime,
693 693 unsigned int sid, unsigned char pa_lfr_pkt_nr, unsigned char * acquisitionTime )
694 694 {
695 695 unsigned long long int acquisitionTimeAsLong;
696 696 unsigned char localAcquisitionTime[BYTES_PER_TIME];
697 697 double deltaT;
698 698
699 699 deltaT = INIT_FLOAT;
700 700
701 701 localAcquisitionTime[BYTE_0] = (unsigned char) ( coarseTime >> SHIFT_3_BYTES );
702 702 localAcquisitionTime[BYTE_1] = (unsigned char) ( coarseTime >> SHIFT_2_BYTES );
703 703 localAcquisitionTime[BYTE_2] = (unsigned char) ( coarseTime >> SHIFT_1_BYTE );
704 704 localAcquisitionTime[BYTE_3] = (unsigned char) ( coarseTime );
705 705 localAcquisitionTime[BYTE_4] = (unsigned char) ( fineTime >> SHIFT_1_BYTE );
706 706 localAcquisitionTime[BYTE_5] = (unsigned char) ( fineTime );
707 707
708 708 acquisitionTimeAsLong = ( (unsigned long long int) localAcquisitionTime[BYTE_0] << SHIFT_5_BYTES )
709 709 + ( (unsigned long long int) localAcquisitionTime[BYTE_1] << SHIFT_4_BYTES )
710 710 + ( (unsigned long long int) localAcquisitionTime[BYTE_2] << SHIFT_3_BYTES )
711 711 + ( (unsigned long long int) localAcquisitionTime[BYTE_3] << SHIFT_2_BYTES )
712 712 + ( (unsigned long long int) localAcquisitionTime[BYTE_4] << SHIFT_1_BYTE )
713 713 + ( (unsigned long long int) localAcquisitionTime[BYTE_5] );
714 714
715 715 switch( sid )
716 716 {
717 717 case SID_NORM_SWF_F0:
718 718 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * T0_IN_FINETIME ;
719 719 break;
720 720
721 721 case SID_NORM_SWF_F1:
722 722 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * T1_IN_FINETIME ;
723 723 break;
724 724
725 725 case SID_NORM_SWF_F2:
726 726 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * T2_IN_FINETIME ;
727 727 break;
728 728
729 729 case SID_SBM1_CWF_F1:
730 730 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * T1_IN_FINETIME ;
731 731 break;
732 732
733 733 case SID_SBM2_CWF_F2:
734 734 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * T2_IN_FINETIME ;
735 735 break;
736 736
737 737 case SID_BURST_CWF_F2:
738 738 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * T2_IN_FINETIME ;
739 739 break;
740 740
741 741 case SID_NORM_CWF_F3:
742 742 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF_SHORT_F3 * T3_IN_FINETIME ;
743 743 break;
744 744
745 745 case SID_NORM_CWF_LONG_F3:
746 746 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * T3_IN_FINETIME ;
747 747 break;
748 748
749 749 default:
750 750 PRINTF1("in compute_acquisition_time *** ERR unexpected sid %d\n", sid)
751 751 deltaT = 0.;
752 752 break;
753 753 }
754 754
755 755 acquisitionTimeAsLong = acquisitionTimeAsLong + (unsigned long long int) deltaT;
756 756 //
757 757 acquisitionTime[BYTE_0] = (unsigned char) (acquisitionTimeAsLong >> SHIFT_5_BYTES);
758 758 acquisitionTime[BYTE_1] = (unsigned char) (acquisitionTimeAsLong >> SHIFT_4_BYTES);
759 759 acquisitionTime[BYTE_2] = (unsigned char) (acquisitionTimeAsLong >> SHIFT_3_BYTES);
760 760 acquisitionTime[BYTE_3] = (unsigned char) (acquisitionTimeAsLong >> SHIFT_2_BYTES);
761 761 acquisitionTime[BYTE_4] = (unsigned char) (acquisitionTimeAsLong >> SHIFT_1_BYTE );
762 762 acquisitionTime[BYTE_5] = (unsigned char) (acquisitionTimeAsLong );
763 763
764 764 }
765 765
766 766 void build_snapshot_from_ring( ring_node *ring_node_to_send,
767 767 unsigned char frequencyChannel,
768 768 unsigned long long int acquisitionTimeF0_asLong,
769 769 ring_node *ring_node_swf_extracted,
770 770 int *swf_extracted)
771 771 {
772 772 unsigned int i;
773 unsigned int node;
773 774 unsigned long long int centerTime_asLong;
774 775 unsigned long long int acquisitionTime_asLong;
775 776 unsigned long long int bufferAcquisitionTime_asLong;
776 777 unsigned char *ptr1;
777 778 unsigned char *ptr2;
778 779 unsigned char *timeCharPtr;
779 780 unsigned char nb_ring_nodes;
780 781 unsigned long long int frequency_asLong;
781 782 unsigned long long int nbTicksPerSample_asLong;
782 783 unsigned long long int nbSamplesPart1_asLong;
783 784 unsigned long long int sampleOffset_asLong;
784 785
785 786 unsigned int deltaT_F0;
786 787 unsigned int deltaT_F1;
787 788 unsigned long long int deltaT_F2;
788 789
789 790 deltaT_F0 = DELTAT_F0;
790 791 deltaT_F1 = DELTAF_F1;
791 792 deltaT_F2 = DELTAF_F2;
792 793 sampleOffset_asLong = INIT_CHAR;
793 794
794 795 // (1) get the f0 acquisition time => the value is passed in argument
795 796
796 797 // (2) compute the central reference time
797 798 centerTime_asLong = acquisitionTimeF0_asLong + deltaT_F0;
798 799
799 800 // (3) compute the acquisition time of the current snapshot
800 801 switch(frequencyChannel)
801 802 {
802 803 case CHANNELF1: // 1 is for F1 = 4096 Hz
803 804 acquisitionTime_asLong = centerTime_asLong - deltaT_F1;
804 805 nb_ring_nodes = NB_RING_NODES_F1;
805 806 frequency_asLong = FREQ_F1;
806 807 nbTicksPerSample_asLong = TICKS_PER_T1; // 65536 / 4096;
807 808 break;
808 809 case CHANNELF2: // 2 is for F2 = 256 Hz
809 810 acquisitionTime_asLong = centerTime_asLong - deltaT_F2;
810 811 nb_ring_nodes = NB_RING_NODES_F2;
811 812 frequency_asLong = FREQ_F2;
812 813 nbTicksPerSample_asLong = TICKS_PER_T2; // 65536 / 256;
813 814 break;
814 815 default:
815 816 acquisitionTime_asLong = centerTime_asLong;
817 nb_ring_nodes = 0;
816 818 frequency_asLong = FREQ_F2;
817 819 nbTicksPerSample_asLong = TICKS_PER_T2;
818 820 break;
819 821 }
820 822
821 //****************************************************************************
823 //*****************************************************************************
822 824 // (4) search the ring_node with the acquisition time <= acquisitionTime_asLong
823 for (i=0; i<nb_ring_nodes; i++)
825 node = 0;
826 while ( node < nb_ring_nodes)
824 827 {
825 //PRINTF1("%d ... ", i);
826 bufferAcquisitionTime_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send->coarseTime );
828 //PRINTF1("%d ... ", node);
829 bufferAcquisitionTime_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send->coarseTime );
827 830 if (bufferAcquisitionTime_asLong <= acquisitionTime_asLong)
828 831 {
829 832 //PRINTF1("buffer found with acquisition time = %llx\n", bufferAcquisitionTime_asLong);
830 break;
833 node = nb_ring_nodes;
831 834 }
832 ring_node_to_send = ring_node_to_send->previous;
835 else
836 {
837 node = node + 1;
838 ring_node_to_send = ring_node_to_send->previous;
839 }
833 840 }
834 841
835 842 // (5) compute the number of samples to take in the current buffer
836 843 sampleOffset_asLong = ((acquisitionTime_asLong - bufferAcquisitionTime_asLong) * frequency_asLong ) >> SHIFT_2_BYTES;
837 844 nbSamplesPart1_asLong = NB_SAMPLES_PER_SNAPSHOT - sampleOffset_asLong;
838 845 //PRINTF2("sampleOffset_asLong = %lld, nbSamplesPart1_asLong = %lld\n", sampleOffset_asLong, nbSamplesPart1_asLong);
839 846
840 847 // (6) compute the final acquisition time
841 848 acquisitionTime_asLong = bufferAcquisitionTime_asLong +
842 849 (sampleOffset_asLong * nbTicksPerSample_asLong);
843 850
844 851 // (7) copy the acquisition time at the beginning of the extrated snapshot
845 852 ptr1 = (unsigned char*) &acquisitionTime_asLong;
846 853 // fine time
847 854 ptr2 = (unsigned char*) &ring_node_swf_extracted->fineTime;
848 855 ptr2[BYTE_2] = ptr1[ BYTE_4 + OFFSET_2_BYTES ];
849 856 ptr2[BYTE_3] = ptr1[ BYTE_5 + OFFSET_2_BYTES ];
850 857 // coarse time
851 858 ptr2 = (unsigned char*) &ring_node_swf_extracted->coarseTime;
852 859 ptr2[BYTE_0] = ptr1[ BYTE_0 + OFFSET_2_BYTES ];
853 860 ptr2[BYTE_1] = ptr1[ BYTE_1 + OFFSET_2_BYTES ];
854 861 ptr2[BYTE_2] = ptr1[ BYTE_2 + OFFSET_2_BYTES ];
855 862 ptr2[BYTE_3] = ptr1[ BYTE_3 + OFFSET_2_BYTES ];
856 863
857 864 // re set the synchronization bit
858 865 timeCharPtr = (unsigned char*) &ring_node_to_send->coarseTime;
859 866 ptr2[0] = ptr2[0] | (timeCharPtr[0] & SYNC_BIT); // [1000 0000]
860 867
861 868 if ( (nbSamplesPart1_asLong >= NB_SAMPLES_PER_SNAPSHOT) | (nbSamplesPart1_asLong < 0) )
862 869 {
863 870 nbSamplesPart1_asLong = 0;
864 871 }
865 872 // copy the part 1 of the snapshot in the extracted buffer
866 873 for ( i = 0; i < (nbSamplesPart1_asLong * NB_WORDS_SWF_BLK); i++ )
867 874 {
868 875 swf_extracted[i] =
869 876 ((int*) ring_node_to_send->buffer_address)[ i + (sampleOffset_asLong * NB_WORDS_SWF_BLK) ];
870 877 }
871 878 // copy the part 2 of the snapshot in the extracted buffer
872 879 ring_node_to_send = ring_node_to_send->next;
873 880 for ( i = (nbSamplesPart1_asLong * NB_WORDS_SWF_BLK); i < (NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK); i++ )
874 881 {
875 882 swf_extracted[i] =
876 883 ((int*) ring_node_to_send->buffer_address)[ (i-(nbSamplesPart1_asLong * NB_WORDS_SWF_BLK)) ];
877 884 }
878 885 }
879 886
880 887 double computeCorrection( unsigned char *timePtr )
881 888 {
882 889 unsigned long long int acquisitionTime;
883 890 unsigned long long int centerTime;
884 891 unsigned long long int previousTick;
885 892 unsigned long long int nextTick;
886 893 unsigned long long int deltaPreviousTick;
887 894 unsigned long long int deltaNextTick;
888 895 double deltaPrevious_ms;
889 896 double deltaNext_ms;
890 897 double correctionInF2;
891 898
892 899 // get acquisition time in fine time ticks
893 900 acquisitionTime = get_acquisition_time( timePtr );
894 901
895 902 // compute center time
896 903 centerTime = acquisitionTime + DELTAT_F0; // (2048. / 24576. / 2.) * 65536. = 2730.667;
897 904 previousTick = centerTime - (centerTime & INT16_ALL_F);
898 905 nextTick = previousTick + TICKS_PER_S;
899 906
900 907 deltaPreviousTick = centerTime - previousTick;
901 908 deltaNextTick = nextTick - centerTime;
902 909
903 910 deltaPrevious_ms = (((double) deltaPreviousTick) / TICKS_PER_S) * MS_PER_S;
904 911 deltaNext_ms = (((double) deltaNextTick) / TICKS_PER_S) * MS_PER_S;
905 912
906 913 PRINTF2(" delta previous = %.3f ms, delta next = %.2f ms\n", deltaPrevious_ms, deltaNext_ms);
907 914
908 915 // which tick is the closest?
909 916 if (deltaPreviousTick > deltaNextTick)
910 917 {
911 918 // the snapshot center is just before the second => increase delta_snapshot
912 919 correctionInF2 = + (deltaNext_ms * FREQ_F2 / MS_PER_S );
913 920 }
914 921 else
915 922 {
916 923 // the snapshot center is just after the second => decrease delta_snapshot
917 924 correctionInF2 = - (deltaPrevious_ms * FREQ_F2 / MS_PER_S );
918 925 }
919 926
920 927 PRINTF1(" correctionInF2 = %.2f\n", correctionInF2);
921 928
922 929 return correctionInF2;
923 930 }
924 931
925 932 void applyCorrection( double correction )
926 933 {
927 934 int correctionInt;
928 935
929 936 if (correction >= 0.)
930 937 {
931 938 if ( (ONE_TICK_CORR_INTERVAL_0_MIN < correction) && (correction < ONE_TICK_CORR_INTERVAL_0_MAX) )
932 939 {
933 940 correctionInt = ONE_TICK_CORR;
934 941 }
935 942 else
936 943 {
937 944 correctionInt = CORR_MULT * floor(correction);
938 945 }
939 946 }
940 947 else
941 948 {
942 949 if ( (ONE_TICK_CORR_INTERVAL_1_MIN < correction) && (correction < ONE_TICK_CORR_INTERVAL_1_MAX) )
943 950 {
944 951 correctionInt = -ONE_TICK_CORR;
945 952 }
946 953 else
947 954 {
948 955 correctionInt = CORR_MULT * ceil(correction);
949 956 }
950 957 }
951 958 waveform_picker_regs->delta_snapshot = waveform_picker_regs->delta_snapshot + correctionInt;
952 959 }
953 960
954 961 void snapshot_resynchronization( unsigned char *timePtr )
955 962 {
956 963 /** This function compute a correction to apply on delta_snapshot.
957 964 *
958 965 *
959 966 * @param timePtr is a pointer to the acquisition time of the snapshot being considered.
960 967 *
961 968 * @return void
962 969 *
963 970 */
964 971
965 972 static double correction = INIT_FLOAT;
966 973 static resynchro_state state = MEASURE;
967 974 static unsigned int nbSnapshots = 0;
968 975
969 976 int correctionInt;
970 977
971 978 correctionInt = 0;
972 979
973 980 switch (state)
974 981 {
975 982
976 983 case MEASURE:
977 984 // ********
978 985 PRINTF1("MEASURE === %d\n", nbSnapshots);
979 986 state = CORRECTION;
980 987 correction = computeCorrection( timePtr );
981 988 PRINTF1("MEASURE === correction = %.2f\n", correction );
982 989 applyCorrection( correction );
983 990 PRINTF1("MEASURE === delta_snapshot = %d\n", waveform_picker_regs->delta_snapshot);
984 991 //****
985 992 break;
986 993
987 994 case CORRECTION:
988 995 //************
989 996 PRINTF1("CORRECTION === %d\n", nbSnapshots);
990 997 state = MEASURE;
991 998 computeCorrection( timePtr );
992 999 set_wfp_delta_snapshot();
993 1000 PRINTF1("CORRECTION === delta_snapshot = %d\n", waveform_picker_regs->delta_snapshot);
994 1001 //****
995 1002 break;
996 1003
997 1004 default:
998 1005 break;
999 1006
1000 1007 }
1001 1008
1002 1009 nbSnapshots++;
1003 1010 }
1004 1011
1005 1012 //**************
1006 1013 // wfp registers
1007 1014 void reset_wfp_burst_enable( void )
1008 1015 {
1009 1016 /** This function resets the waveform picker burst_enable register.
1010 1017 *
1011 1018 * The burst bits [f2 f1 f0] and the enable bits [f3 f2 f1 f0] are set to 0.
1012 1019 *
1013 1020 */
1014 1021
1015 1022 // [1000 000] burst f2, f1, f0 enable f3, f2, f1, f0
1016 1023 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable & RST_BITS_RUN_BURST_EN;
1017 1024 }
1018 1025
1019 1026 void reset_wfp_status( void )
1020 1027 {
1021 1028 /** This function resets the waveform picker status register.
1022 1029 *
1023 1030 * All status bits are set to 0 [new_err full_err full].
1024 1031 *
1025 1032 */
1026 1033
1027 1034 waveform_picker_regs->status = INT16_ALL_F;
1028 1035 }
1029 1036
1030 1037 void reset_wfp_buffer_addresses( void )
1031 1038 {
1032 1039 // F0
1033 1040 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->previous->buffer_address; // 0x08
1034 1041 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address; // 0x0c
1035 1042 // F1
1036 1043 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->previous->buffer_address; // 0x10
1037 1044 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address; // 0x14
1038 1045 // F2
1039 1046 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->previous->buffer_address; // 0x18
1040 1047 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address; // 0x1c
1041 1048 // F3
1042 1049 waveform_picker_regs->addr_data_f3_0 = current_ring_node_f3->previous->buffer_address; // 0x20
1043 1050 waveform_picker_regs->addr_data_f3_1 = current_ring_node_f3->buffer_address; // 0x24
1044 1051 }
1045 1052
1046 1053 void reset_waveform_picker_regs( void )
1047 1054 {
1048 1055 /** This function resets the waveform picker module registers.
1049 1056 *
1050 1057 * The registers affected by this function are located at the following offset addresses:
1051 1058 * - 0x00 data_shaping
1052 1059 * - 0x04 run_burst_enable
1053 1060 * - 0x08 addr_data_f0
1054 1061 * - 0x0C addr_data_f1
1055 1062 * - 0x10 addr_data_f2
1056 1063 * - 0x14 addr_data_f3
1057 1064 * - 0x18 status
1058 1065 * - 0x1C delta_snapshot
1059 1066 * - 0x20 delta_f0
1060 1067 * - 0x24 delta_f0_2
1061 1068 * - 0x28 delta_f1 (obsolet parameter)
1062 1069 * - 0x2c delta_f2
1063 1070 * - 0x30 nb_data_by_buffer
1064 1071 * - 0x34 nb_snapshot_param
1065 1072 * - 0x38 start_date
1066 1073 * - 0x3c nb_word_in_buffer
1067 1074 *
1068 1075 */
1069 1076
1070 1077 set_wfp_data_shaping(); // 0x00 *** R1 R0 SP1 SP0 BW
1071 1078
1072 1079 reset_wfp_burst_enable(); // 0x04 *** [run *** burst f2, f1, f0 *** enable f3, f2, f1, f0 ]
1073 1080
1074 1081 reset_wfp_buffer_addresses();
1075 1082
1076 1083 reset_wfp_status(); // 0x18
1077 1084
1078 1085 set_wfp_delta_snapshot(); // 0x1c *** 300 s => 0x12bff
1079 1086
1080 1087 set_wfp_delta_f0_f0_2(); // 0x20, 0x24
1081 1088
1082 1089 //the parameter delta_f1 [0x28] is not used anymore
1083 1090
1084 1091 set_wfp_delta_f2(); // 0x2c
1085 1092
1086 1093 DEBUG_PRINTF1("delta_snapshot %x\n", waveform_picker_regs->delta_snapshot);
1087 1094 DEBUG_PRINTF1("delta_f0 %x\n", waveform_picker_regs->delta_f0);
1088 1095 DEBUG_PRINTF1("delta_f0_2 %x\n", waveform_picker_regs->delta_f0_2);
1089 1096 DEBUG_PRINTF1("delta_f1 %x\n", waveform_picker_regs->delta_f1);
1090 1097 DEBUG_PRINTF1("delta_f2 %x\n", waveform_picker_regs->delta_f2);
1091 1098 // 2688 = 8 * 336
1092 1099 waveform_picker_regs->nb_data_by_buffer = DFLT_WFP_NB_DATA_BY_BUFFER; // 0x30 *** 2688 - 1 => nb samples -1
1093 1100 waveform_picker_regs->snapshot_param = DFLT_WFP_SNAPSHOT_PARAM; // 0x34 *** 2688 => nb samples
1094 1101 waveform_picker_regs->start_date = COARSE_TIME_MASK;
1095 1102 //
1096 1103 // coarse time and fine time registers are not initialized, they are volatile
1097 1104 //
1098 1105 waveform_picker_regs->buffer_length = DFLT_WFP_BUFFER_LENGTH; // buffer length in burst = 3 * 2688 / 16 = 504 = 0x1f8
1099 1106 }
1100 1107
1101 1108 void set_wfp_data_shaping( void )
1102 1109 {
1103 1110 /** This function sets the data_shaping register of the waveform picker module.
1104 1111 *
1105 1112 * The value is read from one field of the parameter_dump_packet structure:\n
1106 1113 * bw_sp0_sp1_r0_r1
1107 1114 *
1108 1115 */
1109 1116
1110 1117 unsigned char data_shaping;
1111 1118
1112 1119 // get the parameters for the data shaping [BW SP0 SP1 R0 R1] in sy_lfr_common1 and configure the register
1113 1120 // waveform picker : [R1 R0 SP1 SP0 BW]
1114 1121
1115 1122 data_shaping = parameter_dump_packet.sy_lfr_common_parameters;
1116 1123
1117 1124 waveform_picker_regs->data_shaping =
1118 1125 ( (data_shaping & BIT_5) >> SHIFT_5_BITS ) // BW
1119 1126 + ( (data_shaping & BIT_4) >> SHIFT_3_BITS ) // SP0
1120 1127 + ( (data_shaping & BIT_3) >> 1 ) // SP1
1121 1128 + ( (data_shaping & BIT_2) << 1 ) // R0
1122 1129 + ( (data_shaping & BIT_1) << SHIFT_3_BITS ) // R1
1123 1130 + ( (data_shaping & BIT_0) << SHIFT_5_BITS ); // R2
1124 1131 }
1125 1132
1126 1133 void set_wfp_burst_enable_register( unsigned char mode )
1127 1134 {
1128 1135 /** This function sets the waveform picker burst_enable register depending on the mode.
1129 1136 *
1130 1137 * @param mode is the LFR mode to launch.
1131 1138 *
1132 1139 * The burst bits shall be before the enable bits.
1133 1140 *
1134 1141 */
1135 1142
1136 1143 // [0000 0000] burst f2, f1, f0 enable f3 f2 f1 f0
1137 1144 // the burst bits shall be set first, before the enable bits
1138 1145 switch(mode) {
1139 1146 case LFR_MODE_NORMAL:
1140 1147 case LFR_MODE_SBM1:
1141 1148 case LFR_MODE_SBM2:
1142 1149 waveform_picker_regs->run_burst_enable = RUN_BURST_ENABLE_SBM2; // [0110 0000] enable f2 and f1 burst
1143 1150 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x0f; // [1111] enable f3 f2 f1 f0
1144 1151 break;
1145 1152 case LFR_MODE_BURST:
1146 1153 waveform_picker_regs->run_burst_enable = RUN_BURST_ENABLE_BURST; // [0100 0000] f2 burst enabled
1147 1154 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x0c; // [1100] enable f3 and f2
1148 1155 break;
1149 1156 default:
1150 1157 waveform_picker_regs->run_burst_enable = INIT_CHAR; // [0000 0000] no burst enabled, no waveform enabled
1151 1158 break;
1152 1159 }
1153 1160 }
1154 1161
1155 1162 void set_wfp_delta_snapshot( void )
1156 1163 {
1157 1164 /** This function sets the delta_snapshot register of the waveform picker module.
1158 1165 *
1159 1166 * The value is read from two (unsigned char) of the parameter_dump_packet structure:
1160 1167 * - sy_lfr_n_swf_p[0]
1161 1168 * - sy_lfr_n_swf_p[1]
1162 1169 *
1163 1170 */
1164 1171
1165 1172 unsigned int delta_snapshot;
1166 1173 unsigned int delta_snapshot_in_T2;
1167 1174
1168 1175 delta_snapshot = (parameter_dump_packet.sy_lfr_n_swf_p[0] * CONST_256)
1169 1176 + parameter_dump_packet.sy_lfr_n_swf_p[1];
1170 1177
1171 1178 delta_snapshot_in_T2 = delta_snapshot * FREQ_F2;
1172 1179 waveform_picker_regs->delta_snapshot = delta_snapshot_in_T2 - 1; // max 4 bytes
1173 1180 }
1174 1181
1175 1182 void set_wfp_delta_f0_f0_2( void )
1176 1183 {
1177 1184 unsigned int delta_snapshot;
1178 1185 unsigned int nb_samples_per_snapshot;
1179 1186 float delta_f0_in_float;
1180 1187
1181 1188 delta_snapshot = waveform_picker_regs->delta_snapshot;
1182 1189 nb_samples_per_snapshot = (parameter_dump_packet.sy_lfr_n_swf_l[0] * CONST_256) + parameter_dump_packet.sy_lfr_n_swf_l[1];
1183 1190 delta_f0_in_float = (nb_samples_per_snapshot / 2.) * ( (1. / FREQ_F2) - (1. / FREQ_F0) ) * FREQ_F2;
1184 1191
1185 1192 waveform_picker_regs->delta_f0 = delta_snapshot - floor( delta_f0_in_float );
1186 1193 waveform_picker_regs->delta_f0_2 = DFLT_WFP_DELTA_F0_2; // 48 = 11 0000, max 7 bits
1187 1194 }
1188 1195
1189 1196 void set_wfp_delta_f1( void )
1190 1197 {
1191 1198 /** Sets the value of the delta_f1 parameter
1192 1199 *
1193 1200 * @param void
1194 1201 *
1195 1202 * @return void
1196 1203 *
1197 1204 * delta_f1 is not used, the snapshots are extracted from CWF_F1 waveforms.
1198 1205 *
1199 1206 */
1200 1207
1201 1208 unsigned int delta_snapshot;
1202 1209 unsigned int nb_samples_per_snapshot;
1203 1210 float delta_f1_in_float;
1204 1211
1205 1212 delta_snapshot = waveform_picker_regs->delta_snapshot;
1206 1213 nb_samples_per_snapshot = (parameter_dump_packet.sy_lfr_n_swf_l[0] * CONST_256) + parameter_dump_packet.sy_lfr_n_swf_l[1];
1207 1214 delta_f1_in_float = (nb_samples_per_snapshot / 2.) * ( (1. / FREQ_F2) - (1. / FREQ_F1) ) * FREQ_F2;
1208 1215
1209 1216 waveform_picker_regs->delta_f1 = delta_snapshot - floor( delta_f1_in_float );
1210 1217 }
1211 1218
1212 1219 void set_wfp_delta_f2( void ) // parameter not used, only delta_f0 and delta_f0_2 are used
1213 1220 {
1214 1221 /** Sets the value of the delta_f2 parameter
1215 1222 *
1216 1223 * @param void
1217 1224 *
1218 1225 * @return void
1219 1226 *
1220 1227 * delta_f2 is used only for the first snapshot generation, even when the snapshots are extracted from CWF_F2
1221 1228 * waveforms (see lpp_waveform_snapshot_controler.vhd for details).
1222 1229 *
1223 1230 */
1224 1231
1225 1232 unsigned int delta_snapshot;
1226 1233 unsigned int nb_samples_per_snapshot;
1227 1234
1228 1235 delta_snapshot = waveform_picker_regs->delta_snapshot;
1229 1236 nb_samples_per_snapshot = (parameter_dump_packet.sy_lfr_n_swf_l[0] * CONST_256) + parameter_dump_packet.sy_lfr_n_swf_l[1];
1230 1237
1231 1238 waveform_picker_regs->delta_f2 = delta_snapshot - (nb_samples_per_snapshot / 2) - 1;
1232 1239 }
1233 1240
1234 1241 //*****************
1235 1242 // local parameters
1236 1243
1237 1244 void increment_seq_counter_source_id( unsigned char *packet_sequence_control, unsigned int sid )
1238 1245 {
1239 1246 /** This function increments the parameter "sequence_cnt" depending on the sid passed in argument.
1240 1247 *
1241 1248 * @param packet_sequence_control is a pointer toward the parameter sequence_cnt to update.
1242 1249 * @param sid is the source identifier of the packet being updated.
1243 1250 *
1244 1251 * REQ-LFR-SRS-5240 / SSS-CP-FS-590
1245 1252 * The sequence counters shall wrap around from 2^14 to zero.
1246 1253 * The sequence counter shall start at zero at startup.
1247 1254 *
1248 1255 * REQ-LFR-SRS-5239 / SSS-CP-FS-580
1249 1256 * All TM_LFR_SCIENCE_ packets are sent to ground, i.e. destination id = 0
1250 1257 *
1251 1258 */
1252 1259
1253 1260 unsigned short *sequence_cnt;
1254 1261 unsigned short segmentation_grouping_flag;
1255 1262 unsigned short new_packet_sequence_control;
1256 1263 rtems_mode initial_mode_set;
1257 1264 rtems_mode current_mode_set;
1258 1265 rtems_status_code status;
1259 1266
1260 1267 //******************************************
1261 1268 // CHANGE THE MODE OF THE CALLING RTEMS TASK
1262 1269 status = rtems_task_mode( RTEMS_NO_PREEMPT, RTEMS_PREEMPT_MASK, &initial_mode_set );
1263 1270
1264 1271 if ( (sid == SID_NORM_SWF_F0) || (sid == SID_NORM_SWF_F1) || (sid == SID_NORM_SWF_F2)
1265 1272 || (sid == SID_NORM_CWF_F3) || (sid == SID_NORM_CWF_LONG_F3)
1266 1273 || (sid == SID_BURST_CWF_F2)
1267 1274 || (sid == SID_NORM_ASM_F0) || (sid == SID_NORM_ASM_F1) || (sid == SID_NORM_ASM_F2)
1268 1275 || (sid == SID_NORM_BP1_F0) || (sid == SID_NORM_BP1_F1) || (sid == SID_NORM_BP1_F2)
1269 1276 || (sid == SID_NORM_BP2_F0) || (sid == SID_NORM_BP2_F1) || (sid == SID_NORM_BP2_F2)
1270 1277 || (sid == SID_BURST_BP1_F0) || (sid == SID_BURST_BP2_F0)
1271 1278 || (sid == SID_BURST_BP1_F1) || (sid == SID_BURST_BP2_F1) )
1272 1279 {
1273 1280 sequence_cnt = (unsigned short *) &sequenceCounters_SCIENCE_NORMAL_BURST;
1274 1281 }
1275 1282 else if ( (sid ==SID_SBM1_CWF_F1) || (sid ==SID_SBM2_CWF_F2)
1276 1283 || (sid == SID_SBM1_BP1_F0) || (sid == SID_SBM1_BP2_F0)
1277 1284 || (sid == SID_SBM2_BP1_F0) || (sid == SID_SBM2_BP2_F0)
1278 1285 || (sid == SID_SBM2_BP1_F1) || (sid == SID_SBM2_BP2_F1) )
1279 1286 {
1280 1287 sequence_cnt = (unsigned short *) &sequenceCounters_SCIENCE_SBM1_SBM2;
1281 1288 }
1282 1289 else
1283 1290 {
1284 1291 sequence_cnt = (unsigned short *) NULL;
1285 1292 PRINTF1("in increment_seq_counter_source_id *** ERR apid_destid %d not known\n", sid)
1286 1293 }
1287 1294
1288 1295 if (sequence_cnt != NULL)
1289 1296 {
1290 1297 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << SHIFT_1_BYTE;
1291 1298 *sequence_cnt = (*sequence_cnt) & SEQ_CNT_MASK;
1292 1299
1293 1300 new_packet_sequence_control = segmentation_grouping_flag | (*sequence_cnt) ;
1294 1301
1295 1302 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> SHIFT_1_BYTE);
1296 1303 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
1297 1304
1298 1305 // increment the sequence counter
1299 1306 if ( *sequence_cnt < SEQ_CNT_MAX)
1300 1307 {
1301 1308 *sequence_cnt = *sequence_cnt + 1;
1302 1309 }
1303 1310 else
1304 1311 {
1305 1312 *sequence_cnt = 0;
1306 1313 }
1307 1314 }
1308 1315
1309 1316 //*************************************
1310 1317 // RESTORE THE MODE OF THE CALLING TASK
1311 1318 status = rtems_task_mode( initial_mode_set, RTEMS_PREEMPT_MASK, &current_mode_set );
1312 1319 }
General Comments 0
You need to be logged in to leave comments. Login now