##// END OF EJS Templates
HG_REPOSITORIES » LPP » INSTRUMENTATION » SOLO_LFR » DEV_PLE
RSS

Clone from http://pc-instru.lpp.polytechnique.fr:5000/DEV_PLE

rev 3.0.0.2
paul -
r205:2bc5b89e7f11 R3
parent child
Show More
Show file before | Show file after | Hide comments
@@ -1,869 +1,869
1 1 /** This is the RTEMS initialization module.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * This module contains two very different information:
7 7 * - specific instructions to configure the compilation of the RTEMS executive
8 8 * - functions related to the fligth softwre initialization, especially the INIT RTEMS task
9 9 *
10 10 */
11 11
12 12 //*************************
13 13 // GPL reminder to be added
14 14 //*************************
15 15
16 16 #include <rtems.h>
17 17
18 18 /* configuration information */
19 19
20 20 #define CONFIGURE_INIT
21 21
22 22 #include <bsp.h> /* for device driver prototypes */
23 23
24 24 /* configuration information */
25 25
26 26 #define CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
27 27 #define CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
28 28
29 29 #define CONFIGURE_MAXIMUM_TASKS 20
30 30 #define CONFIGURE_RTEMS_INIT_TASKS_TABLE
31 31 #define CONFIGURE_EXTRA_TASK_STACKS (3 * RTEMS_MINIMUM_STACK_SIZE)
32 32 #define CONFIGURE_LIBIO_MAXIMUM_FILE_DESCRIPTORS 32
33 33 #define CONFIGURE_INIT_TASK_PRIORITY 1 // instead of 100
34 34 #define CONFIGURE_INIT_TASK_MODE (RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT)
35 35 #define CONFIGURE_INIT_TASK_ATTRIBUTES (RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT)
36 36 #define CONFIGURE_MAXIMUM_DRIVERS 16
37 37 #define CONFIGURE_MAXIMUM_PERIODS 5
38 38 #define CONFIGURE_MAXIMUM_TIMERS 5 // STAT (1s), send SWF (0.3s), send CWF3 (1s)
39 39 #define CONFIGURE_MAXIMUM_MESSAGE_QUEUES 5
40 40 #ifdef PRINT_STACK_REPORT
41 41 #define CONFIGURE_STACK_CHECKER_ENABLED
42 42 #endif
43 43
44 44 #include <rtems/confdefs.h>
45 45
46 46 /* If --drvmgr was enabled during the configuration of the RTEMS kernel */
47 47 #ifdef RTEMS_DRVMGR_STARTUP
48 48 #ifdef LEON3
49 49 /* Add Timer and UART Driver */
50 50 #ifdef CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
51 51 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GPTIMER
52 52 #endif
53 53 #ifdef CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
54 54 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_APBUART
55 55 #endif
56 56 #endif
57 57 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GRSPW /* GRSPW Driver */
58 58 #include <drvmgr/drvmgr_confdefs.h>
59 59 #endif
60 60
61 61 #include "fsw_init.h"
62 62 #include "fsw_config.c"
63 63 #include "GscMemoryLPP.hpp"
64 64
65 65 void initCache()
66 66 {
67 67 unsigned int cacheControlRegister;
68 68
69 69 cacheControlRegister = getCacheControlRegister();
70 70 printf("(0) cacheControlRegister = %x\n", cacheControlRegister);
71 71
72 72 resetCacheControlRegister();
73 73
74 74 enableInstructionCache();
75 75 enableDataCache();
76 76 enableInstructionBurstFetch();
77 77
78 78 cacheControlRegister = getCacheControlRegister();
79 79 printf("(1) cacheControlRegister = %x\n", cacheControlRegister);
80 80 }
81 81
82 82 rtems_task Init( rtems_task_argument ignored )
83 83 {
84 84 /** This is the RTEMS INIT taks, it is the first task launched by the system.
85 85 *
86 86 * @param unused is the starting argument of the RTEMS task
87 87 *
88 88 * The INIT task create and run all other RTEMS tasks.
89 89 *
90 90 */
91 91
92 92 //***********
93 93 // INIT CACHE
94 94
95 95 unsigned char *vhdlVersion;
96 96
97 97 reset_lfr();
98 98
99 99 reset_local_time();
100 100
101 101 rtems_cpu_usage_reset();
102 102
103 103 rtems_status_code status;
104 104 rtems_status_code status_spw;
105 105 rtems_isr_entry old_isr_handler;
106 106
107 107 // UART settings
108 108 send_console_outputs_on_apbuart_port();
109 109 set_apbuart_scaler_reload_register(REGS_ADDR_APBUART, APBUART_SCALER_RELOAD_VALUE);
110 110 enable_apbuart_transmitter();
111 111
112 112 DEBUG_PRINTF("\n\n\n\n\nIn INIT *** Now the console is on port COM1\n")
113 113
114 114
115 115 PRINTF("\n\n\n\n\n")
116 116
117 117 initCache();
118 118
119 119 PRINTF("*************************\n")
120 120 PRINTF("** LFR Flight Software **\n")
121 121 PRINTF1("** %d.", SW_VERSION_N1)
122 122 PRINTF1("%d." , SW_VERSION_N2)
123 123 PRINTF1("%d." , SW_VERSION_N3)
124 124 PRINTF1("%d **\n", SW_VERSION_N4)
125 125
126 126 vhdlVersion = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
127 127 PRINTF("** VHDL **\n")
128 128 PRINTF1("** %d.", vhdlVersion[1])
129 129 PRINTF1("%d." , vhdlVersion[2])
130 130 PRINTF1("%d **\n", vhdlVersion[3])
131 131 PRINTF("*************************\n")
132 132 PRINTF("\n\n")
133 133
134 134 init_parameter_dump();
135 135 init_kcoefficients_dump();
136 136 init_local_mode_parameters();
137 137 init_housekeeping_parameters();
138 138 init_k_coefficients_f0();
139 139 init_k_coefficients_f1();
140 140 init_k_coefficients_f2();
141 141
142 142 // waveform picker initialization
143 143 WFP_init_rings(); // initialize the waveform rings
144 144 WFP_reset_current_ring_nodes();
145 145 reset_waveform_picker_regs();
146 146
147 147 // spectral matrices initialization
148 148 SM_init_rings(); // initialize spectral matrices rings
149 149 SM_reset_current_ring_nodes();
150 150 reset_spectral_matrix_regs();
151 151
152 152 // configure calibration
153 153 configureCalibration( false ); // true means interleaved mode, false is for normal mode
154 154
155 155 updateLFRCurrentMode();
156 156
157 157 BOOT_PRINTF1("in INIT *** lfrCurrentMode is %d\n", lfrCurrentMode)
158 158
159 159 create_names(); // create all names
160 160
161 161 status = create_message_queues(); // create message queues
162 162 if (status != RTEMS_SUCCESSFUL)
163 163 {
164 164 PRINTF1("in INIT *** ERR in create_message_queues, code %d", status)
165 165 }
166 166
167 167 status = create_all_tasks(); // create all tasks
168 168 if (status != RTEMS_SUCCESSFUL)
169 169 {
170 170 PRINTF1("in INIT *** ERR in create_all_tasks, code %d\n", status)
171 171 }
172 172
173 173 // **************************
174 174 // <SPACEWIRE INITIALIZATION>
175 175 grspw_timecode_callback = &timecode_irq_handler;
176 176
177 177 status_spw = spacewire_open_link(); // (1) open the link
178 178 if ( status_spw != RTEMS_SUCCESSFUL )
179 179 {
180 180 PRINTF1("in INIT *** ERR spacewire_open_link code %d\n", status_spw )
181 181 }
182 182
183 183 if ( status_spw == RTEMS_SUCCESSFUL ) // (2) configure the link
184 184 {
185 185 status_spw = spacewire_configure_link( fdSPW );
186 186 if ( status_spw != RTEMS_SUCCESSFUL )
187 187 {
188 188 PRINTF1("in INIT *** ERR spacewire_configure_link code %d\n", status_spw )
189 189 }
190 190 }
191 191
192 192 if ( status_spw == RTEMS_SUCCESSFUL) // (3) start the link
193 193 {
194 194 status_spw = spacewire_start_link( fdSPW );
195 195 if ( status_spw != RTEMS_SUCCESSFUL )
196 196 {
197 197 PRINTF1("in INIT *** ERR spacewire_start_link code %d\n", status_spw )
198 198 }
199 199 }
200 200 // </SPACEWIRE INITIALIZATION>
201 201 // ***************************
202 202
203 203 status = start_all_tasks(); // start all tasks
204 204 if (status != RTEMS_SUCCESSFUL)
205 205 {
206 206 PRINTF1("in INIT *** ERR in start_all_tasks, code %d", status)
207 207 }
208 208
209 209 // start RECV and SEND *AFTER* SpaceWire Initialization, due to the timeout of the start call during the initialization
210 210 status = start_recv_send_tasks();
211 211 if ( status != RTEMS_SUCCESSFUL )
212 212 {
213 213 PRINTF1("in INIT *** ERR start_recv_send_tasks code %d\n", status )
214 214 }
215 215
216 216 // suspend science tasks, they will be restarted later depending on the mode
217 217 status = suspend_science_tasks(); // suspend science tasks (not done in stop_current_mode if current mode = STANDBY)
218 218 if (status != RTEMS_SUCCESSFUL)
219 219 {
220 220 PRINTF1("in INIT *** in suspend_science_tasks *** ERR code: %d\n", status)
221 221 }
222 222
223 223 //******************************
224 224 // <SPECTRAL MATRICES SIMULATOR>
225 225 LEON_Mask_interrupt( IRQ_SM_SIMULATOR );
226 226 configure_timer((gptimer_regs_t*) REGS_ADDR_GPTIMER, TIMER_SM_SIMULATOR, CLKDIV_SM_SIMULATOR,
227 227 IRQ_SPARC_SM_SIMULATOR, spectral_matrices_isr_simu );
228 228 // </SPECTRAL MATRICES SIMULATOR>
229 229 //*******************************
230 230
231 231 // configure IRQ handling for the waveform picker unit
232 232 status = rtems_interrupt_catch( waveforms_isr,
233 233 IRQ_SPARC_WAVEFORM_PICKER,
234 234 &old_isr_handler) ;
235 235 // configure IRQ handling for the spectral matrices unit
236 236 status = rtems_interrupt_catch( spectral_matrices_isr,
237 237 IRQ_SPARC_SPECTRAL_MATRIX,
238 238 &old_isr_handler) ;
239 239
240 240 // if the spacewire link is not up then send an event to the SPIQ task for link recovery
241 241 if ( status_spw != RTEMS_SUCCESSFUL )
242 242 {
243 243 status = rtems_event_send( Task_id[TASKID_SPIQ], SPW_LINKERR_EVENT );
244 244 if ( status != RTEMS_SUCCESSFUL ) {
245 245 PRINTF1("in INIT *** ERR rtems_event_send to SPIQ code %d\n", status )
246 246 }
247 247 }
248 248
249 249 BOOT_PRINTF("delete INIT\n")
250 250
251 251 status = rtems_task_delete(RTEMS_SELF);
252 252
253 253 }
254 254
255 255 void init_local_mode_parameters( void )
256 256 {
257 257 /** This function initialize the param_local global variable with default values.
258 258 *
259 259 */
260 260
261 261 unsigned int i;
262 262
263 263 // LOCAL PARAMETERS
264 264
265 265 BOOT_PRINTF1("local_sbm1_nb_cwf_max %d \n", param_local.local_sbm1_nb_cwf_max)
266 266 BOOT_PRINTF1("local_sbm2_nb_cwf_max %d \n", param_local.local_sbm2_nb_cwf_max)
267 267 BOOT_PRINTF1("nb_interrupt_f0_MAX = %d\n", param_local.local_nb_interrupt_f0_MAX)
268 268
269 269 // init sequence counters
270 270
271 271 for(i = 0; i<SEQ_CNT_NB_DEST_ID; i++)
272 272 {
273 273 sequenceCounters_TC_EXE[i] = 0x00;
274 274 }
275 275 sequenceCounters_SCIENCE_NORMAL_BURST = 0x00;
276 276 sequenceCounters_SCIENCE_SBM1_SBM2 = 0x00;
277 277 sequenceCounterHK = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
278 278 sequenceCounterParameterDump = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
279 279 }
280 280
281 281 void reset_local_time( void )
282 282 {
283 283 time_management_regs->ctrl = time_management_regs->ctrl | 0x02; // [0010] software reset, coarse time = 0x80000000
284 284 }
285 285
286 286 void create_names( void ) // create all names for tasks and queues
287 287 {
288 288 /** This function creates all RTEMS names used in the software for tasks and queues.
289 289 *
290 290 * @return RTEMS directive status codes:
291 291 * - RTEMS_SUCCESSFUL - successful completion
292 292 *
293 293 */
294 294
295 295 // task names
296 296 Task_name[TASKID_RECV] = rtems_build_name( 'R', 'E', 'C', 'V' );
297 297 Task_name[TASKID_ACTN] = rtems_build_name( 'A', 'C', 'T', 'N' );
298 298 Task_name[TASKID_SPIQ] = rtems_build_name( 'S', 'P', 'I', 'Q' );
299 299 Task_name[TASKID_STAT] = rtems_build_name( 'S', 'T', 'A', 'T' );
300 300 Task_name[TASKID_AVF0] = rtems_build_name( 'A', 'V', 'F', '0' );
301 301 Task_name[TASKID_SWBD] = rtems_build_name( 'S', 'W', 'B', 'D' );
302 302 Task_name[TASKID_WFRM] = rtems_build_name( 'W', 'F', 'R', 'M' );
303 303 Task_name[TASKID_DUMB] = rtems_build_name( 'D', 'U', 'M', 'B' );
304 304 Task_name[TASKID_HOUS] = rtems_build_name( 'H', 'O', 'U', 'S' );
305 305 Task_name[TASKID_PRC0] = rtems_build_name( 'P', 'R', 'C', '0' );
306 306 Task_name[TASKID_CWF3] = rtems_build_name( 'C', 'W', 'F', '3' );
307 307 Task_name[TASKID_CWF2] = rtems_build_name( 'C', 'W', 'F', '2' );
308 308 Task_name[TASKID_CWF1] = rtems_build_name( 'C', 'W', 'F', '1' );
309 309 Task_name[TASKID_SEND] = rtems_build_name( 'S', 'E', 'N', 'D' );
310 310 Task_name[TASKID_WTDG] = rtems_build_name( 'W', 'T', 'D', 'G' );
311 311 Task_name[TASKID_AVF1] = rtems_build_name( 'A', 'V', 'F', '1' );
312 312 Task_name[TASKID_PRC1] = rtems_build_name( 'P', 'R', 'C', '1' );
313 313 Task_name[TASKID_AVF2] = rtems_build_name( 'A', 'V', 'F', '2' );
314 314 Task_name[TASKID_PRC2] = rtems_build_name( 'P', 'R', 'C', '2' );
315 315
316 316 // rate monotonic period names
317 317 name_hk_rate_monotonic = rtems_build_name( 'H', 'O', 'U', 'S' );
318 318
319 319 misc_name[QUEUE_RECV] = rtems_build_name( 'Q', '_', 'R', 'V' );
320 320 misc_name[QUEUE_SEND] = rtems_build_name( 'Q', '_', 'S', 'D' );
321 321 misc_name[QUEUE_PRC0] = rtems_build_name( 'Q', '_', 'P', '0' );
322 322 misc_name[QUEUE_PRC1] = rtems_build_name( 'Q', '_', 'P', '1' );
323 323 misc_name[QUEUE_PRC2] = rtems_build_name( 'Q', '_', 'P', '2' );
324 324 }
325 325
326 326 int create_all_tasks( void ) // create all tasks which run in the software
327 327 {
328 328 /** This function creates all RTEMS tasks used in the software.
329 329 *
330 330 * @return RTEMS directive status codes:
331 331 * - RTEMS_SUCCESSFUL - task created successfully
332 332 * - RTEMS_INVALID_ADDRESS - id is NULL
333 333 * - RTEMS_INVALID_NAME - invalid task name
334 334 * - RTEMS_INVALID_PRIORITY - invalid task priority
335 335 * - RTEMS_MP_NOT_CONFIGURED - multiprocessing not configured
336 336 * - RTEMS_TOO_MANY - too many tasks created
337 337 * - RTEMS_UNSATISFIED - not enough memory for stack/FP context
338 338 * - RTEMS_TOO_MANY - too many global objects
339 339 *
340 340 */
341 341
342 342 rtems_status_code status;
343 343
344 344 //**********
345 345 // SPACEWIRE
346 346 // RECV
347 347 status = rtems_task_create(
348 348 Task_name[TASKID_RECV], TASK_PRIORITY_RECV, RTEMS_MINIMUM_STACK_SIZE,
349 349 RTEMS_DEFAULT_MODES,
350 350 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_RECV]
351 351 );
352 352 if (status == RTEMS_SUCCESSFUL) // SEND
353 353 {
354 354 status = rtems_task_create(
355 355 Task_name[TASKID_SEND], TASK_PRIORITY_SEND, RTEMS_MINIMUM_STACK_SIZE * 2,
356 356 RTEMS_DEFAULT_MODES,
357 357 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SEND]
358 358 );
359 359 }
360 360 if (status == RTEMS_SUCCESSFUL) // WTDG
361 361 {
362 362 status = rtems_task_create(
363 363 Task_name[TASKID_WTDG], TASK_PRIORITY_WTDG, RTEMS_MINIMUM_STACK_SIZE,
364 364 RTEMS_DEFAULT_MODES,
365 365 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_WTDG]
366 366 );
367 367 }
368 368 if (status == RTEMS_SUCCESSFUL) // ACTN
369 369 {
370 370 status = rtems_task_create(
371 371 Task_name[TASKID_ACTN], TASK_PRIORITY_ACTN, RTEMS_MINIMUM_STACK_SIZE,
372 372 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
373 373 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_ACTN]
374 374 );
375 375 }
376 376 if (status == RTEMS_SUCCESSFUL) // SPIQ
377 377 {
378 378 status = rtems_task_create(
379 379 Task_name[TASKID_SPIQ], TASK_PRIORITY_SPIQ, RTEMS_MINIMUM_STACK_SIZE,
380 380 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
381 381 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_SPIQ]
382 382 );
383 383 }
384 384
385 385 //******************
386 386 // SPECTRAL MATRICES
387 387 if (status == RTEMS_SUCCESSFUL) // AVF0
388 388 {
389 389 status = rtems_task_create(
390 390 Task_name[TASKID_AVF0], TASK_PRIORITY_AVF0, RTEMS_MINIMUM_STACK_SIZE,
391 391 RTEMS_DEFAULT_MODES,
392 392 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF0]
393 393 );
394 394 }
395 395 if (status == RTEMS_SUCCESSFUL) // PRC0
396 396 {
397 397 status = rtems_task_create(
398 398 Task_name[TASKID_PRC0], TASK_PRIORITY_PRC0, RTEMS_MINIMUM_STACK_SIZE * 2,
399 399 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
400 400 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC0]
401 401 );
402 402 }
403 403 if (status == RTEMS_SUCCESSFUL) // AVF1
404 404 {
405 405 status = rtems_task_create(
406 406 Task_name[TASKID_AVF1], TASK_PRIORITY_AVF1, RTEMS_MINIMUM_STACK_SIZE,
407 407 RTEMS_DEFAULT_MODES,
408 408 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF1]
409 409 );
410 410 }
411 411 if (status == RTEMS_SUCCESSFUL) // PRC1
412 412 {
413 413 status = rtems_task_create(
414 414 Task_name[TASKID_PRC1], TASK_PRIORITY_PRC1, RTEMS_MINIMUM_STACK_SIZE * 2,
415 415 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
416 416 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC1]
417 417 );
418 418 }
419 419 if (status == RTEMS_SUCCESSFUL) // AVF2
420 420 {
421 421 status = rtems_task_create(
422 422 Task_name[TASKID_AVF2], TASK_PRIORITY_AVF2, RTEMS_MINIMUM_STACK_SIZE,
423 423 RTEMS_DEFAULT_MODES,
424 424 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF2]
425 425 );
426 426 }
427 427 if (status == RTEMS_SUCCESSFUL) // PRC2
428 428 {
429 429 status = rtems_task_create(
430 430 Task_name[TASKID_PRC2], TASK_PRIORITY_PRC2, RTEMS_MINIMUM_STACK_SIZE * 2,
431 431 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
432 432 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC2]
433 433 );
434 434 }
435 435
436 436 //****************
437 437 // WAVEFORM PICKER
438 438 if (status == RTEMS_SUCCESSFUL) // WFRM
439 439 {
440 440 status = rtems_task_create(
441 441 Task_name[TASKID_WFRM], TASK_PRIORITY_WFRM, RTEMS_MINIMUM_STACK_SIZE,
442 442 RTEMS_DEFAULT_MODES,
443 443 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_WFRM]
444 444 );
445 445 }
446 446 if (status == RTEMS_SUCCESSFUL) // CWF3
447 447 {
448 448 status = rtems_task_create(
449 449 Task_name[TASKID_CWF3], TASK_PRIORITY_CWF3, RTEMS_MINIMUM_STACK_SIZE,
450 450 RTEMS_DEFAULT_MODES,
451 451 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF3]
452 452 );
453 453 }
454 454 if (status == RTEMS_SUCCESSFUL) // CWF2
455 455 {
456 456 status = rtems_task_create(
457 457 Task_name[TASKID_CWF2], TASK_PRIORITY_CWF2, RTEMS_MINIMUM_STACK_SIZE,
458 458 RTEMS_DEFAULT_MODES,
459 459 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF2]
460 460 );
461 461 }
462 462 if (status == RTEMS_SUCCESSFUL) // CWF1
463 463 {
464 464 status = rtems_task_create(
465 465 Task_name[TASKID_CWF1], TASK_PRIORITY_CWF1, RTEMS_MINIMUM_STACK_SIZE,
466 466 RTEMS_DEFAULT_MODES,
467 467 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF1]
468 468 );
469 469 }
470 470 if (status == RTEMS_SUCCESSFUL) // SWBD
471 471 {
472 472 status = rtems_task_create(
473 473 Task_name[TASKID_SWBD], TASK_PRIORITY_SWBD, RTEMS_MINIMUM_STACK_SIZE,
474 474 RTEMS_DEFAULT_MODES,
475 475 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SWBD]
476 476 );
477 477 }
478 478
479 479 //*****
480 480 // MISC
481 481 if (status == RTEMS_SUCCESSFUL) // STAT
482 482 {
483 483 status = rtems_task_create(
484 484 Task_name[TASKID_STAT], TASK_PRIORITY_STAT, RTEMS_MINIMUM_STACK_SIZE,
485 485 RTEMS_DEFAULT_MODES,
486 486 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_STAT]
487 487 );
488 488 }
489 489 if (status == RTEMS_SUCCESSFUL) // DUMB
490 490 {
491 491 status = rtems_task_create(
492 492 Task_name[TASKID_DUMB], TASK_PRIORITY_DUMB, RTEMS_MINIMUM_STACK_SIZE,
493 493 RTEMS_DEFAULT_MODES,
494 494 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_DUMB]
495 495 );
496 496 }
497 497 if (status == RTEMS_SUCCESSFUL) // HOUS
498 498 {
499 499 status = rtems_task_create(
500 500 Task_name[TASKID_HOUS], TASK_PRIORITY_HOUS, RTEMS_MINIMUM_STACK_SIZE,
501 501 RTEMS_DEFAULT_MODES,
502 502 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_HOUS]
503 503 );
504 504 }
505 505
506 506 return status;
507 507 }
508 508
509 509 int start_recv_send_tasks( void )
510 510 {
511 511 rtems_status_code status;
512 512
513 513 status = rtems_task_start( Task_id[TASKID_RECV], recv_task, 1 );
514 514 if (status!=RTEMS_SUCCESSFUL) {
515 515 BOOT_PRINTF("in INIT *** Error starting TASK_RECV\n")
516 516 }
517 517
518 518 if (status == RTEMS_SUCCESSFUL) // SEND
519 519 {
520 520 status = rtems_task_start( Task_id[TASKID_SEND], send_task, 1 );
521 521 if (status!=RTEMS_SUCCESSFUL) {
522 522 BOOT_PRINTF("in INIT *** Error starting TASK_SEND\n")
523 523 }
524 524 }
525 525
526 526 return status;
527 527 }
528 528
529 529 int start_all_tasks( void ) // start all tasks except SEND RECV and HOUS
530 530 {
531 531 /** This function starts all RTEMS tasks used in the software.
532 532 *
533 533 * @return RTEMS directive status codes:
534 534 * - RTEMS_SUCCESSFUL - ask started successfully
535 535 * - RTEMS_INVALID_ADDRESS - invalid task entry point
536 536 * - RTEMS_INVALID_ID - invalid task id
537 537 * - RTEMS_INCORRECT_STATE - task not in the dormant state
538 538 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot start remote task
539 539 *
540 540 */
541 541 // starts all the tasks fot eh flight software
542 542
543 543 rtems_status_code status;
544 544
545 545 //**********
546 546 // SPACEWIRE
547 547 status = rtems_task_start( Task_id[TASKID_SPIQ], spiq_task, 1 );
548 548 if (status!=RTEMS_SUCCESSFUL) {
549 549 BOOT_PRINTF("in INIT *** Error starting TASK_SPIQ\n")
550 550 }
551 551
552 552 if (status == RTEMS_SUCCESSFUL) // WTDG
553 553 {
554 554 status = rtems_task_start( Task_id[TASKID_WTDG], wtdg_task, 1 );
555 555 if (status!=RTEMS_SUCCESSFUL) {
556 556 BOOT_PRINTF("in INIT *** Error starting TASK_WTDG\n")
557 557 }
558 558 }
559 559
560 560 if (status == RTEMS_SUCCESSFUL) // ACTN
561 561 {
562 562 status = rtems_task_start( Task_id[TASKID_ACTN], actn_task, 1 );
563 563 if (status!=RTEMS_SUCCESSFUL) {
564 564 BOOT_PRINTF("in INIT *** Error starting TASK_ACTN\n")
565 565 }
566 566 }
567 567
568 568 //******************
569 569 // SPECTRAL MATRICES
570 570 if (status == RTEMS_SUCCESSFUL) // AVF0
571 571 {
572 572 status = rtems_task_start( Task_id[TASKID_AVF0], avf0_task, LFR_MODE_STANDBY );
573 573 if (status!=RTEMS_SUCCESSFUL) {
574 574 BOOT_PRINTF("in INIT *** Error starting TASK_AVF0\n")
575 575 }
576 576 }
577 577 if (status == RTEMS_SUCCESSFUL) // PRC0
578 578 {
579 579 status = rtems_task_start( Task_id[TASKID_PRC0], prc0_task, LFR_MODE_STANDBY );
580 580 if (status!=RTEMS_SUCCESSFUL) {
581 581 BOOT_PRINTF("in INIT *** Error starting TASK_PRC0\n")
582 582 }
583 583 }
584 584 if (status == RTEMS_SUCCESSFUL) // AVF1
585 585 {
586 586 status = rtems_task_start( Task_id[TASKID_AVF1], avf1_task, LFR_MODE_STANDBY );
587 587 if (status!=RTEMS_SUCCESSFUL) {
588 588 BOOT_PRINTF("in INIT *** Error starting TASK_AVF1\n")
589 589 }
590 590 }
591 591 if (status == RTEMS_SUCCESSFUL) // PRC1
592 592 {
593 593 status = rtems_task_start( Task_id[TASKID_PRC1], prc1_task, LFR_MODE_STANDBY );
594 594 if (status!=RTEMS_SUCCESSFUL) {
595 595 BOOT_PRINTF("in INIT *** Error starting TASK_PRC1\n")
596 596 }
597 597 }
598 598 if (status == RTEMS_SUCCESSFUL) // AVF2
599 599 {
600 600 status = rtems_task_start( Task_id[TASKID_AVF2], avf2_task, 1 );
601 601 if (status!=RTEMS_SUCCESSFUL) {
602 602 BOOT_PRINTF("in INIT *** Error starting TASK_AVF2\n")
603 603 }
604 604 }
605 605 if (status == RTEMS_SUCCESSFUL) // PRC2
606 606 {
607 607 status = rtems_task_start( Task_id[TASKID_PRC2], prc2_task, 1 );
608 608 if (status!=RTEMS_SUCCESSFUL) {
609 609 BOOT_PRINTF("in INIT *** Error starting TASK_PRC2\n")
610 610 }
611 611 }
612 612
613 613 //****************
614 614 // WAVEFORM PICKER
615 615 if (status == RTEMS_SUCCESSFUL) // WFRM
616 616 {
617 617 status = rtems_task_start( Task_id[TASKID_WFRM], wfrm_task, 1 );
618 618 if (status!=RTEMS_SUCCESSFUL) {
619 619 BOOT_PRINTF("in INIT *** Error starting TASK_WFRM\n")
620 620 }
621 621 }
622 622 if (status == RTEMS_SUCCESSFUL) // CWF3
623 623 {
624 624 status = rtems_task_start( Task_id[TASKID_CWF3], cwf3_task, 1 );
625 625 if (status!=RTEMS_SUCCESSFUL) {
626 626 BOOT_PRINTF("in INIT *** Error starting TASK_CWF3\n")
627 627 }
628 628 }
629 629 if (status == RTEMS_SUCCESSFUL) // CWF2
630 630 {
631 631 status = rtems_task_start( Task_id[TASKID_CWF2], cwf2_task, 1 );
632 632 if (status!=RTEMS_SUCCESSFUL) {
633 633 BOOT_PRINTF("in INIT *** Error starting TASK_CWF2\n")
634 634 }
635 635 }
636 636 if (status == RTEMS_SUCCESSFUL) // CWF1
637 637 {
638 638 status = rtems_task_start( Task_id[TASKID_CWF1], cwf1_task, 1 );
639 639 if (status!=RTEMS_SUCCESSFUL) {
640 640 BOOT_PRINTF("in INIT *** Error starting TASK_CWF1\n")
641 641 }
642 642 }
643 643 if (status == RTEMS_SUCCESSFUL) // SWBD
644 644 {
645 645 status = rtems_task_start( Task_id[TASKID_SWBD], swbd_task, 1 );
646 646 if (status!=RTEMS_SUCCESSFUL) {
647 647 BOOT_PRINTF("in INIT *** Error starting TASK_SWBD\n")
648 648 }
649 649 }
650 650
651 651 //*****
652 652 // MISC
653 653 if (status == RTEMS_SUCCESSFUL) // HOUS
654 654 {
655 655 status = rtems_task_start( Task_id[TASKID_HOUS], hous_task, 1 );
656 656 if (status!=RTEMS_SUCCESSFUL) {
657 657 BOOT_PRINTF("in INIT *** Error starting TASK_HOUS\n")
658 658 }
659 659 }
660 660 if (status == RTEMS_SUCCESSFUL) // DUMB
661 661 {
662 662 status = rtems_task_start( Task_id[TASKID_DUMB], dumb_task, 1 );
663 663 if (status!=RTEMS_SUCCESSFUL) {
664 664 BOOT_PRINTF("in INIT *** Error starting TASK_DUMB\n")
665 665 }
666 666 }
667 667 if (status == RTEMS_SUCCESSFUL) // STAT
668 668 {
669 669 status = rtems_task_start( Task_id[TASKID_STAT], stat_task, 1 );
670 670 if (status!=RTEMS_SUCCESSFUL) {
671 671 BOOT_PRINTF("in INIT *** Error starting TASK_STAT\n")
672 672 }
673 673 }
674 674
675 675 return status;
676 676 }
677 677
678 678 rtems_status_code create_message_queues( void ) // create the two message queues used in the software
679 679 {
680 680 rtems_status_code status_recv;
681 681 rtems_status_code status_send;
682 682 rtems_status_code status_q_p0;
683 683 rtems_status_code status_q_p1;
684 684 rtems_status_code status_q_p2;
685 685 rtems_status_code ret;
686 686 rtems_id queue_id;
687 687
688 688 //****************************************
689 689 // create the queue for handling valid TCs
690 690 status_recv = rtems_message_queue_create( misc_name[QUEUE_RECV],
691 691 MSG_QUEUE_COUNT_RECV, CCSDS_TC_PKT_MAX_SIZE,
692 692 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
693 693 if ( status_recv != RTEMS_SUCCESSFUL ) {
694 694 PRINTF1("in create_message_queues *** ERR creating QUEU queue, %d\n", status_recv)
695 695 }
696 696
697 697 //************************************************
698 698 // create the queue for handling TM packet sending
699 699 status_send = rtems_message_queue_create( misc_name[QUEUE_SEND],
700 700 MSG_QUEUE_COUNT_SEND, MSG_QUEUE_SIZE_SEND,
701 701 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
702 702 if ( status_send != RTEMS_SUCCESSFUL ) {
703 703 PRINTF1("in create_message_queues *** ERR creating PKTS queue, %d\n", status_send)
704 704 }
705 705
706 706 //*****************************************************************************
707 707 // create the queue for handling averaged spectral matrices for processing @ f0
708 708 status_q_p0 = rtems_message_queue_create( misc_name[QUEUE_PRC0],
709 709 MSG_QUEUE_COUNT_PRC0, MSG_QUEUE_SIZE_PRC0,
710 710 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
711 711 if ( status_q_p0 != RTEMS_SUCCESSFUL ) {
712 712 PRINTF1("in create_message_queues *** ERR creating Q_P0 queue, %d\n", status_q_p0)
713 713 }
714 714
715 715 //*****************************************************************************
716 716 // create the queue for handling averaged spectral matrices for processing @ f1
717 717 status_q_p1 = rtems_message_queue_create( misc_name[QUEUE_PRC1],
718 718 MSG_QUEUE_COUNT_PRC1, MSG_QUEUE_SIZE_PRC1,
719 719 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
720 720 if ( status_q_p1 != RTEMS_SUCCESSFUL ) {
721 721 PRINTF1("in create_message_queues *** ERR creating Q_P1 queue, %d\n", status_q_p1)
722 722 }
723 723
724 724 //*****************************************************************************
725 725 // create the queue for handling averaged spectral matrices for processing @ f2
726 726 status_q_p2 = rtems_message_queue_create( misc_name[QUEUE_PRC2],
727 727 MSG_QUEUE_COUNT_PRC2, MSG_QUEUE_SIZE_PRC2,
728 728 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
729 729 if ( status_q_p2 != RTEMS_SUCCESSFUL ) {
730 730 PRINTF1("in create_message_queues *** ERR creating Q_P2 queue, %d\n", status_q_p2)
731 731 }
732 732
733 733 if ( status_recv != RTEMS_SUCCESSFUL )
734 734 {
735 735 ret = status_recv;
736 736 }
737 737 else if( status_send != RTEMS_SUCCESSFUL )
738 738 {
739 739 ret = status_send;
740 740 }
741 741 else if( status_q_p0 != RTEMS_SUCCESSFUL )
742 742 {
743 743 ret = status_q_p0;
744 744 }
745 745 else if( status_q_p1 != RTEMS_SUCCESSFUL )
746 746 {
747 747 ret = status_q_p1;
748 748 }
749 749 else
750 750 {
751 751 ret = status_q_p2;
752 752 }
753 753
754 754 return ret;
755 755 }
756 756
757 757 rtems_status_code get_message_queue_id_send( rtems_id *queue_id )
758 758 {
759 759 rtems_status_code status;
760 760 rtems_name queue_name;
761 761
762 762 queue_name = rtems_build_name( 'Q', '_', 'S', 'D' );
763 763
764 764 status = rtems_message_queue_ident( queue_name, 0, queue_id );
765 765
766 766 return status;
767 767 }
768 768
769 769 rtems_status_code get_message_queue_id_recv( rtems_id *queue_id )
770 770 {
771 771 rtems_status_code status;
772 772 rtems_name queue_name;
773 773
774 774 queue_name = rtems_build_name( 'Q', '_', 'R', 'V' );
775 775
776 776 status = rtems_message_queue_ident( queue_name, 0, queue_id );
777 777
778 778 return status;
779 779 }
780 780
781 781 rtems_status_code get_message_queue_id_prc0( rtems_id *queue_id )
782 782 {
783 783 rtems_status_code status;
784 784 rtems_name queue_name;
785 785
786 786 queue_name = rtems_build_name( 'Q', '_', 'P', '0' );
787 787
788 788 status = rtems_message_queue_ident( queue_name, 0, queue_id );
789 789
790 790 return status;
791 791 }
792 792
793 793 rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id )
794 794 {
795 795 rtems_status_code status;
796 796 rtems_name queue_name;
797 797
798 798 queue_name = rtems_build_name( 'Q', '_', 'P', '1' );
799 799
800 800 status = rtems_message_queue_ident( queue_name, 0, queue_id );
801 801
802 802 return status;
803 803 }
804 804
805 805 rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id )
806 806 {
807 807 rtems_status_code status;
808 808 rtems_name queue_name;
809 809
810 810 queue_name = rtems_build_name( 'Q', '_', 'P', '2' );
811 811
812 812 status = rtems_message_queue_ident( queue_name, 0, queue_id );
813 813
814 814 return status;
815 815 }
816 816
817 817 void update_queue_max_count( rtems_id queue_id, unsigned char*fifo_size_max )
818 818 {
819 819 u_int32_t count;
820 820 rtems_status_code status;
821 821
822 822 status = rtems_message_queue_get_number_pending( queue_id, &count );
823 823
824 824 count = count + 1;
825 825
826 826 if (status != RTEMS_SUCCESSFUL)
827 827 {
828 828 PRINTF1("in update_queue_max_count *** ERR = %d\n", status)
829 829 }
830 830 else
831 831 {
832 832 if (count > *fifo_size_max)
833 833 {
834 834 *fifo_size_max = count;
835 835 }
836 836 }
837 837 }
838 838
839 839 void init_ring(ring_node ring[], unsigned char nbNodes, volatile int buffer[], unsigned int bufferSize )
840 840 {
841 841 unsigned char i;
842 842
843 843 //***************
844 844 // BUFFER ADDRESS
845 845 for(i=0; i<nbNodes; i++)
846 846 {
847 ring[i].coarseTime = 0x00;
848 ring[i].fineTime = 0x00;
847 ring[i].coarseTime = 0xffffffff;
848 ring[i].fineTime = 0xffffffff;
849 849 ring[i].sid = 0x00;
850 850 ring[i].status = 0x00;
851 851 ring[i].buffer_address = (int) &buffer[ i * bufferSize ];
852 852 }
853 853
854 854 //*****
855 855 // NEXT
856 856 ring[ nbNodes - 1 ].next = (ring_node*) &ring[ 0 ];
857 857 for(i=0; i<nbNodes-1; i++)
858 858 {
859 859 ring[i].next = (ring_node*) &ring[ i + 1 ];
860 860 }
861 861
862 862 //*********
863 863 // PREVIOUS
864 864 ring[ 0 ].previous = (ring_node*) &ring[ nbNodes - 1 ];
865 865 for(i=1; i<nbNodes; i++)
866 866 {
867 867 ring[i].previous = (ring_node*) &ring[ i - 1 ];
868 868 }
869 869 }
Show file before | Show file after | Hide comments
@@ -1,1370 +1,1372
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 extractSWF = false;
35 35 bool swf_f0_ready = false;
36 36 bool swf_f1_ready = false;
37 37 bool swf_f2_ready = false;
38 38
39 39 int wf_snap_extracted[ (NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK) ];
40 40 ring_node ring_node_wf_snap_extracted;
41 41
42 42 //*********************
43 43 // Interrupt SubRoutine
44 44
45 45 ring_node * getRingNodeToSendCWF( unsigned char frequencyChannel)
46 46 {
47 47 ring_node *node;
48 48
49 49 node = NULL;
50 50 switch ( frequencyChannel ) {
51 51 case 1:
52 52 node = ring_node_to_send_cwf_f1;
53 53 break;
54 54 case 2:
55 55 node = ring_node_to_send_cwf_f2;
56 56 break;
57 57 case 3:
58 58 node = ring_node_to_send_cwf_f3;
59 59 break;
60 60 default:
61 61 break;
62 62 }
63 63
64 64 return node;
65 65 }
66 66
67 67 ring_node * getRingNodeToSendSWF( unsigned char frequencyChannel)
68 68 {
69 69 ring_node *node;
70 70
71 71 node = NULL;
72 72 switch ( frequencyChannel ) {
73 73 case 0:
74 74 node = ring_node_to_send_swf_f0;
75 75 break;
76 76 case 1:
77 77 node = ring_node_to_send_swf_f1;
78 78 break;
79 79 case 2:
80 80 node = ring_node_to_send_swf_f2;
81 81 break;
82 82 default:
83 83 break;
84 84 }
85 85
86 86 return node;
87 87 }
88 88
89 89 void reset_extractSWF( void )
90 90 {
91 91 extractSWF = false;
92 92 swf_f0_ready = false;
93 93 swf_f1_ready = false;
94 94 swf_f2_ready = false;
95 95 }
96 96
97 97 inline void waveforms_isr_f3( void )
98 98 {
99 99 rtems_status_code spare_status;
100 100
101 101 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
102 102 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
103 103 { // in modes other than STANDBY and BURST, send the CWF_F3 data
104 104 //***
105 105 // F3
106 106 if ( (waveform_picker_regs->status & 0xc0) != 0x00 ) { // [1100 0000] check the f3 full bits
107 107 ring_node_to_send_cwf_f3 = current_ring_node_f3->previous;
108 108 current_ring_node_f3 = current_ring_node_f3->next;
109 109 if ((waveform_picker_regs->status & 0x40) == 0x40){ // [0100 0000] f3 buffer 0 is full
110 110 ring_node_to_send_cwf_f3->coarseTime = waveform_picker_regs->f3_0_coarse_time;
111 111 ring_node_to_send_cwf_f3->fineTime = waveform_picker_regs->f3_0_fine_time;
112 112 waveform_picker_regs->addr_data_f3_0 = current_ring_node_f3->buffer_address;
113 113 waveform_picker_regs->status = waveform_picker_regs->status & 0x00008840; // [1000 1000 0100 0000]
114 114 }
115 115 else if ((waveform_picker_regs->status & 0x80) == 0x80){ // [1000 0000] f3 buffer 1 is full
116 116 ring_node_to_send_cwf_f3->coarseTime = waveform_picker_regs->f3_1_coarse_time;
117 117 ring_node_to_send_cwf_f3->fineTime = waveform_picker_regs->f3_1_fine_time;
118 118 waveform_picker_regs->addr_data_f3_1 = current_ring_node_f3->buffer_address;
119 119 waveform_picker_regs->status = waveform_picker_regs->status & 0x00008880; // [1000 1000 1000 0000]
120 120 }
121 121 if (rtems_event_send( Task_id[TASKID_CWF3], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
122 122 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
123 123 }
124 124 }
125 125 }
126 126 }
127 127
128 128 inline void waveforms_isr_normal( void )
129 129 {
130 130 rtems_status_code status;
131 131
132 132 if ( ( (waveform_picker_regs->status & 0x30) != 0x00 ) // [0011 0000] check the f2 full bits
133 133 && ( (waveform_picker_regs->status & 0x0c) != 0x00 ) // [0000 1100] check the f1 full bits
134 134 && ( (waveform_picker_regs->status & 0x03) != 0x00 )) // [0000 0011] check the f0 full bits
135 135 {
136 136 //***
137 137 // F0
138 138 ring_node_to_send_swf_f0 = current_ring_node_f0->previous;
139 139 current_ring_node_f0 = current_ring_node_f0->next;
140 140 if ( (waveform_picker_regs->status & 0x01) == 0x01)
141 141 {
142 142
143 143 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_0_coarse_time;
144 144 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_0_fine_time;
145 145 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->buffer_address;
146 146 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001101; // [0001 0001 0000 0001]
147 147 }
148 148 else if ( (waveform_picker_regs->status & 0x02) == 0x02)
149 149 {
150 150 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_1_coarse_time;
151 151 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_1_fine_time;
152 152 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address;
153 153 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001102; // [0001 0001 0000 0010]
154 154 }
155 155
156 156 //***
157 157 // F1
158 158 ring_node_to_send_swf_f1 = current_ring_node_f1->previous;
159 159 current_ring_node_f1 = current_ring_node_f1->next;
160 160 if ( (waveform_picker_regs->status & 0x04) == 0x04)
161 161 {
162 162 ring_node_to_send_swf_f1->coarseTime = waveform_picker_regs->f1_0_coarse_time;
163 163 ring_node_to_send_swf_f1->fineTime = waveform_picker_regs->f1_0_fine_time;
164 164 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->buffer_address;
165 165 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002204; // [0010 0010 0000 0100] f1 bits = 0
166 166 }
167 167 else if ( (waveform_picker_regs->status & 0x08) == 0x08)
168 168 {
169 169 ring_node_to_send_swf_f1->coarseTime = waveform_picker_regs->f1_1_coarse_time;
170 170 ring_node_to_send_swf_f1->fineTime = waveform_picker_regs->f1_1_fine_time;
171 171 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address;
172 172 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002208; // [0010 0010 0000 1000] f1 bits = 0
173 173 }
174 174
175 175 //***
176 176 // F2
177 177 ring_node_to_send_swf_f2 = current_ring_node_f2->previous;
178 178 current_ring_node_f2 = current_ring_node_f2->next;
179 179 if ( (waveform_picker_regs->status & 0x10) == 0x10)
180 180 {
181 181 ring_node_to_send_swf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
182 182 ring_node_to_send_swf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
183 183 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
184 184 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
185 185 }
186 186 else if ( (waveform_picker_regs->status & 0x20) == 0x20)
187 187 {
188 188 ring_node_to_send_swf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
189 189 ring_node_to_send_swf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
190 190 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
191 191 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
192 192 }
193 193 //
194 194 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_NORMAL );
195 195 if ( status != RTEMS_SUCCESSFUL)
196 196 {
197 197 status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
198 198 }
199 199 }
200 200 }
201 201
202 202 inline void waveforms_isr_burst( void )
203 203 {
204 204 unsigned char status;
205 205 rtems_status_code spare_status;
206 206
207 207 status = (waveform_picker_regs->status & 0x30) >> 4; // [0011 0000] get the status bits for f2
208 208
209 209
210 210 switch(status)
211 211 {
212 212 case 1:
213 213 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
214 214 ring_node_to_send_cwf_f2->sid = SID_BURST_CWF_F2;
215 215 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
216 216 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
217 217 current_ring_node_f2 = current_ring_node_f2->next;
218 218 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
219 219 if (rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_BURST ) != RTEMS_SUCCESSFUL) {
220 220 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
221 221 }
222 222 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
223 223 break;
224 224 case 2:
225 225 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
226 226 ring_node_to_send_cwf_f2->sid = SID_BURST_CWF_F2;
227 227 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
228 228 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
229 229 current_ring_node_f2 = current_ring_node_f2->next;
230 230 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
231 231 if (rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_BURST ) != RTEMS_SUCCESSFUL) {
232 232 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
233 233 }
234 234 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
235 235 break;
236 236 default:
237 237 break;
238 238 }
239 239 }
240 240
241 241 inline void waveforms_isr_sbm1( void )
242 242 {
243 243 rtems_status_code status;
244 244
245 245 //***
246 246 // F1
247 247 if ( (waveform_picker_regs->status & 0x0c) != 0x00 ) { // [0000 1100] check the f1 full bits
248 248 // (1) change the receiving buffer for the waveform picker
249 249 ring_node_to_send_cwf_f1 = current_ring_node_f1->previous;
250 250 current_ring_node_f1 = current_ring_node_f1->next;
251 251 if ( (waveform_picker_regs->status & 0x04) == 0x04)
252 252 {
253 253 ring_node_to_send_cwf_f1->coarseTime = waveform_picker_regs->f1_0_coarse_time;
254 254 ring_node_to_send_cwf_f1->fineTime = waveform_picker_regs->f1_0_fine_time;
255 255 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->buffer_address;
256 256 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002204; // [0010 0010 0000 0100] f1 bits = 0
257 257 }
258 258 else if ( (waveform_picker_regs->status & 0x08) == 0x08)
259 259 {
260 260 ring_node_to_send_cwf_f1->coarseTime = waveform_picker_regs->f1_1_coarse_time;
261 261 ring_node_to_send_cwf_f1->fineTime = waveform_picker_regs->f1_1_fine_time;
262 262 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address;
263 263 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002208; // [0010 0010 0000 1000] f1 bits = 0
264 264 }
265 265 // (2) send an event for the the CWF1 task for transmission (and snapshot extraction if needed)
266 266 status = rtems_event_send( Task_id[TASKID_CWF1], RTEMS_EVENT_MODE_SBM1 );
267 267 }
268 268
269 269 //***
270 270 // F0
271 271 if ( (waveform_picker_regs->status & 0x03) != 0x00 ) { // [0000 0011] check the f0 full bits
272 272 swf_f0_ready = true;
273 273 // change f0 buffer
274 274 ring_node_to_send_swf_f0 = current_ring_node_f0->previous;
275 275 current_ring_node_f0 = current_ring_node_f0->next;
276 276 if ( (waveform_picker_regs->status & 0x01) == 0x01)
277 277 {
278 278
279 279 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_0_coarse_time;
280 280 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_0_fine_time;
281 281 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->buffer_address;
282 282 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001101; // [0001 0001 0000 0001]
283 283 }
284 284 else if ( (waveform_picker_regs->status & 0x02) == 0x02)
285 285 {
286 286 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_1_coarse_time;
287 287 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_1_fine_time;
288 288 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address;
289 289 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001102; // [0001 0001 0000 0010]
290 290 }
291 291 }
292 292
293 293 //***
294 294 // F2
295 295 if ( (waveform_picker_regs->status & 0x30) != 0x00 ) { // [0011 0000] check the f2 full bits
296 296 swf_f2_ready = true;
297 297 // change f2 buffer
298 298 ring_node_to_send_swf_f2 = current_ring_node_f2->previous;
299 299 current_ring_node_f2 = current_ring_node_f2->next;
300 300 if ( (waveform_picker_regs->status & 0x10) == 0x10)
301 301 {
302 302 ring_node_to_send_swf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
303 303 ring_node_to_send_swf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
304 304 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
305 305 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
306 306 }
307 307 else if ( (waveform_picker_regs->status & 0x20) == 0x20)
308 308 {
309 309 ring_node_to_send_swf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
310 310 ring_node_to_send_swf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
311 311 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
312 312 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
313 313 }
314 314 }
315 315 }
316 316
317 317 inline void waveforms_isr_sbm2( void )
318 318 {
319 319 rtems_status_code status;
320 320
321 321 //***
322 322 // F2
323 323 if ( (waveform_picker_regs->status & 0x30) != 0x00 ) { // [0011 0000] check the f2 full bit
324 324 // (1) change the receiving buffer for the waveform picker
325 325 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
326 326 ring_node_to_send_cwf_f2->sid = SID_SBM2_CWF_F2;
327 327 current_ring_node_f2 = current_ring_node_f2->next;
328 328 if ( (waveform_picker_regs->status & 0x10) == 0x10)
329 329 {
330 330 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
331 331 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
332 332 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
333 333 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
334 334 }
335 335 else if ( (waveform_picker_regs->status & 0x20) == 0x20)
336 336 {
337 337 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
338 338 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
339 339 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
340 340 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
341 341 }
342 342 // (2) send an event for the waveforms transmission
343 343 status = rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_SBM2 );
344 344 }
345 345
346 346 //***
347 347 // F0
348 348 if ( (waveform_picker_regs->status & 0x03) != 0x00 ) { // [0000 0011] check the f0 full bit
349 349 swf_f0_ready = true;
350 350 // change f0 buffer
351 351 ring_node_to_send_swf_f0 = current_ring_node_f0->previous;
352 352 current_ring_node_f0 = current_ring_node_f0->next;
353 353 if ( (waveform_picker_regs->status & 0x01) == 0x01)
354 354 {
355 355
356 356 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_0_coarse_time;
357 357 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_0_fine_time;
358 358 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->buffer_address;
359 359 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001101; // [0001 0001 0000 0001]
360 360 }
361 361 else if ( (waveform_picker_regs->status & 0x02) == 0x02)
362 362 {
363 363 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_1_coarse_time;
364 364 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_1_fine_time;
365 365 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address;
366 366 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001102; // [0001 0001 0000 0010]
367 367 }
368 368 }
369 369
370 370 //***
371 371 // F1
372 372 if ( (waveform_picker_regs->status & 0x0c) != 0x00 ) { // [0000 1100] check the f1 full bit
373 373 swf_f1_ready = true;
374 374 ring_node_to_send_swf_f1 = current_ring_node_f1->previous;
375 375 current_ring_node_f1 = current_ring_node_f1->next;
376 376 if ( (waveform_picker_regs->status & 0x04) == 0x04)
377 377 {
378 378 ring_node_to_send_swf_f1->coarseTime = waveform_picker_regs->f1_0_coarse_time;
379 379 ring_node_to_send_swf_f1->fineTime = waveform_picker_regs->f1_0_fine_time;
380 380 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->buffer_address;
381 381 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002204; // [0010 0010 0000 0100] f1 bits = 0
382 382 }
383 383 else if ( (waveform_picker_regs->status & 0x08) == 0x08)
384 384 {
385 385 ring_node_to_send_swf_f1->coarseTime = waveform_picker_regs->f1_1_coarse_time;
386 386 ring_node_to_send_swf_f1->fineTime = waveform_picker_regs->f1_1_fine_time;
387 387 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address;
388 388 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002208; // [0010 0010 0000 1000] f1 bits = 0
389 389 }
390 390 }
391 391 }
392 392
393 393 rtems_isr waveforms_isr( rtems_vector_number vector )
394 394 {
395 395 /** This is the interrupt sub routine called by the waveform picker core.
396 396 *
397 397 * This ISR launch different actions depending mainly on two pieces of information:
398 398 * 1. the values read in the registers of the waveform picker.
399 399 * 2. the current LFR mode.
400 400 *
401 401 */
402 402
403 403 // STATUS
404 404 // new error error buffer full
405 405 // 15 14 13 12 11 10 9 8
406 406 // f3 f2 f1 f0 f3 f2 f1 f0
407 407 //
408 408 // ready buffer
409 409 // 7 6 5 4 3 2 1 0
410 410 // f3_1 f3_0 f2_1 f2_0 f1_1 f1_0 f0_1 f0_0
411 411
412 412 rtems_status_code spare_status;
413 413
414 414 waveforms_isr_f3();
415 415
416 416 if ( (waveform_picker_regs->status & 0xff00) != 0x00) // [1111 1111 0000 0000] check the error bits
417 417 {
418 418 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_10 );
419 419 }
420 420
421 421 switch(lfrCurrentMode)
422 422 {
423 423 //********
424 424 // STANDBY
425 425 case(LFR_MODE_STANDBY):
426 426 break;
427 427
428 428 //******
429 429 // NORMAL
430 430 case(LFR_MODE_NORMAL):
431 431 waveforms_isr_normal();
432 432 break;
433 433
434 434 //******
435 435 // BURST
436 436 case(LFR_MODE_BURST):
437 437 waveforms_isr_burst();
438 438 break;
439 439
440 440 //*****
441 441 // SBM1
442 442 case(LFR_MODE_SBM1):
443 443 waveforms_isr_sbm1();
444 444 break;
445 445
446 446 //*****
447 447 // SBM2
448 448 case(LFR_MODE_SBM2):
449 449 waveforms_isr_sbm2();
450 450 break;
451 451
452 452 //********
453 453 // DEFAULT
454 454 default:
455 455 break;
456 456 }
457 457 }
458 458
459 459 //************
460 460 // RTEMS TASKS
461 461
462 462 rtems_task wfrm_task(rtems_task_argument argument) //used with the waveform picker VHDL IP
463 463 {
464 464 /** This RTEMS task is dedicated to the transmission of snapshots of the NORMAL mode.
465 465 *
466 466 * @param unused is the starting argument of the RTEMS task
467 467 *
468 468 * The following data packets are sent by this task:
469 469 * - TM_LFR_SCIENCE_NORMAL_SWF_F0
470 470 * - TM_LFR_SCIENCE_NORMAL_SWF_F1
471 471 * - TM_LFR_SCIENCE_NORMAL_SWF_F2
472 472 *
473 473 */
474 474
475 475 rtems_event_set event_out;
476 476 rtems_id queue_id;
477 477 rtems_status_code status;
478 478 bool resynchronisationEngaged;
479 479 ring_node *ring_node_wf_snap_extracted_ptr;
480 480
481 481 ring_node_wf_snap_extracted_ptr = (ring_node *) &ring_node_wf_snap_extracted;
482 482
483 483 resynchronisationEngaged = false;
484 484
485 485 status = get_message_queue_id_send( &queue_id );
486 486 if (status != RTEMS_SUCCESSFUL)
487 487 {
488 488 PRINTF1("in WFRM *** ERR get_message_queue_id_send %d\n", status)
489 489 }
490 490
491 491 BOOT_PRINTF("in WFRM ***\n")
492 492
493 493 while(1){
494 494 // wait for an RTEMS_EVENT
495 495 rtems_event_receive(RTEMS_EVENT_MODE_NORMAL | RTEMS_EVENT_MODE_SBM1
496 496 | RTEMS_EVENT_MODE_SBM2 | RTEMS_EVENT_MODE_SBM2_WFRM,
497 497 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
498 498 if(resynchronisationEngaged == false)
499 499 { // engage resynchronisation
500 500 snapshot_resynchronization( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
501 501 resynchronisationEngaged = true;
502 502 }
503 503 else
504 504 { // reset delta_snapshot to the nominal value
505 505 PRINTF("no resynchronisation, reset delta_snapshot to the nominal value\n")
506 506 set_wfp_delta_snapshot();
507 507 resynchronisationEngaged = false;
508 508 }
509 509 //
510 510
511 511 if (event_out == RTEMS_EVENT_MODE_NORMAL)
512 512 {
513 513 DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_NORMAL\n")
514 514 ring_node_to_send_swf_f0->sid = SID_NORM_SWF_F0;
515 515 ring_node_to_send_swf_f1->sid = SID_NORM_SWF_F1;
516 516 ring_node_to_send_swf_f2->sid = SID_NORM_SWF_F2;
517 517 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f0, sizeof( ring_node* ) );
518 518 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f1, sizeof( ring_node* ) );
519 519 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f2, sizeof( ring_node* ) );
520 520 }
521 521 if (event_out == RTEMS_EVENT_MODE_SBM1)
522 522 {
523 523 DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_SBM1\n")
524 524 ring_node_to_send_swf_f0->sid = SID_NORM_SWF_F0;
525 525 ring_node_wf_snap_extracted_ptr->sid = SID_NORM_SWF_F1;
526 526 ring_node_to_send_swf_f2->sid = SID_NORM_SWF_F2;
527 527 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f0, sizeof( ring_node* ) );
528 528 status = rtems_message_queue_send( queue_id, &ring_node_wf_snap_extracted_ptr, sizeof( ring_node* ) );
529 529 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f2, sizeof( ring_node* ) );
530 530 }
531 531 if (event_out == RTEMS_EVENT_MODE_SBM2)
532 532 {
533 533 DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_SBM2\n")
534 534 ring_node_to_send_swf_f0->sid = SID_NORM_SWF_F0;
535 535 ring_node_to_send_swf_f1->sid = SID_NORM_SWF_F1;
536 536 ring_node_wf_snap_extracted_ptr->sid = SID_NORM_SWF_F2;
537 537 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f0, sizeof( ring_node* ) );
538 538 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f1, sizeof( ring_node* ) );
539 539 status = rtems_message_queue_send( queue_id, &ring_node_wf_snap_extracted_ptr, sizeof( ring_node* ) );
540 540 }
541 541 }
542 542 }
543 543
544 544 rtems_task cwf3_task(rtems_task_argument argument) //used with the waveform picker VHDL IP
545 545 {
546 546 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f3.
547 547 *
548 548 * @param unused is the starting argument of the RTEMS task
549 549 *
550 550 * The following data packet is sent by this task:
551 551 * - TM_LFR_SCIENCE_NORMAL_CWF_F3
552 552 *
553 553 */
554 554
555 555 rtems_event_set event_out;
556 556 rtems_id queue_id;
557 557 rtems_status_code status;
558 558 ring_node ring_node_cwf3_light;
559 ring_node *ring_node_to_send_cwf;
559 560
560 561 status = get_message_queue_id_send( &queue_id );
561 562 if (status != RTEMS_SUCCESSFUL)
562 563 {
563 564 PRINTF1("in CWF3 *** ERR get_message_queue_id_send %d\n", status)
564 565 }
565 566
566 567 ring_node_to_send_cwf_f3->sid = SID_NORM_CWF_LONG_F3;
567 568
568 569 // init the ring_node_cwf3_light structure
569 570 ring_node_cwf3_light.buffer_address = (int) wf_cont_f3_light;
570 571 ring_node_cwf3_light.coarseTime = 0x00;
571 572 ring_node_cwf3_light.fineTime = 0x00;
572 573 ring_node_cwf3_light.next = NULL;
573 574 ring_node_cwf3_light.previous = NULL;
574 575 ring_node_cwf3_light.sid = SID_NORM_CWF_F3;
575 576 ring_node_cwf3_light.status = 0x00;
576 577
577 578 BOOT_PRINTF("in CWF3 ***\n")
578 579
579 580 while(1){
580 581 // wait for an RTEMS_EVENT
581 582 rtems_event_receive( RTEMS_EVENT_0,
582 583 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
583 584 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
584 585 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode==LFR_MODE_SBM2) )
585 586 {
587 ring_node_to_send_cwf = getRingNodeToSendCWF( 3 );
586 588 if ( (parameter_dump_packet.sy_lfr_n_cwf_long_f3 & 0x01) == 0x01)
587 589 {
588 590 PRINTF("send CWF_LONG_F3\n")
589 591 ring_node_to_send_cwf_f3->sid = SID_NORM_CWF_LONG_F3;
590 status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf_f3, sizeof( ring_node* ) );
592 status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf, sizeof( ring_node* ) );
591 593 }
592 594 else
593 595 {
594 596 PRINTF("send CWF_F3 (light)\n")
595 send_waveform_CWF3_light( ring_node_to_send_cwf_f3, &ring_node_cwf3_light, queue_id );
597 send_waveform_CWF3_light( ring_node_to_send_cwf, &ring_node_cwf3_light, queue_id );
596 598 }
597 599
598 600 }
599 601 else
600 602 {
601 603 PRINTF1("in CWF3 *** lfrCurrentMode is %d, no data will be sent\n", lfrCurrentMode)
602 604 }
603 605 }
604 606 }
605 607
606 608 rtems_task cwf2_task(rtems_task_argument argument) // ONLY USED IN BURST AND SBM2
607 609 {
608 610 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f2.
609 611 *
610 612 * @param unused is the starting argument of the RTEMS task
611 613 *
612 614 * The following data packet is sent by this function:
613 615 * - TM_LFR_SCIENCE_BURST_CWF_F2
614 616 * - TM_LFR_SCIENCE_SBM2_CWF_F2
615 617 *
616 618 */
617 619
618 620 rtems_event_set event_out;
619 621 rtems_id queue_id;
620 622 rtems_status_code status;
621 623 ring_node *ring_node_to_send;
622 624 unsigned long long int acquisitionTimeF0_asLong;
623 625
624 626 acquisitionTimeF0_asLong = 0x00;
625 627
626 628 status = get_message_queue_id_send( &queue_id );
627 629 if (status != RTEMS_SUCCESSFUL)
628 630 {
629 631 PRINTF1("in CWF2 *** ERR get_message_queue_id_send %d\n", status)
630 632 }
631 633
632 634 BOOT_PRINTF("in CWF2 ***\n")
633 635
634 636 while(1){
635 637 // wait for an RTEMS_EVENT
636 638 rtems_event_receive( RTEMS_EVENT_MODE_BURST | RTEMS_EVENT_MODE_SBM2,
637 639 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
638 640 ring_node_to_send = getRingNodeToSendCWF( 2 );
639 641 if (event_out == RTEMS_EVENT_MODE_BURST)
640 642 {
641 643 status = rtems_message_queue_send( queue_id, &ring_node_to_send, sizeof( ring_node* ) );
642 644 }
643 645 if (event_out == RTEMS_EVENT_MODE_SBM2)
644 646 {
645 647 status = rtems_message_queue_send( queue_id, &ring_node_to_send, sizeof( ring_node* ) );
646 648 // launch snapshot extraction if needed
647 649 if (extractSWF == true)
648 650 {
649 651 ring_node_to_send_swf_f2 = ring_node_to_send_cwf_f2;
650 652 // extract the snapshot
651 653 build_snapshot_from_ring( ring_node_to_send_swf_f2, 2, acquisitionTimeF0_asLong );
652 654 // send the snapshot when built
653 655 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_SBM2 );
654 656 extractSWF = false;
655 657 }
656 658 if (swf_f0_ready && swf_f1_ready)
657 659 {
658 660 extractSWF = true;
659 661 // record the acquition time of the fΓ  snapshot to use to build the snapshot at f2
660 662 acquisitionTimeF0_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
661 663 swf_f0_ready = false;
662 664 swf_f1_ready = false;
663 665 }
664 666 }
665 667 }
666 668 }
667 669
668 670 rtems_task cwf1_task(rtems_task_argument argument) // ONLY USED IN SBM1
669 671 {
670 672 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f1.
671 673 *
672 674 * @param unused is the starting argument of the RTEMS task
673 675 *
674 676 * The following data packet is sent by this function:
675 677 * - TM_LFR_SCIENCE_SBM1_CWF_F1
676 678 *
677 679 */
678 680
679 681 rtems_event_set event_out;
680 682 rtems_id queue_id;
681 683 rtems_status_code status;
682 684
683 685 ring_node *ring_node_to_send_cwf;
684 686
685 687 status = get_message_queue_id_send( &queue_id );
686 688 if (status != RTEMS_SUCCESSFUL)
687 689 {
688 690 PRINTF1("in CWF1 *** ERR get_message_queue_id_send %d\n", status)
689 691 }
690 692
691 693 BOOT_PRINTF("in CWF1 ***\n")
692 694
693 695 while(1){
694 696 // wait for an RTEMS_EVENT
695 697 rtems_event_receive( RTEMS_EVENT_MODE_SBM1,
696 698 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
697 699 ring_node_to_send_cwf = getRingNodeToSendCWF( 1 );
698 700 ring_node_to_send_cwf_f1->sid = SID_SBM1_CWF_F1;
699 701 status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf, sizeof( ring_node* ) );
700 702 if (status != 0)
701 703 printf("cwf sending failed\n");
702 704 // launch snapshot extraction if needed
703 705 if (extractSWF == true)
704 706 {
705 707 ring_node_to_send_swf_f1 = ring_node_to_send_cwf;
706 708 // launch the snapshot extraction
707 709 status = rtems_event_send( Task_id[TASKID_SWBD], RTEMS_EVENT_MODE_SBM1 );
708 710 extractSWF = false;
709 711 }
710 712 if (swf_f0_ready == true)
711 713 {
712 714 extractSWF = true;
713 715 swf_f0_ready = false; // this step shall be executed only one time
714 716 }
715 717 if ((swf_f1_ready == true) && (swf_f2_ready == true)) // swf_f1 is ready after the extraction
716 718 {
717 719 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_SBM1 );
718 720 swf_f1_ready = false;
719 721 swf_f2_ready = false;
720 722 }
721 723 }
722 724 }
723 725
724 726 rtems_task swbd_task(rtems_task_argument argument)
725 727 {
726 728 /** This RTEMS task is dedicated to the building of snapshots from different continuous waveforms buffers.
727 729 *
728 730 * @param unused is the starting argument of the RTEMS task
729 731 *
730 732 */
731 733
732 734 rtems_event_set event_out;
733 735 unsigned long long int acquisitionTimeF0_asLong;
734 736
735 737 acquisitionTimeF0_asLong = 0x00;
736 738
737 739 BOOT_PRINTF("in SWBD ***\n")
738 740
739 741 while(1){
740 742 // wait for an RTEMS_EVENT
741 743 rtems_event_receive( RTEMS_EVENT_MODE_SBM1 | RTEMS_EVENT_MODE_SBM2,
742 744 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
743 745 if (event_out == RTEMS_EVENT_MODE_SBM1)
744 746 {
745 747 acquisitionTimeF0_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
746 748 build_snapshot_from_ring( ring_node_to_send_swf_f1, 1, acquisitionTimeF0_asLong );
747 749 swf_f1_ready = true; // the snapshot has been extracted and is ready to be sent
748 750 }
749 751 else
750 752 {
751 753 PRINTF1("in SWBD *** unexpected rtems event received %x\n", (int) event_out)
752 754 }
753 755 }
754 756 }
755 757
756 758 //******************
757 759 // general functions
758 760
759 761 void WFP_init_rings( void )
760 762 {
761 763 // F0 RING
762 764 init_ring( waveform_ring_f0, NB_RING_NODES_F0, wf_buffer_f0, WFRM_BUFFER );
763 765 // F1 RING
764 766 init_ring( waveform_ring_f1, NB_RING_NODES_F1, wf_buffer_f1, WFRM_BUFFER );
765 767 // F2 RING
766 768 init_ring( waveform_ring_f2, NB_RING_NODES_F2, wf_buffer_f2, WFRM_BUFFER );
767 769 // F3 RING
768 770 init_ring( waveform_ring_f3, NB_RING_NODES_F3, wf_buffer_f3, WFRM_BUFFER );
769 771
770 772 ring_node_wf_snap_extracted.buffer_address = (int) wf_snap_extracted;
771 773
772 774 DEBUG_PRINTF1("waveform_ring_f0 @%x\n", (unsigned int) waveform_ring_f0)
773 775 DEBUG_PRINTF1("waveform_ring_f1 @%x\n", (unsigned int) waveform_ring_f1)
774 776 DEBUG_PRINTF1("waveform_ring_f2 @%x\n", (unsigned int) waveform_ring_f2)
775 777 DEBUG_PRINTF1("waveform_ring_f3 @%x\n", (unsigned int) waveform_ring_f3)
776 778 DEBUG_PRINTF1("wf_buffer_f0 @%x\n", (unsigned int) wf_buffer_f0)
777 779 DEBUG_PRINTF1("wf_buffer_f1 @%x\n", (unsigned int) wf_buffer_f1)
778 780 DEBUG_PRINTF1("wf_buffer_f2 @%x\n", (unsigned int) wf_buffer_f2)
779 781 DEBUG_PRINTF1("wf_buffer_f3 @%x\n", (unsigned int) wf_buffer_f3)
780 782
781 783 }
782 784
783 785 void WFP_reset_current_ring_nodes( void )
784 786 {
785 787 current_ring_node_f0 = waveform_ring_f0[0].next;
786 788 current_ring_node_f1 = waveform_ring_f1[0].next;
787 789 current_ring_node_f2 = waveform_ring_f2[0].next;
788 790 current_ring_node_f3 = waveform_ring_f3[0].next;
789 791
790 792 ring_node_to_send_swf_f0 = waveform_ring_f0;
791 793 ring_node_to_send_swf_f1 = waveform_ring_f1;
792 794 ring_node_to_send_swf_f2 = waveform_ring_f2;
793 795
794 796 ring_node_to_send_cwf_f1 = waveform_ring_f1;
795 797 ring_node_to_send_cwf_f2 = waveform_ring_f2;
796 798 ring_node_to_send_cwf_f3 = waveform_ring_f3;
797 799 }
798 800
799 801 int send_waveform_CWF3_light( ring_node *ring_node_to_send, ring_node *ring_node_cwf3_light, rtems_id queue_id )
800 802 {
801 803 /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data.
802 804 *
803 805 * @param waveform points to the buffer containing the data that will be send.
804 806 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
805 807 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
806 808 * contain information to setup the transmission of the data packets.
807 809 *
808 810 * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer
809 811 * from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks.
810 812 *
811 813 */
812 814
813 815 unsigned int i;
814 816 int ret;
815 817 rtems_status_code status;
816 818
817 819 char *sample;
818 820 int *dataPtr;
819 821
820 822 ret = LFR_DEFAULT;
821 823
822 824 dataPtr = (int*) ring_node_to_send->buffer_address;
823 825
824 826 ring_node_cwf3_light->coarseTime = ring_node_to_send->coarseTime;
825 827 ring_node_cwf3_light->fineTime = ring_node_to_send->fineTime;
826 828
827 829 //**********************
828 830 // BUILD CWF3_light DATA
829 831 for ( i=0; i< NB_SAMPLES_PER_SNAPSHOT; i++)
830 832 {
831 833 sample = (char*) &dataPtr[ (i * NB_WORDS_SWF_BLK) ];
832 834 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) ] = sample[ 0 ];
833 835 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 1 ] = sample[ 1 ];
834 836 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 2 ] = sample[ 2 ];
835 837 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 3 ] = sample[ 3 ];
836 838 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 4 ] = sample[ 4 ];
837 839 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 5 ] = sample[ 5 ];
838 840 }
839 841
840 842 // SEND PACKET
841 843 status = rtems_message_queue_send( queue_id, &ring_node_cwf3_light, sizeof( ring_node* ) );
842 844 if (status != RTEMS_SUCCESSFUL) {
843 845 printf("%d-%d, ERR %d\n", SID_NORM_CWF_F3, i, (int) status);
844 846 ret = LFR_DEFAULT;
845 847 }
846 848
847 849 return ret;
848 850 }
849 851
850 852 void compute_acquisition_time( unsigned int coarseTime, unsigned int fineTime,
851 853 unsigned int sid, unsigned char pa_lfr_pkt_nr, unsigned char * acquisitionTime )
852 854 {
853 855 unsigned long long int acquisitionTimeAsLong;
854 856 unsigned char localAcquisitionTime[6];
855 857 double deltaT;
856 858
857 859 deltaT = 0.;
858 860
859 861 localAcquisitionTime[0] = (unsigned char) ( coarseTime >> 24 );
860 862 localAcquisitionTime[1] = (unsigned char) ( coarseTime >> 16 );
861 863 localAcquisitionTime[2] = (unsigned char) ( coarseTime >> 8 );
862 864 localAcquisitionTime[3] = (unsigned char) ( coarseTime );
863 865 localAcquisitionTime[4] = (unsigned char) ( fineTime >> 8 );
864 866 localAcquisitionTime[5] = (unsigned char) ( fineTime );
865 867
866 868 acquisitionTimeAsLong = ( (unsigned long long int) localAcquisitionTime[0] << 40 )
867 869 + ( (unsigned long long int) localAcquisitionTime[1] << 32 )
868 870 + ( (unsigned long long int) localAcquisitionTime[2] << 24 )
869 871 + ( (unsigned long long int) localAcquisitionTime[3] << 16 )
870 872 + ( (unsigned long long int) localAcquisitionTime[4] << 8 )
871 873 + ( (unsigned long long int) localAcquisitionTime[5] );
872 874
873 875 switch( sid )
874 876 {
875 877 case SID_NORM_SWF_F0:
876 878 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * 65536. / 24576. ;
877 879 break;
878 880
879 881 case SID_NORM_SWF_F1:
880 882 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * 65536. / 4096. ;
881 883 break;
882 884
883 885 case SID_NORM_SWF_F2:
884 886 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * 65536. / 256. ;
885 887 break;
886 888
887 889 case SID_SBM1_CWF_F1:
888 890 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 4096. ;
889 891 break;
890 892
891 893 case SID_SBM2_CWF_F2:
892 894 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 256. ;
893 895 break;
894 896
895 897 case SID_BURST_CWF_F2:
896 898 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 256. ;
897 899 break;
898 900
899 901 case SID_NORM_CWF_F3:
900 902 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF_SHORT_F3 * 65536. / 16. ;
901 903 break;
902 904
903 905 case SID_NORM_CWF_LONG_F3:
904 906 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 16. ;
905 907 break;
906 908
907 909 default:
908 910 PRINTF1("in compute_acquisition_time *** ERR unexpected sid %d\n", sid)
909 911 deltaT = 0.;
910 912 break;
911 913 }
912 914
913 915 acquisitionTimeAsLong = acquisitionTimeAsLong + (unsigned long long int) deltaT;
914 916 //
915 917 acquisitionTime[0] = (unsigned char) (acquisitionTimeAsLong >> 40);
916 918 acquisitionTime[1] = (unsigned char) (acquisitionTimeAsLong >> 32);
917 919 acquisitionTime[2] = (unsigned char) (acquisitionTimeAsLong >> 24);
918 920 acquisitionTime[3] = (unsigned char) (acquisitionTimeAsLong >> 16);
919 921 acquisitionTime[4] = (unsigned char) (acquisitionTimeAsLong >> 8 );
920 922 acquisitionTime[5] = (unsigned char) (acquisitionTimeAsLong );
921 923
922 924 }
923 925
924 926 void build_snapshot_from_ring( ring_node *ring_node_to_send, unsigned char frequencyChannel, unsigned long long int acquisitionTimeF0_asLong )
925 927 {
926 928 unsigned int i;
927 929 unsigned long long int centerTime_asLong;
928 930 unsigned long long int acquisitionTime_asLong;
929 931 unsigned long long int bufferAcquisitionTime_asLong;
930 932 unsigned char *ptr1;
931 933 unsigned char *ptr2;
932 934 unsigned char *timeCharPtr;
933 935 unsigned char nb_ring_nodes;
934 936 unsigned long long int frequency_asLong;
935 937 unsigned long long int nbTicksPerSample_asLong;
936 938 unsigned long long int nbSamplesPart1_asLong;
937 939 unsigned long long int sampleOffset_asLong;
938 940
939 941 unsigned int deltaT_F0;
940 942 unsigned int deltaT_F1;
941 943 unsigned long long int deltaT_F2;
942 944
943 945 deltaT_F0 = 2731; // (2048. / 24576. / 2.) * 65536. = 2730.667;
944 946 deltaT_F1 = 16384; // (2048. / 4096. / 2.) * 65536. = 16384;
945 947 deltaT_F2 = 262144; // (2048. / 256. / 2.) * 65536. = 262144;
946 948 sampleOffset_asLong = 0x00;
947 949
948 950 // (1) get the f0 acquisition time => the value is passed in argument
949 951
950 952 // (2) compute the central reference time
951 953 centerTime_asLong = acquisitionTimeF0_asLong + deltaT_F0;
952 954
953 955 // (3) compute the acquisition time of the current snapshot
954 956 switch(frequencyChannel)
955 957 {
956 958 case 1: // 1 is for F1 = 4096 Hz
957 959 acquisitionTime_asLong = centerTime_asLong - deltaT_F1;
958 960 nb_ring_nodes = NB_RING_NODES_F1;
959 961 frequency_asLong = 4096;
960 962 nbTicksPerSample_asLong = 16; // 65536 / 4096;
961 963 break;
962 964 case 2: // 2 is for F2 = 256 Hz
963 965 acquisitionTime_asLong = centerTime_asLong - deltaT_F2;
964 966 nb_ring_nodes = NB_RING_NODES_F2;
965 967 frequency_asLong = 256;
966 968 nbTicksPerSample_asLong = 256; // 65536 / 256;
967 969 break;
968 970 default:
969 971 acquisitionTime_asLong = centerTime_asLong;
970 972 frequency_asLong = 256;
971 973 nbTicksPerSample_asLong = 256;
972 974 break;
973 975 }
974 976
975 977 //****************************************************************************
976 978 // (4) search the ring_node with the acquisition time <= acquisitionTime_asLong
977 979 for (i=0; i<nb_ring_nodes; i++)
978 980 {
979 981 PRINTF1("%d ... ", i)
980 982 bufferAcquisitionTime_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send->coarseTime );
981 983 if (bufferAcquisitionTime_asLong <= acquisitionTime_asLong)
982 984 {
983 985 PRINTF1("buffer found with acquisition time = %llx\n", bufferAcquisitionTime_asLong)
984 986 break;
985 987 }
986 988 ring_node_to_send = ring_node_to_send->previous;
987 989 }
988 990
989 991 // (5) compute the number of samples to take in the current buffer
990 992 sampleOffset_asLong = ((acquisitionTime_asLong - bufferAcquisitionTime_asLong) * frequency_asLong ) >> 16;
991 993 nbSamplesPart1_asLong = NB_SAMPLES_PER_SNAPSHOT - sampleOffset_asLong;
992 994 PRINTF2("sampleOffset_asLong = %lld, nbSamplesPart1_asLong = %lld\n", sampleOffset_asLong, nbSamplesPart1_asLong)
993 995
994 996 // (6) compute the final acquisition time
995 997 acquisitionTime_asLong = bufferAcquisitionTime_asLong +
996 998 sampleOffset_asLong * nbTicksPerSample_asLong;
997 999
998 1000 // (7) copy the acquisition time at the beginning of the extrated snapshot
999 1001 ptr1 = (unsigned char*) &acquisitionTime_asLong;
1000 1002 // fine time
1001 1003 ptr2 = (unsigned char*) &ring_node_wf_snap_extracted.fineTime;
1002 1004 ptr2[2] = ptr1[ 4 + 2 ];
1003 1005 ptr2[3] = ptr1[ 5 + 2 ];
1004 1006 // coarse time
1005 1007 ptr2 = (unsigned char*) &ring_node_wf_snap_extracted.coarseTime;
1006 1008 ptr2[0] = ptr1[ 0 + 2 ];
1007 1009 ptr2[1] = ptr1[ 1 + 2 ];
1008 1010 ptr2[2] = ptr1[ 2 + 2 ];
1009 1011 ptr2[3] = ptr1[ 3 + 2 ];
1010 1012
1011 1013 // re set the synchronization bit
1012 1014 timeCharPtr = (unsigned char*) &ring_node_to_send->coarseTime;
1013 1015 ptr2[0] = ptr2[0] | (timeCharPtr[0] & 0x80); // [1000 0000]
1014 1016
1015 1017 if ( (nbSamplesPart1_asLong >= NB_SAMPLES_PER_SNAPSHOT) | (nbSamplesPart1_asLong < 0) )
1016 1018 {
1017 1019 nbSamplesPart1_asLong = 0;
1018 1020 }
1019 1021 // copy the part 1 of the snapshot in the extracted buffer
1020 1022 for ( i = 0; i < (nbSamplesPart1_asLong * NB_WORDS_SWF_BLK); i++ )
1021 1023 {
1022 1024 wf_snap_extracted[i] =
1023 1025 ((int*) ring_node_to_send->buffer_address)[ i + (sampleOffset_asLong * NB_WORDS_SWF_BLK) ];
1024 1026 }
1025 1027 // copy the part 2 of the snapshot in the extracted buffer
1026 1028 ring_node_to_send = ring_node_to_send->next;
1027 1029 for ( i = (nbSamplesPart1_asLong * NB_WORDS_SWF_BLK); i < (NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK); i++ )
1028 1030 {
1029 1031 wf_snap_extracted[i] =
1030 1032 ((int*) ring_node_to_send->buffer_address)[ (i-(nbSamplesPart1_asLong * NB_WORDS_SWF_BLK)) ];
1031 1033 }
1032 1034 }
1033 1035
1034 1036 void snapshot_resynchronization( unsigned char *timePtr )
1035 1037 {
1036 1038 unsigned long long int acquisitionTime;
1037 1039 unsigned long long int centerTime;
1038 1040 unsigned long long int previousTick;
1039 1041 unsigned long long int nextTick;
1040 1042 unsigned long long int deltaPreviousTick;
1041 1043 unsigned long long int deltaNextTick;
1042 1044 unsigned int deltaTickInF2;
1043 1045 double deltaPrevious;
1044 1046 double deltaNext;
1045 1047
1046 1048 acquisitionTime = get_acquisition_time( timePtr );
1047 1049
1048 1050 // compute center time
1049 1051 centerTime = acquisitionTime + 2731; // (2048. / 24576. / 2.) * 65536. = 2730.667;
1050 1052 previousTick = centerTime - (centerTime & 0xffff);
1051 1053 nextTick = previousTick + 65536;
1052 1054
1053 1055 deltaPreviousTick = centerTime - previousTick;
1054 1056 deltaNextTick = nextTick - centerTime;
1055 1057
1056 1058 deltaPrevious = ((double) deltaPreviousTick) / 65536. * 1000.;
1057 1059 deltaNext = ((double) deltaNextTick) / 65536. * 1000.;
1058 1060
1059 1061 PRINTF2("delta previous = %f ms, delta next = %f ms\n", deltaPrevious, deltaNext)
1060 1062 PRINTF2("delta previous = %llu, delta next = %llu\n", deltaPreviousTick, deltaNextTick)
1061 1063
1062 1064 // which tick is the closest
1063 1065 if (deltaPreviousTick > deltaNextTick)
1064 1066 {
1065 1067 deltaTickInF2 = floor( (deltaNext * 256. / 1000.) ); // the division by 2 is important here
1066 1068 waveform_picker_regs->delta_snapshot = waveform_picker_regs->delta_snapshot + deltaTickInF2;
1067 1069 printf("correction of = + %u\n", deltaTickInF2);
1068 1070 }
1069 1071 else
1070 1072 {
1071 1073 deltaTickInF2 = floor( (deltaPrevious * 256. / 1000.) ); // the division by 2 is important here
1072 1074 waveform_picker_regs->delta_snapshot = waveform_picker_regs->delta_snapshot - deltaTickInF2;
1073 1075 printf("correction of = - %u\n", deltaTickInF2);
1074 1076 }
1075 1077 }
1076 1078
1077 1079 //**************
1078 1080 // wfp registers
1079 1081 void reset_wfp_burst_enable( void )
1080 1082 {
1081 1083 /** This function resets the waveform picker burst_enable register.
1082 1084 *
1083 1085 * The burst bits [f2 f1 f0] and the enable bits [f3 f2 f1 f0] are set to 0.
1084 1086 *
1085 1087 */
1086 1088
1087 1089 // [1000 000] burst f2, f1, f0 enable f3, f2, f1, f0
1088 1090 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable & 0x80;
1089 1091 }
1090 1092
1091 1093 void reset_wfp_status( void )
1092 1094 {
1093 1095 /** This function resets the waveform picker status register.
1094 1096 *
1095 1097 * All status bits are set to 0 [new_err full_err full].
1096 1098 *
1097 1099 */
1098 1100
1099 1101 waveform_picker_regs->status = 0xffff;
1100 1102 }
1101 1103
1102 1104 void reset_wfp_buffer_addresses( void )
1103 1105 {
1104 1106 // F0
1105 1107 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->previous->buffer_address; // 0x08
1106 1108 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address; // 0x0c
1107 1109 // F1
1108 1110 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->previous->buffer_address; // 0x10
1109 1111 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address; // 0x14
1110 1112 // F2
1111 1113 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->previous->buffer_address; // 0x18
1112 1114 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address; // 0x1c
1113 1115 // F3
1114 1116 waveform_picker_regs->addr_data_f3_0 = current_ring_node_f3->previous->buffer_address; // 0x20
1115 1117 waveform_picker_regs->addr_data_f3_1 = current_ring_node_f3->buffer_address; // 0x24
1116 1118 }
1117 1119
1118 1120 void reset_waveform_picker_regs( void )
1119 1121 {
1120 1122 /** This function resets the waveform picker module registers.
1121 1123 *
1122 1124 * The registers affected by this function are located at the following offset addresses:
1123 1125 * - 0x00 data_shaping
1124 1126 * - 0x04 run_burst_enable
1125 1127 * - 0x08 addr_data_f0
1126 1128 * - 0x0C addr_data_f1
1127 1129 * - 0x10 addr_data_f2
1128 1130 * - 0x14 addr_data_f3
1129 1131 * - 0x18 status
1130 1132 * - 0x1C delta_snapshot
1131 1133 * - 0x20 delta_f0
1132 1134 * - 0x24 delta_f0_2
1133 1135 * - 0x28 delta_f1
1134 1136 * - 0x2c delta_f2
1135 1137 * - 0x30 nb_data_by_buffer
1136 1138 * - 0x34 nb_snapshot_param
1137 1139 * - 0x38 start_date
1138 1140 * - 0x3c nb_word_in_buffer
1139 1141 *
1140 1142 */
1141 1143
1142 1144 set_wfp_data_shaping(); // 0x00 *** R1 R0 SP1 SP0 BW
1143 1145
1144 1146 reset_wfp_burst_enable(); // 0x04 *** [run *** burst f2, f1, f0 *** enable f3, f2, f1, f0 ]
1145 1147
1146 1148 reset_wfp_buffer_addresses();
1147 1149
1148 1150 reset_wfp_status(); // 0x18
1149 1151
1150 1152 set_wfp_delta_snapshot(); // 0x1c *** 300 s => 0x12bff
1151 1153
1152 1154 set_wfp_delta_f0_f0_2(); // 0x20, 0x24
1153 1155
1154 1156 set_wfp_delta_f1(); // 0x28
1155 1157
1156 1158 set_wfp_delta_f2(); // 0x2c
1157 1159
1158 1160 DEBUG_PRINTF1("delta_snapshot %x\n", waveform_picker_regs->delta_snapshot)
1159 1161 DEBUG_PRINTF1("delta_f0 %x\n", waveform_picker_regs->delta_f0)
1160 1162 DEBUG_PRINTF1("delta_f0_2 %x\n", waveform_picker_regs->delta_f0_2)
1161 1163 DEBUG_PRINTF1("delta_f1 %x\n", waveform_picker_regs->delta_f1)
1162 1164 DEBUG_PRINTF1("delta_f2 %x\n", waveform_picker_regs->delta_f2)
1163 1165 // 2688 = 8 * 336
1164 1166 waveform_picker_regs->nb_data_by_buffer = 0xa7f; // 0x30 *** 2688 - 1 => nb samples -1
1165 1167 waveform_picker_regs->snapshot_param = 0xa80; // 0x34 *** 2688 => nb samples
1166 1168 waveform_picker_regs->start_date = 0x7fffffff; // 0x38
1167 1169 //
1168 1170 // coarse time and fine time registers are not initialized, they are volatile
1169 1171 //
1170 1172 waveform_picker_regs->buffer_length = 0x1f8;// buffer length in burst = 3 * 2688 / 16 = 504 = 0x1f8
1171 1173 }
1172 1174
1173 1175 void set_wfp_data_shaping( void )
1174 1176 {
1175 1177 /** This function sets the data_shaping register of the waveform picker module.
1176 1178 *
1177 1179 * The value is read from one field of the parameter_dump_packet structure:\n
1178 1180 * bw_sp0_sp1_r0_r1
1179 1181 *
1180 1182 */
1181 1183
1182 1184 unsigned char data_shaping;
1183 1185
1184 1186 // get the parameters for the data shaping [BW SP0 SP1 R0 R1] in sy_lfr_common1 and configure the register
1185 1187 // waveform picker : [R1 R0 SP1 SP0 BW]
1186 1188
1187 1189 data_shaping = parameter_dump_packet.sy_lfr_common_parameters;
1188 1190
1189 1191 waveform_picker_regs->data_shaping =
1190 1192 ( (data_shaping & 0x20) >> 5 ) // BW
1191 1193 + ( (data_shaping & 0x10) >> 3 ) // SP0
1192 1194 + ( (data_shaping & 0x08) >> 1 ) // SP1
1193 1195 + ( (data_shaping & 0x04) ) // R0
1194 1196 + ( (data_shaping & 0x02) << 3 ) // R1
1195 1197 + ( (data_shaping & 0x01) << 5 ); // R2
1196 1198 }
1197 1199
1198 1200 void set_wfp_burst_enable_register( unsigned char mode )
1199 1201 {
1200 1202 /** This function sets the waveform picker burst_enable register depending on the mode.
1201 1203 *
1202 1204 * @param mode is the LFR mode to launch.
1203 1205 *
1204 1206 * The burst bits shall be before the enable bits.
1205 1207 *
1206 1208 */
1207 1209
1208 1210 // [0000 0000] burst f2, f1, f0 enable f3 f2 f1 f0
1209 1211 // the burst bits shall be set first, before the enable bits
1210 1212 switch(mode) {
1211 1213 case(LFR_MODE_NORMAL):
1212 1214 waveform_picker_regs->run_burst_enable = 0x00; // [0000 0000] no burst enable
1213 1215 waveform_picker_regs->run_burst_enable = 0x0f; // [0000 1111] enable f3 f2 f1 f0
1214 1216 break;
1215 1217 case(LFR_MODE_BURST):
1216 1218 waveform_picker_regs->run_burst_enable = 0x40; // [0100 0000] f2 burst enabled
1217 1219 // waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x04; // [0100] enable f2
1218 1220 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x0c; // [1100] enable f3 AND f2
1219 1221 break;
1220 1222 case(LFR_MODE_SBM1):
1221 1223 waveform_picker_regs->run_burst_enable = 0x20; // [0010 0000] f1 burst enabled
1222 1224 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x0f; // [1111] enable f3 f2 f1 f0
1223 1225 break;
1224 1226 case(LFR_MODE_SBM2):
1225 1227 waveform_picker_regs->run_burst_enable = 0x40; // [0100 0000] f2 burst enabled
1226 1228 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x0f; // [1111] enable f3 f2 f1 f0
1227 1229 break;
1228 1230 default:
1229 1231 waveform_picker_regs->run_burst_enable = 0x00; // [0000 0000] no burst enabled, no waveform enabled
1230 1232 break;
1231 1233 }
1232 1234 }
1233 1235
1234 1236 void set_wfp_delta_snapshot( void )
1235 1237 {
1236 1238 /** This function sets the delta_snapshot register of the waveform picker module.
1237 1239 *
1238 1240 * The value is read from two (unsigned char) of the parameter_dump_packet structure:
1239 1241 * - sy_lfr_n_swf_p[0]
1240 1242 * - sy_lfr_n_swf_p[1]
1241 1243 *
1242 1244 */
1243 1245
1244 1246 unsigned int delta_snapshot;
1245 1247 unsigned int delta_snapshot_in_T2;
1246 1248
1247 1249 delta_snapshot = parameter_dump_packet.sy_lfr_n_swf_p[0]*256
1248 1250 + parameter_dump_packet.sy_lfr_n_swf_p[1];
1249 1251
1250 1252 delta_snapshot_in_T2 = delta_snapshot * 256;
1251 1253 waveform_picker_regs->delta_snapshot = delta_snapshot_in_T2 - 1; // max 4 bytes
1252 1254 }
1253 1255
1254 1256 void set_wfp_delta_f0_f0_2( void )
1255 1257 {
1256 1258 unsigned int delta_snapshot;
1257 1259 unsigned int nb_samples_per_snapshot;
1258 1260 float delta_f0_in_float;
1259 1261
1260 1262 delta_snapshot = waveform_picker_regs->delta_snapshot;
1261 1263 nb_samples_per_snapshot = parameter_dump_packet.sy_lfr_n_swf_l[0] * 256 + parameter_dump_packet.sy_lfr_n_swf_l[1];
1262 1264 delta_f0_in_float =nb_samples_per_snapshot / 2. * ( 1. / 256. - 1. / 24576.) * 256.;
1263 1265
1264 1266 waveform_picker_regs->delta_f0 = delta_snapshot - floor( delta_f0_in_float );
1265 1267 waveform_picker_regs->delta_f0_2 = 0x30; // 48 = 11 0000, max 7 bits
1266 1268 }
1267 1269
1268 1270 void set_wfp_delta_f1( void )
1269 1271 {
1270 1272 unsigned int delta_snapshot;
1271 1273 unsigned int nb_samples_per_snapshot;
1272 1274 float delta_f1_in_float;
1273 1275
1274 1276 delta_snapshot = waveform_picker_regs->delta_snapshot;
1275 1277 nb_samples_per_snapshot = parameter_dump_packet.sy_lfr_n_swf_l[0] * 256 + parameter_dump_packet.sy_lfr_n_swf_l[1];
1276 1278 delta_f1_in_float = nb_samples_per_snapshot / 2. * ( 1. / 256. - 1. / 4096.) * 256.;
1277 1279
1278 1280 waveform_picker_regs->delta_f1 = delta_snapshot - floor( delta_f1_in_float );
1279 1281 }
1280 1282
1281 1283 void set_wfp_delta_f2()
1282 1284 {
1283 1285 unsigned int delta_snapshot;
1284 1286 unsigned int nb_samples_per_snapshot;
1285 1287
1286 1288 delta_snapshot = waveform_picker_regs->delta_snapshot;
1287 1289 nb_samples_per_snapshot = parameter_dump_packet.sy_lfr_n_swf_l[0] * 256 + parameter_dump_packet.sy_lfr_n_swf_l[1];
1288 1290
1289 1291 waveform_picker_regs->delta_f2 = delta_snapshot - nb_samples_per_snapshot / 2;
1290 1292 }
1291 1293
1292 1294 //*****************
1293 1295 // local parameters
1294 1296
1295 1297 void increment_seq_counter_source_id( unsigned char *packet_sequence_control, unsigned int sid )
1296 1298 {
1297 1299 /** This function increments the parameter "sequence_cnt" depending on the sid passed in argument.
1298 1300 *
1299 1301 * @param packet_sequence_control is a pointer toward the parameter sequence_cnt to update.
1300 1302 * @param sid is the source identifier of the packet being updated.
1301 1303 *
1302 1304 * REQ-LFR-SRS-5240 / SSS-CP-FS-590
1303 1305 * The sequence counters shall wrap around from 2^14 to zero.
1304 1306 * The sequence counter shall start at zero at startup.
1305 1307 *
1306 1308 * REQ-LFR-SRS-5239 / SSS-CP-FS-580
1307 1309 * All TM_LFR_SCIENCE_ packets are sent to ground, i.e. destination id = 0
1308 1310 *
1309 1311 */
1310 1312
1311 1313 unsigned short *sequence_cnt;
1312 1314 unsigned short segmentation_grouping_flag;
1313 1315 unsigned short new_packet_sequence_control;
1314 1316 rtems_mode initial_mode_set;
1315 1317 rtems_mode current_mode_set;
1316 1318 rtems_status_code status;
1317 1319
1318 1320 //******************************************
1319 1321 // CHANGE THE MODE OF THE CALLING RTEMS TASK
1320 1322 status = rtems_task_mode( RTEMS_NO_PREEMPT, RTEMS_PREEMPT_MASK, &initial_mode_set );
1321 1323
1322 1324 if ( (sid == SID_NORM_SWF_F0) || (sid == SID_NORM_SWF_F1) || (sid == SID_NORM_SWF_F2)
1323 1325 || (sid == SID_NORM_CWF_F3) || (sid == SID_NORM_CWF_LONG_F3)
1324 1326 || (sid == SID_BURST_CWF_F2)
1325 1327 || (sid == SID_NORM_ASM_F0) || (sid == SID_NORM_ASM_F1) || (sid == SID_NORM_ASM_F2)
1326 1328 || (sid == SID_NORM_BP1_F0) || (sid == SID_NORM_BP1_F1) || (sid == SID_NORM_BP1_F2)
1327 1329 || (sid == SID_NORM_BP2_F0) || (sid == SID_NORM_BP2_F1) || (sid == SID_NORM_BP2_F2)
1328 1330 || (sid == SID_BURST_BP1_F0) || (sid == SID_BURST_BP2_F0)
1329 1331 || (sid == SID_BURST_BP1_F1) || (sid == SID_BURST_BP2_F1) )
1330 1332 {
1331 1333 sequence_cnt = (unsigned short *) &sequenceCounters_SCIENCE_NORMAL_BURST;
1332 1334 }
1333 1335 else if ( (sid ==SID_SBM1_CWF_F1) || (sid ==SID_SBM2_CWF_F2)
1334 1336 || (sid == SID_SBM1_BP1_F0) || (sid == SID_SBM1_BP2_F0)
1335 1337 || (sid == SID_SBM2_BP1_F0) || (sid == SID_SBM2_BP2_F0)
1336 1338 || (sid == SID_SBM2_BP1_F1) || (sid == SID_SBM2_BP2_F1) )
1337 1339 {
1338 1340 sequence_cnt = (unsigned short *) &sequenceCounters_SCIENCE_SBM1_SBM2;
1339 1341 }
1340 1342 else
1341 1343 {
1342 1344 sequence_cnt = (unsigned short *) NULL;
1343 1345 PRINTF1("in increment_seq_counter_source_id *** ERR apid_destid %d not known\n", sid)
1344 1346 }
1345 1347
1346 1348 if (sequence_cnt != NULL)
1347 1349 {
1348 1350 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
1349 1351 *sequence_cnt = (*sequence_cnt) & 0x3fff;
1350 1352
1351 1353 new_packet_sequence_control = segmentation_grouping_flag | (*sequence_cnt) ;
1352 1354
1353 1355 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> 8);
1354 1356 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
1355 1357
1356 1358 // increment the sequence counter
1357 1359 if ( *sequence_cnt < SEQ_CNT_MAX)
1358 1360 {
1359 1361 *sequence_cnt = *sequence_cnt + 1;
1360 1362 }
1361 1363 else
1362 1364 {
1363 1365 *sequence_cnt = 0;
1364 1366 }
1365 1367 }
1366 1368
1367 1369 //***********************************
1368 1370 // RESET THE MODE OF THE CALLING TASK
1369 1371 status = rtems_task_mode( initial_mode_set, RTEMS_PREEMPT_MASK, &current_mode_set );
1370 1372 }
General Comments 0
You need to be logged in to leave comments. Login now