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

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

3.1.0.0
paul -
r287:0eea705dfb8e R3_plus draft
parent child
Show More
Show file before | Show file after | Hide comments
@@ -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