1 |
|
|
/*------------------------------------------------------------------------------ |
2 |
|
|
-- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW), |
3 |
|
|
-- This file is a part of the LFR FSW |
4 |
|
|
-- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS |
5 |
|
|
-- |
6 |
|
|
-- This program is free software; you can redistribute it and/or modify |
7 |
|
|
-- it under the terms of the GNU General Public License as published by |
8 |
|
|
-- the Free Software Foundation; either version 2 of the License, or |
9 |
|
|
-- (at your option) any later version. |
10 |
|
|
-- |
11 |
|
|
-- This program is distributed in the hope that it will be useful, |
12 |
|
|
-- but WITHOUT ANY WARRANTY; without even the implied warranty of |
13 |
|
|
-- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
14 |
|
|
-- GNU General Public License for more details. |
15 |
|
|
-- |
16 |
|
|
-- You should have received a copy of the GNU General Public License |
17 |
|
|
-- along with this program; if not, write to the Free Software |
18 |
|
|
-- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
19 |
|
|
-------------------------------------------------------------------------------*/ |
20 |
|
|
/*-- Author : Paul Leroy |
21 |
|
|
-- Contact : Alexis Jeandet |
22 |
|
|
-- Mail : alexis.jeandet@lpp.polytechnique.fr |
23 |
|
|
----------------------------------------------------------------------------*/ |
24 |
|
|
|
25 |
|
|
/** Functions related to data processing. |
26 |
|
|
* |
27 |
|
|
* @file |
28 |
|
|
* @author P. LEROY |
29 |
|
|
* |
30 |
|
|
* These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation. |
31 |
|
|
* |
32 |
|
|
*/ |
33 |
|
|
|
34 |
|
|
#include "fsw_processing.h" |
35 |
|
|
#include "fsw_processing_globals.c" |
36 |
|
|
#include "fsw_init.h" |
37 |
|
|
|
38 |
|
|
unsigned int nb_sm_f0 = 0; |
39 |
|
|
unsigned int nb_sm_f0_aux_f1= 0; |
40 |
|
|
unsigned int nb_sm_f1 = 0; |
41 |
|
|
unsigned int nb_sm_f0_aux_f2= 0; |
42 |
|
|
|
43 |
|
|
typedef enum restartState_t |
44 |
|
|
{ |
45 |
|
|
WAIT_FOR_F2, |
46 |
|
|
WAIT_FOR_F1, |
47 |
|
|
WAIT_FOR_F0 |
48 |
|
|
} restartState; |
49 |
|
|
|
50 |
|
|
//************************ |
51 |
|
|
// spectral matrices rings |
52 |
|
|
ring_node sm_ring_f0[ NB_RING_NODES_SM_F0 ] = {0}; |
53 |
|
|
ring_node sm_ring_f1[ NB_RING_NODES_SM_F1 ] = {0}; |
54 |
|
|
ring_node sm_ring_f2[ NB_RING_NODES_SM_F2 ] = {0}; |
55 |
|
|
ring_node *current_ring_node_sm_f0 = NULL; |
56 |
|
|
ring_node *current_ring_node_sm_f1 = NULL; |
57 |
|
|
ring_node *current_ring_node_sm_f2 = NULL; |
58 |
|
|
ring_node *ring_node_for_averaging_sm_f0= NULL; |
59 |
|
|
ring_node *ring_node_for_averaging_sm_f1= NULL; |
60 |
|
|
ring_node *ring_node_for_averaging_sm_f2= NULL; |
61 |
|
|
|
62 |
|
|
// |
63 |
|
1615952 |
ring_node * getRingNodeForAveraging( unsigned char frequencyChannel) |
64 |
|
|
{ |
65 |
|
|
ring_node *node; |
66 |
|
|
|
67 |
|
1615952 |
node = NULL; |
68 |
✓✓✓✗
|
1615952 |
switch ( frequencyChannel ) { |
69 |
|
|
case CHANNELF0: |
70 |
|
1293416 |
node = ring_node_for_averaging_sm_f0; |
71 |
|
1293416 |
break; |
72 |
|
|
case CHANNELF1: |
73 |
|
215184 |
node = ring_node_for_averaging_sm_f1; |
74 |
|
215184 |
break; |
75 |
|
|
case CHANNELF2: |
76 |
|
107392 |
node = ring_node_for_averaging_sm_f2; |
77 |
|
|
break; |
78 |
|
|
default: |
79 |
|
|
break; |
80 |
|
|
} |
81 |
|
|
|
82 |
|
1615952 |
return node; |
83 |
|
|
} |
84 |
|
|
|
85 |
|
|
//*********************************************************** |
86 |
|
|
// Interrupt Service Routine for spectral matrices processing |
87 |
|
|
|
88 |
|
12171101 |
void spectral_matrices_isr_f0( int statusReg ) |
89 |
|
|
{ |
90 |
|
|
unsigned char status; |
91 |
|
|
rtems_status_code status_code; |
92 |
|
|
ring_node *full_ring_node; |
93 |
|
|
|
94 |
|
12171101 |
status = (unsigned char) (statusReg & BITS_STATUS_F0); // [0011] get the status_ready_matrix_f0_x bits |
95 |
|
|
|
96 |
✗✓✓✓
|
12171101 |
switch(status) |
97 |
|
|
{ |
98 |
|
|
case 0: |
99 |
|
|
break; |
100 |
|
|
case BIT_READY_0_1: |
101 |
|
|
// UNEXPECTED VALUE |
102 |
|
|
spectral_matrix_regs->status = BIT_READY_0_1; // [0011] |
103 |
|
|
status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 ); |
104 |
|
|
break; |
105 |
|
|
case BIT_READY_0: |
106 |
|
5175734 |
full_ring_node = current_ring_node_sm_f0->previous; |
107 |
|
5175734 |
full_ring_node->coarseTime = spectral_matrix_regs->f0_0_coarse_time; |
108 |
|
5175734 |
full_ring_node->fineTime = spectral_matrix_regs->f0_0_fine_time; |
109 |
|
5175734 |
current_ring_node_sm_f0 = current_ring_node_sm_f0->next; |
110 |
|
5175734 |
spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->buffer_address; |
111 |
|
|
// if there are enough ring nodes ready, wake up an AVFx task |
112 |
|
5175734 |
nb_sm_f0 = nb_sm_f0 + 1; |
113 |
✓✓ |
5175734 |
if (nb_sm_f0 == NB_SM_BEFORE_AVF0_F1) |
114 |
|
|
{ |
115 |
|
3659 |
ring_node_for_averaging_sm_f0 = full_ring_node; |
116 |
✗✓ |
3659 |
if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) |
117 |
|
|
{ |
118 |
|
|
status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 ); |
119 |
|
|
} |
120 |
|
3659 |
nb_sm_f0 = 0; |
121 |
|
|
} |
122 |
|
5175734 |
spectral_matrix_regs->status = BIT_READY_0; // [0000 0001] |
123 |
|
5175734 |
break; |
124 |
|
|
case BIT_READY_1: |
125 |
|
5175328 |
full_ring_node = current_ring_node_sm_f0->previous; |
126 |
|
5175328 |
full_ring_node->coarseTime = spectral_matrix_regs->f0_1_coarse_time; |
127 |
|
5175328 |
full_ring_node->fineTime = spectral_matrix_regs->f0_1_fine_time; |
128 |
|
5175328 |
current_ring_node_sm_f0 = current_ring_node_sm_f0->next; |
129 |
|
5175328 |
spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address; |
130 |
|
|
// if there are enough ring nodes ready, wake up an AVFx task |
131 |
|
5175328 |
nb_sm_f0 = nb_sm_f0 + 1; |
132 |
✓✓ |
5175328 |
if (nb_sm_f0 == NB_SM_BEFORE_AVF0_F1) |
133 |
|
|
{ |
134 |
|
1289878 |
ring_node_for_averaging_sm_f0 = full_ring_node; |
135 |
✗✓ |
1289878 |
if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) |
136 |
|
|
{ |
137 |
|
|
status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 ); |
138 |
|
|
} |
139 |
|
1289878 |
nb_sm_f0 = 0; |
140 |
|
|
} |
141 |
|
5175328 |
spectral_matrix_regs->status = BIT_READY_1; // [0000 0010] |
142 |
|
|
break; |
143 |
|
|
default: |
144 |
|
|
break; |
145 |
|
|
} |
146 |
|
12171101 |
} |
147 |
|
|
|
148 |
|
12171101 |
void spectral_matrices_isr_f1( int statusReg ) |
149 |
|
|
{ |
150 |
|
|
rtems_status_code status_code; |
151 |
|
|
unsigned char status; |
152 |
|
|
ring_node *full_ring_node; |
153 |
|
|
|
154 |
|
12171101 |
status = (unsigned char) ((statusReg & BITS_STATUS_F1) >> SHIFT_2_BITS); // [1100] get the status_ready_matrix_f1_x bits |
155 |
|
|
|
156 |
✗✓✓✓
|
12171101 |
switch(status) |
157 |
|
|
{ |
158 |
|
|
case 0: |
159 |
|
|
break; |
160 |
|
|
case BIT_READY_0_1: |
161 |
|
|
// UNEXPECTED VALUE |
162 |
|
|
spectral_matrix_regs->status = BITS_STATUS_F1; // [1100] |
163 |
|
|
status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 ); |
164 |
|
|
break; |
165 |
|
|
case BIT_READY_0: |
166 |
|
862608 |
full_ring_node = current_ring_node_sm_f1->previous; |
167 |
|
862608 |
full_ring_node->coarseTime = spectral_matrix_regs->f1_0_coarse_time; |
168 |
|
862608 |
full_ring_node->fineTime = spectral_matrix_regs->f1_0_fine_time; |
169 |
|
862608 |
current_ring_node_sm_f1 = current_ring_node_sm_f1->next; |
170 |
|
862608 |
spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->buffer_address; |
171 |
|
|
// if there are enough ring nodes ready, wake up an AVFx task |
172 |
|
862608 |
nb_sm_f1 = nb_sm_f1 + 1; |
173 |
✓✓ |
862608 |
if (nb_sm_f1 == NB_SM_BEFORE_AVF0_F1) |
174 |
|
|
{ |
175 |
|
16035 |
ring_node_for_averaging_sm_f1 = full_ring_node; |
176 |
✗✓ |
16035 |
if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) |
177 |
|
|
{ |
178 |
|
|
status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 ); |
179 |
|
|
} |
180 |
|
16035 |
nb_sm_f1 = 0; |
181 |
|
|
} |
182 |
|
862608 |
spectral_matrix_regs->status = BIT_STATUS_F1_0; // [0000 0100] |
183 |
|
862608 |
break; |
184 |
|
|
case BIT_READY_1: |
185 |
|
862251 |
full_ring_node = current_ring_node_sm_f1->previous; |
186 |
|
862251 |
full_ring_node->coarseTime = spectral_matrix_regs->f1_1_coarse_time; |
187 |
|
862251 |
full_ring_node->fineTime = spectral_matrix_regs->f1_1_fine_time; |
188 |
|
862251 |
current_ring_node_sm_f1 = current_ring_node_sm_f1->next; |
189 |
|
862251 |
spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address; |
190 |
|
|
// if there are enough ring nodes ready, wake up an AVFx task |
191 |
|
862251 |
nb_sm_f1 = nb_sm_f1 + 1; |
192 |
✓✓ |
862251 |
if (nb_sm_f1 == NB_SM_BEFORE_AVF0_F1) |
193 |
|
|
{ |
194 |
|
199225 |
ring_node_for_averaging_sm_f1 = full_ring_node; |
195 |
✗✓ |
199225 |
if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) |
196 |
|
|
{ |
197 |
|
|
status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 ); |
198 |
|
|
} |
199 |
|
199225 |
nb_sm_f1 = 0; |
200 |
|
|
} |
201 |
|
862251 |
spectral_matrix_regs->status = BIT_STATUS_F1_1; // [1000 0000] |
202 |
|
|
break; |
203 |
|
|
default: |
204 |
|
|
break; |
205 |
|
|
} |
206 |
|
12171101 |
} |
207 |
|
|
|
208 |
|
12171101 |
void spectral_matrices_isr_f2( int statusReg ) |
209 |
|
|
{ |
210 |
|
|
unsigned char status; |
211 |
|
|
rtems_status_code status_code; |
212 |
|
|
|
213 |
|
12171101 |
status = (unsigned char) ((statusReg & BITS_STATUS_F2) >> SHIFT_4_BITS); // [0011 0000] get the status_ready_matrix_f2_x bits |
214 |
|
|
|
215 |
✗✓✓✓
|
12171101 |
switch(status) |
216 |
|
|
{ |
217 |
|
|
case 0: |
218 |
|
|
break; |
219 |
|
|
case BIT_READY_0_1: |
220 |
|
|
// UNEXPECTED VALUE |
221 |
|
|
spectral_matrix_regs->status = BITS_STATUS_F2; // [0011 0000] |
222 |
|
|
status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 ); |
223 |
|
|
break; |
224 |
|
|
case BIT_READY_0: |
225 |
|
53801 |
ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous; |
226 |
|
53801 |
current_ring_node_sm_f2 = current_ring_node_sm_f2->next; |
227 |
|
53801 |
ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_0_coarse_time; |
228 |
|
53801 |
ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_0_fine_time; |
229 |
|
53801 |
spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->buffer_address; |
230 |
|
53801 |
spectral_matrix_regs->status = BIT_STATUS_F2_0; // [0001 0000] |
231 |
✗✓ |
53801 |
if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) |
232 |
|
|
{ |
233 |
|
|
status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 ); |
234 |
|
|
} |
235 |
|
|
break; |
236 |
|
|
case BIT_READY_1: |
237 |
|
53633 |
ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous; |
238 |
|
53633 |
current_ring_node_sm_f2 = current_ring_node_sm_f2->next; |
239 |
|
53633 |
ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_1_coarse_time; |
240 |
|
53633 |
ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_1_fine_time; |
241 |
|
53633 |
spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address; |
242 |
|
53633 |
spectral_matrix_regs->status = BIT_STATUS_F2_1; // [0010 0000] |
243 |
✗✓ |
53633 |
if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) |
244 |
|
|
{ |
245 |
|
|
status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 ); |
246 |
|
|
} |
247 |
|
|
break; |
248 |
|
|
default: |
249 |
|
|
break; |
250 |
|
|
} |
251 |
|
12171101 |
} |
252 |
|
|
|
253 |
|
12186204 |
void spectral_matrix_isr_error_handler( int statusReg ) |
254 |
|
|
{ |
255 |
|
|
// STATUS REGISTER |
256 |
|
|
// input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0) |
257 |
|
|
// 10 9 8 |
258 |
|
|
// buffer_full ** [bad_component_err] ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0 |
259 |
|
|
// 7 6 5 4 3 2 1 0 |
260 |
|
|
// [bad_component_err] not defined in the last version of the VHDL code |
261 |
|
|
|
262 |
|
|
rtems_status_code status_code; |
263 |
|
|
|
264 |
|
|
//*************************************************** |
265 |
|
|
// the ASM status register is copied in the HK packet |
266 |
|
12186204 |
housekeeping_packet.hk_lfr_vhdl_aa_sm = (unsigned char) ((statusReg & BITS_HK_AA_SM) >> SHIFT_7_BITS); // [0111 1000 0000] |
267 |
|
|
|
268 |
✗✓ |
12186204 |
if (statusReg & BITS_SM_ERR) // [0111 1100 0000] |
269 |
|
|
{ |
270 |
|
|
status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_8 ); |
271 |
|
|
} |
272 |
|
|
|
273 |
|
12186204 |
spectral_matrix_regs->status = spectral_matrix_regs->status & BITS_SM_ERR; |
274 |
|
|
|
275 |
|
12186204 |
} |
276 |
|
|
|
277 |
|
12186204 |
rtems_isr spectral_matrices_isr( rtems_vector_number vector ) |
278 |
|
|
{ |
279 |
|
|
// STATUS REGISTER |
280 |
|
|
// input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0) |
281 |
|
|
// 10 9 8 |
282 |
|
|
// buffer_full ** bad_component_err ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0 |
283 |
|
|
// 7 6 5 4 3 2 1 0 |
284 |
|
|
|
285 |
|
|
int statusReg; |
286 |
|
|
|
287 |
|
|
static restartState state = WAIT_FOR_F2; |
288 |
|
|
|
289 |
|
12186204 |
statusReg = spectral_matrix_regs->status; |
290 |
|
|
|
291 |
✓✓ |
12186204 |
if (thisIsAnASMRestart == 0) |
292 |
|
|
{ // this is not a restart sequence, process incoming matrices normally |
293 |
|
12171101 |
spectral_matrices_isr_f0( statusReg ); |
294 |
|
|
|
295 |
|
12171101 |
spectral_matrices_isr_f1( statusReg ); |
296 |
|
|
|
297 |
|
12171101 |
spectral_matrices_isr_f2( statusReg ); |
298 |
|
|
} |
299 |
|
|
else |
300 |
|
|
{ // a restart sequence has to be launched |
301 |
✓✓✓✗
|
15103 |
switch (state) { |
302 |
|
|
case WAIT_FOR_F2: |
303 |
✓✓ |
14484 |
if ((statusReg & BITS_STATUS_F2) != INIT_CHAR) // [0011 0000] check the status_ready_matrix_f2_x bits |
304 |
|
|
{ |
305 |
|
234 |
state = WAIT_FOR_F1; |
306 |
|
|
} |
307 |
|
|
break; |
308 |
|
|
case WAIT_FOR_F1: |
309 |
✓✓ |
384 |
if ((statusReg & BITS_STATUS_F1) != INIT_CHAR) // [0000 1100] check the status_ready_matrix_f1_x bits |
310 |
|
|
{ |
311 |
|
234 |
state = WAIT_FOR_F0; |
312 |
|
|
} |
313 |
|
|
break; |
314 |
|
|
case WAIT_FOR_F0: |
315 |
✓✓ |
235 |
if ((statusReg & BITS_STATUS_F0) != INIT_CHAR) // [0000 0011] check the status_ready_matrix_f0_x bits |
316 |
|
|
{ |
317 |
|
234 |
state = WAIT_FOR_F2; |
318 |
|
234 |
thisIsAnASMRestart = 0; |
319 |
|
|
} |
320 |
|
|
break; |
321 |
|
|
default: |
322 |
|
|
break; |
323 |
|
|
} |
324 |
|
15103 |
reset_sm_status(); |
325 |
|
|
} |
326 |
|
|
|
327 |
|
12186204 |
spectral_matrix_isr_error_handler( statusReg ); |
328 |
|
|
|
329 |
|
12186204 |
} |
330 |
|
|
|
331 |
|
|
//****************** |
332 |
|
|
// Spectral Matrices |
333 |
|
|
|
334 |
|
1606 |
void reset_nb_sm( void ) |
335 |
|
|
{ |
336 |
|
1606 |
nb_sm_f0 = 0; |
337 |
|
1606 |
nb_sm_f0_aux_f1 = 0; |
338 |
|
1606 |
nb_sm_f0_aux_f2 = 0; |
339 |
|
|
|
340 |
|
1606 |
nb_sm_f1 = 0; |
341 |
|
1606 |
} |
342 |
|
|
|
343 |
|
87 |
void SM_init_rings( void ) |
344 |
|
|
{ |
345 |
|
87 |
init_ring( sm_ring_f0, NB_RING_NODES_SM_F0, sm_f0, TOTAL_SIZE_SM ); |
346 |
|
87 |
init_ring( sm_ring_f1, NB_RING_NODES_SM_F1, sm_f1, TOTAL_SIZE_SM ); |
347 |
|
87 |
init_ring( sm_ring_f2, NB_RING_NODES_SM_F2, sm_f2, TOTAL_SIZE_SM ); |
348 |
|
|
|
349 |
|
|
DEBUG_PRINTF1("sm_ring_f0 @%x\n", (unsigned int) sm_ring_f0) |
350 |
|
|
DEBUG_PRINTF1("sm_ring_f1 @%x\n", (unsigned int) sm_ring_f1) |
351 |
|
|
DEBUG_PRINTF1("sm_ring_f2 @%x\n", (unsigned int) sm_ring_f2) |
352 |
|
|
DEBUG_PRINTF1("sm_f0 @%x\n", (unsigned int) sm_f0) |
353 |
|
|
DEBUG_PRINTF1("sm_f1 @%x\n", (unsigned int) sm_f1) |
354 |
|
|
DEBUG_PRINTF1("sm_f2 @%x\n", (unsigned int) sm_f2) |
355 |
|
87 |
} |
356 |
|
|
|
357 |
|
8030 |
void ASM_generic_init_ring( ring_node_asm *ring, unsigned char nbNodes ) |
358 |
|
|
{ |
359 |
|
|
unsigned char i; |
360 |
|
|
|
361 |
|
8030 |
ring[ nbNodes - 1 ].next |
362 |
|
|
= (ring_node_asm*) &ring[ 0 ]; |
363 |
|
|
|
364 |
✓✓ |
52998 |
for(i=0; i<nbNodes-1; i++) |
365 |
|
|
{ |
366 |
|
44968 |
ring[ i ].next = (ring_node_asm*) &ring[ i + 1 ]; |
367 |
|
|
} |
368 |
|
8030 |
} |
369 |
|
|
|
370 |
|
1693 |
void SM_reset_current_ring_nodes( void ) |
371 |
|
|
{ |
372 |
|
1693 |
current_ring_node_sm_f0 = sm_ring_f0[0].next; |
373 |
|
1693 |
current_ring_node_sm_f1 = sm_ring_f1[0].next; |
374 |
|
1693 |
current_ring_node_sm_f2 = sm_ring_f2[0].next; |
375 |
|
|
|
376 |
|
1693 |
ring_node_for_averaging_sm_f0 = NULL; |
377 |
|
1693 |
ring_node_for_averaging_sm_f1 = NULL; |
378 |
|
1693 |
ring_node_for_averaging_sm_f2 = NULL; |
379 |
|
1693 |
} |
380 |
|
|
|
381 |
|
|
//***************** |
382 |
|
|
// Basic Parameters |
383 |
|
|
|
384 |
|
10450 |
void BP_init_header( bp_packet *packet, |
385 |
|
|
unsigned int apid, unsigned char sid, |
386 |
|
|
unsigned int packetLength, unsigned char blkNr ) |
387 |
|
|
{ |
388 |
|
10450 |
packet->targetLogicalAddress = CCSDS_DESTINATION_ID; |
389 |
|
10450 |
packet->protocolIdentifier = CCSDS_PROTOCOLE_ID; |
390 |
|
10450 |
packet->reserved = INIT_CHAR; |
391 |
|
10450 |
packet->userApplication = CCSDS_USER_APP; |
392 |
|
10450 |
packet->packetID[0] = (unsigned char) (apid >> SHIFT_1_BYTE); |
393 |
|
10450 |
packet->packetID[1] = (unsigned char) (apid); |
394 |
|
10450 |
packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE; |
395 |
|
10450 |
packet->packetSequenceControl[1] = INIT_CHAR; |
396 |
|
10450 |
packet->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE); |
397 |
|
10450 |
packet->packetLength[1] = (unsigned char) (packetLength); |
398 |
|
|
// DATA FIELD HEADER |
399 |
|
10450 |
packet->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2; |
400 |
|
10450 |
packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type |
401 |
|
10450 |
packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype |
402 |
|
10450 |
packet->destinationID = TM_DESTINATION_ID_GROUND; |
403 |
|
10450 |
packet->time[BYTE_0] = INIT_CHAR; |
404 |
|
10450 |
packet->time[BYTE_1] = INIT_CHAR; |
405 |
|
10450 |
packet->time[BYTE_2] = INIT_CHAR; |
406 |
|
10450 |
packet->time[BYTE_3] = INIT_CHAR; |
407 |
|
10450 |
packet->time[BYTE_4] = INIT_CHAR; |
408 |
|
10450 |
packet->time[BYTE_5] = INIT_CHAR; |
409 |
|
|
// AUXILIARY DATA HEADER |
410 |
|
10450 |
packet->sid = sid; |
411 |
|
10450 |
packet->pa_bia_status_info = INIT_CHAR; |
412 |
|
10450 |
packet->sy_lfr_common_parameters_spare = INIT_CHAR; |
413 |
|
10450 |
packet->sy_lfr_common_parameters = INIT_CHAR; |
414 |
|
10450 |
packet->acquisitionTime[BYTE_0] = INIT_CHAR; |
415 |
|
10450 |
packet->acquisitionTime[BYTE_1] = INIT_CHAR; |
416 |
|
10450 |
packet->acquisitionTime[BYTE_2] = INIT_CHAR; |
417 |
|
10450 |
packet->acquisitionTime[BYTE_3] = INIT_CHAR; |
418 |
|
10450 |
packet->acquisitionTime[BYTE_4] = INIT_CHAR; |
419 |
|
10450 |
packet->acquisitionTime[BYTE_5] = INIT_CHAR; |
420 |
|
10450 |
packet->pa_lfr_bp_blk_nr[0] = INIT_CHAR; // BLK_NR MSB |
421 |
|
10450 |
packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB |
422 |
|
10450 |
} |
423 |
|
|
|
424 |
|
3212 |
void BP_init_header_with_spare( bp_packet_with_spare *packet, |
425 |
|
|
unsigned int apid, unsigned char sid, |
426 |
|
|
unsigned int packetLength , unsigned char blkNr) |
427 |
|
|
{ |
428 |
|
3212 |
packet->targetLogicalAddress = CCSDS_DESTINATION_ID; |
429 |
|
3212 |
packet->protocolIdentifier = CCSDS_PROTOCOLE_ID; |
430 |
|
3212 |
packet->reserved = INIT_CHAR; |
431 |
|
3212 |
packet->userApplication = CCSDS_USER_APP; |
432 |
|
3212 |
packet->packetID[0] = (unsigned char) (apid >> SHIFT_1_BYTE); |
433 |
|
3212 |
packet->packetID[1] = (unsigned char) (apid); |
434 |
|
3212 |
packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE; |
435 |
|
3212 |
packet->packetSequenceControl[1] = INIT_CHAR; |
436 |
|
3212 |
packet->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE); |
437 |
|
3212 |
packet->packetLength[1] = (unsigned char) (packetLength); |
438 |
|
|
// DATA FIELD HEADER |
439 |
|
3212 |
packet->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2; |
440 |
|
3212 |
packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type |
441 |
|
3212 |
packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype |
442 |
|
3212 |
packet->destinationID = TM_DESTINATION_ID_GROUND; |
443 |
|
|
// AUXILIARY DATA HEADER |
444 |
|
3212 |
packet->sid = sid; |
445 |
|
3212 |
packet->pa_bia_status_info = INIT_CHAR; |
446 |
|
3212 |
packet->sy_lfr_common_parameters_spare = INIT_CHAR; |
447 |
|
3212 |
packet->sy_lfr_common_parameters = INIT_CHAR; |
448 |
|
3212 |
packet->time[BYTE_0] = INIT_CHAR; |
449 |
|
3212 |
packet->time[BYTE_1] = INIT_CHAR; |
450 |
|
3212 |
packet->time[BYTE_2] = INIT_CHAR; |
451 |
|
3212 |
packet->time[BYTE_3] = INIT_CHAR; |
452 |
|
3212 |
packet->time[BYTE_4] = INIT_CHAR; |
453 |
|
3212 |
packet->time[BYTE_5] = INIT_CHAR; |
454 |
|
3212 |
packet->source_data_spare = INIT_CHAR; |
455 |
|
3212 |
packet->pa_lfr_bp_blk_nr[0] = INIT_CHAR; // BLK_NR MSB |
456 |
|
3212 |
packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB |
457 |
|
3212 |
} |
458 |
|
|
|
459 |
|
85946 |
void BP_send(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid ) |
460 |
|
|
{ |
461 |
|
|
rtems_status_code status; |
462 |
|
|
|
463 |
|
|
// SEND PACKET |
464 |
|
85946 |
status = rtems_message_queue_send( queue_id, data, nbBytesToSend); |
465 |
|
|
if (status != RTEMS_SUCCESSFUL) |
466 |
|
|
{ |
467 |
|
|
PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status) |
468 |
|
|
} |
469 |
|
85946 |
} |
470 |
|
|
|
471 |
|
213326 |
void BP_send_s1_s2(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid ) |
472 |
|
|
{ |
473 |
|
|
/** This function is used to send the BP paquets when needed. |
474 |
|
|
* |
475 |
|
|
* @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE |
476 |
|
|
* |
477 |
|
|
* @return void |
478 |
|
|
* |
479 |
|
|
* SBM1 and SBM2 paquets are sent depending on the type of the LFR mode transition. |
480 |
|
|
* BURST paquets are sent everytime. |
481 |
|
|
* |
482 |
|
|
*/ |
483 |
|
|
|
484 |
|
|
rtems_status_code status; |
485 |
|
|
|
486 |
|
|
// SEND PACKET |
487 |
|
|
// before lastValidTransitionDate, the data are drops even if they are ready |
488 |
|
|
// this guarantees that no SBM packets will be received before the requested enter mode time |
489 |
✓✓ |
213326 |
if ( time_management_regs->coarse_time >= lastValidEnterModeTime) |
490 |
|
|
{ |
491 |
|
209896 |
status = rtems_message_queue_send( queue_id, data, nbBytesToSend); |
492 |
|
|
if (status != RTEMS_SUCCESSFUL) |
493 |
|
|
{ |
494 |
|
|
PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status) |
495 |
|
|
} |
496 |
|
|
} |
497 |
|
213326 |
} |
498 |
|
|
|
499 |
|
|
//****************** |
500 |
|
|
// general functions |
501 |
|
|
|
502 |
|
18449 |
void reset_sm_status( void ) |
503 |
|
|
{ |
504 |
|
|
// error |
505 |
|
|
// 10 --------------- 9 ---------------- 8 ---------------- 7 --------- |
506 |
|
|
// input_fif0_write_2 input_fifo_write_1 input_fifo_write_0 buffer_full |
507 |
|
|
// ---------- 5 -- 4 -- 3 -- 2 -- 1 -- 0 -- |
508 |
|
|
// ready bits f2_1 f2_0 f1_1 f1_1 f0_1 f0_0 |
509 |
|
|
|
510 |
|
18449 |
spectral_matrix_regs->status = BITS_STATUS_REG; // [0111 1111 1111] |
511 |
|
18449 |
} |
512 |
|
|
|
513 |
|
1693 |
void reset_spectral_matrix_regs( void ) |
514 |
|
|
{ |
515 |
|
|
/** This function resets the spectral matrices module registers. |
516 |
|
|
* |
517 |
|
|
* The registers affected by this function are located at the following offset addresses: |
518 |
|
|
* |
519 |
|
|
* - 0x00 config |
520 |
|
|
* - 0x04 status |
521 |
|
|
* - 0x08 matrixF0_Address0 |
522 |
|
|
* - 0x10 matrixFO_Address1 |
523 |
|
|
* - 0x14 matrixF1_Address |
524 |
|
|
* - 0x18 matrixF2_Address |
525 |
|
|
* |
526 |
|
|
*/ |
527 |
|
|
|
528 |
|
1693 |
set_sm_irq_onError( 0 ); |
529 |
|
|
|
530 |
|
1693 |
set_sm_irq_onNewMatrix( 0 ); |
531 |
|
|
|
532 |
|
1693 |
reset_sm_status(); |
533 |
|
|
|
534 |
|
|
// F1 |
535 |
|
1693 |
spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->previous->buffer_address; |
536 |
|
1693 |
spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address; |
537 |
|
|
// F2 |
538 |
|
1693 |
spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->previous->buffer_address; |
539 |
|
1693 |
spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address; |
540 |
|
|
// F3 |
541 |
|
1693 |
spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->previous->buffer_address; |
542 |
|
1693 |
spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address; |
543 |
|
|
|
544 |
|
1693 |
spectral_matrix_regs->matrix_length = DEFAULT_MATRIX_LENGTH; // 25 * 128 / 16 = 200 = 0xc8 |
545 |
|
1693 |
} |
546 |
|
|
|
547 |
|
598544 |
void set_time( unsigned char *time, unsigned char * timeInBuffer ) |
548 |
|
|
{ |
549 |
|
598544 |
time[BYTE_0] = timeInBuffer[BYTE_0]; |
550 |
|
598544 |
time[BYTE_1] = timeInBuffer[BYTE_1]; |
551 |
|
598544 |
time[BYTE_2] = timeInBuffer[BYTE_2]; |
552 |
|
598544 |
time[BYTE_3] = timeInBuffer[BYTE_3]; |
553 |
|
598544 |
time[BYTE_4] = timeInBuffer[BYTE_6]; |
554 |
|
598544 |
time[BYTE_5] = timeInBuffer[BYTE_7]; |
555 |
|
598544 |
} |
556 |
|
|
|
557 |
|
14688 |
unsigned long long int get_acquisition_time( unsigned char *timePtr ) |
558 |
|
|
{ |
559 |
|
|
unsigned long long int acquisitionTimeAslong; |
560 |
|
14688 |
acquisitionTimeAslong = INIT_CHAR; |
561 |
|
14688 |
acquisitionTimeAslong = |
562 |
|
14688 |
( (unsigned long long int) (timePtr[BYTE_0] & SYNC_BIT_MASK) << SHIFT_5_BYTES ) // [0111 1111] mask the synchronization bit |
563 |
|
14688 |
+ ( (unsigned long long int) timePtr[BYTE_1] << SHIFT_4_BYTES ) |
564 |
|
14688 |
+ ( (unsigned long long int) timePtr[BYTE_2] << SHIFT_3_BYTES ) |
565 |
|
14688 |
+ ( (unsigned long long int) timePtr[BYTE_3] << SHIFT_2_BYTES ) |
566 |
|
14688 |
+ ( (unsigned long long int) timePtr[BYTE_6] << SHIFT_1_BYTE ) |
567 |
|
14688 |
+ ( (unsigned long long int) timePtr[BYTE_7] ); |
568 |
|
14688 |
return acquisitionTimeAslong; |
569 |
|
|
} |
570 |
|
|
|
571 |
|
173645 |
unsigned char getSID( rtems_event_set event ) |
572 |
|
|
{ |
573 |
|
|
unsigned char sid; |
574 |
|
|
|
575 |
|
|
rtems_event_set eventSetBURST; |
576 |
|
|
rtems_event_set eventSetSBM; |
577 |
|
|
|
578 |
|
173645 |
sid = 0; |
579 |
|
|
|
580 |
|
|
//****** |
581 |
|
|
// BURST |
582 |
|
173645 |
eventSetBURST = RTEMS_EVENT_BURST_BP1_F0 |
583 |
|
|
| RTEMS_EVENT_BURST_BP1_F1 |
584 |
|
|
| RTEMS_EVENT_BURST_BP2_F0 |
585 |
|
|
| RTEMS_EVENT_BURST_BP2_F1; |
586 |
|
|
|
587 |
|
|
//**** |
588 |
|
|
// SBM |
589 |
|
173645 |
eventSetSBM = RTEMS_EVENT_SBM_BP1_F0 |
590 |
|
|
| RTEMS_EVENT_SBM_BP1_F1 |
591 |
|
|
| RTEMS_EVENT_SBM_BP2_F0 |
592 |
|
|
| RTEMS_EVENT_SBM_BP2_F1; |
593 |
|
|
|
594 |
✓✓ |
173645 |
if (event & eventSetBURST) |
595 |
|
|
{ |
596 |
|
21003 |
sid = SID_BURST_BP1_F0; |
597 |
|
|
} |
598 |
✓✗ |
152642 |
else if (event & eventSetSBM) |
599 |
|
|
{ |
600 |
|
152642 |
sid = SID_SBM1_BP1_F0; |
601 |
|
|
} |
602 |
|
|
else |
603 |
|
|
{ |
604 |
|
|
sid = 0; |
605 |
|
|
} |
606 |
|
|
|
607 |
|
173645 |
return sid; |
608 |
|
|
} |
609 |
|
|
|
610 |
|
|
/** |
611 |
|
|
* @brief extractReImVectors converts a given ASM component from interleaved to split representation |
612 |
|
|
* @param inputASM |
613 |
|
|
* @param outputASM |
614 |
|
|
* @param asmComponent |
615 |
|
|
*/ |
616 |
|
4301780 |
void extractReImVectors( float *inputASM, float *outputASM, unsigned int asmComponent ) |
617 |
|
|
{ |
618 |
|
|
unsigned int i; |
619 |
|
|
float re; |
620 |
|
|
float im; |
621 |
|
|
|
622 |
✓✓ |
554929620 |
for (i=0; i<NB_BINS_PER_SM; i++){ |
623 |
|
550627840 |
re = inputASM[ (asmComponent*NB_BINS_PER_SM) + (i * SM_BYTES_PER_VAL) ]; |
624 |
|
550627840 |
im = inputASM[ (asmComponent*NB_BINS_PER_SM) + (i * SM_BYTES_PER_VAL) + 1]; |
625 |
|
550627840 |
outputASM[ ( asmComponent *NB_BINS_PER_SM) + i] = re; |
626 |
|
550627840 |
outputASM[ ((asmComponent+1)*NB_BINS_PER_SM) + i] = im; |
627 |
|
|
} |
628 |
|
4301780 |
} |
629 |
|
|
|
630 |
|
|
/** |
631 |
|
|
* @brief copyReVectors copies real part of a given ASM from inputASM to outputASM |
632 |
|
|
* @param inputASM |
633 |
|
|
* @param outputASM |
634 |
|
|
* @param asmComponent |
635 |
|
|
*/ |
636 |
|
2150890 |
void copyReVectors( float *inputASM, float *outputASM, unsigned int asmComponent ) |
637 |
|
|
{ |
638 |
|
|
unsigned int i; |
639 |
|
|
float re; |
640 |
|
|
|
641 |
✓✓ |
277464810 |
for (i=0; i<NB_BINS_PER_SM; i++){ |
642 |
|
275313920 |
re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i]; |
643 |
|
275313920 |
outputASM[ (asmComponent*NB_BINS_PER_SM) + i] = re; |
644 |
|
|
} |
645 |
|
2150890 |
} |
646 |
|
|
|
647 |
|
|
/** |
648 |
|
|
* @brief ASM_patch, converts ASM from interleaved to split representation |
649 |
|
|
* @param inputASM |
650 |
|
|
* @param outputASM |
651 |
|
|
* @note inputASM and outputASM must be different, in other words this function can't do in place convertion |
652 |
|
|
* @see extractReImVectors |
653 |
|
|
*/ |
654 |
|
430178 |
void ASM_patch( float *inputASM, float *outputASM ) |
655 |
|
|
{ |
656 |
|
430178 |
extractReImVectors( inputASM, outputASM, ASM_COMP_B1B2); // b1b2 |
657 |
|
430178 |
extractReImVectors( inputASM, outputASM, ASM_COMP_B1B3 ); // b1b3 |
658 |
|
430178 |
extractReImVectors( inputASM, outputASM, ASM_COMP_B1E1 ); // b1e1 |
659 |
|
430178 |
extractReImVectors( inputASM, outputASM, ASM_COMP_B1E2 ); // b1e2 |
660 |
|
430178 |
extractReImVectors( inputASM, outputASM, ASM_COMP_B2B3 ); // b2b3 |
661 |
|
430178 |
extractReImVectors( inputASM, outputASM, ASM_COMP_B2E1 ); // b2e1 |
662 |
|
430178 |
extractReImVectors( inputASM, outputASM, ASM_COMP_B2E2 ); // b2e2 |
663 |
|
430178 |
extractReImVectors( inputASM, outputASM, ASM_COMP_B3E1 ); // b3e1 |
664 |
|
430178 |
extractReImVectors( inputASM, outputASM, ASM_COMP_B3E2 ); // b3e2 |
665 |
|
430178 |
extractReImVectors( inputASM, outputASM, ASM_COMP_E1E2 ); // e1e2 |
666 |
|
|
|
667 |
|
430178 |
copyReVectors(inputASM, outputASM, ASM_COMP_B1B1 ); // b1b1 |
668 |
|
430178 |
copyReVectors(inputASM, outputASM, ASM_COMP_B2B2 ); // b2b2 |
669 |
|
430178 |
copyReVectors(inputASM, outputASM, ASM_COMP_B3B3); // b3b3 |
670 |
|
430178 |
copyReVectors(inputASM, outputASM, ASM_COMP_E1E1); // e1e1 |
671 |
|
430178 |
copyReVectors(inputASM, outputASM, ASM_COMP_E2E2); // e2e2 |
672 |
|
430178 |
} |
673 |
|
|
|
674 |
|
245524 |
void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat , float divider, |
675 |
|
|
unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage, |
676 |
|
|
unsigned char ASMIndexStart, |
677 |
|
|
unsigned char channel ) |
678 |
|
|
{ |
679 |
|
|
//************* |
680 |
|
|
// input format |
681 |
|
|
// component0[0 .. 127] component1[0 .. 127] .. component24[0 .. 127] |
682 |
|
|
//************** |
683 |
|
|
// output format |
684 |
|
|
// matr0[0 .. 24] matr1[0 .. 24] .. matr127[0 .. 24] |
685 |
|
|
//************ |
686 |
|
|
// compression |
687 |
|
|
// matr0[0 .. 24] matr1[0 .. 24] .. matr11[0 .. 24] => f0 NORM |
688 |
|
|
// matr0[0 .. 24] matr1[0 .. 24] .. matr22[0 .. 24] => f0 BURST, SBM |
689 |
|
|
|
690 |
|
|
int frequencyBin; |
691 |
|
|
int asmComponent; |
692 |
|
|
int offsetASM; |
693 |
|
|
int offsetCompressed; |
694 |
|
|
int offsetFBin; |
695 |
|
|
int fBinMask; |
696 |
|
|
int k; |
697 |
|
|
|
698 |
|
|
// BUILD DATA |
699 |
✓✓ |
6383624 |
for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++) |
700 |
|
|
{ |
701 |
✓✓ |
126633125 |
for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ ) |
702 |
|
|
{ |
703 |
|
120495025 |
offsetCompressed = // NO TIME OFFSET |
704 |
|
120495025 |
(frequencyBin * NB_VALUES_PER_SM) |
705 |
|
|
+ asmComponent; |
706 |
|
120495025 |
offsetASM = // NO TIME OFFSET |
707 |
|
240990050 |
(asmComponent * NB_BINS_PER_SM) |
708 |
|
|
+ ASMIndexStart |
709 |
|
120495025 |
+ (frequencyBin * nbBinsToAverage); |
710 |
|
240990050 |
offsetFBin = ASMIndexStart |
711 |
|
120495025 |
+ (frequencyBin * nbBinsToAverage); |
712 |
|
120495025 |
compressed_spec_mat[ offsetCompressed ] = 0; |
713 |
✓✓ |
688729425 |
for ( k = 0; k < nbBinsToAverage; k++ ) |
714 |
|
|
{ |
715 |
|
568234400 |
fBinMask = getFBinMask( offsetFBin + k, channel ); |
716 |
|
1136468800 |
compressed_spec_mat[offsetCompressed ] = compressed_spec_mat[ offsetCompressed ] |
717 |
|
568234400 |
+ (averaged_spec_mat[ offsetASM + k ] * fBinMask); |
718 |
|
|
} |
719 |
✓✓ |
120495025 |
if (divider != 0) |
720 |
|
|
{ |
721 |
|
118999375 |
compressed_spec_mat[ offsetCompressed ] = compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage); |
722 |
|
|
} |
723 |
|
|
else |
724 |
|
|
{ |
725 |
|
1495650 |
compressed_spec_mat[ offsetCompressed ] = INIT_FLOAT; |
726 |
|
|
} |
727 |
|
|
} |
728 |
|
|
} |
729 |
|
|
|
730 |
|
245524 |
} |
731 |
|
|
|
732 |
|
568234400 |
int getFBinMask( int index, unsigned char channel ) |
733 |
|
|
{ |
734 |
|
|
unsigned int indexInChar; |
735 |
|
|
unsigned int indexInTheChar; |
736 |
|
|
int fbin; |
737 |
|
|
unsigned char *sy_lfr_fbins_fx_word1; |
738 |
|
|
|
739 |
|
568234400 |
sy_lfr_fbins_fx_word1 = parameter_dump_packet.sy_lfr_fbins_f0_word1; |
740 |
|
|
|
741 |
✓✓✓✗
|
568234400 |
switch(channel) |
742 |
|
|
{ |
743 |
|
|
case CHANNELF0: |
744 |
|
359367800 |
sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f0; |
745 |
|
359367800 |
break; |
746 |
|
|
case CHANNELF1: |
747 |
|
151405800 |
sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f1; |
748 |
|
151405800 |
break; |
749 |
|
|
case CHANNELF2: |
750 |
|
57460800 |
sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f2; |
751 |
|
|
break; |
752 |
|
|
default: |
753 |
|
|
PRINTF("ERR *** in getFBinMask, wrong frequency channel") |
754 |
|
|
} |
755 |
|
|
|
756 |
|
568234400 |
indexInChar = index >> SHIFT_3_BITS; |
757 |
|
568234400 |
indexInTheChar = index - (indexInChar * BITS_PER_BYTE); |
758 |
|
|
|
759 |
|
568234400 |
fbin = (int) ((sy_lfr_fbins_fx_word1[ BYTES_PER_MASK - 1 - indexInChar] >> indexInTheChar) & 1); |
760 |
|
|
|
761 |
|
568234400 |
return fbin; |
762 |
|
|
} |
763 |
|
|
|
764 |
|
|
/** |
765 |
|
|
* @brief isPolluted returns MATRIX_IS_POLLUTED if there is any overlap between t0:t1 and tbad0:tbad1 ranges |
766 |
|
|
* @param t0 Start acquisition time |
767 |
|
|
* @param t1 End of acquisition time |
768 |
|
|
* @param tbad0 Start time of poluting signal |
769 |
|
|
* @param tbad1 End time of poluting signal |
770 |
|
|
* @return |
771 |
|
|
*/ |
772 |
|
1938941 |
unsigned char isPolluted( u_int64_t t0, u_int64_t t1, u_int64_t tbad0, u_int64_t tbad1 ) |
773 |
|
|
{ |
774 |
|
|
unsigned char polluted; |
775 |
|
|
|
776 |
|
1938941 |
polluted = MATRIX_IS_NOT_POLLUTED; |
777 |
|
|
|
778 |
✓✓✓✓ ✓✗✗✓
|
7207188 |
if ( ((tbad0 < t0) && (t0 < tbad1)) // t0 is inside the polluted range |
779 |
|
1759205 |
|| ((tbad0 < t1) && (t1 < tbad1)) // t1 is inside the polluted range |
780 |
|
1754521 |
|| ((t0 < tbad0) && (tbad1 < t1)) // the polluted range is inside the signal range |
781 |
|
1754521 |
|| ((tbad0 < t0) && (t1 < tbad1))) // the signal range is inside the polluted range |
782 |
|
|
{ |
783 |
|
184420 |
polluted = MATRIX_IS_POLLUTED; |
784 |
|
|
} |
785 |
|
|
|
786 |
|
1938941 |
return polluted; |
787 |
|
|
} |
788 |
|
|
|
789 |
|
|
/** |
790 |
|
|
* @brief acquisitionTimeIsValid checks if the given acquisition time is poluted by PAS |
791 |
|
|
* @param coarseTime Coarse acquisition time of the given SM |
792 |
|
|
* @param fineTime Fine acquisition time of the given ASM |
793 |
|
|
* @param channel Frequency channel to check, will impact SM time footprint |
794 |
|
|
* @return MATRIX_IS_POLLUTED if there is any time overlap between SM and PAS poluting signal |
795 |
|
|
*/ |
796 |
|
12175874 |
unsigned char acquisitionTimeIsValid( unsigned int coarseTime, unsigned int fineTime, unsigned char channel) |
797 |
|
|
{ |
798 |
|
|
u_int64_t t0; |
799 |
|
|
u_int64_t t1; |
800 |
|
|
u_int64_t tc; |
801 |
|
|
u_int64_t tbad0; |
802 |
|
|
u_int64_t tbad1; |
803 |
|
|
|
804 |
|
|
u_int64_t modulusInFineTime; |
805 |
|
|
u_int64_t offsetInFineTime; |
806 |
|
|
u_int64_t shiftInFineTime; |
807 |
|
|
u_int64_t tbadInFineTime; |
808 |
|
|
|
809 |
|
|
u_int64_t timecodeReference; |
810 |
|
|
|
811 |
|
|
unsigned char pasFilteringIsEnabled; |
812 |
|
|
unsigned char ret; |
813 |
|
|
|
814 |
|
|
// compute acquisition time from caoarseTime and fineTime |
815 |
|
12175874 |
t0 = ( ((u_int64_t)coarseTime) << SHIFT_2_BYTES ) + (u_int64_t) fineTime; |
816 |
|
12175874 |
t1 = t0; |
817 |
|
12175874 |
tc = t0; |
818 |
|
12175874 |
tbad0 = t0; |
819 |
|
12175874 |
tbad1 = t0; |
820 |
|
|
|
821 |
✓✓✓✗
|
12175874 |
switch(channel) |
822 |
|
|
{ |
823 |
|
|
case CHANNELF0: |
824 |
|
10347295 |
t1 = t0 + ACQUISITION_DURATION_F0; |
825 |
|
10347295 |
tc = t0 + HALF_ACQUISITION_DURATION_F0; |
826 |
|
10347295 |
break; |
827 |
|
|
case CHANNELF1: |
828 |
|
1721472 |
t1 = t0 + ACQUISITION_DURATION_F1; |
829 |
|
1721472 |
tc = t0 + HALF_ACQUISITION_DURATION_F1; |
830 |
|
1721472 |
break; |
831 |
|
|
case CHANNELF2: |
832 |
|
107392 |
t1 = t0 + ACQUISITION_DURATION_F2; |
833 |
|
107392 |
tc = t0 + HALF_ACQUISITION_DURATION_F2; |
834 |
|
|
break; |
835 |
|
|
default: |
836 |
|
|
break; |
837 |
|
|
} |
838 |
|
|
|
839 |
|
|
// compute the acquitionTime range |
840 |
|
12175874 |
modulusInFineTime = filterPar.modulus_in_finetime; |
841 |
|
12175874 |
offsetInFineTime = filterPar.offset_in_finetime; |
842 |
|
12175874 |
shiftInFineTime = filterPar.shift_in_finetime; |
843 |
|
12175874 |
tbadInFineTime = filterPar.tbad_in_finetime; |
844 |
|
12175874 |
timecodeReference = INIT_INT; |
845 |
|
|
|
846 |
|
12175874 |
pasFilteringIsEnabled = (filterPar.spare_sy_lfr_pas_filter_enabled & 1); // [0000 0001] |
847 |
|
12175874 |
ret = MATRIX_IS_NOT_POLLUTED; |
848 |
|
|
|
849 |
✓✓ |
12175874 |
if ( (tbadInFineTime == 0) || (pasFilteringIsEnabled == 0) ) |
850 |
|
|
{ |
851 |
|
11468094 |
ret = MATRIX_IS_NOT_POLLUTED; |
852 |
|
|
} |
853 |
|
|
else |
854 |
|
|
{ |
855 |
|
|
// INTERSECTION TEST #1 |
856 |
|
707780 |
timecodeReference = (tc - (tc % modulusInFineTime)) - modulusInFineTime ; |
857 |
|
707780 |
tbad0 = timecodeReference + offsetInFineTime + shiftInFineTime; |
858 |
|
707780 |
tbad1 = timecodeReference + offsetInFineTime + shiftInFineTime + tbadInFineTime; |
859 |
|
707780 |
ret = isPolluted( t0, t1, tbad0, tbad1 ); |
860 |
|
|
|
861 |
|
|
// INTERSECTION TEST #2 |
862 |
✓✓ |
707796 |
if (ret == MATRIX_IS_NOT_POLLUTED) |
863 |
|
|
{ |
864 |
|
707769 |
timecodeReference = (tc - (tc % modulusInFineTime)) ; |
865 |
|
707769 |
tbad0 = timecodeReference + offsetInFineTime + shiftInFineTime; |
866 |
|
707769 |
tbad1 = timecodeReference + offsetInFineTime + shiftInFineTime + tbadInFineTime; |
867 |
|
707769 |
ret = isPolluted( t0, t1, tbad0, tbad1 ); |
868 |
|
|
} |
869 |
|
|
|
870 |
|
|
// INTERSECTION TEST #3 |
871 |
✓✓ |
707652 |
if (ret == MATRIX_IS_NOT_POLLUTED) |
872 |
|
|
{ |
873 |
|
523376 |
timecodeReference = (tc - (tc % modulusInFineTime)) + modulusInFineTime ; |
874 |
|
523376 |
tbad0 = timecodeReference + offsetInFineTime + shiftInFineTime; |
875 |
|
523376 |
tbad1 = timecodeReference + offsetInFineTime + shiftInFineTime + tbadInFineTime; |
876 |
|
523376 |
ret = isPolluted( t0, t1, tbad0, tbad1 ); |
877 |
|
|
} |
878 |
|
|
} |
879 |
|
|
|
880 |
|
12175746 |
return ret; |
881 |
|
|
} |
882 |
|
|
|
883 |
|
174 |
void init_kcoeff_sbm_from_kcoeff_norm(float *input_kcoeff, float *output_kcoeff, unsigned char nb_bins_norm) |
884 |
|
|
{ |
885 |
|
|
unsigned char bin; |
886 |
|
|
unsigned char kcoeff; |
887 |
|
|
|
888 |
✓✓ |
2262 |
for (bin=0; bin<nb_bins_norm; bin++) |
889 |
|
|
{ |
890 |
✓✓ |
68904 |
for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++) |
891 |
|
|
{ |
892 |
|
133632 |
output_kcoeff[ ( (bin * NB_K_COEFF_PER_BIN) + kcoeff ) * SBM_COEFF_PER_NORM_COEFF ] |
893 |
|
66816 |
= input_kcoeff[ (bin*NB_K_COEFF_PER_BIN) + kcoeff ]; |
894 |
|
133632 |
output_kcoeff[ ( ( (bin * NB_K_COEFF_PER_BIN ) + kcoeff) * SBM_COEFF_PER_NORM_COEFF ) + 1 ] |
895 |
|
66816 |
= input_kcoeff[ (bin*NB_K_COEFF_PER_BIN) + kcoeff ]; |
896 |
|
|
} |
897 |
|
|
} |
898 |
|
174 |
} |