@@ -1,2 +1,2 | |||||
1 | 3081d1f9bb20b2b64a192585337a292a9804e0c5 LFR_basic-parameters |
|
1 | 3081d1f9bb20b2b64a192585337a292a9804e0c5 LFR_basic-parameters | |
2 | 5dfc0745a617f0b14b9b4c6d6c12d01f1fb9a801 header/lfr_common_headers |
|
2 | 26659466eb11170e587645c796142ac8a7fd0add header/lfr_common_headers |
@@ -1,371 +1,373 | |||||
1 | #ifndef FSW_PROCESSING_H_INCLUDED |
|
1 | #ifndef FSW_PROCESSING_H_INCLUDED | |
2 | #define FSW_PROCESSING_H_INCLUDED |
|
2 | #define FSW_PROCESSING_H_INCLUDED | |
3 |
|
3 | |||
4 | #include <rtems.h> |
|
4 | #include <rtems.h> | |
5 | #include <grspw.h> |
|
5 | #include <grspw.h> | |
6 | #include <math.h> |
|
6 | #include <math.h> | |
7 | #include <stdlib.h> // abs() is in the stdlib |
|
7 | #include <stdlib.h> // abs() is in the stdlib | |
8 | #include <stdio.h> |
|
8 | #include <stdio.h> | |
9 | #include <math.h> |
|
9 | #include <math.h> | |
10 | #include <grlib_regs.h> |
|
10 | #include <grlib_regs.h> | |
11 |
|
11 | |||
12 | #include "fsw_params.h" |
|
12 | #include "fsw_params.h" | |
13 |
|
13 | |||
14 | #define SBM_COEFF_PER_NORM_COEFF 2 |
|
14 | #define SBM_COEFF_PER_NORM_COEFF 2 | |
15 | #define MAX_SRC_DATA 780 // MAX size is 26 bins * 30 Bytes [TM_LFR_SCIENCE_BURST_BP2_F1] |
|
15 | #define MAX_SRC_DATA 780 // MAX size is 26 bins * 30 Bytes [TM_LFR_SCIENCE_BURST_BP2_F1] | |
16 | #define MAX_SRC_DATA_WITH_SPARE 143 // 13 bins * 11 Bytes |
|
16 | #define MAX_SRC_DATA_WITH_SPARE 143 // 13 bins * 11 Bytes | |
17 |
|
17 | |||
18 | #define NODE_0 0 |
|
18 | #define NODE_0 0 | |
19 | #define NODE_1 1 |
|
19 | #define NODE_1 1 | |
20 | #define NODE_2 2 |
|
20 | #define NODE_2 2 | |
21 | #define NODE_3 3 |
|
21 | #define NODE_3 3 | |
22 | #define NODE_4 4 |
|
22 | #define NODE_4 4 | |
23 | #define NODE_5 5 |
|
23 | #define NODE_5 5 | |
24 | #define NODE_6 6 |
|
24 | #define NODE_6 6 | |
25 | #define NODE_7 7 |
|
25 | #define NODE_7 7 | |
26 |
|
26 | |||
27 | typedef struct ring_node_asm |
|
27 | typedef struct ring_node_asm | |
28 | { |
|
28 | { | |
29 | struct ring_node_asm *next; |
|
29 | struct ring_node_asm *next; | |
30 | float matrix[ TOTAL_SIZE_SM ]; |
|
30 | float matrix[ TOTAL_SIZE_SM ]; | |
31 | unsigned int status; |
|
31 | unsigned int status; | |
32 | } ring_node_asm; |
|
32 | } ring_node_asm; | |
33 |
|
33 | |||
34 | typedef struct |
|
34 | typedef struct | |
35 | { |
|
35 | { | |
36 | unsigned char targetLogicalAddress; |
|
36 | unsigned char targetLogicalAddress; | |
37 | unsigned char protocolIdentifier; |
|
37 | unsigned char protocolIdentifier; | |
38 | unsigned char reserved; |
|
38 | unsigned char reserved; | |
39 | unsigned char userApplication; |
|
39 | unsigned char userApplication; | |
40 | unsigned char packetID[BYTES_PER_PACKETID]; |
|
40 | unsigned char packetID[BYTES_PER_PACKETID]; | |
41 | unsigned char packetSequenceControl[BYTES_PER_SEQ_CTRL]; |
|
41 | unsigned char packetSequenceControl[BYTES_PER_SEQ_CTRL]; | |
42 | unsigned char packetLength[BYTES_PER_PKT_LEN]; |
|
42 | unsigned char packetLength[BYTES_PER_PKT_LEN]; | |
43 | // DATA FIELD HEADER |
|
43 | // DATA FIELD HEADER | |
44 | unsigned char spare1_pusVersion_spare2; |
|
44 | unsigned char spare1_pusVersion_spare2; | |
45 | unsigned char serviceType; |
|
45 | unsigned char serviceType; | |
46 | unsigned char serviceSubType; |
|
46 | unsigned char serviceSubType; | |
47 | unsigned char destinationID; |
|
47 | unsigned char destinationID; | |
48 | unsigned char time[BYTES_PER_TIME]; |
|
48 | unsigned char time[BYTES_PER_TIME]; | |
49 | // AUXILIARY HEADER |
|
49 | // AUXILIARY HEADER | |
50 | unsigned char sid; |
|
50 | unsigned char sid; | |
51 | unsigned char pa_bia_status_info; |
|
51 | unsigned char pa_bia_status_info; | |
52 | unsigned char sy_lfr_common_parameters_spare; |
|
52 | unsigned char sy_lfr_common_parameters_spare; | |
53 | unsigned char sy_lfr_common_parameters; |
|
53 | unsigned char sy_lfr_common_parameters; | |
54 | unsigned char acquisitionTime[BYTES_PER_TIME]; |
|
54 | unsigned char acquisitionTime[BYTES_PER_TIME]; | |
55 | unsigned char pa_lfr_bp_blk_nr[BYTES_PER_BLKNR]; |
|
55 | unsigned char pa_lfr_bp_blk_nr[BYTES_PER_BLKNR]; | |
56 | // SOURCE DATA |
|
56 | // SOURCE DATA | |
57 | unsigned char data[ MAX_SRC_DATA ]; // MAX size is 26 bins * 30 Bytes [TM_LFR_SCIENCE_BURST_BP2_F1] |
|
57 | unsigned char data[ MAX_SRC_DATA ]; // MAX size is 26 bins * 30 Bytes [TM_LFR_SCIENCE_BURST_BP2_F1] | |
58 | } bp_packet; |
|
58 | } bp_packet; | |
59 |
|
59 | |||
60 | typedef struct |
|
60 | typedef struct | |
61 | { |
|
61 | { | |
62 | unsigned char targetLogicalAddress; |
|
62 | unsigned char targetLogicalAddress; | |
63 | unsigned char protocolIdentifier; |
|
63 | unsigned char protocolIdentifier; | |
64 | unsigned char reserved; |
|
64 | unsigned char reserved; | |
65 | unsigned char userApplication; |
|
65 | unsigned char userApplication; | |
66 | unsigned char packetID[BYTES_PER_PACKETID]; |
|
66 | unsigned char packetID[BYTES_PER_PACKETID]; | |
67 | unsigned char packetSequenceControl[BYTES_PER_SEQ_CTRL]; |
|
67 | unsigned char packetSequenceControl[BYTES_PER_SEQ_CTRL]; | |
68 | unsigned char packetLength[BYTES_PER_PKT_LEN]; |
|
68 | unsigned char packetLength[BYTES_PER_PKT_LEN]; | |
69 | // DATA FIELD HEADER |
|
69 | // DATA FIELD HEADER | |
70 | unsigned char spare1_pusVersion_spare2; |
|
70 | unsigned char spare1_pusVersion_spare2; | |
71 | unsigned char serviceType; |
|
71 | unsigned char serviceType; | |
72 | unsigned char serviceSubType; |
|
72 | unsigned char serviceSubType; | |
73 | unsigned char destinationID; |
|
73 | unsigned char destinationID; | |
74 | unsigned char time[BYTES_PER_TIME]; |
|
74 | unsigned char time[BYTES_PER_TIME]; | |
75 | // AUXILIARY HEADER |
|
75 | // AUXILIARY HEADER | |
76 | unsigned char sid; |
|
76 | unsigned char sid; | |
77 | unsigned char pa_bia_status_info; |
|
77 | unsigned char pa_bia_status_info; | |
78 | unsigned char sy_lfr_common_parameters_spare; |
|
78 | unsigned char sy_lfr_common_parameters_spare; | |
79 | unsigned char sy_lfr_common_parameters; |
|
79 | unsigned char sy_lfr_common_parameters; | |
80 | unsigned char acquisitionTime[BYTES_PER_TIME]; |
|
80 | unsigned char acquisitionTime[BYTES_PER_TIME]; | |
81 | unsigned char source_data_spare; |
|
81 | unsigned char source_data_spare; | |
82 | unsigned char pa_lfr_bp_blk_nr[BYTES_PER_BLKNR]; |
|
82 | unsigned char pa_lfr_bp_blk_nr[BYTES_PER_BLKNR]; | |
83 | // SOURCE DATA |
|
83 | // SOURCE DATA | |
84 | unsigned char data[ MAX_SRC_DATA_WITH_SPARE ]; // 13 bins * 11 Bytes |
|
84 | unsigned char data[ MAX_SRC_DATA_WITH_SPARE ]; // 13 bins * 11 Bytes | |
85 | } bp_packet_with_spare; // only for TM_LFR_SCIENCE_NORMAL_BP1_F0 and F1 |
|
85 | } bp_packet_with_spare; // only for TM_LFR_SCIENCE_NORMAL_BP1_F0 and F1 | |
86 |
|
86 | |||
87 | typedef struct asm_msg |
|
87 | typedef struct asm_msg | |
88 | { |
|
88 | { | |
89 | ring_node_asm *norm; |
|
89 | ring_node_asm *norm; | |
90 | ring_node_asm *burst_sbm; |
|
90 | ring_node_asm *burst_sbm; | |
91 | rtems_event_set event; |
|
91 | rtems_event_set event; | |
92 | unsigned int coarseTimeNORM; |
|
92 | unsigned int coarseTimeNORM; | |
93 | unsigned int fineTimeNORM; |
|
93 | unsigned int fineTimeNORM; | |
94 | unsigned int coarseTimeSBM; |
|
94 | unsigned int coarseTimeSBM; | |
95 | unsigned int fineTimeSBM; |
|
95 | unsigned int fineTimeSBM; | |
96 | unsigned int numberOfSMInASMNORM; |
|
96 | unsigned int numberOfSMInASMNORM; | |
97 | unsigned int numberOfSMInASMSBM; |
|
97 | unsigned int numberOfSMInASMSBM; | |
98 | } asm_msg; |
|
98 | } asm_msg; | |
99 |
|
99 | |||
100 | extern unsigned char thisIsAnASMRestart; |
|
100 | extern unsigned char thisIsAnASMRestart; | |
101 |
|
101 | |||
102 | extern volatile int sm_f0[ ]; |
|
102 | extern volatile int sm_f0[ ]; | |
103 | extern volatile int sm_f1[ ]; |
|
103 | extern volatile int sm_f1[ ]; | |
104 | extern volatile int sm_f2[ ]; |
|
104 | extern volatile int sm_f2[ ]; | |
105 | extern unsigned int acquisitionDurations[]; |
|
105 | extern unsigned int acquisitionDurations[]; | |
106 |
|
106 | |||
107 | // parameters |
|
107 | // parameters | |
108 | extern struct param_local_str param_local; |
|
108 | extern struct param_local_str param_local; | |
109 | extern Packet_TM_LFR_PARAMETER_DUMP_t parameter_dump_packet; |
|
109 | extern Packet_TM_LFR_PARAMETER_DUMP_t parameter_dump_packet; | |
110 |
|
110 | |||
111 | // registers |
|
111 | // registers | |
112 | extern time_management_regs_t *time_management_regs; |
|
112 | extern time_management_regs_t *time_management_regs; | |
113 | extern volatile spectral_matrix_regs_t *spectral_matrix_regs; |
|
113 | extern volatile spectral_matrix_regs_t *spectral_matrix_regs; | |
114 |
|
114 | |||
115 | extern rtems_name misc_name[]; |
|
115 | extern rtems_name misc_name[]; | |
116 | extern rtems_id Task_id[]; /* array of task ids */ |
|
116 | extern rtems_id Task_id[]; /* array of task ids */ | |
117 |
|
117 | |||
118 | ring_node * getRingNodeForAveraging( unsigned char frequencyChannel); |
|
118 | ring_node * getRingNodeForAveraging( unsigned char frequencyChannel); | |
119 | // ISR |
|
119 | // ISR | |
120 | rtems_isr spectral_matrices_isr( rtems_vector_number vector ); |
|
120 | rtems_isr spectral_matrices_isr( rtems_vector_number vector ); | |
121 |
|
121 | |||
122 | //****************** |
|
122 | //****************** | |
123 | // Spectral Matrices |
|
123 | // Spectral Matrices | |
124 | void reset_nb_sm( void ); |
|
124 | void reset_nb_sm( void ); | |
125 | // SM |
|
125 | // SM | |
126 | void SM_init_rings( void ); |
|
126 | void SM_init_rings( void ); | |
127 | void SM_reset_current_ring_nodes( void ); |
|
127 | void SM_reset_current_ring_nodes( void ); | |
128 | // ASM |
|
128 | // ASM | |
129 | void ASM_generic_init_ring(ring_node_asm *ring, unsigned char nbNodes ); |
|
129 | void ASM_generic_init_ring(ring_node_asm *ring, unsigned char nbNodes ); | |
130 |
|
130 | |||
131 | //***************** |
|
131 | //***************** | |
132 | // Basic Parameters |
|
132 | // Basic Parameters | |
133 |
|
133 | |||
134 | void BP_reset_current_ring_nodes( void ); |
|
134 | void BP_reset_current_ring_nodes( void ); | |
135 | void BP_init_header(bp_packet *packet, |
|
135 | void BP_init_header(bp_packet *packet, | |
136 | unsigned int apid, unsigned char sid, |
|
136 | unsigned int apid, unsigned char sid, | |
137 | unsigned int packetLength , unsigned char blkNr); |
|
137 | unsigned int packetLength , unsigned char blkNr); | |
138 | void BP_init_header_with_spare(bp_packet_with_spare *packet, |
|
138 | void BP_init_header_with_spare(bp_packet_with_spare *packet, | |
139 | unsigned int apid, unsigned char sid, |
|
139 | unsigned int apid, unsigned char sid, | |
140 | unsigned int packetLength, unsigned char blkNr ); |
|
140 | unsigned int packetLength, unsigned char blkNr ); | |
141 | void BP_send( char *data, |
|
141 | void BP_send( char *data, | |
142 | rtems_id queue_id, |
|
142 | rtems_id queue_id, | |
143 | unsigned int nbBytesToSend , unsigned int sid ); |
|
143 | unsigned int nbBytesToSend , unsigned int sid ); | |
144 | void BP_send_s1_s2(char *data, |
|
144 | void BP_send_s1_s2(char *data, | |
145 | rtems_id queue_id, |
|
145 | rtems_id queue_id, | |
146 | unsigned int nbBytesToSend, unsigned int sid ); |
|
146 | unsigned int nbBytesToSend, unsigned int sid ); | |
147 |
|
147 | |||
148 | //****************** |
|
148 | //****************** | |
149 | // general functions |
|
149 | // general functions | |
150 | void reset_sm_status( void ); |
|
150 | void reset_sm_status( void ); | |
151 | void reset_spectral_matrix_regs( void ); |
|
151 | void reset_spectral_matrix_regs( void ); | |
152 | void set_time(unsigned char *time, unsigned char *timeInBuffer ); |
|
152 | void set_time(unsigned char *time, unsigned char *timeInBuffer ); | |
153 | unsigned long long int get_acquisition_time( unsigned char *timePtr ); |
|
153 | unsigned long long int get_acquisition_time( unsigned char *timePtr ); | |
154 | unsigned char getSID( rtems_event_set event ); |
|
154 | unsigned char getSID( rtems_event_set event ); | |
155 |
|
155 | |||
156 | extern rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id ); |
|
156 | extern rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id ); | |
157 | extern rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id ); |
|
157 | extern rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id ); | |
158 |
|
158 | |||
159 | //*************************************** |
|
159 | //*************************************** | |
160 | // DEFINITIONS OF STATIC INLINE FUNCTIONS |
|
160 | // DEFINITIONS OF STATIC INLINE FUNCTIONS | |
161 | static inline void SM_average(float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM, |
|
161 | static inline void SM_average(float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM, | |
162 | ring_node *ring_node_tab[], |
|
162 | ring_node *ring_node_tab[], | |
163 | unsigned int nbAverageNORM, unsigned int nbAverageSBM, |
|
163 | unsigned int nbAverageNORM, unsigned int nbAverageSBM, | |
164 | asm_msg *msgForMATR , unsigned char channel); |
|
164 | asm_msg *msgForMATR , unsigned char channel); | |
165 |
|
165 | |||
166 | void ASM_patch( float *inputASM, float *outputASM ); |
|
166 | void ASM_patch( float *inputASM, float *outputASM ); | |
167 |
|
167 | |||
168 | void extractReImVectors(float *inputASM, float *outputASM, unsigned int asmComponent ); |
|
168 | void extractReImVectors(float *inputASM, float *outputASM, unsigned int asmComponent ); | |
169 |
|
169 | |||
170 | static inline void ASM_reorganize_and_divide(float *averaged_spec_mat, float *averaged_spec_mat_reorganized, |
|
170 | static inline void ASM_reorganize_and_divide(float *averaged_spec_mat, float *averaged_spec_mat_reorganized, | |
171 | float divider ); |
|
171 | float divider ); | |
172 |
|
172 | |||
173 | static inline void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat, |
|
173 | static inline void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat, | |
174 | float divider, |
|
174 | float divider, | |
175 | unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart); |
|
175 | unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart); | |
176 |
|
176 | |||
177 | static inline void ASM_convert(volatile float *input_matrix, char *output_matrix); |
|
177 | static inline void ASM_convert(volatile float *input_matrix, char *output_matrix); | |
178 |
|
178 | |||
|
179 | unsigned char isPolluted( u_int64_t t0, u_int64_t t1, u_int64_t tbad0, u_int64_t tbad1 ); | |||
|
180 | ||||
179 | unsigned char acquisitionTimeIsValid(unsigned int coarseTime, unsigned int fineTime, unsigned char channel); |
|
181 | unsigned char acquisitionTimeIsValid(unsigned int coarseTime, unsigned int fineTime, unsigned char channel); | |
180 |
|
182 | |||
181 | void SM_average( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM, |
|
183 | void SM_average( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM, | |
182 | ring_node *ring_node_tab[], |
|
184 | ring_node *ring_node_tab[], | |
183 | unsigned int nbAverageNORM, unsigned int nbAverageSBM, |
|
185 | unsigned int nbAverageNORM, unsigned int nbAverageSBM, | |
184 | asm_msg *msgForMATR, unsigned char channel ) |
|
186 | asm_msg *msgForMATR, unsigned char channel ) | |
185 | { |
|
187 | { | |
186 | float sum; |
|
188 | float sum; | |
187 | unsigned int i; |
|
189 | unsigned int i; | |
188 | unsigned int k; |
|
190 | unsigned int k; | |
189 | unsigned char incomingSMIsValid[NB_SM_BEFORE_AVF0_F1]; |
|
191 | unsigned char incomingSMIsValid[NB_SM_BEFORE_AVF0_F1]; | |
190 | unsigned int numberOfValidSM; |
|
192 | unsigned int numberOfValidSM; | |
191 | unsigned char isValid; |
|
193 | unsigned char isValid; | |
192 |
|
194 | |||
193 | //************** |
|
195 | //************** | |
194 | // PAS FILTERING |
|
196 | // PAS FILTERING | |
195 | // check acquisitionTime of the incoming data |
|
197 | // check acquisitionTime of the incoming data | |
196 | numberOfValidSM = 0; |
|
198 | numberOfValidSM = 0; | |
197 | for (k=0; k<NB_SM_BEFORE_AVF0_F1; k++) |
|
199 | for (k=0; k<NB_SM_BEFORE_AVF0_F1; k++) | |
198 | { |
|
200 | { | |
199 | isValid = acquisitionTimeIsValid( ring_node_tab[k]->coarseTime, ring_node_tab[k]->fineTime, channel ); |
|
201 | isValid = acquisitionTimeIsValid( ring_node_tab[k]->coarseTime, ring_node_tab[k]->fineTime, channel ); | |
200 | incomingSMIsValid[k] = isValid; |
|
202 | incomingSMIsValid[k] = isValid; | |
201 | numberOfValidSM = numberOfValidSM + isValid; |
|
203 | numberOfValidSM = numberOfValidSM + isValid; | |
202 | } |
|
204 | } | |
203 |
|
205 | |||
204 | //************************ |
|
206 | //************************ | |
205 | // AVERAGE SPECTRAL MATRIX |
|
207 | // AVERAGE SPECTRAL MATRIX | |
206 | for(i=0; i<TOTAL_SIZE_SM; i++) |
|
208 | for(i=0; i<TOTAL_SIZE_SM; i++) | |
207 | { |
|
209 | { | |
208 | sum = INIT_FLOAT; |
|
210 | sum = INIT_FLOAT; | |
209 | for ( k = 0; k < NB_SM_BEFORE_AVF0_F1; k++ ) |
|
211 | for ( k = 0; k < NB_SM_BEFORE_AVF0_F1; k++ ) | |
210 | { |
|
212 | { | |
211 |
if (incomingSMIsValid[k] == |
|
213 | if (incomingSMIsValid[k] == MATRIX_IS_NOT_POLLUTED) | |
212 | { |
|
214 | { | |
213 | sum = sum + ( (int *) (ring_node_tab[0]->buffer_address) ) [ i ] ; |
|
215 | sum = sum + ( (int *) (ring_node_tab[0]->buffer_address) ) [ i ] ; | |
214 | } |
|
216 | } | |
215 | } |
|
217 | } | |
216 |
|
218 | |||
217 | if ( (nbAverageNORM == 0) && (nbAverageSBM == 0) ) |
|
219 | if ( (nbAverageNORM == 0) && (nbAverageSBM == 0) ) | |
218 | { |
|
220 | { | |
219 | averaged_spec_mat_NORM[ i ] = sum; |
|
221 | averaged_spec_mat_NORM[ i ] = sum; | |
220 | averaged_spec_mat_SBM[ i ] = sum; |
|
222 | averaged_spec_mat_SBM[ i ] = sum; | |
221 | msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime; |
|
223 | msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime; | |
222 | msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime; |
|
224 | msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime; | |
223 | msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime; |
|
225 | msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime; | |
224 | msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime; |
|
226 | msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime; | |
225 | } |
|
227 | } | |
226 | else if ( (nbAverageNORM != 0) && (nbAverageSBM != 0) ) |
|
228 | else if ( (nbAverageNORM != 0) && (nbAverageSBM != 0) ) | |
227 | { |
|
229 | { | |
228 | averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum ); |
|
230 | averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum ); | |
229 | averaged_spec_mat_SBM[ i ] = ( averaged_spec_mat_SBM[ i ] + sum ); |
|
231 | averaged_spec_mat_SBM[ i ] = ( averaged_spec_mat_SBM[ i ] + sum ); | |
230 | } |
|
232 | } | |
231 | else if ( (nbAverageNORM != 0) && (nbAverageSBM == 0) ) |
|
233 | else if ( (nbAverageNORM != 0) && (nbAverageSBM == 0) ) | |
232 | { |
|
234 | { | |
233 | averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum ); |
|
235 | averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum ); | |
234 | averaged_spec_mat_SBM[ i ] = sum; |
|
236 | averaged_spec_mat_SBM[ i ] = sum; | |
235 | msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime; |
|
237 | msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime; | |
236 | msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime; |
|
238 | msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime; | |
237 | } |
|
239 | } | |
238 | else |
|
240 | else | |
239 | { |
|
241 | { | |
240 | averaged_spec_mat_NORM[ i ] = sum; |
|
242 | averaged_spec_mat_NORM[ i ] = sum; | |
241 | averaged_spec_mat_SBM[ i ] = ( averaged_spec_mat_SBM[ i ] + sum ); |
|
243 | averaged_spec_mat_SBM[ i ] = ( averaged_spec_mat_SBM[ i ] + sum ); | |
242 | msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime; |
|
244 | msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime; | |
243 | msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime; |
|
245 | msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime; | |
244 | // PRINTF2("ERR *** in SM_average *** unexpected parameters %d %d\n", nbAverageNORM, nbAverageSBM) |
|
246 | // PRINTF2("ERR *** in SM_average *** unexpected parameters %d %d\n", nbAverageNORM, nbAverageSBM) | |
245 | } |
|
247 | } | |
246 | } |
|
248 | } | |
247 |
|
249 | |||
248 | //******************* |
|
250 | //******************* | |
249 | // UPDATE SM COUNTERS |
|
251 | // UPDATE SM COUNTERS | |
250 | if ( (nbAverageNORM == 0) && (nbAverageSBM == 0) ) |
|
252 | if ( (nbAverageNORM == 0) && (nbAverageSBM == 0) ) | |
251 | { |
|
253 | { | |
252 | msgForMATR->numberOfSMInASMNORM = numberOfValidSM; |
|
254 | msgForMATR->numberOfSMInASMNORM = numberOfValidSM; | |
253 | msgForMATR->numberOfSMInASMSBM = numberOfValidSM; |
|
255 | msgForMATR->numberOfSMInASMSBM = numberOfValidSM; | |
254 | } |
|
256 | } | |
255 | else if ( (nbAverageNORM != 0) && (nbAverageSBM != 0) ) |
|
257 | else if ( (nbAverageNORM != 0) && (nbAverageSBM != 0) ) | |
256 | { |
|
258 | { | |
257 | msgForMATR->numberOfSMInASMNORM = msgForMATR->numberOfSMInASMNORM + numberOfValidSM; |
|
259 | msgForMATR->numberOfSMInASMNORM = msgForMATR->numberOfSMInASMNORM + numberOfValidSM; | |
258 | msgForMATR->numberOfSMInASMSBM = msgForMATR->numberOfSMInASMSBM + numberOfValidSM; |
|
260 | msgForMATR->numberOfSMInASMSBM = msgForMATR->numberOfSMInASMSBM + numberOfValidSM; | |
259 | } |
|
261 | } | |
260 | else if ( (nbAverageNORM != 0) && (nbAverageSBM == 0) ) |
|
262 | else if ( (nbAverageNORM != 0) && (nbAverageSBM == 0) ) | |
261 | { |
|
263 | { | |
262 | msgForMATR->numberOfSMInASMNORM = msgForMATR->numberOfSMInASMNORM + numberOfValidSM; |
|
264 | msgForMATR->numberOfSMInASMNORM = msgForMATR->numberOfSMInASMNORM + numberOfValidSM; | |
263 | msgForMATR->numberOfSMInASMSBM = numberOfValidSM; |
|
265 | msgForMATR->numberOfSMInASMSBM = numberOfValidSM; | |
264 | } |
|
266 | } | |
265 | else |
|
267 | else | |
266 | { |
|
268 | { | |
267 | msgForMATR->numberOfSMInASMNORM = numberOfValidSM; |
|
269 | msgForMATR->numberOfSMInASMNORM = numberOfValidSM; | |
268 | msgForMATR->numberOfSMInASMSBM = msgForMATR->numberOfSMInASMSBM + numberOfValidSM; |
|
270 | msgForMATR->numberOfSMInASMSBM = msgForMATR->numberOfSMInASMSBM + numberOfValidSM; | |
269 | } |
|
271 | } | |
270 | } |
|
272 | } | |
271 |
|
273 | |||
272 | void ASM_reorganize_and_divide( float *averaged_spec_mat, float *averaged_spec_mat_reorganized, float divider ) |
|
274 | void ASM_reorganize_and_divide( float *averaged_spec_mat, float *averaged_spec_mat_reorganized, float divider ) | |
273 | { |
|
275 | { | |
274 | int frequencyBin; |
|
276 | int frequencyBin; | |
275 | int asmComponent; |
|
277 | int asmComponent; | |
276 | unsigned int offsetASM; |
|
278 | unsigned int offsetASM; | |
277 | unsigned int offsetASMReorganized; |
|
279 | unsigned int offsetASMReorganized; | |
278 |
|
280 | |||
279 | // BUILD DATA |
|
281 | // BUILD DATA | |
280 | for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++) |
|
282 | for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++) | |
281 | { |
|
283 | { | |
282 | for( frequencyBin = 0; frequencyBin < NB_BINS_PER_SM; frequencyBin++ ) |
|
284 | for( frequencyBin = 0; frequencyBin < NB_BINS_PER_SM; frequencyBin++ ) | |
283 | { |
|
285 | { | |
284 | offsetASMReorganized = |
|
286 | offsetASMReorganized = | |
285 | (frequencyBin * NB_VALUES_PER_SM) |
|
287 | (frequencyBin * NB_VALUES_PER_SM) | |
286 | + asmComponent; |
|
288 | + asmComponent; | |
287 | offsetASM = |
|
289 | offsetASM = | |
288 | (asmComponent * NB_BINS_PER_SM) |
|
290 | (asmComponent * NB_BINS_PER_SM) | |
289 | + frequencyBin; |
|
291 | + frequencyBin; | |
290 | if ( divider != INIT_FLOAT ) |
|
292 | if ( divider != INIT_FLOAT ) | |
291 | { |
|
293 | { | |
292 | averaged_spec_mat_reorganized[offsetASMReorganized ] = averaged_spec_mat[ offsetASM ] / divider; |
|
294 | averaged_spec_mat_reorganized[offsetASMReorganized ] = averaged_spec_mat[ offsetASM ] / divider; | |
293 | } |
|
295 | } | |
294 | else |
|
296 | else | |
295 | { |
|
297 | { | |
296 | averaged_spec_mat_reorganized[offsetASMReorganized ] = INIT_FLOAT; |
|
298 | averaged_spec_mat_reorganized[offsetASMReorganized ] = INIT_FLOAT; | |
297 | } |
|
299 | } | |
298 | } |
|
300 | } | |
299 | } |
|
301 | } | |
300 | } |
|
302 | } | |
301 |
|
303 | |||
302 | void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat , float divider, |
|
304 | void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat , float divider, | |
303 | unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage, unsigned char ASMIndexStart ) |
|
305 | unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage, unsigned char ASMIndexStart ) | |
304 | { |
|
306 | { | |
305 | int frequencyBin; |
|
307 | int frequencyBin; | |
306 | int asmComponent; |
|
308 | int asmComponent; | |
307 | int offsetASM; |
|
309 | int offsetASM; | |
308 | int offsetCompressed; |
|
310 | int offsetCompressed; | |
309 | int k; |
|
311 | int k; | |
310 |
|
312 | |||
311 | // BUILD DATA |
|
313 | // BUILD DATA | |
312 | for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++) |
|
314 | for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++) | |
313 | { |
|
315 | { | |
314 | for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ ) |
|
316 | for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ ) | |
315 | { |
|
317 | { | |
316 | offsetCompressed = // NO TIME OFFSET |
|
318 | offsetCompressed = // NO TIME OFFSET | |
317 | (frequencyBin * NB_VALUES_PER_SM) |
|
319 | (frequencyBin * NB_VALUES_PER_SM) | |
318 | + asmComponent; |
|
320 | + asmComponent; | |
319 | offsetASM = // NO TIME OFFSET |
|
321 | offsetASM = // NO TIME OFFSET | |
320 | (asmComponent * NB_BINS_PER_SM) |
|
322 | (asmComponent * NB_BINS_PER_SM) | |
321 | + ASMIndexStart |
|
323 | + ASMIndexStart | |
322 | + (frequencyBin * nbBinsToAverage); |
|
324 | + (frequencyBin * nbBinsToAverage); | |
323 | compressed_spec_mat[ offsetCompressed ] = 0; |
|
325 | compressed_spec_mat[ offsetCompressed ] = 0; | |
324 | for ( k = 0; k < nbBinsToAverage; k++ ) |
|
326 | for ( k = 0; k < nbBinsToAverage; k++ ) | |
325 | { |
|
327 | { | |
326 | compressed_spec_mat[offsetCompressed ] = |
|
328 | compressed_spec_mat[offsetCompressed ] = | |
327 | ( compressed_spec_mat[ offsetCompressed ] |
|
329 | ( compressed_spec_mat[ offsetCompressed ] | |
328 | + averaged_spec_mat[ offsetASM + k ] ); |
|
330 | + averaged_spec_mat[ offsetASM + k ] ); | |
329 | } |
|
331 | } | |
330 | compressed_spec_mat[ offsetCompressed ] = |
|
332 | compressed_spec_mat[ offsetCompressed ] = | |
331 | compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage); |
|
333 | compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage); | |
332 | } |
|
334 | } | |
333 | } |
|
335 | } | |
334 | } |
|
336 | } | |
335 |
|
337 | |||
336 | void ASM_convert( volatile float *input_matrix, char *output_matrix) |
|
338 | void ASM_convert( volatile float *input_matrix, char *output_matrix) | |
337 | { |
|
339 | { | |
338 | unsigned int frequencyBin; |
|
340 | unsigned int frequencyBin; | |
339 | unsigned int asmComponent; |
|
341 | unsigned int asmComponent; | |
340 | char * pt_char_input; |
|
342 | char * pt_char_input; | |
341 | char * pt_char_output; |
|
343 | char * pt_char_output; | |
342 | unsigned int offsetInput; |
|
344 | unsigned int offsetInput; | |
343 | unsigned int offsetOutput; |
|
345 | unsigned int offsetOutput; | |
344 |
|
346 | |||
345 | pt_char_input = (char*) &input_matrix; |
|
347 | pt_char_input = (char*) &input_matrix; | |
346 | pt_char_output = (char*) &output_matrix; |
|
348 | pt_char_output = (char*) &output_matrix; | |
347 |
|
349 | |||
348 | // convert all other data |
|
350 | // convert all other data | |
349 | for( frequencyBin=0; frequencyBin<NB_BINS_PER_SM; frequencyBin++) |
|
351 | for( frequencyBin=0; frequencyBin<NB_BINS_PER_SM; frequencyBin++) | |
350 | { |
|
352 | { | |
351 | for ( asmComponent=0; asmComponent<NB_VALUES_PER_SM; asmComponent++) |
|
353 | for ( asmComponent=0; asmComponent<NB_VALUES_PER_SM; asmComponent++) | |
352 | { |
|
354 | { | |
353 | offsetInput = (frequencyBin*NB_VALUES_PER_SM) + asmComponent ; |
|
355 | offsetInput = (frequencyBin*NB_VALUES_PER_SM) + asmComponent ; | |
354 | offsetOutput = SM_BYTES_PER_VAL * ( (frequencyBin*NB_VALUES_PER_SM) + asmComponent ) ; |
|
356 | offsetOutput = SM_BYTES_PER_VAL * ( (frequencyBin*NB_VALUES_PER_SM) + asmComponent ) ; | |
355 | pt_char_input = (char*) &input_matrix [ offsetInput ]; |
|
357 | pt_char_input = (char*) &input_matrix [ offsetInput ]; | |
356 | pt_char_output = (char*) &output_matrix[ offsetOutput ]; |
|
358 | pt_char_output = (char*) &output_matrix[ offsetOutput ]; | |
357 | pt_char_output[0] = pt_char_input[0]; // bits 31 downto 24 of the float |
|
359 | pt_char_output[0] = pt_char_input[0]; // bits 31 downto 24 of the float | |
358 | pt_char_output[1] = pt_char_input[1]; // bits 23 downto 16 of the float |
|
360 | pt_char_output[1] = pt_char_input[1]; // bits 23 downto 16 of the float | |
359 | } |
|
361 | } | |
360 | } |
|
362 | } | |
361 | } |
|
363 | } | |
362 |
|
364 | |||
363 | void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat, |
|
365 | void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat, | |
364 | float divider, |
|
366 | float divider, | |
365 | unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart, unsigned char channel); |
|
367 | unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart, unsigned char channel); | |
366 |
|
368 | |||
367 | int getFBinMask(int k, unsigned char channel); |
|
369 | int getFBinMask(int k, unsigned char channel); | |
368 |
|
370 | |||
369 | void init_kcoeff_sbm_from_kcoeff_norm( float *input_kcoeff, float *output_kcoeff, unsigned char nb_bins_norm); |
|
371 | void init_kcoeff_sbm_from_kcoeff_norm( float *input_kcoeff, float *output_kcoeff, unsigned char nb_bins_norm); | |
370 |
|
372 | |||
371 | #endif // FSW_PROCESSING_H_INCLUDED |
|
373 | #endif // FSW_PROCESSING_H_INCLUDED |
@@ -1,107 +1,107 | |||||
1 | cmake_minimum_required (VERSION 2.6) |
|
1 | cmake_minimum_required (VERSION 2.6) | |
2 | project (fsw) |
|
2 | project (fsw) | |
3 |
|
3 | |||
4 | include(sparc-rtems) |
|
4 | include(sparc-rtems) | |
5 | include(cppcheck) |
|
5 | include(cppcheck) | |
6 |
|
6 | |||
7 | include_directories("../header" |
|
7 | include_directories("../header" | |
8 | "../header/lfr_common_headers" |
|
8 | "../header/lfr_common_headers" | |
9 | "../header/processing" |
|
9 | "../header/processing" | |
10 | "../LFR_basic-parameters" |
|
10 | "../LFR_basic-parameters" | |
11 | "../src") |
|
11 | "../src") | |
12 |
|
12 | |||
13 | set(SOURCES wf_handler.c |
|
13 | set(SOURCES wf_handler.c | |
14 | tc_handler.c |
|
14 | tc_handler.c | |
15 | fsw_misc.c |
|
15 | fsw_misc.c | |
16 | fsw_init.c |
|
16 | fsw_init.c | |
17 | fsw_globals.c |
|
17 | fsw_globals.c | |
18 | fsw_spacewire.c |
|
18 | fsw_spacewire.c | |
19 | tc_load_dump_parameters.c |
|
19 | tc_load_dump_parameters.c | |
20 | tm_lfr_tc_exe.c |
|
20 | tm_lfr_tc_exe.c | |
21 | tc_acceptance.c |
|
21 | tc_acceptance.c | |
22 | processing/fsw_processing.c |
|
22 | processing/fsw_processing.c | |
23 | processing/avf0_prc0.c |
|
23 | processing/avf0_prc0.c | |
24 | processing/avf1_prc1.c |
|
24 | processing/avf1_prc1.c | |
25 | processing/avf2_prc2.c |
|
25 | processing/avf2_prc2.c | |
26 | lfr_cpu_usage_report.c |
|
26 | lfr_cpu_usage_report.c | |
27 | ${LFR_BP_SRC} |
|
27 | ${LFR_BP_SRC} | |
28 | ../header/wf_handler.h |
|
28 | ../header/wf_handler.h | |
29 | ../header/tc_handler.h |
|
29 | ../header/tc_handler.h | |
30 | ../header/grlib_regs.h |
|
30 | ../header/grlib_regs.h | |
31 | ../header/fsw_misc.h |
|
31 | ../header/fsw_misc.h | |
32 | ../header/fsw_init.h |
|
32 | ../header/fsw_init.h | |
33 | ../header/fsw_spacewire.h |
|
33 | ../header/fsw_spacewire.h | |
34 | ../header/tc_load_dump_parameters.h |
|
34 | ../header/tc_load_dump_parameters.h | |
35 | ../header/tm_lfr_tc_exe.h |
|
35 | ../header/tm_lfr_tc_exe.h | |
36 | ../header/tc_acceptance.h |
|
36 | ../header/tc_acceptance.h | |
37 | ../header/processing/fsw_processing.h |
|
37 | ../header/processing/fsw_processing.h | |
38 | ../header/processing/avf0_prc0.h |
|
38 | ../header/processing/avf0_prc0.h | |
39 | ../header/processing/avf1_prc1.h |
|
39 | ../header/processing/avf1_prc1.h | |
40 | ../header/processing/avf2_prc2.h |
|
40 | ../header/processing/avf2_prc2.h | |
41 | ../header/fsw_params_wf_handler.h |
|
41 | ../header/fsw_params_wf_handler.h | |
42 | ../header/lfr_cpu_usage_report.h |
|
42 | ../header/lfr_cpu_usage_report.h | |
43 | ../header/lfr_common_headers/ccsds_types.h |
|
43 | ../header/lfr_common_headers/ccsds_types.h | |
44 | ../header/lfr_common_headers/fsw_params.h |
|
44 | ../header/lfr_common_headers/fsw_params.h | |
45 | ../header/lfr_common_headers/fsw_params_nb_bytes.h |
|
45 | ../header/lfr_common_headers/fsw_params_nb_bytes.h | |
46 | ../header/lfr_common_headers/fsw_params_processing.h |
|
46 | ../header/lfr_common_headers/fsw_params_processing.h | |
47 | ../header/lfr_common_headers/tm_byte_positions.h |
|
47 | ../header/lfr_common_headers/tm_byte_positions.h | |
48 | ../LFR_basic-parameters/basic_parameters.h |
|
48 | ../LFR_basic-parameters/basic_parameters.h | |
49 | ../LFR_basic-parameters/basic_parameters_params.h |
|
49 | ../LFR_basic-parameters/basic_parameters_params.h | |
50 | ../header/GscMemoryLPP.hpp |
|
50 | ../header/GscMemoryLPP.hpp | |
51 | ) |
|
51 | ) | |
52 |
|
52 | |||
53 |
|
53 | |||
54 | option(FSW_verbose "Enable verbose LFR" OFF) |
|
54 | option(FSW_verbose "Enable verbose LFR" OFF) | |
55 | option(FSW_boot_messages "Enable LFR boot messages" OFF) |
|
55 | option(FSW_boot_messages "Enable LFR boot messages" OFF) | |
56 | option(FSW_debug_messages "Enable LFR debug messages" OFF) |
|
56 | option(FSW_debug_messages "Enable LFR debug messages" OFF) | |
57 | option(FSW_cpu_usage_report "Enable LFR cpu usage report" OFF) |
|
57 | option(FSW_cpu_usage_report "Enable LFR cpu usage report" OFF) | |
58 | option(FSW_stack_report "Enable LFR stack report" OFF) |
|
58 | option(FSW_stack_report "Enable LFR stack report" OFF) | |
59 | option(FSW_vhdl_dev "?" OFF) |
|
59 | option(FSW_vhdl_dev "?" OFF) | |
60 | option(FSW_lpp_dpu_destid "Set to debug at LPP" ON) |
|
60 | option(FSW_lpp_dpu_destid "Set to debug at LPP" ON) | |
61 | option(FSW_debug_watchdog "Enable debug watchdog" OFF) |
|
61 | option(FSW_debug_watchdog "Enable debug watchdog" OFF) | |
62 | option(FSW_debug_tch "?" OFF) |
|
62 | option(FSW_debug_tch "?" OFF) | |
63 |
|
63 | |||
64 | set(SW_VERSION_N1 "3" CACHE STRING "Choose N1 FSW Version." FORCE) |
|
64 | set(SW_VERSION_N1 "3" CACHE STRING "Choose N1 FSW Version." FORCE) | |
65 | set(SW_VERSION_N2 "2" CACHE STRING "Choose N2 FSW Version." FORCE) |
|
65 | set(SW_VERSION_N2 "2" CACHE STRING "Choose N2 FSW Version." FORCE) | |
66 | set(SW_VERSION_N3 "0" CACHE STRING "Choose N3 FSW Version." FORCE) |
|
66 | set(SW_VERSION_N3 "0" CACHE STRING "Choose N3 FSW Version." FORCE) | |
67 |
set(SW_VERSION_N4 " |
|
67 | set(SW_VERSION_N4 "5" CACHE STRING "Choose N4 FSW Version." FORCE) | |
68 |
|
68 | |||
69 | if(FSW_verbose) |
|
69 | if(FSW_verbose) | |
70 | add_definitions(-DPRINT_MESSAGES_ON_CONSOLE) |
|
70 | add_definitions(-DPRINT_MESSAGES_ON_CONSOLE) | |
71 | endif() |
|
71 | endif() | |
72 | if(FSW_boot_messages) |
|
72 | if(FSW_boot_messages) | |
73 | add_definitions(-DBOOT_MESSAGES) |
|
73 | add_definitions(-DBOOT_MESSAGES) | |
74 | endif() |
|
74 | endif() | |
75 | if(FSW_debug_messages) |
|
75 | if(FSW_debug_messages) | |
76 | add_definitions(-DDEBUG_MESSAGES) |
|
76 | add_definitions(-DDEBUG_MESSAGES) | |
77 | endif() |
|
77 | endif() | |
78 | if(FSW_cpu_usage_report) |
|
78 | if(FSW_cpu_usage_report) | |
79 | add_definitions(-DPRINT_TASK_STATISTICS) |
|
79 | add_definitions(-DPRINT_TASK_STATISTICS) | |
80 | endif() |
|
80 | endif() | |
81 | if(FSW_stack_report) |
|
81 | if(FSW_stack_report) | |
82 | add_definitions(-DPRINT_STACK_REPORT) |
|
82 | add_definitions(-DPRINT_STACK_REPORT) | |
83 | endif() |
|
83 | endif() | |
84 | if(FSW_vhdl_dev) |
|
84 | if(FSW_vhdl_dev) | |
85 | add_definitions(-DVHDL_DEV) |
|
85 | add_definitions(-DVHDL_DEV) | |
86 | endif() |
|
86 | endif() | |
87 | if(FSW_lpp_dpu_destid) |
|
87 | if(FSW_lpp_dpu_destid) | |
88 | add_definitions(-DLPP_DPU_DESTID) |
|
88 | add_definitions(-DLPP_DPU_DESTID) | |
89 | endif() |
|
89 | endif() | |
90 | if(FSW_debug_watchdog) |
|
90 | if(FSW_debug_watchdog) | |
91 | add_definitions(-DDEBUG_WATCHDOG) |
|
91 | add_definitions(-DDEBUG_WATCHDOG) | |
92 | endif() |
|
92 | endif() | |
93 | if(FSW_debug_tch) |
|
93 | if(FSW_debug_tch) | |
94 | add_definitions(-DDEBUG_TCH) |
|
94 | add_definitions(-DDEBUG_TCH) | |
95 | endif() |
|
95 | endif() | |
96 |
|
96 | |||
97 | add_definitions(-DMSB_FIRST_TCH) |
|
97 | add_definitions(-DMSB_FIRST_TCH) | |
98 |
|
98 | |||
99 | add_definitions(-DSWVERSION=-1-0) |
|
99 | add_definitions(-DSWVERSION=-1-0) | |
100 | add_definitions(-DSW_VERSION_N1=${SW_VERSION_N1}) |
|
100 | add_definitions(-DSW_VERSION_N1=${SW_VERSION_N1}) | |
101 | add_definitions(-DSW_VERSION_N2=${SW_VERSION_N2}) |
|
101 | add_definitions(-DSW_VERSION_N2=${SW_VERSION_N2}) | |
102 | add_definitions(-DSW_VERSION_N3=${SW_VERSION_N3}) |
|
102 | add_definitions(-DSW_VERSION_N3=${SW_VERSION_N3}) | |
103 | add_definitions(-DSW_VERSION_N4=${SW_VERSION_N4}) |
|
103 | add_definitions(-DSW_VERSION_N4=${SW_VERSION_N4}) | |
104 |
|
104 | |||
105 | add_executable(fsw ${SOURCES}) |
|
105 | add_executable(fsw ${SOURCES}) | |
106 | add_test_cppcheck(fsw STYLE UNUSED_FUNCTIONS POSSIBLE_ERROR MISSING_INCLUDE) |
|
106 | add_test_cppcheck(fsw STYLE UNUSED_FUNCTIONS POSSIBLE_ERROR MISSING_INCLUDE) | |
107 |
|
107 |
@@ -1,98 +1,96 | |||||
1 | /** Global variables of the LFR flight software. |
|
1 | /** Global variables of the LFR flight software. | |
2 | * |
|
2 | * | |
3 | * @file |
|
3 | * @file | |
4 | * @author P. LEROY |
|
4 | * @author P. LEROY | |
5 | * |
|
5 | * | |
6 | * Among global variables, there are: |
|
6 | * Among global variables, there are: | |
7 | * - RTEMS names and id. |
|
7 | * - RTEMS names and id. | |
8 | * - APB configuration registers. |
|
8 | * - APB configuration registers. | |
9 | * - waveforms global buffers, used by the waveform picker hardware module to store data. |
|
9 | * - waveforms global buffers, used by the waveform picker hardware module to store data. | |
10 | * - spectral matrices buffesr, used by the hardware module to store data. |
|
10 | * - spectral matrices buffesr, used by the hardware module to store data. | |
11 | * - variable related to LFR modes parameters. |
|
11 | * - variable related to LFR modes parameters. | |
12 | * - the global HK packet buffer. |
|
12 | * - the global HK packet buffer. | |
13 | * - the global dump parameter buffer. |
|
13 | * - the global dump parameter buffer. | |
14 | * |
|
14 | * | |
15 | */ |
|
15 | */ | |
16 |
|
16 | |||
17 | #include <rtems.h> |
|
17 | #include <rtems.h> | |
18 | #include <grspw.h> |
|
18 | #include <grspw.h> | |
19 |
|
19 | |||
20 | #include "ccsds_types.h" |
|
20 | #include "ccsds_types.h" | |
21 | #include "grlib_regs.h" |
|
21 | #include "grlib_regs.h" | |
22 | #include "fsw_params.h" |
|
22 | #include "fsw_params.h" | |
23 | #include "fsw_params_wf_handler.h" |
|
23 | #include "fsw_params_wf_handler.h" | |
24 |
|
24 | |||
25 | #define NB_OF_TASKS 20 |
|
25 | #define NB_OF_TASKS 20 | |
26 | #define NB_OF_MISC_NAMES 5 |
|
26 | #define NB_OF_MISC_NAMES 5 | |
27 |
|
27 | |||
28 | // RTEMS GLOBAL VARIABLES |
|
28 | // RTEMS GLOBAL VARIABLES | |
29 | rtems_name misc_name[NB_OF_MISC_NAMES] = {0}; |
|
29 | rtems_name misc_name[NB_OF_MISC_NAMES] = {0}; | |
30 | rtems_name Task_name[NB_OF_TASKS] = {0}; /* array of task names */ |
|
30 | rtems_name Task_name[NB_OF_TASKS] = {0}; /* array of task names */ | |
31 | rtems_id Task_id[NB_OF_TASKS] = {0}; /* array of task ids */ |
|
31 | rtems_id Task_id[NB_OF_TASKS] = {0}; /* array of task ids */ | |
32 | rtems_name timecode_timer_name = 0; |
|
32 | rtems_name timecode_timer_name = 0; | |
33 | rtems_id timecode_timer_id = RTEMS_ID_NONE; |
|
33 | rtems_id timecode_timer_id = RTEMS_ID_NONE; | |
34 | rtems_name name_hk_rate_monotonic = 0; // name of the HK rate monotonic |
|
34 | rtems_name name_hk_rate_monotonic = 0; // name of the HK rate monotonic | |
35 | rtems_id HK_id = RTEMS_ID_NONE;// id of the HK rate monotonic period |
|
35 | rtems_id HK_id = RTEMS_ID_NONE;// id of the HK rate monotonic period | |
36 | rtems_name name_avgv_rate_monotonic = 0; // name of the AVGV rate monotonic |
|
36 | rtems_name name_avgv_rate_monotonic = 0; // name of the AVGV rate monotonic | |
37 | rtems_id AVGV_id = RTEMS_ID_NONE;// id of the AVGV rate monotonic period |
|
37 | rtems_id AVGV_id = RTEMS_ID_NONE;// id of the AVGV rate monotonic period | |
38 | int fdSPW = 0; |
|
38 | int fdSPW = 0; | |
39 | int fdUART = 0; |
|
39 | int fdUART = 0; | |
40 | unsigned char lfrCurrentMode = 0; |
|
40 | unsigned char lfrCurrentMode = 0; | |
41 | unsigned char pa_bia_status_info = 0; |
|
41 | unsigned char pa_bia_status_info = 0; | |
42 | unsigned char thisIsAnASMRestart = 0; |
|
42 | unsigned char thisIsAnASMRestart = 0; | |
43 | unsigned char oneTcLfrUpdateTimeReceived = 0; |
|
43 | unsigned char oneTcLfrUpdateTimeReceived = 0; | |
44 |
|
44 | |||
45 | // WAVEFORMS GLOBAL VARIABLES // 2048 * 3 * 4 + 2 * 4 = 24576 + 8 bytes = 24584 |
|
45 | // WAVEFORMS GLOBAL VARIABLES // 2048 * 3 * 4 + 2 * 4 = 24576 + 8 bytes = 24584 | |
46 | // 97 * 256 = 24832 => delta = 248 bytes = 62 words |
|
46 | // 97 * 256 = 24832 => delta = 248 bytes = 62 words | |
47 | // WAVEFORMS GLOBAL VARIABLES // 2688 * 3 * 4 + 2 * 4 = 32256 + 8 bytes = 32264 |
|
47 | // WAVEFORMS GLOBAL VARIABLES // 2688 * 3 * 4 + 2 * 4 = 32256 + 8 bytes = 32264 | |
48 | // 127 * 256 = 32512 => delta = 248 bytes = 62 words |
|
48 | // 127 * 256 = 32512 => delta = 248 bytes = 62 words | |
49 | // F0 F1 F2 F3 |
|
49 | // F0 F1 F2 F3 | |
50 | volatile int wf_buffer_f0[ NB_RING_NODES_F0 * WFRM_BUFFER ] __attribute__((aligned(0x100))) = {0}; |
|
50 | volatile int wf_buffer_f0[ NB_RING_NODES_F0 * WFRM_BUFFER ] __attribute__((aligned(0x100))) = {0}; | |
51 | volatile int wf_buffer_f1[ NB_RING_NODES_F1 * WFRM_BUFFER ] __attribute__((aligned(0x100))) = {0}; |
|
51 | volatile int wf_buffer_f1[ NB_RING_NODES_F1 * WFRM_BUFFER ] __attribute__((aligned(0x100))) = {0}; | |
52 | volatile int wf_buffer_f2[ NB_RING_NODES_F2 * WFRM_BUFFER ] __attribute__((aligned(0x100))) = {0}; |
|
52 | volatile int wf_buffer_f2[ NB_RING_NODES_F2 * WFRM_BUFFER ] __attribute__((aligned(0x100))) = {0}; | |
53 | volatile int wf_buffer_f3[ NB_RING_NODES_F3 * WFRM_BUFFER ] __attribute__((aligned(0x100))) = {0}; |
|
53 | volatile int wf_buffer_f3[ NB_RING_NODES_F3 * WFRM_BUFFER ] __attribute__((aligned(0x100))) = {0}; | |
54 |
|
54 | |||
55 | //*********************************** |
|
55 | //*********************************** | |
56 | // SPECTRAL MATRICES GLOBAL VARIABLES |
|
56 | // SPECTRAL MATRICES GLOBAL VARIABLES | |
57 |
|
57 | |||
58 | // alignment constraints for the spectral matrices buffers => the first data after the time (8 bytes) shall be aligned on 0x00 |
|
58 | // alignment constraints for the spectral matrices buffers => the first data after the time (8 bytes) shall be aligned on 0x00 | |
59 | volatile int sm_f0[ NB_RING_NODES_SM_F0 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100))) = {0}; |
|
59 | volatile int sm_f0[ NB_RING_NODES_SM_F0 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100))) = {0}; | |
60 | volatile int sm_f1[ NB_RING_NODES_SM_F1 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100))) = {0}; |
|
60 | volatile int sm_f1[ NB_RING_NODES_SM_F1 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100))) = {0}; | |
61 | volatile int sm_f2[ NB_RING_NODES_SM_F2 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100))) = {0}; |
|
61 | volatile int sm_f2[ NB_RING_NODES_SM_F2 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100))) = {0}; | |
62 |
|
62 | |||
63 | // APB CONFIGURATION REGISTERS |
|
63 | // APB CONFIGURATION REGISTERS | |
64 | time_management_regs_t *time_management_regs = (time_management_regs_t*) REGS_ADDR_TIME_MANAGEMENT; |
|
64 | time_management_regs_t *time_management_regs = (time_management_regs_t*) REGS_ADDR_TIME_MANAGEMENT; | |
65 | gptimer_regs_t *gptimer_regs = (gptimer_regs_t *) REGS_ADDR_GPTIMER; |
|
65 | gptimer_regs_t *gptimer_regs = (gptimer_regs_t *) REGS_ADDR_GPTIMER; | |
66 | waveform_picker_regs_0_1_18_t *waveform_picker_regs = (waveform_picker_regs_0_1_18_t*) REGS_ADDR_WAVEFORM_PICKER; |
|
66 | waveform_picker_regs_0_1_18_t *waveform_picker_regs = (waveform_picker_regs_0_1_18_t*) REGS_ADDR_WAVEFORM_PICKER; | |
67 | spectral_matrix_regs_t *spectral_matrix_regs = (spectral_matrix_regs_t*) REGS_ADDR_SPECTRAL_MATRIX; |
|
67 | spectral_matrix_regs_t *spectral_matrix_regs = (spectral_matrix_regs_t*) REGS_ADDR_SPECTRAL_MATRIX; | |
68 |
|
68 | |||
69 | // MODE PARAMETERS |
|
69 | // MODE PARAMETERS | |
70 | Packet_TM_LFR_PARAMETER_DUMP_t parameter_dump_packet = {0}; |
|
70 | Packet_TM_LFR_PARAMETER_DUMP_t parameter_dump_packet = {0}; | |
71 | struct param_local_str param_local = {0}; |
|
71 | struct param_local_str param_local = {0}; | |
72 | unsigned int lastValidEnterModeTime = {0}; |
|
72 | unsigned int lastValidEnterModeTime = {0}; | |
73 |
|
73 | |||
74 | // HK PACKETS |
|
74 | // HK PACKETS | |
75 | Packet_TM_LFR_HK_t housekeeping_packet = {0}; |
|
75 | Packet_TM_LFR_HK_t housekeeping_packet = {0}; | |
76 | // message queues occupancy |
|
76 | // message queues occupancy | |
77 | unsigned char hk_lfr_q_sd_fifo_size_max = 0; |
|
77 | unsigned char hk_lfr_q_sd_fifo_size_max = 0; | |
78 | unsigned char hk_lfr_q_rv_fifo_size_max = 0; |
|
78 | unsigned char hk_lfr_q_rv_fifo_size_max = 0; | |
79 | unsigned char hk_lfr_q_p0_fifo_size_max = 0; |
|
79 | unsigned char hk_lfr_q_p0_fifo_size_max = 0; | |
80 | unsigned char hk_lfr_q_p1_fifo_size_max = 0; |
|
80 | unsigned char hk_lfr_q_p1_fifo_size_max = 0; | |
81 | unsigned char hk_lfr_q_p2_fifo_size_max = 0; |
|
81 | unsigned char hk_lfr_q_p2_fifo_size_max = 0; | |
82 | // sequence counters are incremented by APID (PID + CAT) and destination ID |
|
82 | // sequence counters are incremented by APID (PID + CAT) and destination ID | |
83 | unsigned short sequenceCounters_SCIENCE_NORMAL_BURST __attribute__((aligned(0x4))) = 0; |
|
83 | unsigned short sequenceCounters_SCIENCE_NORMAL_BURST __attribute__((aligned(0x4))) = 0; | |
84 | unsigned short sequenceCounters_SCIENCE_SBM1_SBM2 __attribute__((aligned(0x4))) = 0; |
|
84 | unsigned short sequenceCounters_SCIENCE_SBM1_SBM2 __attribute__((aligned(0x4))) = 0; | |
85 | unsigned short sequenceCounters_TC_EXE[SEQ_CNT_NB_DEST_ID] __attribute__((aligned(0x4))) = {0}; |
|
85 | unsigned short sequenceCounters_TC_EXE[SEQ_CNT_NB_DEST_ID] __attribute__((aligned(0x4))) = {0}; | |
86 | unsigned short sequenceCounters_TM_DUMP[SEQ_CNT_NB_DEST_ID] __attribute__((aligned(0x4))) = {0}; |
|
86 | unsigned short sequenceCounters_TM_DUMP[SEQ_CNT_NB_DEST_ID] __attribute__((aligned(0x4))) = {0}; | |
87 | unsigned short sequenceCounterHK __attribute__((aligned(0x4))) = {0}; |
|
87 | unsigned short sequenceCounterHK __attribute__((aligned(0x4))) = {0}; | |
88 | spw_stats grspw_stats __attribute__((aligned(0x4))) = {0}; |
|
88 | spw_stats grspw_stats __attribute__((aligned(0x4))) = {0}; | |
89 |
|
89 | |||
90 | // TC_LFR_UPDATE_INFO |
|
90 | // TC_LFR_UPDATE_INFO | |
91 | rw_f_t rw_f; |
|
91 | rw_f_t rw_f; | |
92 |
|
92 | |||
93 | // TC_LFR_LOAD_FILTER_PAR |
|
93 | // TC_LFR_LOAD_FILTER_PAR | |
94 | filterPar_t filterPar = {0}; |
|
94 | filterPar_t filterPar = {0}; | |
95 |
|
95 | |||
96 | fbins_masks_t fbins_masks = {0}; |
|
96 | fbins_masks_t fbins_masks = {0}; | |
97 | unsigned int acquisitionDurations[NB_ACQUISITION_DURATION] |
|
|||
98 | = {ACQUISITION_DURATION_F0, ACQUISITION_DURATION_F1, ACQUISITION_DURATION_F2}; |
|
@@ -1,332 +1,332 | |||||
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 "avf2_prc2.h" |
|
10 | #include "avf2_prc2.h" | |
11 |
|
11 | |||
12 | nb_sm_before_bp_asm_f2 nb_sm_before_f2 = {0}; |
|
12 | nb_sm_before_bp_asm_f2 nb_sm_before_f2 = {0}; | |
13 |
|
13 | |||
14 | //*** |
|
14 | //*** | |
15 | // F2 |
|
15 | // F2 | |
16 | ring_node_asm asm_ring_norm_f2 [ NB_RING_NODES_ASM_NORM_F2 ] = {0}; |
|
16 | ring_node_asm asm_ring_norm_f2 [ NB_RING_NODES_ASM_NORM_F2 ] = {0}; | |
17 |
|
17 | |||
18 | ring_node ring_to_send_asm_f2 [ NB_RING_NODES_ASM_F2 ] = {0}; |
|
18 | ring_node ring_to_send_asm_f2 [ NB_RING_NODES_ASM_F2 ] = {0}; | |
19 | int buffer_asm_f2 [ NB_RING_NODES_ASM_F2 * TOTAL_SIZE_SM ] = {0}; |
|
19 | int buffer_asm_f2 [ NB_RING_NODES_ASM_F2 * TOTAL_SIZE_SM ] = {0}; | |
20 |
|
20 | |||
21 | float asm_f2_patched_norm [ TOTAL_SIZE_SM ] = {0}; |
|
21 | float asm_f2_patched_norm [ TOTAL_SIZE_SM ] = {0}; | |
22 | float asm_f2_reorganized [ TOTAL_SIZE_SM ] = {0}; |
|
22 | float asm_f2_reorganized [ TOTAL_SIZE_SM ] = {0}; | |
23 |
|
23 | |||
24 | float compressed_sm_norm_f2[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F2] = {0}; |
|
24 | float compressed_sm_norm_f2[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F2] = {0}; | |
25 |
|
25 | |||
26 | float k_coeff_intercalib_f2[ NB_BINS_COMPRESSED_SM_F2 * NB_K_COEFF_PER_BIN ] = {0}; // 12 * 32 = 384 |
|
26 | float k_coeff_intercalib_f2[ NB_BINS_COMPRESSED_SM_F2 * NB_K_COEFF_PER_BIN ] = {0}; // 12 * 32 = 384 | |
27 |
|
27 | |||
28 | //************ |
|
28 | //************ | |
29 | // RTEMS TASKS |
|
29 | // RTEMS TASKS | |
30 |
|
30 | |||
31 | //*** |
|
31 | //*** | |
32 | // F2 |
|
32 | // F2 | |
33 | rtems_task avf2_task( rtems_task_argument argument ) |
|
33 | rtems_task avf2_task( rtems_task_argument argument ) | |
34 | { |
|
34 | { | |
35 | rtems_event_set event_out; |
|
35 | rtems_event_set event_out; | |
36 | rtems_status_code status; |
|
36 | rtems_status_code status; | |
37 | rtems_id queue_id_prc2; |
|
37 | rtems_id queue_id_prc2; | |
38 | asm_msg msgForPRC; |
|
38 | asm_msg msgForPRC; | |
39 | ring_node *nodeForAveraging; |
|
39 | ring_node *nodeForAveraging; | |
40 | ring_node_asm *current_ring_node_asm_norm_f2; |
|
40 | ring_node_asm *current_ring_node_asm_norm_f2; | |
41 |
|
41 | |||
42 | unsigned int nb_norm_bp1; |
|
42 | unsigned int nb_norm_bp1; | |
43 | unsigned int nb_norm_bp2; |
|
43 | unsigned int nb_norm_bp2; | |
44 | unsigned int nb_norm_asm; |
|
44 | unsigned int nb_norm_asm; | |
45 |
|
45 | |||
46 | event_out = EVENT_SETS_NONE_PENDING; |
|
46 | event_out = EVENT_SETS_NONE_PENDING; | |
47 | queue_id_prc2 = RTEMS_ID_NONE; |
|
47 | queue_id_prc2 = RTEMS_ID_NONE; | |
48 | nb_norm_bp1 = 0; |
|
48 | nb_norm_bp1 = 0; | |
49 | nb_norm_bp2 = 0; |
|
49 | nb_norm_bp2 = 0; | |
50 | nb_norm_asm = 0; |
|
50 | nb_norm_asm = 0; | |
51 |
|
51 | |||
52 | reset_nb_sm_f2( ); // reset the sm counters that drive the BP and ASM computations / transmissions |
|
52 | reset_nb_sm_f2( ); // reset the sm counters that drive the BP and ASM computations / transmissions | |
53 | ASM_generic_init_ring( asm_ring_norm_f2, NB_RING_NODES_ASM_NORM_F2 ); |
|
53 | ASM_generic_init_ring( asm_ring_norm_f2, NB_RING_NODES_ASM_NORM_F2 ); | |
54 | current_ring_node_asm_norm_f2 = asm_ring_norm_f2; |
|
54 | current_ring_node_asm_norm_f2 = asm_ring_norm_f2; | |
55 |
|
55 | |||
56 | BOOT_PRINTF("in AVF2 ***\n") |
|
56 | BOOT_PRINTF("in AVF2 ***\n") | |
57 |
|
57 | |||
58 | status = get_message_queue_id_prc2( &queue_id_prc2 ); |
|
58 | status = get_message_queue_id_prc2( &queue_id_prc2 ); | |
59 | if (status != RTEMS_SUCCESSFUL) |
|
59 | if (status != RTEMS_SUCCESSFUL) | |
60 | { |
|
60 | { | |
61 | PRINTF1("in AVF2 *** ERR get_message_queue_id_prc2 %d\n", status) |
|
61 | PRINTF1("in AVF2 *** ERR get_message_queue_id_prc2 %d\n", status) | |
62 | } |
|
62 | } | |
63 |
|
63 | |||
64 | while(1){ |
|
64 | while(1){ | |
65 | rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0 |
|
65 | rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0 | |
66 |
|
66 | |||
67 | //**************************************** |
|
67 | //**************************************** | |
68 | // initialize the mesage for the MATR task |
|
68 | // initialize the mesage for the MATR task | |
69 | msgForPRC.norm = current_ring_node_asm_norm_f2; |
|
69 | msgForPRC.norm = current_ring_node_asm_norm_f2; | |
70 | msgForPRC.burst_sbm = NULL; |
|
70 | msgForPRC.burst_sbm = NULL; | |
71 | msgForPRC.event = EVENT_SETS_NONE_PENDING; // this composite event will be sent to the PRC2 task |
|
71 | msgForPRC.event = EVENT_SETS_NONE_PENDING; // this composite event will be sent to the PRC2 task | |
72 | // |
|
72 | // | |
73 | //**************************************** |
|
73 | //**************************************** | |
74 |
|
74 | |||
75 | nodeForAveraging = getRingNodeForAveraging( CHANNELF2 ); |
|
75 | nodeForAveraging = getRingNodeForAveraging( CHANNELF2 ); | |
76 |
|
76 | |||
77 | // compute the average and store it in the averaged_sm_f2 buffer |
|
77 | // compute the average and store it in the averaged_sm_f2 buffer | |
78 | SM_average_f2( current_ring_node_asm_norm_f2->matrix, |
|
78 | SM_average_f2( current_ring_node_asm_norm_f2->matrix, | |
79 | nodeForAveraging, |
|
79 | nodeForAveraging, | |
80 | nb_norm_bp1, |
|
80 | nb_norm_bp1, | |
81 | &msgForPRC ); |
|
81 | &msgForPRC ); | |
82 |
|
82 | |||
83 | // update nb_average |
|
83 | // update nb_average | |
84 | nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF2; |
|
84 | nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF2; | |
85 | nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF2; |
|
85 | nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF2; | |
86 | nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF2; |
|
86 | nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF2; | |
87 |
|
87 | |||
88 | if (nb_norm_bp1 == nb_sm_before_f2.norm_bp1) |
|
88 | if (nb_norm_bp1 == nb_sm_before_f2.norm_bp1) | |
89 | { |
|
89 | { | |
90 | nb_norm_bp1 = 0; |
|
90 | nb_norm_bp1 = 0; | |
91 | // set another ring for the ASM storage |
|
91 | // set another ring for the ASM storage | |
92 | current_ring_node_asm_norm_f2 = current_ring_node_asm_norm_f2->next; |
|
92 | current_ring_node_asm_norm_f2 = current_ring_node_asm_norm_f2->next; | |
93 | if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1) |
|
93 | if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1) | |
94 | || (lfrCurrentMode == LFR_MODE_SBM2) ) |
|
94 | || (lfrCurrentMode == LFR_MODE_SBM2) ) | |
95 | { |
|
95 | { | |
96 | msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_BP1_F2; |
|
96 | msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_BP1_F2; | |
97 | } |
|
97 | } | |
98 | } |
|
98 | } | |
99 |
|
99 | |||
100 | if (nb_norm_bp2 == nb_sm_before_f2.norm_bp2) |
|
100 | if (nb_norm_bp2 == nb_sm_before_f2.norm_bp2) | |
101 | { |
|
101 | { | |
102 | nb_norm_bp2 = 0; |
|
102 | nb_norm_bp2 = 0; | |
103 | if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1) |
|
103 | if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1) | |
104 | || (lfrCurrentMode == LFR_MODE_SBM2) ) |
|
104 | || (lfrCurrentMode == LFR_MODE_SBM2) ) | |
105 | { |
|
105 | { | |
106 | msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_BP2_F2; |
|
106 | msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_BP2_F2; | |
107 | } |
|
107 | } | |
108 | } |
|
108 | } | |
109 |
|
109 | |||
110 | if (nb_norm_asm == nb_sm_before_f2.norm_asm) |
|
110 | if (nb_norm_asm == nb_sm_before_f2.norm_asm) | |
111 | { |
|
111 | { | |
112 | nb_norm_asm = 0; |
|
112 | nb_norm_asm = 0; | |
113 | if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1) |
|
113 | if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1) | |
114 | || (lfrCurrentMode == LFR_MODE_SBM2) ) |
|
114 | || (lfrCurrentMode == LFR_MODE_SBM2) ) | |
115 | { |
|
115 | { | |
116 | msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_ASM_F2; |
|
116 | msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_ASM_F2; | |
117 | } |
|
117 | } | |
118 | } |
|
118 | } | |
119 |
|
119 | |||
120 | //************************* |
|
120 | //************************* | |
121 | // send the message to PRC2 |
|
121 | // send the message to PRC2 | |
122 | if (msgForPRC.event != EVENT_SETS_NONE_PENDING) |
|
122 | if (msgForPRC.event != EVENT_SETS_NONE_PENDING) | |
123 | { |
|
123 | { | |
124 | status = rtems_message_queue_send( queue_id_prc2, (char *) &msgForPRC, MSG_QUEUE_SIZE_PRC2); |
|
124 | status = rtems_message_queue_send( queue_id_prc2, (char *) &msgForPRC, MSG_QUEUE_SIZE_PRC2); | |
125 | } |
|
125 | } | |
126 |
|
126 | |||
127 | if (status != RTEMS_SUCCESSFUL) { |
|
127 | if (status != RTEMS_SUCCESSFUL) { | |
128 | PRINTF1("in AVF2 *** Error sending message to PRC2, code %d\n", status) |
|
128 | PRINTF1("in AVF2 *** Error sending message to PRC2, code %d\n", status) | |
129 | } |
|
129 | } | |
130 | } |
|
130 | } | |
131 | } |
|
131 | } | |
132 |
|
132 | |||
133 | rtems_task prc2_task( rtems_task_argument argument ) |
|
133 | rtems_task prc2_task( rtems_task_argument argument ) | |
134 | { |
|
134 | { | |
135 | char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer |
|
135 | char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer | |
136 | size_t size; // size of the incoming TC packet |
|
136 | size_t size; // size of the incoming TC packet | |
137 | asm_msg *incomingMsg; |
|
137 | asm_msg *incomingMsg; | |
138 | // |
|
138 | // | |
139 | rtems_status_code status; |
|
139 | rtems_status_code status; | |
140 | rtems_id queue_id_send; |
|
140 | rtems_id queue_id_send; | |
141 | rtems_id queue_id_q_p2; |
|
141 | rtems_id queue_id_q_p2; | |
142 | bp_packet __attribute__((aligned(4))) packet_norm_bp1; |
|
142 | bp_packet __attribute__((aligned(4))) packet_norm_bp1; | |
143 | bp_packet __attribute__((aligned(4))) packet_norm_bp2; |
|
143 | bp_packet __attribute__((aligned(4))) packet_norm_bp2; | |
144 | ring_node *current_ring_node_to_send_asm_f2; |
|
144 | ring_node *current_ring_node_to_send_asm_f2; | |
145 | float nbSMInASMNORM; |
|
145 | float nbSMInASMNORM; | |
146 |
|
146 | |||
147 | unsigned long long int localTime; |
|
147 | unsigned long long int localTime; | |
148 |
|
148 | |||
149 | size = 0; |
|
149 | size = 0; | |
150 | queue_id_send = RTEMS_ID_NONE; |
|
150 | queue_id_send = RTEMS_ID_NONE; | |
151 | queue_id_q_p2 = RTEMS_ID_NONE; |
|
151 | queue_id_q_p2 = RTEMS_ID_NONE; | |
152 | memset( &packet_norm_bp1, 0, sizeof(bp_packet) ); |
|
152 | memset( &packet_norm_bp1, 0, sizeof(bp_packet) ); | |
153 | memset( &packet_norm_bp2, 0, sizeof(bp_packet) ); |
|
153 | memset( &packet_norm_bp2, 0, sizeof(bp_packet) ); | |
154 |
|
154 | |||
155 | // init the ring of the averaged spectral matrices which will be transmitted to the DPU |
|
155 | // init the ring of the averaged spectral matrices which will be transmitted to the DPU | |
156 | init_ring( ring_to_send_asm_f2, NB_RING_NODES_ASM_F2, (volatile int*) buffer_asm_f2, TOTAL_SIZE_SM ); |
|
156 | init_ring( ring_to_send_asm_f2, NB_RING_NODES_ASM_F2, (volatile int*) buffer_asm_f2, TOTAL_SIZE_SM ); | |
157 | current_ring_node_to_send_asm_f2 = ring_to_send_asm_f2; |
|
157 | current_ring_node_to_send_asm_f2 = ring_to_send_asm_f2; | |
158 |
|
158 | |||
159 | //************* |
|
159 | //************* | |
160 | // NORM headers |
|
160 | // NORM headers | |
161 | BP_init_header( &packet_norm_bp1, |
|
161 | BP_init_header( &packet_norm_bp1, | |
162 | APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F2, |
|
162 | APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F2, | |
163 | PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F2, NB_BINS_COMPRESSED_SM_F2 ); |
|
163 | PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F2, NB_BINS_COMPRESSED_SM_F2 ); | |
164 | BP_init_header( &packet_norm_bp2, |
|
164 | BP_init_header( &packet_norm_bp2, | |
165 | APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F2, |
|
165 | APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F2, | |
166 | PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F2, NB_BINS_COMPRESSED_SM_F2 ); |
|
166 | PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F2, NB_BINS_COMPRESSED_SM_F2 ); | |
167 |
|
167 | |||
168 | status = get_message_queue_id_send( &queue_id_send ); |
|
168 | status = get_message_queue_id_send( &queue_id_send ); | |
169 | if (status != RTEMS_SUCCESSFUL) |
|
169 | if (status != RTEMS_SUCCESSFUL) | |
170 | { |
|
170 | { | |
171 | PRINTF1("in PRC2 *** ERR get_message_queue_id_send %d\n", status) |
|
171 | PRINTF1("in PRC2 *** ERR get_message_queue_id_send %d\n", status) | |
172 | } |
|
172 | } | |
173 | status = get_message_queue_id_prc2( &queue_id_q_p2); |
|
173 | status = get_message_queue_id_prc2( &queue_id_q_p2); | |
174 | if (status != RTEMS_SUCCESSFUL) |
|
174 | if (status != RTEMS_SUCCESSFUL) | |
175 | { |
|
175 | { | |
176 | PRINTF1("in PRC2 *** ERR get_message_queue_id_prc2 %d\n", status) |
|
176 | PRINTF1("in PRC2 *** ERR get_message_queue_id_prc2 %d\n", status) | |
177 | } |
|
177 | } | |
178 |
|
178 | |||
179 | BOOT_PRINTF("in PRC2 ***\n") |
|
179 | BOOT_PRINTF("in PRC2 ***\n") | |
180 |
|
180 | |||
181 | while(1){ |
|
181 | while(1){ | |
182 | status = rtems_message_queue_receive( queue_id_q_p2, incomingData, &size, //************************************ |
|
182 | status = rtems_message_queue_receive( queue_id_q_p2, incomingData, &size, //************************************ | |
183 | RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF2 |
|
183 | RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF2 | |
184 |
|
184 | |||
185 | incomingMsg = (asm_msg*) incomingData; |
|
185 | incomingMsg = (asm_msg*) incomingData; | |
186 |
|
186 | |||
187 | ASM_patch( incomingMsg->norm->matrix, asm_f2_patched_norm ); |
|
187 | ASM_patch( incomingMsg->norm->matrix, asm_f2_patched_norm ); | |
188 |
|
188 | |||
189 | localTime = getTimeAsUnsignedLongLongInt( ); |
|
189 | localTime = getTimeAsUnsignedLongLongInt( ); | |
190 |
|
190 | |||
191 | nbSMInASMNORM = incomingMsg->numberOfSMInASMNORM; |
|
191 | nbSMInASMNORM = incomingMsg->numberOfSMInASMNORM; | |
192 |
|
192 | |||
193 | //***** |
|
193 | //***** | |
194 | //***** |
|
194 | //***** | |
195 | // NORM |
|
195 | // NORM | |
196 | //***** |
|
196 | //***** | |
197 | //***** |
|
197 | //***** | |
198 | // 1) compress the matrix for Basic Parameters calculation |
|
198 | // 1) compress the matrix for Basic Parameters calculation | |
199 | ASM_compress_reorganize_and_divide_mask( asm_f2_patched_norm, compressed_sm_norm_f2, |
|
199 | ASM_compress_reorganize_and_divide_mask( asm_f2_patched_norm, compressed_sm_norm_f2, | |
200 | nbSMInASMNORM, |
|
200 | nbSMInASMNORM, | |
201 | NB_BINS_COMPRESSED_SM_F2, NB_BINS_TO_AVERAGE_ASM_F2, |
|
201 | NB_BINS_COMPRESSED_SM_F2, NB_BINS_TO_AVERAGE_ASM_F2, | |
202 | ASM_F2_INDICE_START, CHANNELF2 ); |
|
202 | ASM_F2_INDICE_START, CHANNELF2 ); | |
203 | // BP1_F2 |
|
203 | // BP1_F2 | |
204 | if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F2) |
|
204 | if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F2) | |
205 | { |
|
205 | { | |
206 | // 1) compute the BP1 set |
|
206 | // 1) compute the BP1 set | |
207 | BP1_set( compressed_sm_norm_f2, k_coeff_intercalib_f2, NB_BINS_COMPRESSED_SM_F2, packet_norm_bp1.data ); |
|
207 | BP1_set( compressed_sm_norm_f2, k_coeff_intercalib_f2, NB_BINS_COMPRESSED_SM_F2, packet_norm_bp1.data ); | |
208 | // 2) send the BP1 set |
|
208 | // 2) send the BP1 set | |
209 | set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM ); |
|
209 | set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM ); | |
210 | set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM ); |
|
210 | set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM ); | |
211 | packet_norm_bp1.pa_bia_status_info = pa_bia_status_info; |
|
211 | packet_norm_bp1.pa_bia_status_info = pa_bia_status_info; | |
212 | packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters; |
|
212 | packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters; | |
213 | BP_send( (char *) &packet_norm_bp1, queue_id_send, |
|
213 | BP_send( (char *) &packet_norm_bp1, queue_id_send, | |
214 | PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F2 + PACKET_LENGTH_DELTA, |
|
214 | PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F2 + PACKET_LENGTH_DELTA, | |
215 | SID_NORM_BP1_F2 ); |
|
215 | SID_NORM_BP1_F2 ); | |
216 | } |
|
216 | } | |
217 | // BP2_F2 |
|
217 | // BP2_F2 | |
218 | if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F2) |
|
218 | if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F2) | |
219 | { |
|
219 | { | |
220 | // 1) compute the BP2 set |
|
220 | // 1) compute the BP2 set | |
221 | BP2_set( compressed_sm_norm_f2, NB_BINS_COMPRESSED_SM_F2, packet_norm_bp2.data ); |
|
221 | BP2_set( compressed_sm_norm_f2, NB_BINS_COMPRESSED_SM_F2, packet_norm_bp2.data ); | |
222 | // 2) send the BP2 set |
|
222 | // 2) send the BP2 set | |
223 | set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM ); |
|
223 | set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM ); | |
224 | set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM ); |
|
224 | set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM ); | |
225 | packet_norm_bp2.pa_bia_status_info = pa_bia_status_info; |
|
225 | packet_norm_bp2.pa_bia_status_info = pa_bia_status_info; | |
226 | packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters; |
|
226 | packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters; | |
227 | BP_send( (char *) &packet_norm_bp2, queue_id_send, |
|
227 | BP_send( (char *) &packet_norm_bp2, queue_id_send, | |
228 | PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F2 + PACKET_LENGTH_DELTA, |
|
228 | PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F2 + PACKET_LENGTH_DELTA, | |
229 | SID_NORM_BP2_F2 ); |
|
229 | SID_NORM_BP2_F2 ); | |
230 | } |
|
230 | } | |
231 |
|
231 | |||
232 | if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F2) |
|
232 | if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F2) | |
233 | { |
|
233 | { | |
234 | // 1) reorganize the ASM and divide |
|
234 | // 1) reorganize the ASM and divide | |
235 | ASM_reorganize_and_divide( asm_f2_patched_norm, |
|
235 | ASM_reorganize_and_divide( asm_f2_patched_norm, | |
236 | (float*) current_ring_node_to_send_asm_f2->buffer_address, |
|
236 | (float*) current_ring_node_to_send_asm_f2->buffer_address, | |
237 | nb_sm_before_f2.norm_bp1 ); |
|
237 | nb_sm_before_f2.norm_bp1 ); | |
238 | current_ring_node_to_send_asm_f2->coarseTime = incomingMsg->coarseTimeNORM; |
|
238 | current_ring_node_to_send_asm_f2->coarseTime = incomingMsg->coarseTimeNORM; | |
239 | current_ring_node_to_send_asm_f2->fineTime = incomingMsg->fineTimeNORM; |
|
239 | current_ring_node_to_send_asm_f2->fineTime = incomingMsg->fineTimeNORM; | |
240 | current_ring_node_to_send_asm_f2->sid = SID_NORM_ASM_F2; |
|
240 | current_ring_node_to_send_asm_f2->sid = SID_NORM_ASM_F2; | |
241 |
|
241 | |||
242 | // 3) send the spectral matrix packets |
|
242 | // 3) send the spectral matrix packets | |
243 | status = rtems_message_queue_send( queue_id_send, ¤t_ring_node_to_send_asm_f2, sizeof( ring_node* ) ); |
|
243 | status = rtems_message_queue_send( queue_id_send, ¤t_ring_node_to_send_asm_f2, sizeof( ring_node* ) ); | |
244 |
|
244 | |||
245 | // change asm ring node |
|
245 | // change asm ring node | |
246 | current_ring_node_to_send_asm_f2 = current_ring_node_to_send_asm_f2->next; |
|
246 | current_ring_node_to_send_asm_f2 = current_ring_node_to_send_asm_f2->next; | |
247 | } |
|
247 | } | |
248 |
|
248 | |||
249 | update_queue_max_count( queue_id_q_p2, &hk_lfr_q_p2_fifo_size_max ); |
|
249 | update_queue_max_count( queue_id_q_p2, &hk_lfr_q_p2_fifo_size_max ); | |
250 |
|
250 | |||
251 | } |
|
251 | } | |
252 | } |
|
252 | } | |
253 |
|
253 | |||
254 | //********** |
|
254 | //********** | |
255 | // FUNCTIONS |
|
255 | // FUNCTIONS | |
256 |
|
256 | |||
257 | void reset_nb_sm_f2( void ) |
|
257 | void reset_nb_sm_f2( void ) | |
258 | { |
|
258 | { | |
259 | nb_sm_before_f2.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0; |
|
259 | nb_sm_before_f2.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0; | |
260 | nb_sm_before_f2.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1; |
|
260 | nb_sm_before_f2.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1; | |
261 | nb_sm_before_f2.norm_asm = (parameter_dump_packet.sy_lfr_n_asm_p[0] * CONST_256) + parameter_dump_packet.sy_lfr_n_asm_p[1]; |
|
261 | nb_sm_before_f2.norm_asm = (parameter_dump_packet.sy_lfr_n_asm_p[0] * CONST_256) + parameter_dump_packet.sy_lfr_n_asm_p[1]; | |
262 | } |
|
262 | } | |
263 |
|
263 | |||
264 | void SM_average_f2( float *averaged_spec_mat_f2, |
|
264 | void SM_average_f2( float *averaged_spec_mat_f2, | |
265 | ring_node *ring_node, |
|
265 | ring_node *ring_node, | |
266 | unsigned int nbAverageNormF2, |
|
266 | unsigned int nbAverageNormF2, | |
267 | asm_msg *msgForMATR ) |
|
267 | asm_msg *msgForMATR ) | |
268 | { |
|
268 | { | |
269 | float sum; |
|
269 | float sum; | |
270 | unsigned int i; |
|
270 | unsigned int i; | |
271 | unsigned char keepMatrix; |
|
271 | unsigned char keepMatrix; | |
272 |
|
272 | |||
273 | // test acquisitionTime validity |
|
273 | // test acquisitionTime validity | |
274 | keepMatrix = acquisitionTimeIsValid( ring_node->coarseTime, ring_node->fineTime, CHANNELF2 ); |
|
274 | keepMatrix = acquisitionTimeIsValid( ring_node->coarseTime, ring_node->fineTime, CHANNELF2 ); | |
275 |
|
275 | |||
276 | for(i=0; i<TOTAL_SIZE_SM; i++) |
|
276 | for(i=0; i<TOTAL_SIZE_SM; i++) | |
277 | { |
|
277 | { | |
278 | sum = ( (int *) (ring_node->buffer_address) ) [ i ]; |
|
278 | sum = ( (int *) (ring_node->buffer_address) ) [ i ]; | |
279 | if ( (nbAverageNormF2 == 0) ) // average initialization |
|
279 | if ( (nbAverageNormF2 == 0) ) // average initialization | |
280 | { |
|
280 | { | |
281 |
if (keepMatrix == |
|
281 | if (keepMatrix == MATRIX_IS_NOT_POLLUTED) // keep the matrix and add it to the average | |
282 | { |
|
282 | { | |
283 | averaged_spec_mat_f2[ i ] = sum; |
|
283 | averaged_spec_mat_f2[ i ] = sum; | |
284 | } |
|
284 | } | |
285 | else // drop the matrix and initialize the average |
|
285 | else // drop the matrix and initialize the average | |
286 | { |
|
286 | { | |
287 | averaged_spec_mat_f2[ i ] = INIT_FLOAT; |
|
287 | averaged_spec_mat_f2[ i ] = INIT_FLOAT; | |
288 | } |
|
288 | } | |
289 | msgForMATR->coarseTimeNORM = ring_node->coarseTime; |
|
289 | msgForMATR->coarseTimeNORM = ring_node->coarseTime; | |
290 | msgForMATR->fineTimeNORM = ring_node->fineTime; |
|
290 | msgForMATR->fineTimeNORM = ring_node->fineTime; | |
291 | } |
|
291 | } | |
292 | else |
|
292 | else | |
293 | { |
|
293 | { | |
294 |
if (keepMatrix == |
|
294 | if (keepMatrix == MATRIX_IS_NOT_POLLUTED) // keep the matrix and add it to the average | |
295 | { |
|
295 | { | |
296 | averaged_spec_mat_f2[ i ] = ( averaged_spec_mat_f2[ i ] + sum ); |
|
296 | averaged_spec_mat_f2[ i ] = ( averaged_spec_mat_f2[ i ] + sum ); | |
297 | } |
|
297 | } | |
298 | else |
|
298 | else | |
299 | { |
|
299 | { | |
300 | // nothing to do, the matrix is not valid |
|
300 | // nothing to do, the matrix is not valid | |
301 | } |
|
301 | } | |
302 | } |
|
302 | } | |
303 | } |
|
303 | } | |
304 |
|
304 | |||
305 | if (keepMatrix == 1) |
|
305 | if (keepMatrix == 1) | |
306 | { |
|
306 | { | |
307 | if ( (nbAverageNormF2 == 0) ) |
|
307 | if ( (nbAverageNormF2 == 0) ) | |
308 | { |
|
308 | { | |
309 | msgForMATR->numberOfSMInASMNORM = 1; |
|
309 | msgForMATR->numberOfSMInASMNORM = 1; | |
310 | } |
|
310 | } | |
311 | else |
|
311 | else | |
312 | { |
|
312 | { | |
313 | msgForMATR->numberOfSMInASMNORM++; |
|
313 | msgForMATR->numberOfSMInASMNORM++; | |
314 | } |
|
314 | } | |
315 | } |
|
315 | } | |
316 | else |
|
316 | else | |
317 | { |
|
317 | { | |
318 | if ( (nbAverageNormF2 == 0) ) |
|
318 | if ( (nbAverageNormF2 == 0) ) | |
319 | { |
|
319 | { | |
320 | msgForMATR->numberOfSMInASMNORM = 0; |
|
320 | msgForMATR->numberOfSMInASMNORM = 0; | |
321 | } |
|
321 | } | |
322 | else |
|
322 | else | |
323 | { |
|
323 | { | |
324 | // nothing to do |
|
324 | // nothing to do | |
325 | } |
|
325 | } | |
326 | } |
|
326 | } | |
327 | } |
|
327 | } | |
328 |
|
328 | |||
329 | void init_k_coefficients_prc2( void ) |
|
329 | void init_k_coefficients_prc2( void ) | |
330 | { |
|
330 | { | |
331 | init_k_coefficients( k_coeff_intercalib_f2, NB_BINS_COMPRESSED_SM_F2); |
|
331 | init_k_coefficients( k_coeff_intercalib_f2, NB_BINS_COMPRESSED_SM_F2); | |
332 | } |
|
332 | } |
@@ -1,817 +1,830 | |||||
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 = 0; |
|
14 | unsigned int nb_sm_f0 = 0; | |
15 | unsigned int nb_sm_f0_aux_f1= 0; |
|
15 | unsigned int nb_sm_f0_aux_f1= 0; | |
16 | unsigned int nb_sm_f1 = 0; |
|
16 | unsigned int nb_sm_f1 = 0; | |
17 | unsigned int nb_sm_f0_aux_f2= 0; |
|
17 | unsigned int nb_sm_f0_aux_f2= 0; | |
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 ] = {0}; |
|
28 | ring_node sm_ring_f0[ NB_RING_NODES_SM_F0 ] = {0}; | |
29 | ring_node sm_ring_f1[ NB_RING_NODES_SM_F1 ] = {0}; |
|
29 | ring_node sm_ring_f1[ NB_RING_NODES_SM_F1 ] = {0}; | |
30 | ring_node sm_ring_f2[ NB_RING_NODES_SM_F2 ] = {0}; |
|
30 | ring_node sm_ring_f2[ NB_RING_NODES_SM_F2 ] = {0}; | |
31 | ring_node *current_ring_node_sm_f0 = NULL; |
|
31 | ring_node *current_ring_node_sm_f0 = NULL; | |
32 | ring_node *current_ring_node_sm_f1 = NULL; |
|
32 | ring_node *current_ring_node_sm_f1 = NULL; | |
33 | ring_node *current_ring_node_sm_f2 = NULL; |
|
33 | ring_node *current_ring_node_sm_f2 = NULL; | |
34 | ring_node *ring_node_for_averaging_sm_f0= NULL; |
|
34 | ring_node *ring_node_for_averaging_sm_f0= NULL; | |
35 | ring_node *ring_node_for_averaging_sm_f1= NULL; |
|
35 | ring_node *ring_node_for_averaging_sm_f1= NULL; | |
36 | ring_node *ring_node_for_averaging_sm_f2= NULL; |
|
36 | ring_node *ring_node_for_averaging_sm_f2= NULL; | |
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 CHANNELF0: |
|
45 | case CHANNELF0: | |
46 | node = ring_node_for_averaging_sm_f0; |
|
46 | node = ring_node_for_averaging_sm_f0; | |
47 | break; |
|
47 | break; | |
48 | case CHANNELF1: |
|
48 | case CHANNELF1: | |
49 | node = ring_node_for_averaging_sm_f1; |
|
49 | node = ring_node_for_averaging_sm_f1; | |
50 | break; |
|
50 | break; | |
51 | case CHANNELF2: |
|
51 | case CHANNELF2: | |
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 & BITS_STATUS_F0); // [0011] get the status_ready_matrix_f0_x bits |
|
70 | status = (unsigned char) (statusReg & BITS_STATUS_F0); // [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 BIT_READY_0_1: |
|
76 | case BIT_READY_0_1: | |
77 | // UNEXPECTED VALUE |
|
77 | // UNEXPECTED VALUE | |
78 | spectral_matrix_regs->status = BIT_READY_0_1; // [0011] |
|
78 | spectral_matrix_regs->status = BIT_READY_0_1; // [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 BIT_READY_0: |
|
81 | case BIT_READY_0: | |
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_F1) |
|
89 | if (nb_sm_f0 == NB_SM_BEFORE_AVF0_F1) | |
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 = BIT_READY_0; // [0000 0001] |
|
98 | spectral_matrix_regs->status = BIT_READY_0; // [0000 0001] | |
99 | break; |
|
99 | break; | |
100 | case BIT_READY_1: |
|
100 | case BIT_READY_1: | |
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_F1) |
|
108 | if (nb_sm_f0 == NB_SM_BEFORE_AVF0_F1) | |
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 = BIT_READY_1; // [0000 0010] |
|
117 | spectral_matrix_regs->status = BIT_READY_1; // [0000 0010] | |
118 | break; |
|
118 | break; | |
119 | default: |
|
119 | default: | |
120 | break; |
|
120 | break; | |
121 | } |
|
121 | } | |
122 | } |
|
122 | } | |
123 |
|
123 | |||
124 | void spectral_matrices_isr_f1( int statusReg ) |
|
124 | void spectral_matrices_isr_f1( int statusReg ) | |
125 | { |
|
125 | { | |
126 | rtems_status_code status_code; |
|
126 | rtems_status_code status_code; | |
127 | unsigned char status; |
|
127 | unsigned char status; | |
128 | ring_node *full_ring_node; |
|
128 | ring_node *full_ring_node; | |
129 |
|
129 | |||
130 | status = (unsigned char) ((statusReg & BITS_STATUS_F1) >> SHIFT_2_BITS); // [1100] get the status_ready_matrix_f1_x bits |
|
130 | status = (unsigned char) ((statusReg & BITS_STATUS_F1) >> SHIFT_2_BITS); // [1100] get the status_ready_matrix_f1_x bits | |
131 |
|
131 | |||
132 | switch(status) |
|
132 | switch(status) | |
133 | { |
|
133 | { | |
134 | case 0: |
|
134 | case 0: | |
135 | break; |
|
135 | break; | |
136 | case BIT_READY_0_1: |
|
136 | case BIT_READY_0_1: | |
137 | // UNEXPECTED VALUE |
|
137 | // UNEXPECTED VALUE | |
138 | spectral_matrix_regs->status = BITS_STATUS_F1; // [1100] |
|
138 | spectral_matrix_regs->status = BITS_STATUS_F1; // [1100] | |
139 | status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 ); |
|
139 | status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 ); | |
140 | break; |
|
140 | break; | |
141 | case BIT_READY_0: |
|
141 | case BIT_READY_0: | |
142 | full_ring_node = current_ring_node_sm_f1->previous; |
|
142 | full_ring_node = current_ring_node_sm_f1->previous; | |
143 | full_ring_node->coarseTime = spectral_matrix_regs->f1_0_coarse_time; |
|
143 | full_ring_node->coarseTime = spectral_matrix_regs->f1_0_coarse_time; | |
144 | full_ring_node->fineTime = spectral_matrix_regs->f1_0_fine_time; |
|
144 | full_ring_node->fineTime = spectral_matrix_regs->f1_0_fine_time; | |
145 | current_ring_node_sm_f1 = current_ring_node_sm_f1->next; |
|
145 | current_ring_node_sm_f1 = current_ring_node_sm_f1->next; | |
146 | spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->buffer_address; |
|
146 | spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->buffer_address; | |
147 | // if there are enough ring nodes ready, wake up an AVFx task |
|
147 | // if there are enough ring nodes ready, wake up an AVFx task | |
148 | nb_sm_f1 = nb_sm_f1 + 1; |
|
148 | nb_sm_f1 = nb_sm_f1 + 1; | |
149 | if (nb_sm_f1 == NB_SM_BEFORE_AVF0_F1) |
|
149 | if (nb_sm_f1 == NB_SM_BEFORE_AVF0_F1) | |
150 | { |
|
150 | { | |
151 | ring_node_for_averaging_sm_f1 = full_ring_node; |
|
151 | ring_node_for_averaging_sm_f1 = full_ring_node; | |
152 | if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) |
|
152 | if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) | |
153 | { |
|
153 | { | |
154 | status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 ); |
|
154 | status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 ); | |
155 | } |
|
155 | } | |
156 | nb_sm_f1 = 0; |
|
156 | nb_sm_f1 = 0; | |
157 | } |
|
157 | } | |
158 | spectral_matrix_regs->status = BIT_STATUS_F1_0; // [0000 0100] |
|
158 | spectral_matrix_regs->status = BIT_STATUS_F1_0; // [0000 0100] | |
159 | break; |
|
159 | break; | |
160 | case BIT_READY_1: |
|
160 | case BIT_READY_1: | |
161 | full_ring_node = current_ring_node_sm_f1->previous; |
|
161 | full_ring_node = current_ring_node_sm_f1->previous; | |
162 | full_ring_node->coarseTime = spectral_matrix_regs->f1_1_coarse_time; |
|
162 | full_ring_node->coarseTime = spectral_matrix_regs->f1_1_coarse_time; | |
163 | full_ring_node->fineTime = spectral_matrix_regs->f1_1_fine_time; |
|
163 | full_ring_node->fineTime = spectral_matrix_regs->f1_1_fine_time; | |
164 | current_ring_node_sm_f1 = current_ring_node_sm_f1->next; |
|
164 | current_ring_node_sm_f1 = current_ring_node_sm_f1->next; | |
165 | spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address; |
|
165 | spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address; | |
166 | // if there are enough ring nodes ready, wake up an AVFx task |
|
166 | // if there are enough ring nodes ready, wake up an AVFx task | |
167 | nb_sm_f1 = nb_sm_f1 + 1; |
|
167 | nb_sm_f1 = nb_sm_f1 + 1; | |
168 | if (nb_sm_f1 == NB_SM_BEFORE_AVF0_F1) |
|
168 | if (nb_sm_f1 == NB_SM_BEFORE_AVF0_F1) | |
169 | { |
|
169 | { | |
170 | ring_node_for_averaging_sm_f1 = full_ring_node; |
|
170 | ring_node_for_averaging_sm_f1 = full_ring_node; | |
171 | if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) |
|
171 | if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) | |
172 | { |
|
172 | { | |
173 | status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 ); |
|
173 | status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 ); | |
174 | } |
|
174 | } | |
175 | nb_sm_f1 = 0; |
|
175 | nb_sm_f1 = 0; | |
176 | } |
|
176 | } | |
177 | spectral_matrix_regs->status = BIT_STATUS_F1_1; // [1000 0000] |
|
177 | spectral_matrix_regs->status = BIT_STATUS_F1_1; // [1000 0000] | |
178 | break; |
|
178 | break; | |
179 | default: |
|
179 | default: | |
180 | break; |
|
180 | break; | |
181 | } |
|
181 | } | |
182 | } |
|
182 | } | |
183 |
|
183 | |||
184 | void spectral_matrices_isr_f2( int statusReg ) |
|
184 | void spectral_matrices_isr_f2( int statusReg ) | |
185 | { |
|
185 | { | |
186 | unsigned char status; |
|
186 | unsigned char status; | |
187 | rtems_status_code status_code; |
|
187 | rtems_status_code status_code; | |
188 |
|
188 | |||
189 | status = (unsigned char) ((statusReg & BITS_STATUS_F2) >> SHIFT_4_BITS); // [0011 0000] get the status_ready_matrix_f2_x bits |
|
189 | status = (unsigned char) ((statusReg & BITS_STATUS_F2) >> SHIFT_4_BITS); // [0011 0000] get the status_ready_matrix_f2_x bits | |
190 |
|
190 | |||
191 | switch(status) |
|
191 | switch(status) | |
192 | { |
|
192 | { | |
193 | case 0: |
|
193 | case 0: | |
194 | break; |
|
194 | break; | |
195 | case BIT_READY_0_1: |
|
195 | case BIT_READY_0_1: | |
196 | // UNEXPECTED VALUE |
|
196 | // UNEXPECTED VALUE | |
197 | spectral_matrix_regs->status = BITS_STATUS_F2; // [0011 0000] |
|
197 | spectral_matrix_regs->status = BITS_STATUS_F2; // [0011 0000] | |
198 | status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 ); |
|
198 | status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 ); | |
199 | break; |
|
199 | break; | |
200 | case BIT_READY_0: |
|
200 | case BIT_READY_0: | |
201 | ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous; |
|
201 | ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous; | |
202 | current_ring_node_sm_f2 = current_ring_node_sm_f2->next; |
|
202 | current_ring_node_sm_f2 = current_ring_node_sm_f2->next; | |
203 | ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_0_coarse_time; |
|
203 | ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_0_coarse_time; | |
204 | ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_0_fine_time; |
|
204 | ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_0_fine_time; | |
205 | spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->buffer_address; |
|
205 | spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->buffer_address; | |
206 | spectral_matrix_regs->status = BIT_STATUS_F2_0; // [0001 0000] |
|
206 | spectral_matrix_regs->status = BIT_STATUS_F2_0; // [0001 0000] | |
207 | if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) |
|
207 | if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) | |
208 | { |
|
208 | { | |
209 | status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 ); |
|
209 | status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 ); | |
210 | } |
|
210 | } | |
211 | break; |
|
211 | break; | |
212 | case BIT_READY_1: |
|
212 | case BIT_READY_1: | |
213 | ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous; |
|
213 | ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous; | |
214 | current_ring_node_sm_f2 = current_ring_node_sm_f2->next; |
|
214 | current_ring_node_sm_f2 = current_ring_node_sm_f2->next; | |
215 | ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_1_coarse_time; |
|
215 | ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_1_coarse_time; | |
216 | ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_1_fine_time; |
|
216 | ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_1_fine_time; | |
217 | spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address; |
|
217 | spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address; | |
218 | spectral_matrix_regs->status = BIT_STATUS_F2_1; // [0010 0000] |
|
218 | spectral_matrix_regs->status = BIT_STATUS_F2_1; // [0010 0000] | |
219 | if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) |
|
219 | if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) | |
220 | { |
|
220 | { | |
221 | status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 ); |
|
221 | status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 ); | |
222 | } |
|
222 | } | |
223 | break; |
|
223 | break; | |
224 | default: |
|
224 | default: | |
225 | break; |
|
225 | break; | |
226 | } |
|
226 | } | |
227 | } |
|
227 | } | |
228 |
|
228 | |||
229 | void spectral_matrix_isr_error_handler( int statusReg ) |
|
229 | void spectral_matrix_isr_error_handler( int statusReg ) | |
230 | { |
|
230 | { | |
231 | // STATUS REGISTER |
|
231 | // STATUS REGISTER | |
232 | // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0) |
|
232 | // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0) | |
233 | // 10 9 8 |
|
233 | // 10 9 8 | |
234 | // buffer_full ** [bad_component_err] ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0 |
|
234 | // buffer_full ** [bad_component_err] ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0 | |
235 | // 7 6 5 4 3 2 1 0 |
|
235 | // 7 6 5 4 3 2 1 0 | |
236 | // [bad_component_err] not defined in the last version of the VHDL code |
|
236 | // [bad_component_err] not defined in the last version of the VHDL code | |
237 |
|
237 | |||
238 | rtems_status_code status_code; |
|
238 | rtems_status_code status_code; | |
239 |
|
239 | |||
240 | //*************************************************** |
|
240 | //*************************************************** | |
241 | // the ASM status register is copied in the HK packet |
|
241 | // the ASM status register is copied in the HK packet | |
242 | housekeeping_packet.hk_lfr_vhdl_aa_sm = (unsigned char) ((statusReg & BITS_HK_AA_SM) >> SHIFT_7_BITS); // [0111 1000 0000] |
|
242 | housekeeping_packet.hk_lfr_vhdl_aa_sm = (unsigned char) ((statusReg & BITS_HK_AA_SM) >> SHIFT_7_BITS); // [0111 1000 0000] | |
243 |
|
243 | |||
244 | if (statusReg & BITS_SM_ERR) // [0111 1100 0000] |
|
244 | if (statusReg & BITS_SM_ERR) // [0111 1100 0000] | |
245 | { |
|
245 | { | |
246 | status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_8 ); |
|
246 | status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_8 ); | |
247 | } |
|
247 | } | |
248 |
|
248 | |||
249 | spectral_matrix_regs->status = spectral_matrix_regs->status & BITS_SM_ERR; |
|
249 | spectral_matrix_regs->status = spectral_matrix_regs->status & BITS_SM_ERR; | |
250 |
|
250 | |||
251 | } |
|
251 | } | |
252 |
|
252 | |||
253 | rtems_isr spectral_matrices_isr( rtems_vector_number vector ) |
|
253 | rtems_isr spectral_matrices_isr( rtems_vector_number vector ) | |
254 | { |
|
254 | { | |
255 | // STATUS REGISTER |
|
255 | // STATUS REGISTER | |
256 | // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0) |
|
256 | // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0) | |
257 | // 10 9 8 |
|
257 | // 10 9 8 | |
258 | // buffer_full ** bad_component_err ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0 |
|
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 |
|
259 | // 7 6 5 4 3 2 1 0 | |
260 |
|
260 | |||
261 | int statusReg; |
|
261 | int statusReg; | |
262 |
|
262 | |||
263 | static restartState state = WAIT_FOR_F2; |
|
263 | static restartState state = WAIT_FOR_F2; | |
264 |
|
264 | |||
265 | statusReg = spectral_matrix_regs->status; |
|
265 | statusReg = spectral_matrix_regs->status; | |
266 |
|
266 | |||
267 | if (thisIsAnASMRestart == 0) |
|
267 | if (thisIsAnASMRestart == 0) | |
268 | { // this is not a restart sequence, process incoming matrices normally |
|
268 | { // this is not a restart sequence, process incoming matrices normally | |
269 | spectral_matrices_isr_f0( statusReg ); |
|
269 | spectral_matrices_isr_f0( statusReg ); | |
270 |
|
270 | |||
271 | spectral_matrices_isr_f1( statusReg ); |
|
271 | spectral_matrices_isr_f1( statusReg ); | |
272 |
|
272 | |||
273 | spectral_matrices_isr_f2( statusReg ); |
|
273 | spectral_matrices_isr_f2( statusReg ); | |
274 | } |
|
274 | } | |
275 | else |
|
275 | else | |
276 | { // a restart sequence has to be launched |
|
276 | { // a restart sequence has to be launched | |
277 | switch (state) { |
|
277 | switch (state) { | |
278 | case WAIT_FOR_F2: |
|
278 | case WAIT_FOR_F2: | |
279 | if ((statusReg & BITS_STATUS_F2) != INIT_CHAR) // [0011 0000] check the status_ready_matrix_f2_x bits |
|
279 | if ((statusReg & BITS_STATUS_F2) != INIT_CHAR) // [0011 0000] check the status_ready_matrix_f2_x bits | |
280 | { |
|
280 | { | |
281 | state = WAIT_FOR_F1; |
|
281 | state = WAIT_FOR_F1; | |
282 | } |
|
282 | } | |
283 | break; |
|
283 | break; | |
284 | case WAIT_FOR_F1: |
|
284 | case WAIT_FOR_F1: | |
285 | if ((statusReg & BITS_STATUS_F1) != INIT_CHAR) // [0000 1100] check the status_ready_matrix_f1_x bits |
|
285 | if ((statusReg & BITS_STATUS_F1) != INIT_CHAR) // [0000 1100] check the status_ready_matrix_f1_x bits | |
286 | { |
|
286 | { | |
287 | state = WAIT_FOR_F0; |
|
287 | state = WAIT_FOR_F0; | |
288 | } |
|
288 | } | |
289 | break; |
|
289 | break; | |
290 | case WAIT_FOR_F0: |
|
290 | case WAIT_FOR_F0: | |
291 | if ((statusReg & BITS_STATUS_F0) != INIT_CHAR) // [0000 0011] check the status_ready_matrix_f0_x bits |
|
291 | if ((statusReg & BITS_STATUS_F0) != INIT_CHAR) // [0000 0011] check the status_ready_matrix_f0_x bits | |
292 | { |
|
292 | { | |
293 | state = WAIT_FOR_F2; |
|
293 | state = WAIT_FOR_F2; | |
294 | thisIsAnASMRestart = 0; |
|
294 | thisIsAnASMRestart = 0; | |
295 | } |
|
295 | } | |
296 | break; |
|
296 | break; | |
297 | default: |
|
297 | default: | |
298 | break; |
|
298 | break; | |
299 | } |
|
299 | } | |
300 | reset_sm_status(); |
|
300 | reset_sm_status(); | |
301 | } |
|
301 | } | |
302 |
|
302 | |||
303 | spectral_matrix_isr_error_handler( statusReg ); |
|
303 | spectral_matrix_isr_error_handler( statusReg ); | |
304 |
|
304 | |||
305 | } |
|
305 | } | |
306 |
|
306 | |||
307 | //****************** |
|
307 | //****************** | |
308 | // Spectral Matrices |
|
308 | // Spectral Matrices | |
309 |
|
309 | |||
310 | void reset_nb_sm( void ) |
|
310 | void reset_nb_sm( void ) | |
311 | { |
|
311 | { | |
312 | nb_sm_f0 = 0; |
|
312 | nb_sm_f0 = 0; | |
313 | nb_sm_f0_aux_f1 = 0; |
|
313 | nb_sm_f0_aux_f1 = 0; | |
314 | nb_sm_f0_aux_f2 = 0; |
|
314 | nb_sm_f0_aux_f2 = 0; | |
315 |
|
315 | |||
316 | nb_sm_f1 = 0; |
|
316 | nb_sm_f1 = 0; | |
317 | } |
|
317 | } | |
318 |
|
318 | |||
319 | void SM_init_rings( void ) |
|
319 | void SM_init_rings( void ) | |
320 | { |
|
320 | { | |
321 | init_ring( sm_ring_f0, NB_RING_NODES_SM_F0, sm_f0, TOTAL_SIZE_SM ); |
|
321 | init_ring( sm_ring_f0, NB_RING_NODES_SM_F0, sm_f0, TOTAL_SIZE_SM ); | |
322 | init_ring( sm_ring_f1, NB_RING_NODES_SM_F1, sm_f1, TOTAL_SIZE_SM ); |
|
322 | init_ring( sm_ring_f1, NB_RING_NODES_SM_F1, sm_f1, TOTAL_SIZE_SM ); | |
323 | init_ring( sm_ring_f2, NB_RING_NODES_SM_F2, sm_f2, TOTAL_SIZE_SM ); |
|
323 | init_ring( sm_ring_f2, NB_RING_NODES_SM_F2, sm_f2, TOTAL_SIZE_SM ); | |
324 |
|
324 | |||
325 | DEBUG_PRINTF1("sm_ring_f0 @%x\n", (unsigned int) sm_ring_f0) |
|
325 | DEBUG_PRINTF1("sm_ring_f0 @%x\n", (unsigned int) sm_ring_f0) | |
326 | DEBUG_PRINTF1("sm_ring_f1 @%x\n", (unsigned int) sm_ring_f1) |
|
326 | DEBUG_PRINTF1("sm_ring_f1 @%x\n", (unsigned int) sm_ring_f1) | |
327 | DEBUG_PRINTF1("sm_ring_f2 @%x\n", (unsigned int) sm_ring_f2) |
|
327 | DEBUG_PRINTF1("sm_ring_f2 @%x\n", (unsigned int) sm_ring_f2) | |
328 | DEBUG_PRINTF1("sm_f0 @%x\n", (unsigned int) sm_f0) |
|
328 | DEBUG_PRINTF1("sm_f0 @%x\n", (unsigned int) sm_f0) | |
329 | DEBUG_PRINTF1("sm_f1 @%x\n", (unsigned int) sm_f1) |
|
329 | DEBUG_PRINTF1("sm_f1 @%x\n", (unsigned int) sm_f1) | |
330 | DEBUG_PRINTF1("sm_f2 @%x\n", (unsigned int) sm_f2) |
|
330 | DEBUG_PRINTF1("sm_f2 @%x\n", (unsigned int) sm_f2) | |
331 | } |
|
331 | } | |
332 |
|
332 | |||
333 | void ASM_generic_init_ring( ring_node_asm *ring, unsigned char nbNodes ) |
|
333 | void ASM_generic_init_ring( ring_node_asm *ring, unsigned char nbNodes ) | |
334 | { |
|
334 | { | |
335 | unsigned char i; |
|
335 | unsigned char i; | |
336 |
|
336 | |||
337 | ring[ nbNodes - 1 ].next |
|
337 | ring[ nbNodes - 1 ].next | |
338 | = (ring_node_asm*) &ring[ 0 ]; |
|
338 | = (ring_node_asm*) &ring[ 0 ]; | |
339 |
|
339 | |||
340 | for(i=0; i<nbNodes-1; i++) |
|
340 | for(i=0; i<nbNodes-1; i++) | |
341 | { |
|
341 | { | |
342 | ring[ i ].next = (ring_node_asm*) &ring[ i + 1 ]; |
|
342 | ring[ i ].next = (ring_node_asm*) &ring[ i + 1 ]; | |
343 | } |
|
343 | } | |
344 | } |
|
344 | } | |
345 |
|
345 | |||
346 | void SM_reset_current_ring_nodes( void ) |
|
346 | void SM_reset_current_ring_nodes( void ) | |
347 | { |
|
347 | { | |
348 | current_ring_node_sm_f0 = sm_ring_f0[0].next; |
|
348 | current_ring_node_sm_f0 = sm_ring_f0[0].next; | |
349 | current_ring_node_sm_f1 = sm_ring_f1[0].next; |
|
349 | current_ring_node_sm_f1 = sm_ring_f1[0].next; | |
350 | current_ring_node_sm_f2 = sm_ring_f2[0].next; |
|
350 | current_ring_node_sm_f2 = sm_ring_f2[0].next; | |
351 |
|
351 | |||
352 | ring_node_for_averaging_sm_f0 = NULL; |
|
352 | ring_node_for_averaging_sm_f0 = NULL; | |
353 | ring_node_for_averaging_sm_f1 = NULL; |
|
353 | ring_node_for_averaging_sm_f1 = NULL; | |
354 | ring_node_for_averaging_sm_f2 = NULL; |
|
354 | ring_node_for_averaging_sm_f2 = NULL; | |
355 | } |
|
355 | } | |
356 |
|
356 | |||
357 | //***************** |
|
357 | //***************** | |
358 | // Basic Parameters |
|
358 | // Basic Parameters | |
359 |
|
359 | |||
360 | void BP_init_header( bp_packet *packet, |
|
360 | void BP_init_header( bp_packet *packet, | |
361 | unsigned int apid, unsigned char sid, |
|
361 | unsigned int apid, unsigned char sid, | |
362 | unsigned int packetLength, unsigned char blkNr ) |
|
362 | unsigned int packetLength, unsigned char blkNr ) | |
363 | { |
|
363 | { | |
364 | packet->targetLogicalAddress = CCSDS_DESTINATION_ID; |
|
364 | packet->targetLogicalAddress = CCSDS_DESTINATION_ID; | |
365 | packet->protocolIdentifier = CCSDS_PROTOCOLE_ID; |
|
365 | packet->protocolIdentifier = CCSDS_PROTOCOLE_ID; | |
366 | packet->reserved = INIT_CHAR; |
|
366 | packet->reserved = INIT_CHAR; | |
367 | packet->userApplication = CCSDS_USER_APP; |
|
367 | packet->userApplication = CCSDS_USER_APP; | |
368 | packet->packetID[0] = (unsigned char) (apid >> SHIFT_1_BYTE); |
|
368 | packet->packetID[0] = (unsigned char) (apid >> SHIFT_1_BYTE); | |
369 | packet->packetID[1] = (unsigned char) (apid); |
|
369 | packet->packetID[1] = (unsigned char) (apid); | |
370 | packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE; |
|
370 | packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE; | |
371 | packet->packetSequenceControl[1] = INIT_CHAR; |
|
371 | packet->packetSequenceControl[1] = INIT_CHAR; | |
372 | packet->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE); |
|
372 | packet->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE); | |
373 | packet->packetLength[1] = (unsigned char) (packetLength); |
|
373 | packet->packetLength[1] = (unsigned char) (packetLength); | |
374 | // DATA FIELD HEADER |
|
374 | // DATA FIELD HEADER | |
375 | packet->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2; |
|
375 | packet->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2; | |
376 | packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type |
|
376 | packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type | |
377 | packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype |
|
377 | packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype | |
378 | packet->destinationID = TM_DESTINATION_ID_GROUND; |
|
378 | packet->destinationID = TM_DESTINATION_ID_GROUND; | |
379 | packet->time[BYTE_0] = INIT_CHAR; |
|
379 | packet->time[BYTE_0] = INIT_CHAR; | |
380 | packet->time[BYTE_1] = INIT_CHAR; |
|
380 | packet->time[BYTE_1] = INIT_CHAR; | |
381 | packet->time[BYTE_2] = INIT_CHAR; |
|
381 | packet->time[BYTE_2] = INIT_CHAR; | |
382 | packet->time[BYTE_3] = INIT_CHAR; |
|
382 | packet->time[BYTE_3] = INIT_CHAR; | |
383 | packet->time[BYTE_4] = INIT_CHAR; |
|
383 | packet->time[BYTE_4] = INIT_CHAR; | |
384 | packet->time[BYTE_5] = INIT_CHAR; |
|
384 | packet->time[BYTE_5] = INIT_CHAR; | |
385 | // AUXILIARY DATA HEADER |
|
385 | // AUXILIARY DATA HEADER | |
386 | packet->sid = sid; |
|
386 | packet->sid = sid; | |
387 | packet->pa_bia_status_info = INIT_CHAR; |
|
387 | packet->pa_bia_status_info = INIT_CHAR; | |
388 | packet->sy_lfr_common_parameters_spare = INIT_CHAR; |
|
388 | packet->sy_lfr_common_parameters_spare = INIT_CHAR; | |
389 | packet->sy_lfr_common_parameters = INIT_CHAR; |
|
389 | packet->sy_lfr_common_parameters = INIT_CHAR; | |
390 | packet->acquisitionTime[BYTE_0] = INIT_CHAR; |
|
390 | packet->acquisitionTime[BYTE_0] = INIT_CHAR; | |
391 | packet->acquisitionTime[BYTE_1] = INIT_CHAR; |
|
391 | packet->acquisitionTime[BYTE_1] = INIT_CHAR; | |
392 | packet->acquisitionTime[BYTE_2] = INIT_CHAR; |
|
392 | packet->acquisitionTime[BYTE_2] = INIT_CHAR; | |
393 | packet->acquisitionTime[BYTE_3] = INIT_CHAR; |
|
393 | packet->acquisitionTime[BYTE_3] = INIT_CHAR; | |
394 | packet->acquisitionTime[BYTE_4] = INIT_CHAR; |
|
394 | packet->acquisitionTime[BYTE_4] = INIT_CHAR; | |
395 | packet->acquisitionTime[BYTE_5] = INIT_CHAR; |
|
395 | packet->acquisitionTime[BYTE_5] = INIT_CHAR; | |
396 | packet->pa_lfr_bp_blk_nr[0] = INIT_CHAR; // BLK_NR MSB |
|
396 | packet->pa_lfr_bp_blk_nr[0] = INIT_CHAR; // BLK_NR MSB | |
397 | packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB |
|
397 | packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB | |
398 | } |
|
398 | } | |
399 |
|
399 | |||
400 | void BP_init_header_with_spare( bp_packet_with_spare *packet, |
|
400 | void BP_init_header_with_spare( bp_packet_with_spare *packet, | |
401 | unsigned int apid, unsigned char sid, |
|
401 | unsigned int apid, unsigned char sid, | |
402 | unsigned int packetLength , unsigned char blkNr) |
|
402 | unsigned int packetLength , unsigned char blkNr) | |
403 | { |
|
403 | { | |
404 | packet->targetLogicalAddress = CCSDS_DESTINATION_ID; |
|
404 | packet->targetLogicalAddress = CCSDS_DESTINATION_ID; | |
405 | packet->protocolIdentifier = CCSDS_PROTOCOLE_ID; |
|
405 | packet->protocolIdentifier = CCSDS_PROTOCOLE_ID; | |
406 | packet->reserved = INIT_CHAR; |
|
406 | packet->reserved = INIT_CHAR; | |
407 | packet->userApplication = CCSDS_USER_APP; |
|
407 | packet->userApplication = CCSDS_USER_APP; | |
408 | packet->packetID[0] = (unsigned char) (apid >> SHIFT_1_BYTE); |
|
408 | packet->packetID[0] = (unsigned char) (apid >> SHIFT_1_BYTE); | |
409 | packet->packetID[1] = (unsigned char) (apid); |
|
409 | packet->packetID[1] = (unsigned char) (apid); | |
410 | packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE; |
|
410 | packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE; | |
411 | packet->packetSequenceControl[1] = INIT_CHAR; |
|
411 | packet->packetSequenceControl[1] = INIT_CHAR; | |
412 | packet->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE); |
|
412 | packet->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE); | |
413 | packet->packetLength[1] = (unsigned char) (packetLength); |
|
413 | packet->packetLength[1] = (unsigned char) (packetLength); | |
414 | // DATA FIELD HEADER |
|
414 | // DATA FIELD HEADER | |
415 | packet->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2; |
|
415 | packet->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2; | |
416 | packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type |
|
416 | packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type | |
417 | packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype |
|
417 | packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype | |
418 | packet->destinationID = TM_DESTINATION_ID_GROUND; |
|
418 | packet->destinationID = TM_DESTINATION_ID_GROUND; | |
419 | // AUXILIARY DATA HEADER |
|
419 | // AUXILIARY DATA HEADER | |
420 | packet->sid = sid; |
|
420 | packet->sid = sid; | |
421 | packet->pa_bia_status_info = INIT_CHAR; |
|
421 | packet->pa_bia_status_info = INIT_CHAR; | |
422 | packet->sy_lfr_common_parameters_spare = INIT_CHAR; |
|
422 | packet->sy_lfr_common_parameters_spare = INIT_CHAR; | |
423 | packet->sy_lfr_common_parameters = INIT_CHAR; |
|
423 | packet->sy_lfr_common_parameters = INIT_CHAR; | |
424 | packet->time[BYTE_0] = INIT_CHAR; |
|
424 | packet->time[BYTE_0] = INIT_CHAR; | |
425 | packet->time[BYTE_1] = INIT_CHAR; |
|
425 | packet->time[BYTE_1] = INIT_CHAR; | |
426 | packet->time[BYTE_2] = INIT_CHAR; |
|
426 | packet->time[BYTE_2] = INIT_CHAR; | |
427 | packet->time[BYTE_3] = INIT_CHAR; |
|
427 | packet->time[BYTE_3] = INIT_CHAR; | |
428 | packet->time[BYTE_4] = INIT_CHAR; |
|
428 | packet->time[BYTE_4] = INIT_CHAR; | |
429 | packet->time[BYTE_5] = INIT_CHAR; |
|
429 | packet->time[BYTE_5] = INIT_CHAR; | |
430 | packet->source_data_spare = INIT_CHAR; |
|
430 | packet->source_data_spare = INIT_CHAR; | |
431 | packet->pa_lfr_bp_blk_nr[0] = INIT_CHAR; // BLK_NR MSB |
|
431 | packet->pa_lfr_bp_blk_nr[0] = INIT_CHAR; // BLK_NR MSB | |
432 | packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB |
|
432 | packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB | |
433 | } |
|
433 | } | |
434 |
|
434 | |||
435 | void BP_send(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid ) |
|
435 | void BP_send(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid ) | |
436 | { |
|
436 | { | |
437 | rtems_status_code status; |
|
437 | rtems_status_code status; | |
438 |
|
438 | |||
439 | // SEND PACKET |
|
439 | // SEND PACKET | |
440 | status = rtems_message_queue_send( queue_id, data, nbBytesToSend); |
|
440 | status = rtems_message_queue_send( queue_id, data, nbBytesToSend); | |
441 | if (status != RTEMS_SUCCESSFUL) |
|
441 | if (status != RTEMS_SUCCESSFUL) | |
442 | { |
|
442 | { | |
443 | PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status) |
|
443 | PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status) | |
444 | } |
|
444 | } | |
445 | } |
|
445 | } | |
446 |
|
446 | |||
447 | void BP_send_s1_s2(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid ) |
|
447 | void BP_send_s1_s2(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid ) | |
448 | { |
|
448 | { | |
449 | /** This function is used to send the BP paquets when needed. |
|
449 | /** This function is used to send the BP paquets when needed. | |
450 | * |
|
450 | * | |
451 | * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE |
|
451 | * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE | |
452 | * |
|
452 | * | |
453 | * @return void |
|
453 | * @return void | |
454 | * |
|
454 | * | |
455 | * SBM1 and SBM2 paquets are sent depending on the type of the LFR mode transition. |
|
455 | * SBM1 and SBM2 paquets are sent depending on the type of the LFR mode transition. | |
456 | * BURST paquets are sent everytime. |
|
456 | * BURST paquets are sent everytime. | |
457 | * |
|
457 | * | |
458 | */ |
|
458 | */ | |
459 |
|
459 | |||
460 | rtems_status_code status; |
|
460 | rtems_status_code status; | |
461 |
|
461 | |||
462 | // SEND PACKET |
|
462 | // SEND PACKET | |
463 | // before lastValidTransitionDate, the data are drops even if they are ready |
|
463 | // before lastValidTransitionDate, the data are drops even if they are ready | |
464 | // this guarantees that no SBM packets will be received before the requested enter mode time |
|
464 | // this guarantees that no SBM packets will be received before the requested enter mode time | |
465 | if ( time_management_regs->coarse_time >= lastValidEnterModeTime) |
|
465 | if ( time_management_regs->coarse_time >= lastValidEnterModeTime) | |
466 | { |
|
466 | { | |
467 | status = rtems_message_queue_send( queue_id, data, nbBytesToSend); |
|
467 | status = rtems_message_queue_send( queue_id, data, nbBytesToSend); | |
468 | if (status != RTEMS_SUCCESSFUL) |
|
468 | if (status != RTEMS_SUCCESSFUL) | |
469 | { |
|
469 | { | |
470 | PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status) |
|
470 | PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status) | |
471 | } |
|
471 | } | |
472 | } |
|
472 | } | |
473 | } |
|
473 | } | |
474 |
|
474 | |||
475 | //****************** |
|
475 | //****************** | |
476 | // general functions |
|
476 | // general functions | |
477 |
|
477 | |||
478 | void reset_sm_status( void ) |
|
478 | void reset_sm_status( void ) | |
479 | { |
|
479 | { | |
480 | // error |
|
480 | // error | |
481 | // 10 --------------- 9 ---------------- 8 ---------------- 7 --------- |
|
481 | // 10 --------------- 9 ---------------- 8 ---------------- 7 --------- | |
482 | // input_fif0_write_2 input_fifo_write_1 input_fifo_write_0 buffer_full |
|
482 | // input_fif0_write_2 input_fifo_write_1 input_fifo_write_0 buffer_full | |
483 | // ---------- 5 -- 4 -- 3 -- 2 -- 1 -- 0 -- |
|
483 | // ---------- 5 -- 4 -- 3 -- 2 -- 1 -- 0 -- | |
484 | // ready bits f2_1 f2_0 f1_1 f1_1 f0_1 f0_0 |
|
484 | // ready bits f2_1 f2_0 f1_1 f1_1 f0_1 f0_0 | |
485 |
|
485 | |||
486 | spectral_matrix_regs->status = BITS_STATUS_REG; // [0111 1111 1111] |
|
486 | spectral_matrix_regs->status = BITS_STATUS_REG; // [0111 1111 1111] | |
487 | } |
|
487 | } | |
488 |
|
488 | |||
489 | void reset_spectral_matrix_regs( void ) |
|
489 | void reset_spectral_matrix_regs( void ) | |
490 | { |
|
490 | { | |
491 | /** This function resets the spectral matrices module registers. |
|
491 | /** This function resets the spectral matrices module registers. | |
492 | * |
|
492 | * | |
493 | * The registers affected by this function are located at the following offset addresses: |
|
493 | * The registers affected by this function are located at the following offset addresses: | |
494 | * |
|
494 | * | |
495 | * - 0x00 config |
|
495 | * - 0x00 config | |
496 | * - 0x04 status |
|
496 | * - 0x04 status | |
497 | * - 0x08 matrixF0_Address0 |
|
497 | * - 0x08 matrixF0_Address0 | |
498 | * - 0x10 matrixFO_Address1 |
|
498 | * - 0x10 matrixFO_Address1 | |
499 | * - 0x14 matrixF1_Address |
|
499 | * - 0x14 matrixF1_Address | |
500 | * - 0x18 matrixF2_Address |
|
500 | * - 0x18 matrixF2_Address | |
501 | * |
|
501 | * | |
502 | */ |
|
502 | */ | |
503 |
|
503 | |||
504 | set_sm_irq_onError( 0 ); |
|
504 | set_sm_irq_onError( 0 ); | |
505 |
|
505 | |||
506 | set_sm_irq_onNewMatrix( 0 ); |
|
506 | set_sm_irq_onNewMatrix( 0 ); | |
507 |
|
507 | |||
508 | reset_sm_status(); |
|
508 | reset_sm_status(); | |
509 |
|
509 | |||
510 | // F1 |
|
510 | // F1 | |
511 | spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->previous->buffer_address; |
|
511 | spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->previous->buffer_address; | |
512 | spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address; |
|
512 | spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address; | |
513 | // F2 |
|
513 | // F2 | |
514 | spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->previous->buffer_address; |
|
514 | spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->previous->buffer_address; | |
515 | spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address; |
|
515 | spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address; | |
516 | // F3 |
|
516 | // F3 | |
517 | spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->previous->buffer_address; |
|
517 | spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->previous->buffer_address; | |
518 | spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address; |
|
518 | spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address; | |
519 |
|
519 | |||
520 | spectral_matrix_regs->matrix_length = DEFAULT_MATRIX_LENGTH; // 25 * 128 / 16 = 200 = 0xc8 |
|
520 | spectral_matrix_regs->matrix_length = DEFAULT_MATRIX_LENGTH; // 25 * 128 / 16 = 200 = 0xc8 | |
521 | } |
|
521 | } | |
522 |
|
522 | |||
523 | void set_time( unsigned char *time, unsigned char * timeInBuffer ) |
|
523 | void set_time( unsigned char *time, unsigned char * timeInBuffer ) | |
524 | { |
|
524 | { | |
525 | time[BYTE_0] = timeInBuffer[BYTE_0]; |
|
525 | time[BYTE_0] = timeInBuffer[BYTE_0]; | |
526 | time[BYTE_1] = timeInBuffer[BYTE_1]; |
|
526 | time[BYTE_1] = timeInBuffer[BYTE_1]; | |
527 | time[BYTE_2] = timeInBuffer[BYTE_2]; |
|
527 | time[BYTE_2] = timeInBuffer[BYTE_2]; | |
528 | time[BYTE_3] = timeInBuffer[BYTE_3]; |
|
528 | time[BYTE_3] = timeInBuffer[BYTE_3]; | |
529 | time[BYTE_4] = timeInBuffer[BYTE_6]; |
|
529 | time[BYTE_4] = timeInBuffer[BYTE_6]; | |
530 | time[BYTE_5] = timeInBuffer[BYTE_7]; |
|
530 | time[BYTE_5] = timeInBuffer[BYTE_7]; | |
531 | } |
|
531 | } | |
532 |
|
532 | |||
533 | unsigned long long int get_acquisition_time( unsigned char *timePtr ) |
|
533 | unsigned long long int get_acquisition_time( unsigned char *timePtr ) | |
534 | { |
|
534 | { | |
535 | unsigned long long int acquisitionTimeAslong; |
|
535 | unsigned long long int acquisitionTimeAslong; | |
536 | acquisitionTimeAslong = INIT_CHAR; |
|
536 | acquisitionTimeAslong = INIT_CHAR; | |
537 | acquisitionTimeAslong = |
|
537 | acquisitionTimeAslong = | |
538 | ( (unsigned long long int) (timePtr[BYTE_0] & SYNC_BIT_MASK) << SHIFT_5_BYTES ) // [0111 1111] mask the synchronization bit |
|
538 | ( (unsigned long long int) (timePtr[BYTE_0] & SYNC_BIT_MASK) << SHIFT_5_BYTES ) // [0111 1111] mask the synchronization bit | |
539 | + ( (unsigned long long int) timePtr[BYTE_1] << SHIFT_4_BYTES ) |
|
539 | + ( (unsigned long long int) timePtr[BYTE_1] << SHIFT_4_BYTES ) | |
540 | + ( (unsigned long long int) timePtr[BYTE_2] << SHIFT_3_BYTES ) |
|
540 | + ( (unsigned long long int) timePtr[BYTE_2] << SHIFT_3_BYTES ) | |
541 | + ( (unsigned long long int) timePtr[BYTE_3] << SHIFT_2_BYTES ) |
|
541 | + ( (unsigned long long int) timePtr[BYTE_3] << SHIFT_2_BYTES ) | |
542 | + ( (unsigned long long int) timePtr[BYTE_6] << SHIFT_1_BYTE ) |
|
542 | + ( (unsigned long long int) timePtr[BYTE_6] << SHIFT_1_BYTE ) | |
543 | + ( (unsigned long long int) timePtr[BYTE_7] ); |
|
543 | + ( (unsigned long long int) timePtr[BYTE_7] ); | |
544 | return acquisitionTimeAslong; |
|
544 | return acquisitionTimeAslong; | |
545 | } |
|
545 | } | |
546 |
|
546 | |||
547 | unsigned char getSID( rtems_event_set event ) |
|
547 | unsigned char getSID( rtems_event_set event ) | |
548 | { |
|
548 | { | |
549 | unsigned char sid; |
|
549 | unsigned char sid; | |
550 |
|
550 | |||
551 | rtems_event_set eventSetBURST; |
|
551 | rtems_event_set eventSetBURST; | |
552 | rtems_event_set eventSetSBM; |
|
552 | rtems_event_set eventSetSBM; | |
553 |
|
553 | |||
554 | sid = 0; |
|
554 | sid = 0; | |
555 |
|
555 | |||
556 | //****** |
|
556 | //****** | |
557 | // BURST |
|
557 | // BURST | |
558 | eventSetBURST = RTEMS_EVENT_BURST_BP1_F0 |
|
558 | eventSetBURST = RTEMS_EVENT_BURST_BP1_F0 | |
559 | | RTEMS_EVENT_BURST_BP1_F1 |
|
559 | | RTEMS_EVENT_BURST_BP1_F1 | |
560 | | RTEMS_EVENT_BURST_BP2_F0 |
|
560 | | RTEMS_EVENT_BURST_BP2_F0 | |
561 | | RTEMS_EVENT_BURST_BP2_F1; |
|
561 | | RTEMS_EVENT_BURST_BP2_F1; | |
562 |
|
562 | |||
563 | //**** |
|
563 | //**** | |
564 | // SBM |
|
564 | // SBM | |
565 | eventSetSBM = RTEMS_EVENT_SBM_BP1_F0 |
|
565 | eventSetSBM = RTEMS_EVENT_SBM_BP1_F0 | |
566 | | RTEMS_EVENT_SBM_BP1_F1 |
|
566 | | RTEMS_EVENT_SBM_BP1_F1 | |
567 | | RTEMS_EVENT_SBM_BP2_F0 |
|
567 | | RTEMS_EVENT_SBM_BP2_F0 | |
568 | | RTEMS_EVENT_SBM_BP2_F1; |
|
568 | | RTEMS_EVENT_SBM_BP2_F1; | |
569 |
|
569 | |||
570 | if (event & eventSetBURST) |
|
570 | if (event & eventSetBURST) | |
571 | { |
|
571 | { | |
572 | sid = SID_BURST_BP1_F0; |
|
572 | sid = SID_BURST_BP1_F0; | |
573 | } |
|
573 | } | |
574 | else if (event & eventSetSBM) |
|
574 | else if (event & eventSetSBM) | |
575 | { |
|
575 | { | |
576 | sid = SID_SBM1_BP1_F0; |
|
576 | sid = SID_SBM1_BP1_F0; | |
577 | } |
|
577 | } | |
578 | else |
|
578 | else | |
579 | { |
|
579 | { | |
580 | sid = 0; |
|
580 | sid = 0; | |
581 | } |
|
581 | } | |
582 |
|
582 | |||
583 | return sid; |
|
583 | return sid; | |
584 | } |
|
584 | } | |
585 |
|
585 | |||
586 | void extractReImVectors( float *inputASM, float *outputASM, unsigned int asmComponent ) |
|
586 | void extractReImVectors( float *inputASM, float *outputASM, unsigned int asmComponent ) | |
587 | { |
|
587 | { | |
588 | unsigned int i; |
|
588 | unsigned int i; | |
589 | float re; |
|
589 | float re; | |
590 | float im; |
|
590 | float im; | |
591 |
|
591 | |||
592 | for (i=0; i<NB_BINS_PER_SM; i++){ |
|
592 | for (i=0; i<NB_BINS_PER_SM; i++){ | |
593 | re = inputASM[ (asmComponent*NB_BINS_PER_SM) + (i * SM_BYTES_PER_VAL) ]; |
|
593 | re = inputASM[ (asmComponent*NB_BINS_PER_SM) + (i * SM_BYTES_PER_VAL) ]; | |
594 | im = inputASM[ (asmComponent*NB_BINS_PER_SM) + (i * SM_BYTES_PER_VAL) + 1]; |
|
594 | im = inputASM[ (asmComponent*NB_BINS_PER_SM) + (i * SM_BYTES_PER_VAL) + 1]; | |
595 | outputASM[ ( asmComponent *NB_BINS_PER_SM) + i] = re; |
|
595 | outputASM[ ( asmComponent *NB_BINS_PER_SM) + i] = re; | |
596 | outputASM[ ((asmComponent+1)*NB_BINS_PER_SM) + i] = im; |
|
596 | outputASM[ ((asmComponent+1)*NB_BINS_PER_SM) + i] = im; | |
597 | } |
|
597 | } | |
598 | } |
|
598 | } | |
599 |
|
599 | |||
600 | void copyReVectors( float *inputASM, float *outputASM, unsigned int asmComponent ) |
|
600 | void copyReVectors( float *inputASM, float *outputASM, unsigned int asmComponent ) | |
601 | { |
|
601 | { | |
602 | unsigned int i; |
|
602 | unsigned int i; | |
603 | float re; |
|
603 | float re; | |
604 |
|
604 | |||
605 | for (i=0; i<NB_BINS_PER_SM; i++){ |
|
605 | for (i=0; i<NB_BINS_PER_SM; i++){ | |
606 | re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i]; |
|
606 | re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i]; | |
607 | outputASM[ (asmComponent*NB_BINS_PER_SM) + i] = re; |
|
607 | outputASM[ (asmComponent*NB_BINS_PER_SM) + i] = re; | |
608 | } |
|
608 | } | |
609 | } |
|
609 | } | |
610 |
|
610 | |||
611 | void ASM_patch( float *inputASM, float *outputASM ) |
|
611 | void ASM_patch( float *inputASM, float *outputASM ) | |
612 | { |
|
612 | { | |
613 | extractReImVectors( inputASM, outputASM, ASM_COMP_B1B2); // b1b2 |
|
613 | extractReImVectors( inputASM, outputASM, ASM_COMP_B1B2); // b1b2 | |
614 | extractReImVectors( inputASM, outputASM, ASM_COMP_B1B3 ); // b1b3 |
|
614 | extractReImVectors( inputASM, outputASM, ASM_COMP_B1B3 ); // b1b3 | |
615 | extractReImVectors( inputASM, outputASM, ASM_COMP_B1E1 ); // b1e1 |
|
615 | extractReImVectors( inputASM, outputASM, ASM_COMP_B1E1 ); // b1e1 | |
616 | extractReImVectors( inputASM, outputASM, ASM_COMP_B1E2 ); // b1e2 |
|
616 | extractReImVectors( inputASM, outputASM, ASM_COMP_B1E2 ); // b1e2 | |
617 | extractReImVectors( inputASM, outputASM, ASM_COMP_B2B3 ); // b2b3 |
|
617 | extractReImVectors( inputASM, outputASM, ASM_COMP_B2B3 ); // b2b3 | |
618 | extractReImVectors( inputASM, outputASM, ASM_COMP_B2E1 ); // b2e1 |
|
618 | extractReImVectors( inputASM, outputASM, ASM_COMP_B2E1 ); // b2e1 | |
619 | extractReImVectors( inputASM, outputASM, ASM_COMP_B2E2 ); // b2e2 |
|
619 | extractReImVectors( inputASM, outputASM, ASM_COMP_B2E2 ); // b2e2 | |
620 | extractReImVectors( inputASM, outputASM, ASM_COMP_B3E1 ); // b3e1 |
|
620 | extractReImVectors( inputASM, outputASM, ASM_COMP_B3E1 ); // b3e1 | |
621 | extractReImVectors( inputASM, outputASM, ASM_COMP_B3E2 ); // b3e2 |
|
621 | extractReImVectors( inputASM, outputASM, ASM_COMP_B3E2 ); // b3e2 | |
622 | extractReImVectors( inputASM, outputASM, ASM_COMP_E1E2 ); // e1e2 |
|
622 | extractReImVectors( inputASM, outputASM, ASM_COMP_E1E2 ); // e1e2 | |
623 |
|
623 | |||
624 | copyReVectors(inputASM, outputASM, ASM_COMP_B1B1 ); // b1b1 |
|
624 | copyReVectors(inputASM, outputASM, ASM_COMP_B1B1 ); // b1b1 | |
625 | copyReVectors(inputASM, outputASM, ASM_COMP_B2B2 ); // b2b2 |
|
625 | copyReVectors(inputASM, outputASM, ASM_COMP_B2B2 ); // b2b2 | |
626 | copyReVectors(inputASM, outputASM, ASM_COMP_B3B3); // b3b3 |
|
626 | copyReVectors(inputASM, outputASM, ASM_COMP_B3B3); // b3b3 | |
627 | copyReVectors(inputASM, outputASM, ASM_COMP_E1E1); // e1e1 |
|
627 | copyReVectors(inputASM, outputASM, ASM_COMP_E1E1); // e1e1 | |
628 | copyReVectors(inputASM, outputASM, ASM_COMP_E2E2); // e2e2 |
|
628 | copyReVectors(inputASM, outputASM, ASM_COMP_E2E2); // e2e2 | |
629 | } |
|
629 | } | |
630 |
|
630 | |||
631 | void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat , float divider, |
|
631 | void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat , float divider, | |
632 | unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage, |
|
632 | unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage, | |
633 | unsigned char ASMIndexStart, |
|
633 | unsigned char ASMIndexStart, | |
634 | unsigned char channel ) |
|
634 | unsigned char channel ) | |
635 | { |
|
635 | { | |
636 | //************* |
|
636 | //************* | |
637 | // input format |
|
637 | // input format | |
638 | // component0[0 .. 127] component1[0 .. 127] .. component24[0 .. 127] |
|
638 | // component0[0 .. 127] component1[0 .. 127] .. component24[0 .. 127] | |
639 | //************** |
|
639 | //************** | |
640 | // output format |
|
640 | // output format | |
641 | // matr0[0 .. 24] matr1[0 .. 24] .. matr127[0 .. 24] |
|
641 | // matr0[0 .. 24] matr1[0 .. 24] .. matr127[0 .. 24] | |
642 | //************ |
|
642 | //************ | |
643 | // compression |
|
643 | // compression | |
644 | // matr0[0 .. 24] matr1[0 .. 24] .. matr11[0 .. 24] => f0 NORM |
|
644 | // matr0[0 .. 24] matr1[0 .. 24] .. matr11[0 .. 24] => f0 NORM | |
645 | // matr0[0 .. 24] matr1[0 .. 24] .. matr22[0 .. 24] => f0 BURST, SBM |
|
645 | // matr0[0 .. 24] matr1[0 .. 24] .. matr22[0 .. 24] => f0 BURST, SBM | |
646 |
|
646 | |||
647 | int frequencyBin; |
|
647 | int frequencyBin; | |
648 | int asmComponent; |
|
648 | int asmComponent; | |
649 | int offsetASM; |
|
649 | int offsetASM; | |
650 | int offsetCompressed; |
|
650 | int offsetCompressed; | |
651 | int offsetFBin; |
|
651 | int offsetFBin; | |
652 | int fBinMask; |
|
652 | int fBinMask; | |
653 | int k; |
|
653 | int k; | |
654 |
|
654 | |||
655 | // BUILD DATA |
|
655 | // BUILD DATA | |
656 | for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++) |
|
656 | for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++) | |
657 | { |
|
657 | { | |
658 | for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ ) |
|
658 | for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ ) | |
659 | { |
|
659 | { | |
660 | offsetCompressed = // NO TIME OFFSET |
|
660 | offsetCompressed = // NO TIME OFFSET | |
661 | (frequencyBin * NB_VALUES_PER_SM) |
|
661 | (frequencyBin * NB_VALUES_PER_SM) | |
662 | + asmComponent; |
|
662 | + asmComponent; | |
663 | offsetASM = // NO TIME OFFSET |
|
663 | offsetASM = // NO TIME OFFSET | |
664 | (asmComponent * NB_BINS_PER_SM) |
|
664 | (asmComponent * NB_BINS_PER_SM) | |
665 | + ASMIndexStart |
|
665 | + ASMIndexStart | |
666 | + (frequencyBin * nbBinsToAverage); |
|
666 | + (frequencyBin * nbBinsToAverage); | |
667 | offsetFBin = ASMIndexStart |
|
667 | offsetFBin = ASMIndexStart | |
668 | + (frequencyBin * nbBinsToAverage); |
|
668 | + (frequencyBin * nbBinsToAverage); | |
669 | compressed_spec_mat[ offsetCompressed ] = 0; |
|
669 | compressed_spec_mat[ offsetCompressed ] = 0; | |
670 | for ( k = 0; k < nbBinsToAverage; k++ ) |
|
670 | for ( k = 0; k < nbBinsToAverage; k++ ) | |
671 | { |
|
671 | { | |
672 | fBinMask = getFBinMask( offsetFBin + k, channel ); |
|
672 | fBinMask = getFBinMask( offsetFBin + k, channel ); | |
673 | compressed_spec_mat[offsetCompressed ] = compressed_spec_mat[ offsetCompressed ] |
|
673 | compressed_spec_mat[offsetCompressed ] = compressed_spec_mat[ offsetCompressed ] | |
674 | + (averaged_spec_mat[ offsetASM + k ] * fBinMask); |
|
674 | + (averaged_spec_mat[ offsetASM + k ] * fBinMask); | |
675 | } |
|
675 | } | |
676 | if (divider != 0) |
|
676 | if (divider != 0) | |
677 | { |
|
677 | { | |
678 | compressed_spec_mat[ offsetCompressed ] = compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage); |
|
678 | compressed_spec_mat[ offsetCompressed ] = compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage); | |
679 | } |
|
679 | } | |
680 | else |
|
680 | else | |
681 | { |
|
681 | { | |
682 | compressed_spec_mat[ offsetCompressed ] = INIT_FLOAT; |
|
682 | compressed_spec_mat[ offsetCompressed ] = INIT_FLOAT; | |
683 | } |
|
683 | } | |
684 | } |
|
684 | } | |
685 | } |
|
685 | } | |
686 |
|
686 | |||
687 | } |
|
687 | } | |
688 |
|
688 | |||
689 | int getFBinMask( int index, unsigned char channel ) |
|
689 | int getFBinMask( int index, unsigned char channel ) | |
690 | { |
|
690 | { | |
691 | unsigned int indexInChar; |
|
691 | unsigned int indexInChar; | |
692 | unsigned int indexInTheChar; |
|
692 | unsigned int indexInTheChar; | |
693 | int fbin; |
|
693 | int fbin; | |
694 | unsigned char *sy_lfr_fbins_fx_word1; |
|
694 | unsigned char *sy_lfr_fbins_fx_word1; | |
695 |
|
695 | |||
696 | sy_lfr_fbins_fx_word1 = parameter_dump_packet.sy_lfr_fbins_f0_word1; |
|
696 | sy_lfr_fbins_fx_word1 = parameter_dump_packet.sy_lfr_fbins_f0_word1; | |
697 |
|
697 | |||
698 | switch(channel) |
|
698 | switch(channel) | |
699 | { |
|
699 | { | |
700 | case CHANNELF0: |
|
700 | case CHANNELF0: | |
701 | sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f0; |
|
701 | sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f0; | |
702 | break; |
|
702 | break; | |
703 | case CHANNELF1: |
|
703 | case CHANNELF1: | |
704 | sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f1; |
|
704 | sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f1; | |
705 | break; |
|
705 | break; | |
706 | case CHANNELF2: |
|
706 | case CHANNELF2: | |
707 | sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f2; |
|
707 | sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f2; | |
708 | break; |
|
708 | break; | |
709 | default: |
|
709 | default: | |
710 | PRINTF("ERR *** in getFBinMask, wrong frequency channel") |
|
710 | PRINTF("ERR *** in getFBinMask, wrong frequency channel") | |
711 | } |
|
711 | } | |
712 |
|
712 | |||
713 | indexInChar = index >> SHIFT_3_BITS; |
|
713 | indexInChar = index >> SHIFT_3_BITS; | |
714 | indexInTheChar = index - (indexInChar * BITS_PER_BYTE); |
|
714 | indexInTheChar = index - (indexInChar * BITS_PER_BYTE); | |
715 |
|
715 | |||
716 | fbin = (int) ((sy_lfr_fbins_fx_word1[ BYTES_PER_MASK - 1 - indexInChar] >> indexInTheChar) & 1); |
|
716 | fbin = (int) ((sy_lfr_fbins_fx_word1[ BYTES_PER_MASK - 1 - indexInChar] >> indexInTheChar) & 1); | |
717 |
|
717 | |||
718 | return fbin; |
|
718 | return fbin; | |
719 | } |
|
719 | } | |
720 |
|
720 | |||
|
721 | unsigned char isPolluted( u_int64_t t0, u_int64_t t1, u_int64_t tbad0, u_int64_t tbad1 ) | |||
|
722 | { | |||
|
723 | unsigned char polluted; | |||
|
724 | ||||
|
725 | polluted = MATRIX_IS_NOT_POLLUTED; | |||
|
726 | ||||
|
727 | if ( ((tbad0 < t0) && (t0 < tbad1)) // t0 is inside the polluted range | |||
|
728 | || ((tbad0 < t1) && (t1 < tbad1)) // t1 is inside the polluted range | |||
|
729 | || ((t0 < tbad0) && (tbad1 < t1)) // the polluted range is inside the signal range | |||
|
730 | || ((tbad0 < t0) && (t1 < tbad1))) // the signal range is inside the polluted range | |||
|
731 | { | |||
|
732 | polluted = MATRIX_IS_POLLUTED; | |||
|
733 | } | |||
|
734 | ||||
|
735 | return polluted; | |||
|
736 | } | |||
|
737 | ||||
721 | unsigned char acquisitionTimeIsValid( unsigned int coarseTime, unsigned int fineTime, unsigned char channel) |
|
738 | unsigned char acquisitionTimeIsValid( unsigned int coarseTime, unsigned int fineTime, unsigned char channel) | |
722 | { |
|
739 | { | |
723 |
u_int64_t |
|
740 | u_int64_t t0; | |
724 |
u_int64_t |
|
741 | u_int64_t t1; | |
725 |
u_int64_t t |
|
742 | u_int64_t tc; | |
|
743 | u_int64_t tbad0; | |||
|
744 | u_int64_t tbad1; | |||
|
745 | ||||
|
746 | u_int64_t modulusInFineTime; | |||
726 | u_int64_t offsetInFineTime; |
|
747 | u_int64_t offsetInFineTime; | |
727 | u_int64_t shiftInFineTime; |
|
748 | u_int64_t shiftInFineTime; | |
728 |
u_int64_t t |
|
749 | u_int64_t tbadInFineTime; | |
729 | u_int64_t perturbationTStart; |
|
750 | ||
730 | u_int64_t perturbationTStop; |
|
751 | u_int64_t timecodeReference; | |
|
752 | ||||
731 | unsigned char pasFilteringIsEnabled; |
|
753 | unsigned char pasFilteringIsEnabled; | |
732 | unsigned char ret; |
|
754 | unsigned char ret; | |
733 |
|
755 | |||
734 | pasFilteringIsEnabled = (filterPar.spare_sy_lfr_pas_filter_enabled & 1); // [0000 0001] |
|
756 | pasFilteringIsEnabled = (filterPar.spare_sy_lfr_pas_filter_enabled & 1); // [0000 0001] | |
735 | ret = 1; |
|
757 | ret = MATRIX_IS_NOT_POLLUTED; | |
|
758 | ||||
|
759 | // compute the acquitionTime range | |||
|
760 | modulusInFineTime = ((u_int64_t) filterPar.sy_lfr_pas_filter_modulus) * CONST_65536; | |||
|
761 | offsetInFineTime = ((u_int64_t) filterPar.sy_lfr_pas_filter_offset) * CONST_65536; | |||
|
762 | shiftInFineTime = ((u_int64_t) filterPar.sy_lfr_pas_filter_shift) * CONST_65536; | |||
|
763 | tbadInFineTime = ((u_int64_t) filterPar.sy_lfr_pas_filter_tbad) * CONST_65536; | |||
736 |
|
764 | |||
737 | // compute acquisition time from caoarseTime and fineTime |
|
765 | // compute acquisition time from caoarseTime and fineTime | |
738 |
|
|
766 | t0 = ( ((u_int64_t)coarseTime) << SHIFT_2_BYTES ) + (u_int64_t) fineTime; | |
739 | + (u_int64_t) fineTime; |
|
|||
740 | switch(channel) |
|
767 | switch(channel) | |
741 | { |
|
768 | { | |
742 | case CHANNELF0: |
|
769 | case CHANNELF0: | |
743 |
|
|
770 | t1 = t0 + ACQUISITION_DURATION_F0; | |
|
771 | tc = t0 + HALF_ACQUISITION_DURATION_F0; | |||
744 | break; |
|
772 | break; | |
745 | case CHANNELF1: |
|
773 | case CHANNELF1: | |
746 |
|
|
774 | t1 = t0 + ACQUISITION_DURATION_F1; | |
|
775 | tc = t0 + HALF_ACQUISITION_DURATION_F1; | |||
747 | break; |
|
776 | break; | |
748 | case CHANNELF2: |
|
777 | case CHANNELF2: | |
749 |
|
|
778 | t1 = t0 + ACQUISITION_DURATION_F2; | |
|
779 | tc = t0 + HALF_ACQUISITION_DURATION_F2; | |||
750 | break; |
|
780 | break; | |
751 | } |
|
781 | } | |
752 |
|
782 | |||
753 | // compute the timecode reference |
|
783 | // INTERSECTION TEST #1 | |
754 | timecodeReference = (u_int64_t) ( (floor( ((double) coarseTime) / ((double) filterPar.sy_lfr_pas_filter_modulus) ) |
|
784 | timecodeReference = (tc - (tc % modulusInFineTime)) - modulusInFineTime ; | |
755 | * ((double) filterPar.sy_lfr_pas_filter_modulus)) * CONST_65536 ); |
|
785 | tbad0 = timecodeReference + offsetInFineTime + shiftInFineTime; | |
756 |
|
786 | tbad1 = timecodeReference + offsetInFineTime + shiftInFineTime + tbadInFineTime; | ||
757 | // compute the acquitionTime range |
|
787 | ret = isPolluted( t0, t1, tbad0, tbad1 ); | |
758 | offsetInFineTime = ((double) filterPar.sy_lfr_pas_filter_offset) * CONST_65536; |
|
|||
759 | shiftInFineTime = ((double) filterPar.sy_lfr_pas_filter_shift) * CONST_65536; |
|
|||
760 | tBadInFineTime = ((double) filterPar.sy_lfr_pas_filter_tbad) * CONST_65536; |
|
|||
761 |
|
||||
762 | perturbationTStart = |
|
|||
763 | timecodeReference |
|
|||
764 | + offsetInFineTime |
|
|||
765 | + shiftInFineTime; |
|
|||
766 |
|
788 | |||
767 | perturbationTStop = |
|
789 | // INTERSECTION TEST #2 | |
768 | timecodeReference |
|
790 | timecodeReference = (tc - (tc % modulusInFineTime)) ; | |
769 | + offsetInFineTime |
|
791 | tbad0 = timecodeReference + offsetInFineTime + shiftInFineTime; | |
770 | + shiftInFineTime |
|
792 | tbad1 = timecodeReference + offsetInFineTime + shiftInFineTime + tbadInFineTime; | |
771 | + tBadInFineTime; |
|
793 | if (ret == MATRIX_IS_NOT_POLLUTED) | |
772 |
|
||||
773 | if ( (acquisitionTStart >= perturbationTStart) |
|
|||
774 | && (acquisitionTStart <= perturbationTStop) |
|
|||
775 | && (pasFilteringIsEnabled == 1) ) |
|
|||
776 | { |
|
794 | { | |
777 | ret = 0; // the acquisition time is INSIDE the range, the matrix shall be ignored |
|
795 | ret = isPolluted( t0, t1, tbad0, tbad1 ); | |
778 | } |
|
|||
779 | else |
|
|||
780 | { |
|
|||
781 | ret = 1; // the acquisition time is OUTSIDE the range, the matrix can be used for the averaging |
|
|||
782 | } |
|
796 | } | |
783 |
|
797 | |||
784 | // the last sample of the data used to compute the matrix shall not be INSIDE the range, test it now, it depends on the channel |
|
798 | // INTERSECTION TEST #3 | |
785 | if (ret == 1) |
|
799 | timecodeReference = (tc - (tc % modulusInFineTime)) + modulusInFineTime ; | |
|
800 | tbad0 = timecodeReference + offsetInFineTime + shiftInFineTime; | |||
|
801 | tbad1 = timecodeReference + offsetInFineTime + shiftInFineTime + tbadInFineTime; | |||
|
802 | if (ret == MATRIX_IS_NOT_POLLUTED) | |||
786 | { |
|
803 | { | |
787 | if ( (acquisitionTStop >= perturbationTStart) |
|
804 | ret = isPolluted( t0, t1, tbad0, tbad1 ); | |
788 | && (acquisitionTStop <= perturbationTStop) |
|
805 | } | |
789 | && (pasFilteringIsEnabled == 1) ) |
|
806 | ||
790 | { |
|
807 | if (pasFilteringIsEnabled == 0) | |
791 | ret = 0; // the acquisition time is INSIDE the range, the matrix shall be ignored |
|
808 | { | |
792 | } |
|
809 | ret = MATRIX_IS_NOT_POLLUTED; | |
793 | else |
|
|||
794 | { |
|
|||
795 | ret = 1; // the acquisition time is OUTSIDE the range, the matrix can be used for the averaging |
|
|||
796 | } |
|
|||
797 | } |
|
810 | } | |
798 |
|
811 | |||
799 | return ret; |
|
812 | return ret; | |
800 | } |
|
813 | } | |
801 |
|
814 | |||
802 | void init_kcoeff_sbm_from_kcoeff_norm(float *input_kcoeff, float *output_kcoeff, unsigned char nb_bins_norm) |
|
815 | void init_kcoeff_sbm_from_kcoeff_norm(float *input_kcoeff, float *output_kcoeff, unsigned char nb_bins_norm) | |
803 | { |
|
816 | { | |
804 | unsigned char bin; |
|
817 | unsigned char bin; | |
805 | unsigned char kcoeff; |
|
818 | unsigned char kcoeff; | |
806 |
|
819 | |||
807 | for (bin=0; bin<nb_bins_norm; bin++) |
|
820 | for (bin=0; bin<nb_bins_norm; bin++) | |
808 | { |
|
821 | { | |
809 | for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++) |
|
822 | for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++) | |
810 | { |
|
823 | { | |
811 | output_kcoeff[ ( (bin * NB_K_COEFF_PER_BIN) + kcoeff ) * SBM_COEFF_PER_NORM_COEFF ] |
|
824 | output_kcoeff[ ( (bin * NB_K_COEFF_PER_BIN) + kcoeff ) * SBM_COEFF_PER_NORM_COEFF ] | |
812 | = input_kcoeff[ (bin*NB_K_COEFF_PER_BIN) + kcoeff ]; |
|
825 | = input_kcoeff[ (bin*NB_K_COEFF_PER_BIN) + kcoeff ]; | |
813 | output_kcoeff[ ( ( (bin * NB_K_COEFF_PER_BIN ) + kcoeff) * SBM_COEFF_PER_NORM_COEFF ) + 1 ] |
|
826 | output_kcoeff[ ( ( (bin * NB_K_COEFF_PER_BIN ) + kcoeff) * SBM_COEFF_PER_NORM_COEFF ) + 1 ] | |
814 | = input_kcoeff[ (bin*NB_K_COEFF_PER_BIN) + kcoeff ]; |
|
827 | = input_kcoeff[ (bin*NB_K_COEFF_PER_BIN) + kcoeff ]; | |
815 | } |
|
828 | } | |
816 | } |
|
829 | } | |
817 | } |
|
830 | } |
General Comments 0
You need to be logged in to leave comments.
Login now