##// END OF EJS Templates
Bug 491 => modification of hk_lfr_vhdl_aa_sm
paul -
r273:92f5d3230acc R3a
parent child
Show More
@@ -1,719 +1,720
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 0:
46 46 node = ring_node_for_averaging_sm_f0;
47 47 break;
48 48 case 1:
49 49 node = ring_node_for_averaging_sm_f1;
50 50 break;
51 51 case 2:
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 & 0x03); // [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 3:
77 77 // UNEXPECTED VALUE
78 78 spectral_matrix_regs->status = 0x03; // [0011]
79 79 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
80 80 break;
81 81 case 1:
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)
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 = 0x01; // [0000 0001]
99 99 break;
100 100 case 2:
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)
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 = 0x02; // [0000 0010]
118 118 break;
119 119 }
120 120 }
121 121
122 122 void spectral_matrices_isr_f1( int statusReg )
123 123 {
124 124 rtems_status_code status_code;
125 125 unsigned char status;
126 126 ring_node *full_ring_node;
127 127
128 128 status = (unsigned char) ((statusReg & 0x0c) >> 2); // [1100] get the status_ready_matrix_f1_x bits
129 129
130 130 switch(status)
131 131 {
132 132 case 0:
133 133 break;
134 134 case 3:
135 135 // UNEXPECTED VALUE
136 136 spectral_matrix_regs->status = 0xc0; // [1100]
137 137 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
138 138 break;
139 139 case 1:
140 140 full_ring_node = current_ring_node_sm_f1->previous;
141 141 full_ring_node->coarseTime = spectral_matrix_regs->f1_0_coarse_time;
142 142 full_ring_node->fineTime = spectral_matrix_regs->f1_0_fine_time;
143 143 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
144 144 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->buffer_address;
145 145 // if there are enough ring nodes ready, wake up an AVFx task
146 146 nb_sm_f1 = nb_sm_f1 + 1;
147 147 if (nb_sm_f1 == NB_SM_BEFORE_AVF1)
148 148 {
149 149 ring_node_for_averaging_sm_f1 = full_ring_node;
150 150 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
151 151 {
152 152 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
153 153 }
154 154 nb_sm_f1 = 0;
155 155 }
156 156 spectral_matrix_regs->status = 0x04; // [0000 0100]
157 157 break;
158 158 case 2:
159 159 full_ring_node = current_ring_node_sm_f1->previous;
160 160 full_ring_node->coarseTime = spectral_matrix_regs->f1_1_coarse_time;
161 161 full_ring_node->fineTime = spectral_matrix_regs->f1_1_fine_time;
162 162 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
163 163 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
164 164 // if there are enough ring nodes ready, wake up an AVFx task
165 165 nb_sm_f1 = nb_sm_f1 + 1;
166 166 if (nb_sm_f1 == NB_SM_BEFORE_AVF1)
167 167 {
168 168 ring_node_for_averaging_sm_f1 = full_ring_node;
169 169 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
170 170 {
171 171 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
172 172 }
173 173 nb_sm_f1 = 0;
174 174 }
175 175 spectral_matrix_regs->status = 0x08; // [1000 0000]
176 176 break;
177 177 }
178 178 }
179 179
180 180 void spectral_matrices_isr_f2( int statusReg )
181 181 {
182 182 unsigned char status;
183 183 rtems_status_code status_code;
184 184
185 185 status = (unsigned char) ((statusReg & 0x30) >> 4); // [0011 0000] get the status_ready_matrix_f2_x bits
186 186
187 187 switch(status)
188 188 {
189 189 case 0:
190 190 break;
191 191 case 3:
192 192 // UNEXPECTED VALUE
193 193 spectral_matrix_regs->status = 0x30; // [0011 0000]
194 194 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
195 195 break;
196 196 case 1:
197 197 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
198 198 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
199 199 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_0_coarse_time;
200 200 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_0_fine_time;
201 201 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->buffer_address;
202 202 spectral_matrix_regs->status = 0x10; // [0001 0000]
203 203 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
204 204 {
205 205 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
206 206 }
207 207 break;
208 208 case 2:
209 209 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
210 210 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
211 211 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_1_coarse_time;
212 212 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_1_fine_time;
213 213 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
214 214 spectral_matrix_regs->status = 0x20; // [0010 0000]
215 215 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
216 216 {
217 217 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
218 218 }
219 219 break;
220 220 }
221 221 }
222 222
223 223 void spectral_matrix_isr_error_handler( int statusReg )
224 224 {
225 225 // STATUS REGISTER
226 226 // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
227 227 // 10 9 8
228 // buffer_full ** bad_component_err ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
228 // buffer_full ** [bad_component_err] ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
229 229 // 7 6 5 4 3 2 1 0
230 // [bad_component_err] not defined in the last version of the VHDL code
230 231
231 232 rtems_status_code status_code;
232 233
233 234 //***************************************************
234 235 // the ASM status register is copied in the HK packet
235 housekeeping_packet.hk_lfr_vhdl_aa_sm = (unsigned char) (statusReg & 0x7c0 >> 6); // [0111 1100 0000]
236 housekeeping_packet.hk_lfr_vhdl_aa_sm = (unsigned char) (statusReg & 0x780 >> 7); // [0111 1000 0000]
236 237
237 238 if (statusReg & 0x7c0) // [0111 1100 0000]
238 239 {
239 240 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_8 );
240 241 }
241 242
242 243 spectral_matrix_regs->status = spectral_matrix_regs->status & 0x7c0;
243 244
244 245 }
245 246
246 247 rtems_isr spectral_matrices_isr( rtems_vector_number vector )
247 248 {
248 249 // STATUS REGISTER
249 250 // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
250 251 // 10 9 8
251 252 // buffer_full ** bad_component_err ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
252 253 // 7 6 5 4 3 2 1 0
253 254
254 255 int statusReg;
255 256
256 257 static restartState state = WAIT_FOR_F2;
257 258
258 259 statusReg = spectral_matrix_regs->status;
259 260
260 261 if (thisIsAnASMRestart == 0)
261 262 { // this is not a restart sequence, process incoming matrices normally
262 263 spectral_matrices_isr_f0( statusReg );
263 264
264 265 spectral_matrices_isr_f1( statusReg );
265 266
266 267 spectral_matrices_isr_f2( statusReg );
267 268 }
268 269 else
269 270 { // a restart sequence has to be launched
270 271 switch (state) {
271 272 case WAIT_FOR_F2:
272 273 if ((statusReg & 0x30) != 0x00) // [0011 0000] check the status_ready_matrix_f2_x bits
273 274 {
274 275 state = WAIT_FOR_F1;
275 276 }
276 277 break;
277 278 case WAIT_FOR_F1:
278 279 if ((statusReg & 0x0c) != 0x00) // [0000 1100] check the status_ready_matrix_f1_x bits
279 280 {
280 281 state = WAIT_FOR_F0;
281 282 }
282 283 break;
283 284 case WAIT_FOR_F0:
284 285 if ((statusReg & 0x03) != 0x00) // [0000 0011] check the status_ready_matrix_f0_x bits
285 286 {
286 287 state = WAIT_FOR_F2;
287 288 thisIsAnASMRestart = 0;
288 289 }
289 290 break;
290 291 default:
291 292 break;
292 293 }
293 294 reset_sm_status();
294 295 }
295 296
296 297 spectral_matrix_isr_error_handler( statusReg );
297 298
298 299 }
299 300
300 301 //******************
301 302 // Spectral Matrices
302 303
303 304 void reset_nb_sm( void )
304 305 {
305 306 nb_sm_f0 = 0;
306 307 nb_sm_f0_aux_f1 = 0;
307 308 nb_sm_f0_aux_f2 = 0;
308 309
309 310 nb_sm_f1 = 0;
310 311 }
311 312
312 313 void SM_init_rings( void )
313 314 {
314 315 init_ring( sm_ring_f0, NB_RING_NODES_SM_F0, sm_f0, TOTAL_SIZE_SM );
315 316 init_ring( sm_ring_f1, NB_RING_NODES_SM_F1, sm_f1, TOTAL_SIZE_SM );
316 317 init_ring( sm_ring_f2, NB_RING_NODES_SM_F2, sm_f2, TOTAL_SIZE_SM );
317 318
318 319 DEBUG_PRINTF1("sm_ring_f0 @%x\n", (unsigned int) sm_ring_f0)
319 320 DEBUG_PRINTF1("sm_ring_f1 @%x\n", (unsigned int) sm_ring_f1)
320 321 DEBUG_PRINTF1("sm_ring_f2 @%x\n", (unsigned int) sm_ring_f2)
321 322 DEBUG_PRINTF1("sm_f0 @%x\n", (unsigned int) sm_f0)
322 323 DEBUG_PRINTF1("sm_f1 @%x\n", (unsigned int) sm_f1)
323 324 DEBUG_PRINTF1("sm_f2 @%x\n", (unsigned int) sm_f2)
324 325 }
325 326
326 327 void ASM_generic_init_ring( ring_node_asm *ring, unsigned char nbNodes )
327 328 {
328 329 unsigned char i;
329 330
330 331 ring[ nbNodes - 1 ].next
331 332 = (ring_node_asm*) &ring[ 0 ];
332 333
333 334 for(i=0; i<nbNodes-1; i++)
334 335 {
335 336 ring[ i ].next = (ring_node_asm*) &ring[ i + 1 ];
336 337 }
337 338 }
338 339
339 340 void SM_reset_current_ring_nodes( void )
340 341 {
341 342 current_ring_node_sm_f0 = sm_ring_f0[0].next;
342 343 current_ring_node_sm_f1 = sm_ring_f1[0].next;
343 344 current_ring_node_sm_f2 = sm_ring_f2[0].next;
344 345
345 346 ring_node_for_averaging_sm_f0 = NULL;
346 347 ring_node_for_averaging_sm_f1 = NULL;
347 348 ring_node_for_averaging_sm_f2 = NULL;
348 349 }
349 350
350 351 //*****************
351 352 // Basic Parameters
352 353
353 354 void BP_init_header( bp_packet *packet,
354 355 unsigned int apid, unsigned char sid,
355 356 unsigned int packetLength, unsigned char blkNr )
356 357 {
357 358 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
358 359 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
359 360 packet->reserved = 0x00;
360 361 packet->userApplication = CCSDS_USER_APP;
361 362 packet->packetID[0] = (unsigned char) (apid >> 8);
362 363 packet->packetID[1] = (unsigned char) (apid);
363 364 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
364 365 packet->packetSequenceControl[1] = 0x00;
365 366 packet->packetLength[0] = (unsigned char) (packetLength >> 8);
366 367 packet->packetLength[1] = (unsigned char) (packetLength);
367 368 // DATA FIELD HEADER
368 369 packet->spare1_pusVersion_spare2 = 0x10;
369 370 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
370 371 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
371 372 packet->destinationID = TM_DESTINATION_ID_GROUND;
372 373 packet->time[0] = 0x00;
373 374 packet->time[1] = 0x00;
374 375 packet->time[2] = 0x00;
375 376 packet->time[3] = 0x00;
376 377 packet->time[4] = 0x00;
377 378 packet->time[5] = 0x00;
378 379 // AUXILIARY DATA HEADER
379 380 packet->sid = sid;
380 381 packet->biaStatusInfo = 0x00;
381 382 packet->sy_lfr_common_parameters_spare = 0x00;
382 383 packet->sy_lfr_common_parameters = 0x00;
383 384 packet->acquisitionTime[0] = 0x00;
384 385 packet->acquisitionTime[1] = 0x00;
385 386 packet->acquisitionTime[2] = 0x00;
386 387 packet->acquisitionTime[3] = 0x00;
387 388 packet->acquisitionTime[4] = 0x00;
388 389 packet->acquisitionTime[5] = 0x00;
389 390 packet->pa_lfr_bp_blk_nr[0] = 0x00; // BLK_NR MSB
390 391 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
391 392 }
392 393
393 394 void BP_init_header_with_spare( bp_packet_with_spare *packet,
394 395 unsigned int apid, unsigned char sid,
395 396 unsigned int packetLength , unsigned char blkNr)
396 397 {
397 398 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
398 399 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
399 400 packet->reserved = 0x00;
400 401 packet->userApplication = CCSDS_USER_APP;
401 402 packet->packetID[0] = (unsigned char) (apid >> 8);
402 403 packet->packetID[1] = (unsigned char) (apid);
403 404 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
404 405 packet->packetSequenceControl[1] = 0x00;
405 406 packet->packetLength[0] = (unsigned char) (packetLength >> 8);
406 407 packet->packetLength[1] = (unsigned char) (packetLength);
407 408 // DATA FIELD HEADER
408 409 packet->spare1_pusVersion_spare2 = 0x10;
409 410 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
410 411 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
411 412 packet->destinationID = TM_DESTINATION_ID_GROUND;
412 413 // AUXILIARY DATA HEADER
413 414 packet->sid = sid;
414 415 packet->biaStatusInfo = 0x00;
415 416 packet->sy_lfr_common_parameters_spare = 0x00;
416 417 packet->sy_lfr_common_parameters = 0x00;
417 418 packet->time[0] = 0x00;
418 419 packet->time[0] = 0x00;
419 420 packet->time[0] = 0x00;
420 421 packet->time[0] = 0x00;
421 422 packet->time[0] = 0x00;
422 423 packet->time[0] = 0x00;
423 424 packet->source_data_spare = 0x00;
424 425 packet->pa_lfr_bp_blk_nr[0] = 0x00; // BLK_NR MSB
425 426 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
426 427 }
427 428
428 429 void BP_send(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
429 430 {
430 431 rtems_status_code status;
431 432
432 433 // SEND PACKET
433 434 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
434 435 if (status != RTEMS_SUCCESSFUL)
435 436 {
436 437 PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
437 438 }
438 439 }
439 440
440 441 void BP_send_s1_s2(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
441 442 {
442 443 /** This function is used to send the BP paquets when needed.
443 444 *
444 445 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
445 446 *
446 447 * @return void
447 448 *
448 449 * SBM1 and SBM2 paquets are sent depending on the type of the LFR mode transition.
449 450 * BURST paquets are sent everytime.
450 451 *
451 452 */
452 453
453 454 rtems_status_code status;
454 455
455 456 // SEND PACKET
456 457 // before lastValidTransitionDate, the data are drops even if they are ready
457 458 // this guarantees that no SBM packets will be received before the requested enter mode time
458 459 if ( time_management_regs->coarse_time >= lastValidEnterModeTime)
459 460 {
460 461 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
461 462 if (status != RTEMS_SUCCESSFUL)
462 463 {
463 464 PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
464 465 }
465 466 }
466 467 }
467 468
468 469 //******************
469 470 // general functions
470 471
471 472 void reset_sm_status( void )
472 473 {
473 474 // error
474 475 // 10 --------------- 9 ---------------- 8 ---------------- 7 ---------
475 476 // input_fif0_write_2 input_fifo_write_1 input_fifo_write_0 buffer_full
476 477 // ---------- 5 -- 4 -- 3 -- 2 -- 1 -- 0 --
477 478 // ready bits f2_1 f2_0 f1_1 f1_1 f0_1 f0_0
478 479
479 480 spectral_matrix_regs->status = 0x7ff; // [0111 1111 1111]
480 481 }
481 482
482 483 void reset_spectral_matrix_regs( void )
483 484 {
484 485 /** This function resets the spectral matrices module registers.
485 486 *
486 487 * The registers affected by this function are located at the following offset addresses:
487 488 *
488 489 * - 0x00 config
489 490 * - 0x04 status
490 491 * - 0x08 matrixF0_Address0
491 492 * - 0x10 matrixFO_Address1
492 493 * - 0x14 matrixF1_Address
493 494 * - 0x18 matrixF2_Address
494 495 *
495 496 */
496 497
497 498 set_sm_irq_onError( 0 );
498 499
499 500 set_sm_irq_onNewMatrix( 0 );
500 501
501 502 reset_sm_status();
502 503
503 504 // F1
504 505 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->previous->buffer_address;
505 506 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
506 507 // F2
507 508 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->previous->buffer_address;
508 509 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
509 510 // F3
510 511 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->previous->buffer_address;
511 512 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
512 513
513 514 spectral_matrix_regs->matrix_length = 0xc8; // 25 * 128 / 16 = 200 = 0xc8
514 515 }
515 516
516 517 void set_time( unsigned char *time, unsigned char * timeInBuffer )
517 518 {
518 519 time[0] = timeInBuffer[0];
519 520 time[1] = timeInBuffer[1];
520 521 time[2] = timeInBuffer[2];
521 522 time[3] = timeInBuffer[3];
522 523 time[4] = timeInBuffer[6];
523 524 time[5] = timeInBuffer[7];
524 525 }
525 526
526 527 unsigned long long int get_acquisition_time( unsigned char *timePtr )
527 528 {
528 529 unsigned long long int acquisitionTimeAslong;
529 530 acquisitionTimeAslong = 0x00;
530 531 acquisitionTimeAslong = ( (unsigned long long int) (timePtr[0] & 0x7f) << 40 ) // [0111 1111] mask the synchronization bit
531 532 + ( (unsigned long long int) timePtr[1] << 32 )
532 533 + ( (unsigned long long int) timePtr[2] << 24 )
533 534 + ( (unsigned long long int) timePtr[3] << 16 )
534 535 + ( (unsigned long long int) timePtr[6] << 8 )
535 536 + ( (unsigned long long int) timePtr[7] );
536 537 return acquisitionTimeAslong;
537 538 }
538 539
539 540 unsigned char getSID( rtems_event_set event )
540 541 {
541 542 unsigned char sid;
542 543
543 544 rtems_event_set eventSetBURST;
544 545 rtems_event_set eventSetSBM;
545 546
546 547 //******
547 548 // BURST
548 549 eventSetBURST = RTEMS_EVENT_BURST_BP1_F0
549 550 | RTEMS_EVENT_BURST_BP1_F1
550 551 | RTEMS_EVENT_BURST_BP2_F0
551 552 | RTEMS_EVENT_BURST_BP2_F1;
552 553
553 554 //****
554 555 // SBM
555 556 eventSetSBM = RTEMS_EVENT_SBM_BP1_F0
556 557 | RTEMS_EVENT_SBM_BP1_F1
557 558 | RTEMS_EVENT_SBM_BP2_F0
558 559 | RTEMS_EVENT_SBM_BP2_F1;
559 560
560 561 if (event & eventSetBURST)
561 562 {
562 563 sid = SID_BURST_BP1_F0;
563 564 }
564 565 else if (event & eventSetSBM)
565 566 {
566 567 sid = SID_SBM1_BP1_F0;
567 568 }
568 569 else
569 570 {
570 571 sid = 0;
571 572 }
572 573
573 574 return sid;
574 575 }
575 576
576 577 void extractReImVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
577 578 {
578 579 unsigned int i;
579 580 float re;
580 581 float im;
581 582
582 583 for (i=0; i<NB_BINS_PER_SM; i++){
583 584 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i * 2 ];
584 585 im = inputASM[ (asmComponent*NB_BINS_PER_SM) + i * 2 + 1];
585 586 outputASM[ (asmComponent *NB_BINS_PER_SM) + i] = re;
586 587 outputASM[ (asmComponent+1)*NB_BINS_PER_SM + i] = im;
587 588 }
588 589 }
589 590
590 591 void copyReVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
591 592 {
592 593 unsigned int i;
593 594 float re;
594 595
595 596 for (i=0; i<NB_BINS_PER_SM; i++){
596 597 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i];
597 598 outputASM[ (asmComponent*NB_BINS_PER_SM) + i] = re;
598 599 }
599 600 }
600 601
601 602 void ASM_patch( float *inputASM, float *outputASM )
602 603 {
603 604 extractReImVectors( inputASM, outputASM, 1); // b1b2
604 605 extractReImVectors( inputASM, outputASM, 3 ); // b1b3
605 606 extractReImVectors( inputASM, outputASM, 5 ); // b1e1
606 607 extractReImVectors( inputASM, outputASM, 7 ); // b1e2
607 608 extractReImVectors( inputASM, outputASM, 10 ); // b2b3
608 609 extractReImVectors( inputASM, outputASM, 12 ); // b2e1
609 610 extractReImVectors( inputASM, outputASM, 14 ); // b2e2
610 611 extractReImVectors( inputASM, outputASM, 17 ); // b3e1
611 612 extractReImVectors( inputASM, outputASM, 19 ); // b3e2
612 613 extractReImVectors( inputASM, outputASM, 22 ); // e1e2
613 614
614 615 copyReVectors(inputASM, outputASM, 0 ); // b1b1
615 616 copyReVectors(inputASM, outputASM, 9 ); // b2b2
616 617 copyReVectors(inputASM, outputASM, 16); // b3b3
617 618 copyReVectors(inputASM, outputASM, 21); // e1e1
618 619 copyReVectors(inputASM, outputASM, 24); // e2e2
619 620 }
620 621
621 622 void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
622 623 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage,
623 624 unsigned char ASMIndexStart,
624 625 unsigned char channel )
625 626 {
626 627 //*************
627 628 // input format
628 629 // component0[0 .. 127] component1[0 .. 127] .. component24[0 .. 127]
629 630 //**************
630 631 // output format
631 632 // matr0[0 .. 24] matr1[0 .. 24] .. matr127[0 .. 24]
632 633 //************
633 634 // compression
634 635 // matr0[0 .. 24] matr1[0 .. 24] .. matr11[0 .. 24] => f0 NORM
635 636 // matr0[0 .. 24] matr1[0 .. 24] .. matr22[0 .. 24] => f0 BURST, SBM
636 637
637 638 int frequencyBin;
638 639 int asmComponent;
639 640 int offsetASM;
640 641 int offsetCompressed;
641 642 int offsetFBin;
642 643 int fBinMask;
643 644 int k;
644 645
645 646 // BUILD DATA
646 647 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
647 648 {
648 649 for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
649 650 {
650 651 offsetCompressed = // NO TIME OFFSET
651 652 frequencyBin * NB_VALUES_PER_SM
652 653 + asmComponent;
653 654 offsetASM = // NO TIME OFFSET
654 655 asmComponent * NB_BINS_PER_SM
655 656 + ASMIndexStart
656 657 + frequencyBin * nbBinsToAverage;
657 658 offsetFBin = ASMIndexStart
658 659 + frequencyBin * nbBinsToAverage;
659 660 compressed_spec_mat[ offsetCompressed ] = 0;
660 661 for ( k = 0; k < nbBinsToAverage; k++ )
661 662 {
662 663 fBinMask = getFBinMask( offsetFBin + k, channel );
663 664 compressed_spec_mat[offsetCompressed ] =
664 665 ( compressed_spec_mat[ offsetCompressed ]
665 666 + averaged_spec_mat[ offsetASM + k ] * fBinMask );
666 667 }
667 668 compressed_spec_mat[ offsetCompressed ] =
668 669 compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
669 670 }
670 671 }
671 672
672 673 }
673 674
674 675 int getFBinMask( int index, unsigned char channel )
675 676 {
676 677 unsigned int indexInChar;
677 678 unsigned int indexInTheChar;
678 679 int fbin;
679 680 unsigned char *sy_lfr_fbins_fx_word1;
680 681
681 682 sy_lfr_fbins_fx_word1 = parameter_dump_packet.sy_lfr_fbins_f0_word1;
682 683
683 684 switch(channel)
684 685 {
685 686 case 0:
686 687 sy_lfr_fbins_fx_word1 = parameter_dump_packet.sy_lfr_fbins_f0_word1;
687 688 break;
688 689 case 1:
689 690 sy_lfr_fbins_fx_word1 = parameter_dump_packet.sy_lfr_fbins_f1_word1;
690 691 break;
691 692 case 2:
692 693 sy_lfr_fbins_fx_word1 = parameter_dump_packet.sy_lfr_fbins_f2_word1;
693 694 break;
694 695 default:
695 696 PRINTF("ERR *** in getFBinMask, wrong frequency channel")
696 697 }
697 698
698 699 indexInChar = index >> 3;
699 700 indexInTheChar = index - indexInChar * 8;
700 701
701 702 fbin = (int) ((sy_lfr_fbins_fx_word1[ NB_BYTES_PER_FREQ_MASK - 1 - indexInChar] >> indexInTheChar) & 0x1);
702 703
703 704 return fbin;
704 705 }
705 706
706 707 void init_kcoeff_sbm_from_kcoeff_norm(float *input_kcoeff, float *output_kcoeff, unsigned char nb_bins_norm)
707 708 {
708 709 unsigned char bin;
709 710 unsigned char kcoeff;
710 711
711 712 for (bin=0; bin<nb_bins_norm; bin++)
712 713 {
713 714 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
714 715 {
715 716 output_kcoeff[ (bin*NB_K_COEFF_PER_BIN + kcoeff)*2 ] = input_kcoeff[ bin*NB_K_COEFF_PER_BIN + kcoeff ];
716 717 output_kcoeff[ (bin*NB_K_COEFF_PER_BIN + kcoeff)*2 + 1 ] = input_kcoeff[ bin*NB_K_COEFF_PER_BIN + kcoeff ];
717 718 }
718 719 }
719 720 }
General Comments 0
You need to be logged in to leave comments. Login now