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

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

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