@@ -1,338 +1,332 | |||
|
1 | 1 | #ifndef FSW_PROCESSING_H_INCLUDED |
|
2 | 2 | #define FSW_PROCESSING_H_INCLUDED |
|
3 | 3 | |
|
4 | 4 | #include <rtems.h> |
|
5 | 5 | #include <grspw.h> |
|
6 | 6 | #include <math.h> |
|
7 | 7 | #include <stdlib.h> // abs() is in the stdlib |
|
8 | 8 | #include <stdio.h> |
|
9 | 9 | #include <math.h> |
|
10 | 10 | #include <grlib_regs.h> |
|
11 | 11 | |
|
12 | 12 | #include "fsw_params.h" |
|
13 | 13 | |
|
14 | 14 | typedef struct ring_node_asm |
|
15 | 15 | { |
|
16 | 16 | struct ring_node_asm *next; |
|
17 | 17 | float matrix[ TOTAL_SIZE_SM ]; |
|
18 | 18 | unsigned int status; |
|
19 | 19 | } ring_node_asm; |
|
20 | 20 | |
|
21 | 21 | typedef struct |
|
22 | 22 | { |
|
23 | 23 | unsigned char targetLogicalAddress; |
|
24 | 24 | unsigned char protocolIdentifier; |
|
25 | 25 | unsigned char reserved; |
|
26 | 26 | unsigned char userApplication; |
|
27 | 27 | unsigned char packetID[2]; |
|
28 | 28 | unsigned char packetSequenceControl[2]; |
|
29 | 29 | unsigned char packetLength[2]; |
|
30 | 30 | // DATA FIELD HEADER |
|
31 | 31 | unsigned char spare1_pusVersion_spare2; |
|
32 | 32 | unsigned char serviceType; |
|
33 | 33 | unsigned char serviceSubType; |
|
34 | 34 | unsigned char destinationID; |
|
35 | 35 | unsigned char time[6]; |
|
36 | 36 | // AUXILIARY HEADER |
|
37 | 37 | unsigned char sid; |
|
38 | 38 | unsigned char pa_bia_status_info; |
|
39 | 39 | unsigned char sy_lfr_common_parameters_spare; |
|
40 | 40 | unsigned char sy_lfr_common_parameters; |
|
41 | 41 | unsigned char acquisitionTime[6]; |
|
42 | 42 | unsigned char pa_lfr_bp_blk_nr[2]; |
|
43 | 43 | // SOURCE DATA |
|
44 | 44 | unsigned char data[ 780 ]; // MAX size is 26 bins * 30 Bytes [TM_LFR_SCIENCE_BURST_BP2_F1] |
|
45 | 45 | } bp_packet; |
|
46 | 46 | |
|
47 | 47 | typedef struct |
|
48 | 48 | { |
|
49 | 49 | unsigned char targetLogicalAddress; |
|
50 | 50 | unsigned char protocolIdentifier; |
|
51 | 51 | unsigned char reserved; |
|
52 | 52 | unsigned char userApplication; |
|
53 | 53 | unsigned char packetID[2]; |
|
54 | 54 | unsigned char packetSequenceControl[2]; |
|
55 | 55 | unsigned char packetLength[2]; |
|
56 | 56 | // DATA FIELD HEADER |
|
57 | 57 | unsigned char spare1_pusVersion_spare2; |
|
58 | 58 | unsigned char serviceType; |
|
59 | 59 | unsigned char serviceSubType; |
|
60 | 60 | unsigned char destinationID; |
|
61 | 61 | unsigned char time[6]; |
|
62 | 62 | // AUXILIARY HEADER |
|
63 | 63 | unsigned char sid; |
|
64 | 64 | unsigned char pa_bia_status_info; |
|
65 | 65 | unsigned char sy_lfr_common_parameters_spare; |
|
66 | 66 | unsigned char sy_lfr_common_parameters; |
|
67 | 67 | unsigned char acquisitionTime[6]; |
|
68 | 68 | unsigned char source_data_spare; |
|
69 | 69 | unsigned char pa_lfr_bp_blk_nr[2]; |
|
70 | 70 | // SOURCE DATA |
|
71 | 71 | unsigned char data[ 143 ]; // 13 bins * 11 Bytes |
|
72 | 72 | } bp_packet_with_spare; // only for TM_LFR_SCIENCE_NORMAL_BP1_F0 and F1 |
|
73 | 73 | |
|
74 | 74 | typedef struct asm_msg |
|
75 | 75 | { |
|
76 | 76 | ring_node_asm *norm; |
|
77 | 77 | ring_node_asm *burst_sbm; |
|
78 | 78 | rtems_event_set event; |
|
79 | 79 | unsigned int coarseTimeNORM; |
|
80 | 80 | unsigned int fineTimeNORM; |
|
81 | 81 | unsigned int coarseTimeSBM; |
|
82 | 82 | unsigned int fineTimeSBM; |
|
83 | 83 | } asm_msg; |
|
84 | 84 | |
|
85 | 85 | extern unsigned char thisIsAnASMRestart; |
|
86 | 86 | |
|
87 | 87 | extern volatile int sm_f0[ ]; |
|
88 | 88 | extern volatile int sm_f1[ ]; |
|
89 | 89 | extern volatile int sm_f2[ ]; |
|
90 | 90 | |
|
91 | 91 | // parameters |
|
92 | 92 | extern struct param_local_str param_local; |
|
93 | 93 | extern Packet_TM_LFR_PARAMETER_DUMP_t parameter_dump_packet; |
|
94 | extern unsigned char rw_fbins_mask_f0[16]; | |
|
95 | extern unsigned char rw_fbins_mask_f1[16]; | |
|
96 | extern unsigned char rw_fbins_mask_f2[16]; | |
|
97 | extern unsigned char merged_fbins_mask_f0[16]; | |
|
98 | extern unsigned char merged_fbins_mask_f1[16]; | |
|
99 | extern unsigned char merged_fbins_mask_f2[16]; | |
|
100 | 94 | |
|
101 | 95 | // registers |
|
102 | 96 | extern time_management_regs_t *time_management_regs; |
|
103 | 97 | extern volatile spectral_matrix_regs_t *spectral_matrix_regs; |
|
104 | 98 | |
|
105 | 99 | extern rtems_name misc_name[5]; |
|
106 | 100 | extern rtems_id Task_id[20]; /* array of task ids */ |
|
107 | 101 | |
|
108 | 102 | ring_node * getRingNodeForAveraging( unsigned char frequencyChannel); |
|
109 | 103 | // ISR |
|
110 | 104 | rtems_isr spectral_matrices_isr( rtems_vector_number vector ); |
|
111 | 105 | |
|
112 | 106 | //****************** |
|
113 | 107 | // Spectral Matrices |
|
114 | 108 | void reset_nb_sm( void ); |
|
115 | 109 | // SM |
|
116 | 110 | void SM_init_rings( void ); |
|
117 | 111 | void SM_reset_current_ring_nodes( void ); |
|
118 | 112 | // ASM |
|
119 | 113 | void ASM_generic_init_ring(ring_node_asm *ring, unsigned char nbNodes ); |
|
120 | 114 | |
|
121 | 115 | //***************** |
|
122 | 116 | // Basic Parameters |
|
123 | 117 | |
|
124 | 118 | void BP_reset_current_ring_nodes( void ); |
|
125 | 119 | void BP_init_header(bp_packet *packet, |
|
126 | 120 | unsigned int apid, unsigned char sid, |
|
127 | 121 | unsigned int packetLength , unsigned char blkNr); |
|
128 | 122 | void BP_init_header_with_spare(bp_packet_with_spare *packet, |
|
129 | 123 | unsigned int apid, unsigned char sid, |
|
130 | 124 | unsigned int packetLength, unsigned char blkNr ); |
|
131 | 125 | void BP_send( char *data, |
|
132 | 126 | rtems_id queue_id, |
|
133 | 127 | unsigned int nbBytesToSend , unsigned int sid ); |
|
134 | 128 | void BP_send_s1_s2(char *data, |
|
135 | 129 | rtems_id queue_id, |
|
136 | 130 | unsigned int nbBytesToSend, unsigned int sid ); |
|
137 | 131 | |
|
138 | 132 | //****************** |
|
139 | 133 | // general functions |
|
140 | 134 | void reset_sm_status( void ); |
|
141 | 135 | void reset_spectral_matrix_regs( void ); |
|
142 | 136 | void set_time(unsigned char *time, unsigned char *timeInBuffer ); |
|
143 | 137 | unsigned long long int get_acquisition_time( unsigned char *timePtr ); |
|
144 | 138 | unsigned char getSID( rtems_event_set event ); |
|
145 | 139 | |
|
146 | 140 | extern rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id ); |
|
147 | 141 | extern rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id ); |
|
148 | 142 | |
|
149 | 143 | //*************************************** |
|
150 | 144 | // DEFINITIONS OF STATIC INLINE FUNCTIONS |
|
151 | 145 | static inline void SM_average(float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM, |
|
152 | 146 | ring_node *ring_node_tab[], |
|
153 | 147 | unsigned int nbAverageNORM, unsigned int nbAverageSBM, |
|
154 | 148 | asm_msg *msgForMATR ); |
|
155 | 149 | |
|
156 | 150 | static inline void SM_average_debug(float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM, |
|
157 | 151 | ring_node *ring_node_tab[], |
|
158 | 152 | unsigned int nbAverageNORM, unsigned int nbAverageSBM, |
|
159 | 153 | asm_msg *msgForMATR ); |
|
160 | 154 | |
|
161 | 155 | void ASM_patch( float *inputASM, float *outputASM ); |
|
162 | 156 | |
|
163 | 157 | void extractReImVectors(float *inputASM, float *outputASM, unsigned int asmComponent ); |
|
164 | 158 | |
|
165 | 159 | static inline void ASM_reorganize_and_divide(float *averaged_spec_mat, float *averaged_spec_mat_reorganized, |
|
166 | 160 | float divider ); |
|
167 | 161 | |
|
168 | 162 | static inline void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat, |
|
169 | 163 | float divider, |
|
170 | 164 | unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart); |
|
171 | 165 | |
|
172 | 166 | static inline void ASM_convert(volatile float *input_matrix, char *output_matrix); |
|
173 | 167 | |
|
174 | 168 | void SM_average( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM, |
|
175 | 169 | ring_node *ring_node_tab[], |
|
176 | 170 | unsigned int nbAverageNORM, unsigned int nbAverageSBM, |
|
177 | 171 | asm_msg *msgForMATR ) |
|
178 | 172 | { |
|
179 | 173 | float sum; |
|
180 | 174 | unsigned int i; |
|
181 | 175 | |
|
182 | 176 | for(i=0; i<TOTAL_SIZE_SM; i++) |
|
183 | 177 | { |
|
184 | 178 | sum = ( (int *) (ring_node_tab[0]->buffer_address) ) [ i ] |
|
185 | 179 | + ( (int *) (ring_node_tab[1]->buffer_address) ) [ i ] |
|
186 | 180 | + ( (int *) (ring_node_tab[2]->buffer_address) ) [ i ] |
|
187 | 181 | + ( (int *) (ring_node_tab[3]->buffer_address) ) [ i ] |
|
188 | 182 | + ( (int *) (ring_node_tab[4]->buffer_address) ) [ i ] |
|
189 | 183 | + ( (int *) (ring_node_tab[5]->buffer_address) ) [ i ] |
|
190 | 184 | + ( (int *) (ring_node_tab[6]->buffer_address) ) [ i ] |
|
191 | 185 | + ( (int *) (ring_node_tab[7]->buffer_address) ) [ i ]; |
|
192 | 186 | |
|
193 | 187 | if ( (nbAverageNORM == 0) && (nbAverageSBM == 0) ) |
|
194 | 188 | { |
|
195 | 189 | averaged_spec_mat_NORM[ i ] = sum; |
|
196 | 190 | averaged_spec_mat_SBM[ i ] = sum; |
|
197 | 191 | msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime; |
|
198 | 192 | msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime; |
|
199 | 193 | msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime; |
|
200 | 194 | msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime; |
|
201 | 195 | } |
|
202 | 196 | else if ( (nbAverageNORM != 0) && (nbAverageSBM != 0) ) |
|
203 | 197 | { |
|
204 | 198 | averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum ); |
|
205 | 199 | averaged_spec_mat_SBM[ i ] = ( averaged_spec_mat_SBM[ i ] + sum ); |
|
206 | 200 | } |
|
207 | 201 | else if ( (nbAverageNORM != 0) && (nbAverageSBM == 0) ) |
|
208 | 202 | { |
|
209 | 203 | averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum ); |
|
210 | 204 | averaged_spec_mat_SBM[ i ] = sum; |
|
211 | 205 | msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime; |
|
212 | 206 | msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime; |
|
213 | 207 | } |
|
214 | 208 | else |
|
215 | 209 | { |
|
216 | 210 | averaged_spec_mat_NORM[ i ] = sum; |
|
217 | 211 | averaged_spec_mat_SBM[ i ] = ( averaged_spec_mat_SBM[ i ] + sum ); |
|
218 | 212 | msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime; |
|
219 | 213 | msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime; |
|
220 | 214 | // PRINTF2("ERR *** in SM_average *** unexpected parameters %d %d\n", nbAverageNORM, nbAverageSBM) |
|
221 | 215 | } |
|
222 | 216 | } |
|
223 | 217 | } |
|
224 | 218 | |
|
225 | 219 | void SM_average_debug( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM, |
|
226 | 220 | ring_node *ring_node_tab[], |
|
227 | 221 | unsigned int nbAverageNORM, unsigned int nbAverageSBM, |
|
228 | 222 | asm_msg *msgForMATR ) |
|
229 | 223 | { |
|
230 | 224 | float sum; |
|
231 | 225 | unsigned int i; |
|
232 | 226 | |
|
233 | 227 | for(i=0; i<TOTAL_SIZE_SM; i++) |
|
234 | 228 | { |
|
235 | 229 | sum = ( (int *) (ring_node_tab[0]->buffer_address) ) [ i ]; |
|
236 | 230 | averaged_spec_mat_NORM[ i ] = sum; |
|
237 | 231 | averaged_spec_mat_SBM[ i ] = sum; |
|
238 | 232 | msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime; |
|
239 | 233 | msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime; |
|
240 | 234 | msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime; |
|
241 | 235 | msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime; |
|
242 | 236 | } |
|
243 | 237 | } |
|
244 | 238 | |
|
245 | 239 | void ASM_reorganize_and_divide( float *averaged_spec_mat, float *averaged_spec_mat_reorganized, float divider ) |
|
246 | 240 | { |
|
247 | 241 | int frequencyBin; |
|
248 | 242 | int asmComponent; |
|
249 | 243 | unsigned int offsetASM; |
|
250 | 244 | unsigned int offsetASMReorganized; |
|
251 | 245 | |
|
252 | 246 | // BUILD DATA |
|
253 | 247 | for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++) |
|
254 | 248 | { |
|
255 | 249 | for( frequencyBin = 0; frequencyBin < NB_BINS_PER_SM; frequencyBin++ ) |
|
256 | 250 | { |
|
257 | 251 | offsetASMReorganized = |
|
258 | 252 | frequencyBin * NB_VALUES_PER_SM |
|
259 | 253 | + asmComponent; |
|
260 | 254 | offsetASM = |
|
261 | 255 | asmComponent * NB_BINS_PER_SM |
|
262 | 256 | + frequencyBin; |
|
263 | 257 | averaged_spec_mat_reorganized[offsetASMReorganized ] = |
|
264 | 258 | averaged_spec_mat[ offsetASM ] / divider; |
|
265 | 259 | } |
|
266 | 260 | } |
|
267 | 261 | } |
|
268 | 262 | |
|
269 | 263 | void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat , float divider, |
|
270 | 264 | unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage, unsigned char ASMIndexStart ) |
|
271 | 265 | { |
|
272 | 266 | int frequencyBin; |
|
273 | 267 | int asmComponent; |
|
274 | 268 | int offsetASM; |
|
275 | 269 | int offsetCompressed; |
|
276 | 270 | int k; |
|
277 | 271 | |
|
278 | 272 | // BUILD DATA |
|
279 | 273 | for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++) |
|
280 | 274 | { |
|
281 | 275 | for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ ) |
|
282 | 276 | { |
|
283 | 277 | offsetCompressed = // NO TIME OFFSET |
|
284 | 278 | frequencyBin * NB_VALUES_PER_SM |
|
285 | 279 | + asmComponent; |
|
286 | 280 | offsetASM = // NO TIME OFFSET |
|
287 | 281 | asmComponent * NB_BINS_PER_SM |
|
288 | 282 | + ASMIndexStart |
|
289 | 283 | + frequencyBin * nbBinsToAverage; |
|
290 | 284 | compressed_spec_mat[ offsetCompressed ] = 0; |
|
291 | 285 | for ( k = 0; k < nbBinsToAverage; k++ ) |
|
292 | 286 | { |
|
293 | 287 | compressed_spec_mat[offsetCompressed ] = |
|
294 | 288 | ( compressed_spec_mat[ offsetCompressed ] |
|
295 | 289 | + averaged_spec_mat[ offsetASM + k ] ); |
|
296 | 290 | } |
|
297 | 291 | compressed_spec_mat[ offsetCompressed ] = |
|
298 | 292 | compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage); |
|
299 | 293 | } |
|
300 | 294 | } |
|
301 | 295 | } |
|
302 | 296 | |
|
303 | 297 | void ASM_convert( volatile float *input_matrix, char *output_matrix) |
|
304 | 298 | { |
|
305 | 299 | unsigned int frequencyBin; |
|
306 | 300 | unsigned int asmComponent; |
|
307 | 301 | char * pt_char_input; |
|
308 | 302 | char * pt_char_output; |
|
309 | 303 | unsigned int offsetInput; |
|
310 | 304 | unsigned int offsetOutput; |
|
311 | 305 | |
|
312 | 306 | pt_char_input = (char*) &input_matrix; |
|
313 | 307 | pt_char_output = (char*) &output_matrix; |
|
314 | 308 | |
|
315 | 309 | // convert all other data |
|
316 | 310 | for( frequencyBin=0; frequencyBin<NB_BINS_PER_SM; frequencyBin++) |
|
317 | 311 | { |
|
318 | 312 | for ( asmComponent=0; asmComponent<NB_VALUES_PER_SM; asmComponent++) |
|
319 | 313 | { |
|
320 | 314 | offsetInput = (frequencyBin*NB_VALUES_PER_SM) + asmComponent ; |
|
321 | 315 | offsetOutput = 2 * ( (frequencyBin*NB_VALUES_PER_SM) + asmComponent ) ; |
|
322 | 316 | pt_char_input = (char*) &input_matrix [ offsetInput ]; |
|
323 | 317 | pt_char_output = (char*) &output_matrix[ offsetOutput ]; |
|
324 | 318 | pt_char_output[0] = pt_char_input[0]; // bits 31 downto 24 of the float |
|
325 | 319 | pt_char_output[1] = pt_char_input[1]; // bits 23 downto 16 of the float |
|
326 | 320 | } |
|
327 | 321 | } |
|
328 | 322 | } |
|
329 | 323 | |
|
330 | 324 | void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat, |
|
331 | 325 | float divider, |
|
332 | 326 | unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart, unsigned char channel); |
|
333 | 327 | |
|
334 | 328 | int getFBinMask(int k, unsigned char channel); |
|
335 | 329 | |
|
336 | 330 | void init_kcoeff_sbm_from_kcoeff_norm( float *input_kcoeff, float *output_kcoeff, unsigned char nb_bins_norm); |
|
337 | 331 | |
|
338 | 332 | #endif // FSW_PROCESSING_H_INCLUDED |
General Comments 0
You need to be logged in to leave comments.
Login now