##// END OF EJS Templates
setFBinMask corrected...
paul -
r313:972ae9f0eb8f R3_plus draft
parent child
Show More
@@ -1,2 +1,2
1 1 3081d1f9bb20b2b64a192585337a292a9804e0c5 LFR_basic-parameters
2 94f0f2fccbcb8030d9437ffbb69ee0eefaaea188 header/lfr_common_headers
2 57edc38eadba4601cf0b1e2fa1eeab85082e9f41 header/lfr_common_headers
@@ -1,106 +1,108
1 1 cmake_minimum_required (VERSION 2.6)
2 2 project (fsw)
3 3
4 4 include(sparc-rtems)
5 5 include(cppcheck)
6 6
7 7 include_directories("../header"
8 8 "../header/lfr_common_headers"
9 9 "../header/processing"
10 10 "../LFR_basic-parameters"
11 11 "../src")
12 12
13 13 set(SOURCES wf_handler.c
14 14 tc_handler.c
15 15 fsw_misc.c
16 16 fsw_init.c
17 17 fsw_globals.c
18 18 fsw_spacewire.c
19 19 tc_load_dump_parameters.c
20 20 tm_lfr_tc_exe.c
21 21 tc_acceptance.c
22 22 processing/fsw_processing.c
23 23 processing/avf0_prc0.c
24 24 processing/avf1_prc1.c
25 25 processing/avf2_prc2.c
26 26 lfr_cpu_usage_report.c
27 27 ${LFR_BP_SRC}
28 28 ../header/wf_handler.h
29 29 ../header/tc_handler.h
30 30 ../header/grlib_regs.h
31 31 ../header/fsw_misc.h
32 32 ../header/fsw_init.h
33 33 ../header/fsw_spacewire.h
34 34 ../header/tc_load_dump_parameters.h
35 35 ../header/tm_lfr_tc_exe.h
36 36 ../header/tc_acceptance.h
37 37 ../header/processing/fsw_processing.h
38 38 ../header/processing/avf0_prc0.h
39 39 ../header/processing/avf1_prc1.h
40 40 ../header/processing/avf2_prc2.h
41 41 ../header/fsw_params_wf_handler.h
42 42 ../header/lfr_cpu_usage_report.h
43 43 ../header/lfr_common_headers/ccsds_types.h
44 44 ../header/lfr_common_headers/fsw_params.h
45 45 ../header/lfr_common_headers/fsw_params_nb_bytes.h
46 46 ../header/lfr_common_headers/fsw_params_processing.h
47 47 ../header/lfr_common_headers/tm_byte_positions.h
48 48 ../LFR_basic-parameters/basic_parameters.h
49 49 ../LFR_basic-parameters/basic_parameters_params.h
50 50 ../header/GscMemoryLPP.hpp
51 51 )
52 52
53 53
54 54 option(FSW_verbose "Enable verbose LFR" ON)
55 55 option(FSW_boot_messages "Enable LFR boot messages" ON)
56 56 option(FSW_debug_messages "Enable LFR debug messages" ON)
57 57 option(FSW_cpu_usage_report "Enable LFR cpu usage report" OFF)
58 58 option(FSW_stack_report "Enable LFR stack report" OFF)
59 59 option(FSW_vhdl_dev "?" OFF)
60 60 option(FSW_lpp_dpu_destid "Set to debug at LPP" ON)
61 61 option(FSW_debug_watchdog "Enable debug watchdog" OFF)
62 62 option(FSW_debug_tch "?" OFF)
63 63
64 64 set(SW_VERSION_N1 "3" CACHE STRING "Choose N1 FSW Version." FORCE)
65 65 set(SW_VERSION_N2 "1" CACHE STRING "Choose N2 FSW Version." FORCE)
66 66 set(SW_VERSION_N3 "0" CACHE STRING "Choose N3 FSW Version." FORCE)
67 67 set(SW_VERSION_N4 "4" CACHE STRING "Choose N4 FSW Version." FORCE)
68 68
69 69
70 70 if(FSW_verbose)
71 71 add_definitions(-DPRINT_MESSAGES_ON_CONSOLE)
72 72 endif()
73 73 if(FSW_boot_messages)
74 74 add_definitions(-DBOOT_MESSAGES)
75 75 endif()
76 76 if(FSW_debug_messages)
77 77 add_definitions(-DDEBUG_MESSAGES)
78 78 endif()
79 79 if(FSW_cpu_usage_report)
80 80 add_definitions(-DPRINT_TASK_STATISTICS)
81 81 endif()
82 82 if(FSW_stack_report)
83 83 add_definitions(-DPRINT_STACK_REPORT)
84 84 endif()
85 85 if(FSW_vhdl_dev)
86 86 add_definitions(-DVHDL_DEV)
87 87 endif()
88 88 if(FSW_lpp_dpu_destid)
89 89 add_definitions(-DLPP_DPU_DESTID)
90 90 endif()
91 91 if(FSW_debug_watchdog)
92 92 add_definitions(-DDEBUG_WATCHDOG)
93 93 endif()
94 94 if(FSW_debug_tch)
95 95 add_definitions(-DDEBUG_TCH)
96 96 endif()
97 97
98 98 add_definitions(-DMSB_FIRST_TCH)
99 99
100 100 add_definitions(-DSWVERSION=-1-0)
101 101 add_definitions(-DSW_VERSION_N1=${SW_VERSION_N1})
102 102 add_definitions(-DSW_VERSION_N2=${SW_VERSION_N2})
103 103 add_definitions(-DSW_VERSION_N3=${SW_VERSION_N3})
104 104 add_definitions(-DSW_VERSION_N4=${SW_VERSION_N4})
105 105
106 106 add_executable(FSW ${SOURCES})
107 add_test_cppcheck(FSW STYLE UNUSED_FUNCTIONS POSSIBLE_ERROR MISSING_INCLUDE)
108
@@ -1,786 +1,786
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 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 230 // [bad_component_err] not defined in the last version of the VHDL code
231 231
232 232 rtems_status_code status_code;
233 233
234 234 //***************************************************
235 235 // the ASM status register is copied in the HK packet
236 236 housekeeping_packet.hk_lfr_vhdl_aa_sm = (unsigned char) (statusReg & 0x780 >> 7); // [0111 1000 0000]
237 237
238 238 if (statusReg & 0x7c0) // [0111 1100 0000]
239 239 {
240 240 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_8 );
241 241 }
242 242
243 243 spectral_matrix_regs->status = spectral_matrix_regs->status & 0x7c0;
244 244
245 245 }
246 246
247 247 rtems_isr spectral_matrices_isr( rtems_vector_number vector )
248 248 {
249 249 // STATUS REGISTER
250 250 // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
251 251 // 10 9 8
252 252 // buffer_full ** bad_component_err ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
253 253 // 7 6 5 4 3 2 1 0
254 254
255 255 int statusReg;
256 256
257 257 static restartState state = WAIT_FOR_F2;
258 258
259 259 statusReg = spectral_matrix_regs->status;
260 260
261 261 if (thisIsAnASMRestart == 0)
262 262 { // this is not a restart sequence, process incoming matrices normally
263 263 spectral_matrices_isr_f0( statusReg );
264 264
265 265 spectral_matrices_isr_f1( statusReg );
266 266
267 267 spectral_matrices_isr_f2( statusReg );
268 268 }
269 269 else
270 270 { // a restart sequence has to be launched
271 271 switch (state) {
272 272 case WAIT_FOR_F2:
273 273 if ((statusReg & 0x30) != 0x00) // [0011 0000] check the status_ready_matrix_f2_x bits
274 274 {
275 275 state = WAIT_FOR_F1;
276 276 }
277 277 break;
278 278 case WAIT_FOR_F1:
279 279 if ((statusReg & 0x0c) != 0x00) // [0000 1100] check the status_ready_matrix_f1_x bits
280 280 {
281 281 state = WAIT_FOR_F0;
282 282 }
283 283 break;
284 284 case WAIT_FOR_F0:
285 285 if ((statusReg & 0x03) != 0x00) // [0000 0011] check the status_ready_matrix_f0_x bits
286 286 {
287 287 state = WAIT_FOR_F2;
288 288 thisIsAnASMRestart = 0;
289 289 }
290 290 break;
291 291 default:
292 292 break;
293 293 }
294 294 reset_sm_status();
295 295 }
296 296
297 297 spectral_matrix_isr_error_handler( statusReg );
298 298
299 299 }
300 300
301 301 //******************
302 302 // Spectral Matrices
303 303
304 304 void reset_nb_sm( void )
305 305 {
306 306 nb_sm_f0 = 0;
307 307 nb_sm_f0_aux_f1 = 0;
308 308 nb_sm_f0_aux_f2 = 0;
309 309
310 310 nb_sm_f1 = 0;
311 311 }
312 312
313 313 void SM_init_rings( void )
314 314 {
315 315 init_ring( sm_ring_f0, NB_RING_NODES_SM_F0, sm_f0, TOTAL_SIZE_SM );
316 316 init_ring( sm_ring_f1, NB_RING_NODES_SM_F1, sm_f1, TOTAL_SIZE_SM );
317 317 init_ring( sm_ring_f2, NB_RING_NODES_SM_F2, sm_f2, TOTAL_SIZE_SM );
318 318
319 319 DEBUG_PRINTF1("sm_ring_f0 @%x\n", (unsigned int) sm_ring_f0)
320 320 DEBUG_PRINTF1("sm_ring_f1 @%x\n", (unsigned int) sm_ring_f1)
321 321 DEBUG_PRINTF1("sm_ring_f2 @%x\n", (unsigned int) sm_ring_f2)
322 322 DEBUG_PRINTF1("sm_f0 @%x\n", (unsigned int) sm_f0)
323 323 DEBUG_PRINTF1("sm_f1 @%x\n", (unsigned int) sm_f1)
324 324 DEBUG_PRINTF1("sm_f2 @%x\n", (unsigned int) sm_f2)
325 325 }
326 326
327 327 void ASM_generic_init_ring( ring_node_asm *ring, unsigned char nbNodes )
328 328 {
329 329 unsigned char i;
330 330
331 331 ring[ nbNodes - 1 ].next
332 332 = (ring_node_asm*) &ring[ 0 ];
333 333
334 334 for(i=0; i<nbNodes-1; i++)
335 335 {
336 336 ring[ i ].next = (ring_node_asm*) &ring[ i + 1 ];
337 337 }
338 338 }
339 339
340 340 void SM_reset_current_ring_nodes( void )
341 341 {
342 342 current_ring_node_sm_f0 = sm_ring_f0[0].next;
343 343 current_ring_node_sm_f1 = sm_ring_f1[0].next;
344 344 current_ring_node_sm_f2 = sm_ring_f2[0].next;
345 345
346 346 ring_node_for_averaging_sm_f0 = NULL;
347 347 ring_node_for_averaging_sm_f1 = NULL;
348 348 ring_node_for_averaging_sm_f2 = NULL;
349 349 }
350 350
351 351 //*****************
352 352 // Basic Parameters
353 353
354 354 void BP_init_header( bp_packet *packet,
355 355 unsigned int apid, unsigned char sid,
356 356 unsigned int packetLength, unsigned char blkNr )
357 357 {
358 358 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
359 359 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
360 360 packet->reserved = 0x00;
361 361 packet->userApplication = CCSDS_USER_APP;
362 362 packet->packetID[0] = (unsigned char) (apid >> 8);
363 363 packet->packetID[1] = (unsigned char) (apid);
364 364 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
365 365 packet->packetSequenceControl[1] = 0x00;
366 366 packet->packetLength[0] = (unsigned char) (packetLength >> 8);
367 367 packet->packetLength[1] = (unsigned char) (packetLength);
368 368 // DATA FIELD HEADER
369 369 packet->spare1_pusVersion_spare2 = 0x10;
370 370 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
371 371 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
372 372 packet->destinationID = TM_DESTINATION_ID_GROUND;
373 373 packet->time[0] = 0x00;
374 374 packet->time[1] = 0x00;
375 375 packet->time[2] = 0x00;
376 376 packet->time[3] = 0x00;
377 377 packet->time[4] = 0x00;
378 378 packet->time[5] = 0x00;
379 379 // AUXILIARY DATA HEADER
380 380 packet->sid = sid;
381 381 packet->pa_bia_status_info = 0x00;
382 382 packet->sy_lfr_common_parameters_spare = 0x00;
383 383 packet->sy_lfr_common_parameters = 0x00;
384 384 packet->acquisitionTime[0] = 0x00;
385 385 packet->acquisitionTime[1] = 0x00;
386 386 packet->acquisitionTime[2] = 0x00;
387 387 packet->acquisitionTime[3] = 0x00;
388 388 packet->acquisitionTime[4] = 0x00;
389 389 packet->acquisitionTime[5] = 0x00;
390 390 packet->pa_lfr_bp_blk_nr[0] = 0x00; // BLK_NR MSB
391 391 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
392 392 }
393 393
394 394 void BP_init_header_with_spare( bp_packet_with_spare *packet,
395 395 unsigned int apid, unsigned char sid,
396 396 unsigned int packetLength , unsigned char blkNr)
397 397 {
398 398 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
399 399 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
400 400 packet->reserved = 0x00;
401 401 packet->userApplication = CCSDS_USER_APP;
402 402 packet->packetID[0] = (unsigned char) (apid >> 8);
403 403 packet->packetID[1] = (unsigned char) (apid);
404 404 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
405 405 packet->packetSequenceControl[1] = 0x00;
406 406 packet->packetLength[0] = (unsigned char) (packetLength >> 8);
407 407 packet->packetLength[1] = (unsigned char) (packetLength);
408 408 // DATA FIELD HEADER
409 409 packet->spare1_pusVersion_spare2 = 0x10;
410 410 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
411 411 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
412 412 packet->destinationID = TM_DESTINATION_ID_GROUND;
413 413 // AUXILIARY DATA HEADER
414 414 packet->sid = sid;
415 415 packet->pa_bia_status_info = 0x00;
416 416 packet->sy_lfr_common_parameters_spare = 0x00;
417 417 packet->sy_lfr_common_parameters = 0x00;
418 418 packet->time[0] = 0x00;
419 419 packet->time[0] = 0x00;
420 420 packet->time[0] = 0x00;
421 421 packet->time[0] = 0x00;
422 422 packet->time[0] = 0x00;
423 423 packet->time[0] = 0x00;
424 424 packet->source_data_spare = 0x00;
425 425 packet->pa_lfr_bp_blk_nr[0] = 0x00; // BLK_NR MSB
426 426 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
427 427 }
428 428
429 429 void BP_send(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
430 430 {
431 431 rtems_status_code status;
432 432
433 433 // SEND PACKET
434 434 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
435 435 if (status != RTEMS_SUCCESSFUL)
436 436 {
437 437 PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
438 438 }
439 439 }
440 440
441 441 void BP_send_s1_s2(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
442 442 {
443 443 /** This function is used to send the BP paquets when needed.
444 444 *
445 445 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
446 446 *
447 447 * @return void
448 448 *
449 449 * SBM1 and SBM2 paquets are sent depending on the type of the LFR mode transition.
450 450 * BURST paquets are sent everytime.
451 451 *
452 452 */
453 453
454 454 rtems_status_code status;
455 455
456 456 // SEND PACKET
457 457 // before lastValidTransitionDate, the data are drops even if they are ready
458 458 // this guarantees that no SBM packets will be received before the requested enter mode time
459 459 if ( time_management_regs->coarse_time >= lastValidEnterModeTime)
460 460 {
461 461 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
462 462 if (status != RTEMS_SUCCESSFUL)
463 463 {
464 464 PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
465 465 }
466 466 }
467 467 }
468 468
469 469 //******************
470 470 // general functions
471 471
472 472 void reset_sm_status( void )
473 473 {
474 474 // error
475 475 // 10 --------------- 9 ---------------- 8 ---------------- 7 ---------
476 476 // input_fif0_write_2 input_fifo_write_1 input_fifo_write_0 buffer_full
477 477 // ---------- 5 -- 4 -- 3 -- 2 -- 1 -- 0 --
478 478 // ready bits f2_1 f2_0 f1_1 f1_1 f0_1 f0_0
479 479
480 480 spectral_matrix_regs->status = 0x7ff; // [0111 1111 1111]
481 481 }
482 482
483 483 void reset_spectral_matrix_regs( void )
484 484 {
485 485 /** This function resets the spectral matrices module registers.
486 486 *
487 487 * The registers affected by this function are located at the following offset addresses:
488 488 *
489 489 * - 0x00 config
490 490 * - 0x04 status
491 491 * - 0x08 matrixF0_Address0
492 492 * - 0x10 matrixFO_Address1
493 493 * - 0x14 matrixF1_Address
494 494 * - 0x18 matrixF2_Address
495 495 *
496 496 */
497 497
498 498 set_sm_irq_onError( 0 );
499 499
500 500 set_sm_irq_onNewMatrix( 0 );
501 501
502 502 reset_sm_status();
503 503
504 504 // F1
505 505 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->previous->buffer_address;
506 506 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
507 507 // F2
508 508 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->previous->buffer_address;
509 509 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
510 510 // F3
511 511 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->previous->buffer_address;
512 512 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
513 513
514 514 spectral_matrix_regs->matrix_length = 0xc8; // 25 * 128 / 16 = 200 = 0xc8
515 515 }
516 516
517 517 void set_time( unsigned char *time, unsigned char * timeInBuffer )
518 518 {
519 519 time[0] = timeInBuffer[0];
520 520 time[1] = timeInBuffer[1];
521 521 time[2] = timeInBuffer[2];
522 522 time[3] = timeInBuffer[3];
523 523 time[4] = timeInBuffer[6];
524 524 time[5] = timeInBuffer[7];
525 525 }
526 526
527 527 unsigned long long int get_acquisition_time( unsigned char *timePtr )
528 528 {
529 529 unsigned long long int acquisitionTimeAslong;
530 530 acquisitionTimeAslong = 0x00;
531 531 acquisitionTimeAslong = ( (unsigned long long int) (timePtr[0] & 0x7f) << 40 ) // [0111 1111] mask the synchronization bit
532 532 + ( (unsigned long long int) timePtr[1] << 32 )
533 533 + ( (unsigned long long int) timePtr[2] << 24 )
534 534 + ( (unsigned long long int) timePtr[3] << 16 )
535 535 + ( (unsigned long long int) timePtr[6] << 8 )
536 536 + ( (unsigned long long int) timePtr[7] );
537 537 return acquisitionTimeAslong;
538 538 }
539 539
540 540 unsigned char getSID( rtems_event_set event )
541 541 {
542 542 unsigned char sid;
543 543
544 544 rtems_event_set eventSetBURST;
545 545 rtems_event_set eventSetSBM;
546 546
547 547 //******
548 548 // BURST
549 549 eventSetBURST = RTEMS_EVENT_BURST_BP1_F0
550 550 | RTEMS_EVENT_BURST_BP1_F1
551 551 | RTEMS_EVENT_BURST_BP2_F0
552 552 | RTEMS_EVENT_BURST_BP2_F1;
553 553
554 554 //****
555 555 // SBM
556 556 eventSetSBM = RTEMS_EVENT_SBM_BP1_F0
557 557 | RTEMS_EVENT_SBM_BP1_F1
558 558 | RTEMS_EVENT_SBM_BP2_F0
559 559 | RTEMS_EVENT_SBM_BP2_F1;
560 560
561 561 if (event & eventSetBURST)
562 562 {
563 563 sid = SID_BURST_BP1_F0;
564 564 }
565 565 else if (event & eventSetSBM)
566 566 {
567 567 sid = SID_SBM1_BP1_F0;
568 568 }
569 569 else
570 570 {
571 571 sid = 0;
572 572 }
573 573
574 574 return sid;
575 575 }
576 576
577 577 void extractReImVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
578 578 {
579 579 unsigned int i;
580 580 float re;
581 581 float im;
582 582
583 583 for (i=0; i<NB_BINS_PER_SM; i++){
584 584 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i * 2 ];
585 585 im = inputASM[ (asmComponent*NB_BINS_PER_SM) + i * 2 + 1];
586 586 outputASM[ (asmComponent *NB_BINS_PER_SM) + i] = re;
587 587 outputASM[ (asmComponent+1)*NB_BINS_PER_SM + i] = im;
588 588 }
589 589 }
590 590
591 591 void copyReVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
592 592 {
593 593 unsigned int i;
594 594 float re;
595 595
596 596 for (i=0; i<NB_BINS_PER_SM; i++){
597 597 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i];
598 598 outputASM[ (asmComponent*NB_BINS_PER_SM) + i] = re;
599 599 }
600 600 }
601 601
602 602 void ASM_patch( float *inputASM, float *outputASM )
603 603 {
604 604 extractReImVectors( inputASM, outputASM, 1); // b1b2
605 605 extractReImVectors( inputASM, outputASM, 3 ); // b1b3
606 606 extractReImVectors( inputASM, outputASM, 5 ); // b1e1
607 607 extractReImVectors( inputASM, outputASM, 7 ); // b1e2
608 608 extractReImVectors( inputASM, outputASM, 10 ); // b2b3
609 609 extractReImVectors( inputASM, outputASM, 12 ); // b2e1
610 610 extractReImVectors( inputASM, outputASM, 14 ); // b2e2
611 611 extractReImVectors( inputASM, outputASM, 17 ); // b3e1
612 612 extractReImVectors( inputASM, outputASM, 19 ); // b3e2
613 613 extractReImVectors( inputASM, outputASM, 22 ); // e1e2
614 614
615 615 copyReVectors(inputASM, outputASM, 0 ); // b1b1
616 616 copyReVectors(inputASM, outputASM, 9 ); // b2b2
617 617 copyReVectors(inputASM, outputASM, 16); // b3b3
618 618 copyReVectors(inputASM, outputASM, 21); // e1e1
619 619 copyReVectors(inputASM, outputASM, 24); // e2e2
620 620 }
621 621
622 622 void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
623 623 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage,
624 624 unsigned char ASMIndexStart,
625 625 unsigned char channel )
626 626 {
627 627 //*************
628 628 // input format
629 629 // component0[0 .. 127] component1[0 .. 127] .. component24[0 .. 127]
630 630 //**************
631 631 // output format
632 632 // matr0[0 .. 24] matr1[0 .. 24] .. matr127[0 .. 24]
633 633 //************
634 634 // compression
635 635 // matr0[0 .. 24] matr1[0 .. 24] .. matr11[0 .. 24] => f0 NORM
636 636 // matr0[0 .. 24] matr1[0 .. 24] .. matr22[0 .. 24] => f0 BURST, SBM
637 637
638 638 int frequencyBin;
639 639 int asmComponent;
640 640 int offsetASM;
641 641 int offsetCompressed;
642 642 int offsetFBin;
643 643 int fBinMask;
644 644 int k;
645 645
646 646 // BUILD DATA
647 647 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
648 648 {
649 649 for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
650 650 {
651 651 offsetCompressed = // NO TIME OFFSET
652 652 frequencyBin * NB_VALUES_PER_SM
653 653 + asmComponent;
654 654 offsetASM = // NO TIME OFFSET
655 655 asmComponent * NB_BINS_PER_SM
656 656 + ASMIndexStart
657 657 + frequencyBin * nbBinsToAverage;
658 658 offsetFBin = ASMIndexStart
659 659 + frequencyBin * nbBinsToAverage;
660 660 compressed_spec_mat[ offsetCompressed ] = 0;
661 661 for ( k = 0; k < nbBinsToAverage; k++ )
662 662 {
663 663 fBinMask = getFBinMask( offsetFBin + k, channel );
664 664 compressed_spec_mat[offsetCompressed ] =
665 665 ( compressed_spec_mat[ offsetCompressed ]
666 666 + averaged_spec_mat[ offsetASM + k ] * fBinMask );
667 667 }
668 668 compressed_spec_mat[ offsetCompressed ] =
669 669 (divider != 0.) ? compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage) : 0.0;
670 670 }
671 671 }
672 672
673 673 }
674 674
675 675 int getFBinMask( int index, unsigned char channel )
676 676 {
677 677 unsigned int indexInChar;
678 678 unsigned int indexInTheChar;
679 679 int fbin;
680 680 unsigned char *sy_lfr_fbins_fx_word1;
681 681
682 sy_lfr_fbins_fx_word1 = parameter_dump_packet.sy_lfr_fbins_f0_word1;
682 sy_lfr_fbins_fx_word1 = parameter_dump_packet.sy_lfr_fbins.fx.f0_word1;
683 683
684 684 switch(channel)
685 685 {
686 686 case 0:
687 687 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f0;
688 688 break;
689 689 case 1:
690 690 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f1;
691 691 break;
692 692 case 2:
693 693 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f2;
694 694 break;
695 695 default:
696 696 PRINTF("ERR *** in getFBinMask, wrong frequency channel")
697 697 }
698 698
699 699 indexInChar = index >> 3;
700 700 indexInTheChar = index - indexInChar * 8;
701 701
702 702 fbin = (int) ((sy_lfr_fbins_fx_word1[ NB_BYTES_PER_FREQ_MASK - 1 - indexInChar] >> indexInTheChar) & 0x1);
703 703
704 704 return fbin;
705 705 }
706 706
707 707 unsigned char acquisitionTimeIsValid( unsigned int coarseTime, unsigned int fineTime, unsigned char channel)
708 708 {
709 709 u_int64_t acquisitionTime;
710 710 u_int64_t timecodeReference;
711 711 u_int64_t offsetInFineTime;
712 712 u_int64_t shiftInFineTime;
713 713 u_int64_t tBadInFineTime;
714 714 u_int64_t acquisitionTimeRangeMin;
715 715 u_int64_t acquisitionTimeRangeMax;
716 716 unsigned char pasFilteringIsEnabled;
717 717 unsigned char ret;
718 718
719 719 pasFilteringIsEnabled = (filterPar.spare_sy_lfr_pas_filter_enabled & 0x01); // [0000 0001]
720 720 ret = 1;
721 721
722 722 // compute acquisition time from caoarseTime and fineTime
723 723 acquisitionTime = ( ((u_int64_t)coarseTime) << 16 )
724 724 + (u_int64_t) fineTime;
725 725
726 726 // compute the timecode reference
727 727 timecodeReference = (u_int64_t) (floor( ((double) coarseTime) / ((double) filterPar.sy_lfr_pas_filter_modulus) )
728 728 * ((double) filterPar.sy_lfr_pas_filter_modulus)) * 65536;
729 729
730 730 // compute the acquitionTime range
731 731 offsetInFineTime = ((double) filterPar.sy_lfr_pas_filter_offset) * 65536;
732 732 shiftInFineTime = ((double) filterPar.sy_lfr_pas_filter_shift) * 65536;
733 733 tBadInFineTime = ((double) filterPar.sy_lfr_pas_filter_tbad) * 65536;
734 734
735 735 acquisitionTimeRangeMin =
736 736 timecodeReference
737 737 + offsetInFineTime
738 738 + shiftInFineTime
739 739 - acquisitionDurations[channel];
740 740 acquisitionTimeRangeMax =
741 741 timecodeReference
742 742 + offsetInFineTime
743 743 + shiftInFineTime
744 744 + tBadInFineTime;
745 745
746 746 if ( (acquisitionTime >= acquisitionTimeRangeMin)
747 747 && (acquisitionTime <= acquisitionTimeRangeMax)
748 748 && (pasFilteringIsEnabled == 1) )
749 749 {
750 750 ret = 0; // the acquisition time is INSIDE the range, the matrix shall be ignored
751 751 }
752 752 else
753 753 {
754 754 ret = 1; // the acquisition time is OUTSIDE the range, the matrix can be used for the averaging
755 755 }
756 756
757 757 // printf("coarseTime = %x, fineTime = %x\n",
758 758 // coarseTime,
759 759 // fineTime);
760 760
761 761 // printf("[ret = %d] *** acquisitionTime = %f, Reference = %f",
762 762 // ret,
763 763 // acquisitionTime / 65536.,
764 764 // timecodeReference / 65536.);
765 765
766 766 // printf(", Min = %f, Max = %f\n",
767 767 // acquisitionTimeRangeMin / 65536.,
768 768 // acquisitionTimeRangeMax / 65536.);
769 769
770 770 return ret;
771 771 }
772 772
773 773 void init_kcoeff_sbm_from_kcoeff_norm(float *input_kcoeff, float *output_kcoeff, unsigned char nb_bins_norm)
774 774 {
775 775 unsigned char bin;
776 776 unsigned char kcoeff;
777 777
778 778 for (bin=0; bin<nb_bins_norm; bin++)
779 779 {
780 780 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
781 781 {
782 782 output_kcoeff[ (bin*NB_K_COEFF_PER_BIN + kcoeff)*2 ] = input_kcoeff[ bin*NB_K_COEFF_PER_BIN + kcoeff ];
783 783 output_kcoeff[ (bin*NB_K_COEFF_PER_BIN + kcoeff)*2 + 1 ] = input_kcoeff[ bin*NB_K_COEFF_PER_BIN + kcoeff ];
784 784 }
785 785 }
786 786 }
@@ -1,1628 +1,1619
1 1 /** Functions to load and dump parameters in the LFR registers.
2 2