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

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

BP sending filtered depending on the lastValidEnterModeTime
paul -
r243:e8fa8af1f64c R3a
parent child
Show More
Show file before | Show file after | Hide comments
@@ -1,2 +1,2
1 1 3081d1f9bb20b2b64a192585337a292a9804e0c5 LFR_basic-parameters
2 ff85ce82cd9845f180cb578272717bcb76b62cb5 header/lfr_common_headers
2 ce0c2f17257170a8529605f68687c18f23973087 header/lfr_common_headers
Show file before | Show file after | Hide comments
@@ -1,327 +1,330
1 1 #ifndef FSW_PROCESSING_H_INCLUDED
2 2 #define FSW_PROCESSING_H_INCLUDED
3 3
4 4 #include <rtems.h>
5 5 #include <grspw.h>
6 6 #include <math.h>
7 7 #include <stdlib.h> // abs() is in the stdlib
8 8 #include <stdio.h>
9 9 #include <math.h>
10 10 #include <grlib_regs.h>
11 11
12 12 #include "fsw_params.h"
13 13
14 14 typedef struct ring_node_asm
15 15 {
16 16 struct ring_node_asm *next;
17 17 float matrix[ TOTAL_SIZE_SM ];
18 18 unsigned int status;
19 19 } ring_node_asm;
20 20
21 21 typedef struct
22 22 {
23 23 unsigned char targetLogicalAddress;
24 24 unsigned char protocolIdentifier;
25 25 unsigned char reserved;
26 26 unsigned char userApplication;
27 27 unsigned char packetID[2];
28 28 unsigned char packetSequenceControl[2];
29 29 unsigned char packetLength[2];
30 30 // DATA FIELD HEADER
31 31 unsigned char spare1_pusVersion_spare2;
32 32 unsigned char serviceType;
33 33 unsigned char serviceSubType;
34 34 unsigned char destinationID;
35 35 unsigned char time[6];
36 36 // AUXILIARY HEADER
37 37 unsigned char sid;
38 38 unsigned char biaStatusInfo;
39 39 unsigned char sy_lfr_common_parameters_spare;
40 40 unsigned char sy_lfr_common_parameters;
41 41 unsigned char acquisitionTime[6];
42 42 unsigned char pa_lfr_bp_blk_nr[2];
43 43 // SOURCE DATA
44 44 unsigned char data[ 780 ]; // MAX size is 26 bins * 30 Bytes [TM_LFR_SCIENCE_BURST_BP2_F1]
45 45 } bp_packet;
46 46
47 47 typedef struct
48 48 {
49 49 unsigned char targetLogicalAddress;
50 50 unsigned char protocolIdentifier;
51 51 unsigned char reserved;
52 52 unsigned char userApplication;
53 53 unsigned char packetID[2];
54 54 unsigned char packetSequenceControl[2];
55 55 unsigned char packetLength[2];
56 56 // DATA FIELD HEADER
57 57 unsigned char spare1_pusVersion_spare2;
58 58 unsigned char serviceType;
59 59 unsigned char serviceSubType;
60 60 unsigned char destinationID;
61 61 unsigned char time[6];
62 62 // AUXILIARY HEADER
63 63 unsigned char sid;
64 64 unsigned char biaStatusInfo;
65 65 unsigned char sy_lfr_common_parameters_spare;
66 66 unsigned char sy_lfr_common_parameters;
67 67 unsigned char acquisitionTime[6];
68 68 unsigned char source_data_spare;
69 69 unsigned char pa_lfr_bp_blk_nr[2];
70 70 // SOURCE DATA
71 71 unsigned char data[ 143 ]; // 13 bins * 11 Bytes
72 72 } bp_packet_with_spare; // only for TM_LFR_SCIENCE_NORMAL_BP1_F0 and F1
73 73
74 74 typedef struct asm_msg
75 75 {
76 76 ring_node_asm *norm;
77 77 ring_node_asm *burst_sbm;
78 78 rtems_event_set event;
79 79 unsigned int coarseTimeNORM;
80 80 unsigned int fineTimeNORM;
81 81 unsigned int coarseTimeSBM;
82 82 unsigned int fineTimeSBM;
83 83 } asm_msg;
84 84
85 85 extern volatile int sm_f0[ ];
86 86 extern volatile int sm_f1[ ];
87 87 extern volatile int sm_f2[ ];
88 88
89 89 // parameters
90 90 extern struct param_local_str param_local;
91 91 extern Packet_TM_LFR_PARAMETER_DUMP_t parameter_dump_packet;
92 92
93 93 // registers
94 94 extern time_management_regs_t *time_management_regs;
95 95 extern volatile spectral_matrix_regs_t *spectral_matrix_regs;
96 96
97 97 extern rtems_name misc_name[5];
98 98 extern rtems_id Task_id[20]; /* array of task ids */
99 99
100 100 ring_node * getRingNodeForAveraging( unsigned char frequencyChannel);
101 101 // ISR
102 102 rtems_isr spectral_matrices_isr( rtems_vector_number vector );
103 103
104 104 //******************
105 105 // Spectral Matrices
106 106 void reset_nb_sm( void );
107 107 // SM
108 108 void SM_init_rings( void );
109 109 void SM_reset_current_ring_nodes( void );
110 110 // ASM
111 111 void ASM_generic_init_ring(ring_node_asm *ring, unsigned char nbNodes );
112 112
113 113 //*****************
114 114 // Basic Parameters
115 115
116 116 void BP_reset_current_ring_nodes( void );
117 117 void BP_init_header(bp_packet *packet,
118 unsigned int apid, unsigned char sid,
119 unsigned int packetLength , unsigned char blkNr);
118 unsigned int apid, unsigned char sid,
119 unsigned int packetLength , unsigned char blkNr);
120 120 void BP_init_header_with_spare(bp_packet_with_spare *packet,
121 unsigned int apid, unsigned char sid,
122 unsigned int packetLength, unsigned char blkNr );
121 unsigned int apid, unsigned char sid,
122 unsigned int packetLength, unsigned char blkNr );
123 123 void BP_send( char *data,
124 rtems_id queue_id ,
124 rtems_id queue_id,
125 125 unsigned int nbBytesToSend , unsigned int sid );
126 void BP_send_s1_s2(char *data,
127 rtems_id queue_id,
128 unsigned int nbBytesToSend, unsigned int sid );
126 129
127 130 //******************
128 131 // general functions
129 132 void reset_sm_status( void );
130 133 void reset_spectral_matrix_regs( void );
131 134 void set_time(unsigned char *time, unsigned char *timeInBuffer );
132 135 unsigned long long int get_acquisition_time( unsigned char *timePtr );
133 136 unsigned char getSID( rtems_event_set event );
134 137
135 138 extern rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id );
136 139 extern rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id );
137 140
138 141 //***************************************
139 142 // DEFINITIONS OF STATIC INLINE FUNCTIONS
140 143 static inline void SM_average(float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
141 144 ring_node *ring_node_tab[],
142 145 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
143 146 asm_msg *msgForMATR );
144 147
145 148 static inline void SM_average_debug(float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
146 ring_node *ring_node_tab[],
147 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
148 asm_msg *msgForMATR );
149 ring_node *ring_node_tab[],
150 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
151 asm_msg *msgForMATR );
149 152
150 153 void ASM_patch( float *inputASM, float *outputASM );
151 154
152 155 void extractReImVectors(float *inputASM, float *outputASM, unsigned int asmComponent );
153 156
154 157 static inline void ASM_reorganize_and_divide(float *averaged_spec_mat, float *averaged_spec_mat_reorganized,
155 float divider );
158 float divider );
156 159
157 160 static inline void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat,
158 float divider,
159 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart);
161 float divider,
162 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart);
160 163
161 164 static inline void ASM_convert(volatile float *input_matrix, char *output_matrix);
162 165
163 166 void SM_average( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
164 167 ring_node *ring_node_tab[],
165 168 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
166 169 asm_msg *msgForMATR )
167 170 {
168 171 float sum;
169 172 unsigned int i;
170 173
171 174 for(i=0; i<TOTAL_SIZE_SM; i++)
172 175 {
173 176 sum = ( (int *) (ring_node_tab[0]->buffer_address) ) [ i ]
174 177 + ( (int *) (ring_node_tab[1]->buffer_address) ) [ i ]
175 178 + ( (int *) (ring_node_tab[2]->buffer_address) ) [ i ]
176 179 + ( (int *) (ring_node_tab[3]->buffer_address) ) [ i ]
177 180 + ( (int *) (ring_node_tab[4]->buffer_address) ) [ i ]
178 181 + ( (int *) (ring_node_tab[5]->buffer_address) ) [ i ]
179 182 + ( (int *) (ring_node_tab[6]->buffer_address) ) [ i ]
180 183 + ( (int *) (ring_node_tab[7]->buffer_address) ) [ i ];
181 184
182 185 if ( (nbAverageNORM == 0) && (nbAverageSBM == 0) )
183 186 {
184 187 averaged_spec_mat_NORM[ i ] = sum;
185 188 averaged_spec_mat_SBM[ i ] = sum;
186 189 msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime;
187 190 msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime;
188 191 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
189 192 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
190 193 }
191 194 else if ( (nbAverageNORM != 0) && (nbAverageSBM != 0) )
192 195 {
193 196 averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum );
194 197 averaged_spec_mat_SBM[ i ] = ( averaged_spec_mat_SBM[ i ] + sum );
195 198 }
196 199 else if ( (nbAverageNORM != 0) && (nbAverageSBM == 0) )
197 200 {
198 201 averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum );
199 202 averaged_spec_mat_SBM[ i ] = sum;
200 203 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
201 204 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
202 205 }
203 206 else
204 207 {
205 208 averaged_spec_mat_NORM[ i ] = sum;
206 209 averaged_spec_mat_SBM[ i ] = ( averaged_spec_mat_SBM[ i ] + sum );
207 210 msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime;
208 211 msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime;
209 // PRINTF2("ERR *** in SM_average *** unexpected parameters %d %d\n", nbAverageNORM, nbAverageSBM)
212 // PRINTF2("ERR *** in SM_average *** unexpected parameters %d %d\n", nbAverageNORM, nbAverageSBM)
210 213 }
211 214 }
212 215 }
213 216
214 217 void SM_average_debug( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
215 ring_node *ring_node_tab[],
216 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
217 asm_msg *msgForMATR )
218 ring_node *ring_node_tab[],
219 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
220 asm_msg *msgForMATR )
218 221 {
219 222 float sum;
220 223 unsigned int i;
221 224
222 225 for(i=0; i<TOTAL_SIZE_SM; i++)
223 226 {
224 227 sum = ( (int *) (ring_node_tab[0]->buffer_address) ) [ i ];
225 228 averaged_spec_mat_NORM[ i ] = sum;
226 229 averaged_spec_mat_SBM[ i ] = sum;
227 230 msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime;
228 231 msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime;
229 232 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
230 233 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
231 234 }
232 235 }
233 236
234 237 void ASM_reorganize_and_divide( float *averaged_spec_mat, float *averaged_spec_mat_reorganized, float divider )
235 238 {
236 239 int frequencyBin;
237 240 int asmComponent;
238 241 unsigned int offsetASM;
239 242 unsigned int offsetASMReorganized;
240 243
241 244 // BUILD DATA
242 245 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
243 246 {
244 247 for( frequencyBin = 0; frequencyBin < NB_BINS_PER_SM; frequencyBin++ )
245 248 {
246 249 offsetASMReorganized =
247 250 frequencyBin * NB_VALUES_PER_SM
248 251 + asmComponent;
249 252 offsetASM =
250 253 asmComponent * NB_BINS_PER_SM
251 254 + frequencyBin;
252 255 averaged_spec_mat_reorganized[offsetASMReorganized ] =
253 256 averaged_spec_mat[ offsetASM ] / divider;
254 257 }
255 258 }
256 259 }
257 260
258 261 void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
259 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage, unsigned char ASMIndexStart )
262 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage, unsigned char ASMIndexStart )
260 263 {
261 264 int frequencyBin;
262 265 int asmComponent;
263 266 int offsetASM;
264 267 int offsetCompressed;
265 268 int k;
266 269
267 270 // BUILD DATA
268 271 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
269 272 {
270 273 for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
271 274 {
272 275 offsetCompressed = // NO TIME OFFSET
273 276 frequencyBin * NB_VALUES_PER_SM
274 277 + asmComponent;
275 278 offsetASM = // NO TIME OFFSET
276 279 asmComponent * NB_BINS_PER_SM
277 280 + ASMIndexStart
278 281 + frequencyBin * nbBinsToAverage;
279 282 compressed_spec_mat[ offsetCompressed ] = 0;
280 283 for ( k = 0; k < nbBinsToAverage; k++ )
281 284 {
282 285 compressed_spec_mat[offsetCompressed ] =
283 286 ( compressed_spec_mat[ offsetCompressed ]
284 + averaged_spec_mat[ offsetASM + k ] );
287 + averaged_spec_mat[ offsetASM + k ] );
285 288 }
286 289 compressed_spec_mat[ offsetCompressed ] =
287 290 compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
288 291 }
289 292 }
290 293 }
291 294
292 295 void ASM_convert( volatile float *input_matrix, char *output_matrix)
293 296 {
294 297 unsigned int frequencyBin;
295 298 unsigned int asmComponent;
296 299 char * pt_char_input;
297 300 char * pt_char_output;
298 301 unsigned int offsetInput;
299 302 unsigned int offsetOutput;
300 303
301 304 pt_char_input = (char*) &input_matrix;
302 305 pt_char_output = (char*) &output_matrix;
303 306
304 307 // convert all other data
305 308 for( frequencyBin=0; frequencyBin<NB_BINS_PER_SM; frequencyBin++)
306 309 {
307 310 for ( asmComponent=0; asmComponent<NB_VALUES_PER_SM; asmComponent++)
308 311 {
309 312 offsetInput = (frequencyBin*NB_VALUES_PER_SM) + asmComponent ;
310 313 offsetOutput = 2 * ( (frequencyBin*NB_VALUES_PER_SM) + asmComponent ) ;
311 314 pt_char_input = (char*) &input_matrix [ offsetInput ];
312 315 pt_char_output = (char*) &output_matrix[ offsetOutput ];
313 316 pt_char_output[0] = pt_char_input[0]; // bits 31 downto 24 of the float
314 317 pt_char_output[1] = pt_char_input[1]; // bits 23 downto 16 of the float
315 318 }
316 319 }
317 320 }
318 321
319 322 void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat,
320 float divider,
321 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart, unsigned char channel);
323 float divider,
324 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart, unsigned char channel);
322 325
323 326 int getFBinMask(int k, unsigned char channel);
324 327
325 328 void init_kcoeff_sbm_from_kcoeff_norm( float *input_kcoeff, float *output_kcoeff, unsigned char nb_bins_norm);
326 329
327 330 #endif // FSW_PROCESSING_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -1,80 +1,81
1 1 #ifndef TC_HANDLER_H_INCLUDED
2 2 #define TC_HANDLER_H_INCLUDED
3 3
4 4 #include <rtems.h>
5 5 #include <leon.h>
6 6
7 7 #include "tc_load_dump_parameters.h"
8 8 #include "tc_acceptance.h"
9 9 #include "tm_lfr_tc_exe.h"
10 10 #include "wf_handler.h"
11 11 #include "fsw_processing.h"
12 12
13 13 #include "lfr_cpu_usage_report.h"
14 14
15 extern unsigned int lastValidTransitionDate;
15 extern unsigned int lastValidEnterModeTime;
16 extern enum lfr_transition_type_t lfrTransitionType;
16 17
17 18 //****
18 19 // ISR
19 20 rtems_isr commutation_isr1( rtems_vector_number vector );
20 21 rtems_isr commutation_isr2( rtems_vector_number vector );
21 22
22 23 //***********
23 24 // RTEMS TASK
24 25 rtems_task actn_task( rtems_task_argument unused );
25 26
26 27 //***********
27 28 // TC ACTIONS
28 29 int action_reset( ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time );
29 30 int action_enter_mode(ccsdsTelecommandPacket_t *TC, rtems_id queue_id);
30 31 int action_update_info( ccsdsTelecommandPacket_t *TC, rtems_id queue_id );
31 32 int action_enable_calibration( ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time );
32 33 int action_disable_calibration( ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time );
33 34 int action_update_time( ccsdsTelecommandPacket_t *TC);
34 35
35 36 // mode transition
36 37 int check_mode_value( unsigned char requestedMode );
37 38 int check_mode_transition( unsigned char requestedMode );
38 39 void update_last_valid_transition_date( unsigned int transitionCoarseTime );
39 40 int check_transition_date( unsigned int transitionCoarseTime );
40 41 int stop_spectral_matrices( void );
41 42 int stop_current_mode( void );
42 43 int enter_mode_standby( void );
43 44 int enter_mode_normal( unsigned int transitionCoarseTime );
44 45 int enter_mode_burst( unsigned int transitionCoarseTime );
45 46 int enter_mode_sbm1( unsigned int transitionCoarseTime );
46 47 int enter_mode_sbm2( unsigned int transitionCoarseTime );
47 48 int restart_science_tasks( unsigned char lfrRequestedMode );
48 49 int restart_asm_tasks(unsigned char lfrRequestedMode );
49 50 int suspend_science_tasks(void);
50 51 int suspend_asm_tasks( void );
51 52 void launch_waveform_picker( unsigned char mode , unsigned int transitionCoarseTime );
52 53 void launch_spectral_matrix( void );
53 54 void set_sm_irq_onNewMatrix( unsigned char value );
54 55 void set_sm_irq_onError( unsigned char value );
55 56
56 57 // other functions
57 58 void updateLFRCurrentMode();
58 59 void set_lfr_soft_reset( unsigned char value );
59 60 void reset_lfr( void );
60 61 // CALIBRATION
61 62 void setCalibrationPrescaler( unsigned int prescaler );
62 63 void setCalibrationDivisor( unsigned int divisionFactor );
63 64 void setCalibrationData( void );
64 65 void setCalibrationReload( bool state);
65 66 void setCalibrationEnable( bool state );
66 67 void setCalibrationInterleaved( bool state );
67 68 void setCalibration( bool state );
68 69 void configureCalibration( bool interleaved );
69 70 //
70 71 void update_last_TC_exe( ccsdsTelecommandPacket_t *TC , unsigned char *time );
71 72 void update_last_TC_rej(ccsdsTelecommandPacket_t *TC , unsigned char *time );
72 73 void close_action( ccsdsTelecommandPacket_t *TC, int result, rtems_id queue_id );
73 74
74 75 extern rtems_status_code get_message_queue_id_send( rtems_id *queue_id );
75 76 extern rtems_status_code get_message_queue_id_recv( rtems_id *queue_id );
76 77
77 78 #endif // TC_HANDLER_H_INCLUDED
78 79
79 80
80 81
Show file before | Show file after | Hide comments
@@ -1,80 +1,81
1 1 /** Global variables of the LFR flight software.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * Among global variables, there are:
7 7 * - RTEMS names and id.
8 8 * - APB configuration registers.
9 9 * - waveforms global buffers, used by the waveform picker hardware module to store data.
10 10 * - spectral matrices buffesr, used by the hardware module to store data.
11 11 * - variable related to LFR modes parameters.
12 12 * - the global HK packet buffer.
13 13 * - the global dump parameter buffer.
14 14 *
15 15 */
16 16
17 17 #include <rtems.h>
18 18 #include <grspw.h>
19 19
20 20 #include "ccsds_types.h"
21 21 #include "grlib_regs.h"
22 22 #include "fsw_params.h"
23 23 #include "fsw_params_wf_handler.h"
24 24
25 25 // RTEMS GLOBAL VARIABLES
26 26 rtems_name misc_name[5];
27 27 rtems_name Task_name[20]; /* array of task names */
28 28 rtems_id Task_id[20]; /* array of task ids */
29 29 int fdSPW = 0;
30 30 int fdUART = 0;
31 31 unsigned char lfrCurrentMode;
32 32 unsigned char pa_bia_status_info;
33 33
34 34 // WAVEFORMS GLOBAL VARIABLES // 2048 * 3 * 4 + 2 * 4 = 24576 + 8 bytes = 24584
35 35 // 97 * 256 = 24832 => delta = 248 bytes = 62 words
36 36 // WAVEFORMS GLOBAL VARIABLES // 2688 * 3 * 4 + 2 * 4 = 32256 + 8 bytes = 32264
37 37 // 127 * 256 = 32512 => delta = 248 bytes = 62 words
38 38 // F0 F1 F2 F3
39 39 volatile int wf_buffer_f0[ NB_RING_NODES_F0 * WFRM_BUFFER ] __attribute__((aligned(0x100)));
40 40 volatile int wf_buffer_f1[ NB_RING_NODES_F1 * WFRM_BUFFER ] __attribute__((aligned(0x100)));
41 41 volatile int wf_buffer_f2[ NB_RING_NODES_F2 * WFRM_BUFFER ] __attribute__((aligned(0x100)));
42 42 volatile int wf_buffer_f3[ NB_RING_NODES_F3 * WFRM_BUFFER ] __attribute__((aligned(0x100)));
43 43
44 44 //***********************************
45 45 // SPECTRAL MATRICES GLOBAL VARIABLES
46 46
47 47 // alignment constraints for the spectral matrices buffers => the first data after the time (8 bytes) shall be aligned on 0x00
48 48 volatile int sm_f0[ NB_RING_NODES_SM_F0 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100)));
49 49 volatile int sm_f1[ NB_RING_NODES_SM_F1 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100)));
50 50 volatile int sm_f2[ NB_RING_NODES_SM_F2 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100)));
51 51
52 52 // APB CONFIGURATION REGISTERS
53 53 time_management_regs_t *time_management_regs = (time_management_regs_t*) REGS_ADDR_TIME_MANAGEMENT;
54 54 gptimer_regs_t *gptimer_regs = (gptimer_regs_t *) REGS_ADDR_GPTIMER;
55 55 waveform_picker_regs_0_1_18_t *waveform_picker_regs = (waveform_picker_regs_0_1_18_t*) REGS_ADDR_WAVEFORM_PICKER;
56 56 spectral_matrix_regs_t *spectral_matrix_regs = (spectral_matrix_regs_t*) REGS_ADDR_SPECTRAL_MATRIX;
57 57
58 58 // MODE PARAMETERS
59 59 Packet_TM_LFR_PARAMETER_DUMP_t parameter_dump_packet;
60 60 struct param_local_str param_local;
61 unsigned int lastValidTransitionDate;
61 unsigned int lastValidEnterModeTime;
62 enum lfr_transition_type_t lfrTransitionType;
62 63
63 64 // HK PACKETS
64 65 Packet_TM_LFR_HK_t housekeeping_packet;
65 66 // message queues occupancy
66 67 unsigned char hk_lfr_q_sd_fifo_size_max;
67 68 unsigned char hk_lfr_q_rv_fifo_size_max;
68 69 unsigned char hk_lfr_q_p0_fifo_size_max;
69 70 unsigned char hk_lfr_q_p1_fifo_size_max;
70 71 unsigned char hk_lfr_q_p2_fifo_size_max;
71 72 // sequence counters are incremented by APID (PID + CAT) and destination ID
72 73 unsigned short sequenceCounters_SCIENCE_NORMAL_BURST;
73 74 unsigned short sequenceCounters_SCIENCE_SBM1_SBM2;
74 75 unsigned short sequenceCounters_TC_EXE[SEQ_CNT_NB_DEST_ID];
75 76 unsigned short sequenceCounters_TM_DUMP[SEQ_CNT_NB_DEST_ID];
76 77 unsigned short sequenceCounterHK;
77 78 spw_stats spacewire_stats;
78 79 spw_stats spacewire_stats_backup;
79 80
80 81
Show file before | Show file after | Hide comments
@@ -1,408 +1,408
1 1 /** Functions related to data processing.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 7 *
8 8 */
9 9
10 10 #include "avf0_prc0.h"
11 11 #include "fsw_processing.h"
12 12
13 13 nb_sm_before_bp_asm_f0 nb_sm_before_f0;
14 14
15 15 //***
16 16 // F0
17 17 ring_node_asm asm_ring_norm_f0 [ NB_RING_NODES_ASM_NORM_F0 ];
18 18 ring_node_asm asm_ring_burst_sbm_f0 [ NB_RING_NODES_ASM_BURST_SBM_F0 ];
19 19
20 20 ring_node ring_to_send_asm_f0 [ NB_RING_NODES_ASM_F0 ];
21 21 int buffer_asm_f0 [ NB_RING_NODES_ASM_F0 * TOTAL_SIZE_SM ];
22 22
23 23 float asm_f0_patched_norm [ TOTAL_SIZE_SM ];
24 24 float asm_f0_patched_burst_sbm [ TOTAL_SIZE_SM ];
25 25 float asm_f0_reorganized [ TOTAL_SIZE_SM ];
26 26
27 27 char asm_f0_char [ TIME_OFFSET_IN_BYTES + (TOTAL_SIZE_SM * 2) ];
28 28 float compressed_sm_norm_f0[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F0];
29 29 float compressed_sm_sbm_f0 [ TOTAL_SIZE_COMPRESSED_ASM_SBM_F0 ];
30 30
31 31 float k_coeff_intercalib_f0_norm[ NB_BINS_COMPRESSED_SM_F0 * NB_K_COEFF_PER_BIN ]; // 11 * 32 = 352
32 32 float k_coeff_intercalib_f0_sbm[ NB_BINS_COMPRESSED_SM_SBM_F0 * NB_K_COEFF_PER_BIN ]; // 22 * 32 = 704
33 33
34 34 //************
35 35 // RTEMS TASKS
36 36
37 37 rtems_task avf0_task( rtems_task_argument lfrRequestedMode )
38 38 {
39 39 int i;
40 40
41 41 rtems_event_set event_out;
42 42 rtems_status_code status;
43 43 rtems_id queue_id_prc0;
44 44 asm_msg msgForMATR;
45 45 ring_node *nodeForAveraging;
46 46 ring_node *ring_node_tab[8];
47 47 ring_node_asm *current_ring_node_asm_burst_sbm_f0;
48 48 ring_node_asm *current_ring_node_asm_norm_f0;
49 49
50 50 unsigned int nb_norm_bp1;
51 51 unsigned int nb_norm_bp2;
52 52 unsigned int nb_norm_asm;
53 53 unsigned int nb_sbm_bp1;
54 54 unsigned int nb_sbm_bp2;
55 55
56 56 nb_norm_bp1 = 0;
57 57 nb_norm_bp2 = 0;
58 58 nb_norm_asm = 0;
59 59 nb_sbm_bp1 = 0;
60 60 nb_sbm_bp2 = 0;
61 61
62 62 reset_nb_sm_f0( lfrRequestedMode ); // reset the sm counters that drive the BP and ASM computations / transmissions
63 63 ASM_generic_init_ring( asm_ring_norm_f0, NB_RING_NODES_ASM_NORM_F0 );
64 64 ASM_generic_init_ring( asm_ring_burst_sbm_f0, NB_RING_NODES_ASM_BURST_SBM_F0 );
65 65 current_ring_node_asm_norm_f0 = asm_ring_norm_f0;
66 66 current_ring_node_asm_burst_sbm_f0 = asm_ring_burst_sbm_f0;
67 67
68 68 BOOT_PRINTF1("in AVFO *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
69 69
70 70 status = get_message_queue_id_prc0( &queue_id_prc0 );
71 71 if (status != RTEMS_SUCCESSFUL)
72 72 {
73 73 PRINTF1("in MATR *** ERR get_message_queue_id_prc0 %d\n", status)
74 74 }
75 75
76 76 while(1){
77 77 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
78 78
79 79 //****************************************
80 80 // initialize the mesage for the MATR task
81 81 msgForMATR.norm = current_ring_node_asm_norm_f0;
82 82 msgForMATR.burst_sbm = current_ring_node_asm_burst_sbm_f0;
83 83 msgForMATR.event = 0x00; // this composite event will be sent to the PRC0 task
84 84 //
85 85 //****************************************
86 86
87 87 nodeForAveraging = getRingNodeForAveraging( 0 );
88 88
89 89 ring_node_tab[NB_SM_BEFORE_AVF0-1] = nodeForAveraging;
90 90 for ( i = 2; i < (NB_SM_BEFORE_AVF0+1); i++ )
91 91 {
92 92 nodeForAveraging = nodeForAveraging->previous;
93 93 ring_node_tab[NB_SM_BEFORE_AVF0-i] = nodeForAveraging;
94 94 }
95 95
96 96 // compute the average and store it in the averaged_sm_f1 buffer
97 97 SM_average( current_ring_node_asm_norm_f0->matrix,
98 98 current_ring_node_asm_burst_sbm_f0->matrix,
99 99 ring_node_tab,
100 100 nb_norm_bp1, nb_sbm_bp1,
101 101 &msgForMATR );
102 102
103 103 // update nb_average
104 104 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF0;
105 105 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF0;
106 106 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF0;
107 107 nb_sbm_bp1 = nb_sbm_bp1 + NB_SM_BEFORE_AVF0;
108 108 nb_sbm_bp2 = nb_sbm_bp2 + NB_SM_BEFORE_AVF0;
109 109
110 110 if (nb_sbm_bp1 == nb_sm_before_f0.burst_sbm_bp1)
111 111 {
112 112 nb_sbm_bp1 = 0;
113 113 // set another ring for the ASM storage
114 114 current_ring_node_asm_burst_sbm_f0 = current_ring_node_asm_burst_sbm_f0->next;
115 115 if ( lfrCurrentMode == LFR_MODE_BURST )
116 116 {
117 117 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_BP1_F0;
118 118 }
119 119 else if ( (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
120 120 {
121 121 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_SBM_BP1_F0;
122 122 }
123 123 }
124 124
125 125 if (nb_sbm_bp2 == nb_sm_before_f0.burst_sbm_bp2)
126 126 {
127 127 nb_sbm_bp2 = 0;
128 128 if ( lfrCurrentMode == LFR_MODE_BURST )
129 129 {
130 130 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_BP2_F0;
131 131 }
132 132 else if ( (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
133 133 {
134 134 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_SBM_BP2_F0;
135 135 }
136 136 }
137 137
138 138 if (nb_norm_bp1 == nb_sm_before_f0.norm_bp1)
139 139 {
140 140 nb_norm_bp1 = 0;
141 141 // set another ring for the ASM storage
142 142 current_ring_node_asm_norm_f0 = current_ring_node_asm_norm_f0->next;
143 143 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
144 144 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
145 145 {
146 146 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP1_F0;
147 147 }
148 148 }
149 149
150 150 if (nb_norm_bp2 == nb_sm_before_f0.norm_bp2)
151 151 {
152 152 nb_norm_bp2 = 0;
153 153 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
154 154 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
155 155 {
156 156 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP2_F0;
157 157 }
158 158 }
159 159
160 160 if (nb_norm_asm == nb_sm_before_f0.norm_asm)
161 161 {
162 162 nb_norm_asm = 0;
163 163 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
164 164 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
165 165 {
166 166 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_ASM_F0;
167 167 }
168 168 }
169 169
170 170 //*************************
171 171 // send the message to MATR
172 172 if (msgForMATR.event != 0x00)
173 173 {
174 174 status = rtems_message_queue_send( queue_id_prc0, (char *) &msgForMATR, MSG_QUEUE_SIZE_PRC0);
175 175 }
176 176
177 177 if (status != RTEMS_SUCCESSFUL) {
178 178 PRINTF1("in AVF0 *** Error sending message to MATR, code %d\n", status)
179 179 }
180 180 }
181 181 }
182 182
183 183 rtems_task prc0_task( rtems_task_argument lfrRequestedMode )
184 184 {
185 185 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
186 186 size_t size; // size of the incoming TC packet
187 187 asm_msg *incomingMsg;
188 188 //
189 189 unsigned char sid;
190 190 rtems_status_code status;
191 191 rtems_id queue_id;
192 192 rtems_id queue_id_q_p0;
193 193 bp_packet_with_spare packet_norm_bp1;
194 194 bp_packet packet_norm_bp2;
195 195 bp_packet packet_sbm_bp1;
196 196 bp_packet packet_sbm_bp2;
197 197 ring_node *current_ring_node_to_send_asm_f0;
198 198
199 199 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
200 200 init_ring( ring_to_send_asm_f0, NB_RING_NODES_ASM_F0, (volatile int*) buffer_asm_f0, TOTAL_SIZE_SM );
201 201 current_ring_node_to_send_asm_f0 = ring_to_send_asm_f0;
202 202
203 203 //*************
204 204 // NORM headers
205 205 BP_init_header_with_spare( &packet_norm_bp1,
206 206 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F0,
207 207 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F0, NB_BINS_COMPRESSED_SM_F0 );
208 208 BP_init_header( &packet_norm_bp2,
209 209 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F0,
210 210 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F0, NB_BINS_COMPRESSED_SM_F0);
211 211
212 212 //****************************
213 213 // BURST SBM1 and SBM2 headers
214 214 if ( lfrRequestedMode == LFR_MODE_BURST )
215 215 {
216 216 BP_init_header( &packet_sbm_bp1,
217 217 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP1_F0,
218 218 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
219 219 BP_init_header( &packet_sbm_bp2,
220 220 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP2_F0,
221 221 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
222 222 }
223 223 else if ( lfrRequestedMode == LFR_MODE_SBM1 )
224 224 {
225 225 BP_init_header( &packet_sbm_bp1,
226 226 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM1_BP1_F0,
227 227 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
228 228 BP_init_header( &packet_sbm_bp2,
229 229 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM1_BP2_F0,
230 230 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
231 231 }
232 232 else if ( lfrRequestedMode == LFR_MODE_SBM2 )
233 233 {
234 234 BP_init_header( &packet_sbm_bp1,
235 235 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP1_F0,
236 236 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
237 237 BP_init_header( &packet_sbm_bp2,
238 238 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP2_F0,
239 239 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
240 240 }
241 241 else
242 242 {
243 243 PRINTF1("in PRC0 *** lfrRequestedMode is %d, several headers not initialized\n", (unsigned int) lfrRequestedMode)
244 244 }
245 245
246 246 status = get_message_queue_id_send( &queue_id );
247 247 if (status != RTEMS_SUCCESSFUL)
248 248 {
249 249 PRINTF1("in PRC0 *** ERR get_message_queue_id_send %d\n", status)
250 250 }
251 251 status = get_message_queue_id_prc0( &queue_id_q_p0);
252 252 if (status != RTEMS_SUCCESSFUL)
253 253 {
254 254 PRINTF1("in PRC0 *** ERR get_message_queue_id_prc0 %d\n", status)
255 255 }
256 256
257 257 BOOT_PRINTF1("in PRC0 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
258 258
259 259 while(1){
260 260 status = rtems_message_queue_receive( queue_id_q_p0, incomingData, &size, //************************************
261 261 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF0
262 262
263 263 incomingMsg = (asm_msg*) incomingData;
264 264
265 265 ASM_patch( incomingMsg->norm->matrix, asm_f0_patched_norm );
266 266 ASM_patch( incomingMsg->burst_sbm->matrix, asm_f0_patched_burst_sbm );
267 267
268 268 //****************
269 269 //****************
270 270 // BURST SBM1 SBM2
271 271 //****************
272 272 //****************
273 273 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP1_F0 ) || (incomingMsg->event & RTEMS_EVENT_SBM_BP1_F0 ) )
274 274 {
275 275 sid = getSID( incomingMsg->event );
276 276 // 1) compress the matrix for Basic Parameters calculation
277 277 ASM_compress_reorganize_and_divide_mask( asm_f0_patched_burst_sbm, compressed_sm_sbm_f0,
278 278 nb_sm_before_f0.burst_sbm_bp1,
279 279 NB_BINS_COMPRESSED_SM_SBM_F0, NB_BINS_TO_AVERAGE_ASM_SBM_F0,
280 280 ASM_F0_INDICE_START, CHANNELF0);
281 281 // 2) compute the BP1 set
282 282 BP1_set( compressed_sm_sbm_f0, k_coeff_intercalib_f0_sbm, NB_BINS_COMPRESSED_SM_SBM_F0, packet_sbm_bp1.data );
283 283 // 3) send the BP1 set
284 284 set_time( packet_sbm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
285 285 set_time( packet_sbm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
286 286 packet_sbm_bp1.biaStatusInfo = pa_bia_status_info;
287 287 packet_sbm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
288 BP_send( (char *) &packet_sbm_bp1, queue_id,
288 BP_send_s1_s2( (char *) &packet_sbm_bp1, queue_id,
289 289 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0 + PACKET_LENGTH_DELTA,
290 290 sid);
291 291 // 4) compute the BP2 set if needed
292 292 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP2_F0) || (incomingMsg->event & RTEMS_EVENT_SBM_BP2_F0) )
293 293 {
294 294 // 1) compute the BP2 set
295 295 BP2_set( compressed_sm_sbm_f0, NB_BINS_COMPRESSED_SM_SBM_F0, packet_sbm_bp2.data );
296 296 // 2) send the BP2 set
297 297 set_time( packet_sbm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
298 298 set_time( packet_sbm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
299 299 packet_sbm_bp2.biaStatusInfo = pa_bia_status_info;
300 300 packet_sbm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
301 BP_send( (char *) &packet_sbm_bp2, queue_id,
301 BP_send_s1_s2( (char *) &packet_sbm_bp2, queue_id,
302 302 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0 + PACKET_LENGTH_DELTA,
303 303 sid);
304 304 }
305 305 }
306 306
307 307 //*****
308 308 //*****
309 309 // NORM
310 310 //*****
311 311 //*****
312 312 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F0)
313 313 {
314 314 // 1) compress the matrix for Basic Parameters calculation
315 315 ASM_compress_reorganize_and_divide_mask( asm_f0_patched_norm, compressed_sm_norm_f0,
316 316 nb_sm_before_f0.norm_bp1,
317 317 NB_BINS_COMPRESSED_SM_F0, NB_BINS_TO_AVERAGE_ASM_F0,
318 318 ASM_F0_INDICE_START, CHANNELF0 );
319 319 // 2) compute the BP1 set
320 320 BP1_set( compressed_sm_norm_f0, k_coeff_intercalib_f0_norm, NB_BINS_COMPRESSED_SM_F0, packet_norm_bp1.data );
321 321 // 3) send the BP1 set
322 322 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
323 323 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
324 324 packet_norm_bp1.biaStatusInfo = pa_bia_status_info;
325 325 packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
326 326 BP_send( (char *) &packet_norm_bp1, queue_id,
327 327 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F0 + PACKET_LENGTH_DELTA,
328 328 SID_NORM_BP1_F0 );
329 329 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F0)
330 330 {
331 331 // 1) compute the BP2 set using the same ASM as the one used for BP1
332 332 BP2_set( compressed_sm_norm_f0, NB_BINS_COMPRESSED_SM_F0, packet_norm_bp2.data );
333 333 // 2) send the BP2 set
334 334 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
335 335 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
336 336 packet_norm_bp2.biaStatusInfo = pa_bia_status_info;
337 337 packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
338 338 BP_send( (char *) &packet_norm_bp2, queue_id,
339 339 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F0 + PACKET_LENGTH_DELTA,
340 340 SID_NORM_BP2_F0);
341 341 }
342 342 }
343 343
344 344 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F0)
345 345 {
346 346 // 1) reorganize the ASM and divide
347 347 ASM_reorganize_and_divide( asm_f0_patched_norm,
348 348 (float*) current_ring_node_to_send_asm_f0->buffer_address,
349 349 nb_sm_before_f0.norm_bp1 );
350 350 current_ring_node_to_send_asm_f0->coarseTime = incomingMsg->coarseTimeNORM;
351 351 current_ring_node_to_send_asm_f0->fineTime = incomingMsg->fineTimeNORM;
352 352 current_ring_node_to_send_asm_f0->sid = SID_NORM_ASM_F0;
353 353
354 354 // 3) send the spectral matrix packets
355 355 status = rtems_message_queue_send( queue_id, &current_ring_node_to_send_asm_f0, sizeof( ring_node* ) );
356 356 // change asm ring node
357 357 current_ring_node_to_send_asm_f0 = current_ring_node_to_send_asm_f0->next;
358 358 }
359 359
360 360 update_queue_max_count( queue_id_q_p0, &hk_lfr_q_p0_fifo_size_max );
361 361
362 362 }
363 363 }
364 364
365 365 //**********
366 366 // FUNCTIONS
367 367
368 368 void reset_nb_sm_f0( unsigned char lfrMode )
369 369 {
370 370 nb_sm_before_f0.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0 * 96;
371 371 nb_sm_before_f0.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1 * 96;
372 372 nb_sm_before_f0.norm_asm = (parameter_dump_packet.sy_lfr_n_asm_p[0] * 256 + parameter_dump_packet.sy_lfr_n_asm_p[1]) * 96;
373 373 nb_sm_before_f0.sbm1_bp1 = parameter_dump_packet.sy_lfr_s1_bp_p0 * 24; // 0.25 s per digit
374 374 nb_sm_before_f0.sbm1_bp2 = parameter_dump_packet.sy_lfr_s1_bp_p1 * 96;
375 375 nb_sm_before_f0.sbm2_bp1 = parameter_dump_packet.sy_lfr_s2_bp_p0 * 96;
376 376 nb_sm_before_f0.sbm2_bp2 = parameter_dump_packet.sy_lfr_s2_bp_p1 * 96;
377 377 nb_sm_before_f0.burst_bp1 = parameter_dump_packet.sy_lfr_b_bp_p0 * 96;
378 378 nb_sm_before_f0.burst_bp2 = parameter_dump_packet.sy_lfr_b_bp_p1 * 96;
379 379
380 380 if (lfrMode == LFR_MODE_SBM1)
381 381 {
382 382 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.sbm1_bp1;
383 383 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.sbm1_bp2;
384 384 }
385 385 else if (lfrMode == LFR_MODE_SBM2)
386 386 {
387 387 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.sbm2_bp1;
388 388 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.sbm2_bp2;
389 389 }
390 390 else if (lfrMode == LFR_MODE_BURST)
391 391 {
392 392 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.burst_bp1;
393 393 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.burst_bp2;
394 394 }
395 395 else
396 396 {
397 397 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.burst_bp1;
398 398 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.burst_bp2;
399 399 }
400 400 }
401 401
402 402 void init_k_coefficients_prc0( void )
403 403 {
404 404 init_k_coefficients( k_coeff_intercalib_f0_norm, NB_BINS_COMPRESSED_SM_F0 );
405 405
406 406 init_kcoeff_sbm_from_kcoeff_norm( k_coeff_intercalib_f0_norm, k_coeff_intercalib_f0_sbm, NB_BINS_COMPRESSED_SM_F0);
407 407 }
408 408
Show file before | Show file after | Hide comments
@@ -1,394 +1,394
1 1 /** Functions related to data processing.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 7 *
8 8 */
9 9
10 10 #include "avf1_prc1.h"
11 11
12 12 nb_sm_before_bp_asm_f1 nb_sm_before_f1;
13 13
14 14 extern ring_node sm_ring_f1[ ];
15 15
16 16 //***
17 17 // F1
18 18 ring_node_asm asm_ring_norm_f1 [ NB_RING_NODES_ASM_NORM_F1 ];
19 19 ring_node_asm asm_ring_burst_sbm_f1 [ NB_RING_NODES_ASM_BURST_SBM_F1 ];
20 20
21 21 ring_node ring_to_send_asm_f1 [ NB_RING_NODES_ASM_F1 ];
22 22 int buffer_asm_f1 [ NB_RING_NODES_ASM_F1 * TOTAL_SIZE_SM ];
23 23
24 24 float asm_f1_patched_norm [ TOTAL_SIZE_SM ];
25 25 float asm_f1_patched_burst_sbm [ TOTAL_SIZE_SM ];
26 26 float asm_f1_reorganized [ TOTAL_SIZE_SM ];
27 27
28 28 char asm_f1_char [ TOTAL_SIZE_SM * 2 ];
29 29 float compressed_sm_norm_f1[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F1];
30 30 float compressed_sm_sbm_f1 [ TOTAL_SIZE_COMPRESSED_ASM_SBM_F1 ];
31 31
32 32 float k_coeff_intercalib_f1_norm[ NB_BINS_COMPRESSED_SM_F1 * NB_K_COEFF_PER_BIN ]; // 13 * 32 = 416
33 33 float k_coeff_intercalib_f1_sbm[ NB_BINS_COMPRESSED_SM_SBM_F1 * NB_K_COEFF_PER_BIN ]; // 26 * 32 = 832
34 34
35 35 //************
36 36 // RTEMS TASKS
37 37
38 38 rtems_task avf1_task( rtems_task_argument lfrRequestedMode )
39 39 {
40 40 int i;
41 41
42 42 rtems_event_set event_out;
43 43 rtems_status_code status;
44 44 rtems_id queue_id_prc1;
45 45 asm_msg msgForMATR;
46 46 ring_node *nodeForAveraging;
47 47 ring_node *ring_node_tab[NB_SM_BEFORE_AVF0];
48 48 ring_node_asm *current_ring_node_asm_burst_sbm_f1;
49 49 ring_node_asm *current_ring_node_asm_norm_f1;
50 50
51 51 unsigned int nb_norm_bp1;
52 52 unsigned int nb_norm_bp2;
53 53 unsigned int nb_norm_asm;
54 54 unsigned int nb_sbm_bp1;
55 55 unsigned int nb_sbm_bp2;
56 56
57 57 nb_norm_bp1 = 0;
58 58 nb_norm_bp2 = 0;
59 59 nb_norm_asm = 0;
60 60 nb_sbm_bp1 = 0;
61 61 nb_sbm_bp2 = 0;
62 62
63 63 reset_nb_sm_f1( lfrRequestedMode ); // reset the sm counters that drive the BP and ASM computations / transmissions
64 64 ASM_generic_init_ring( asm_ring_norm_f1, NB_RING_NODES_ASM_NORM_F1 );
65 65 ASM_generic_init_ring( asm_ring_burst_sbm_f1, NB_RING_NODES_ASM_BURST_SBM_F1 );
66 66 current_ring_node_asm_norm_f1 = asm_ring_norm_f1;
67 67 current_ring_node_asm_burst_sbm_f1 = asm_ring_burst_sbm_f1;
68 68
69 69 BOOT_PRINTF1("in AVF1 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
70 70
71 71 status = get_message_queue_id_prc1( &queue_id_prc1 );
72 72 if (status != RTEMS_SUCCESSFUL)
73 73 {
74 74 PRINTF1("in AVF1 *** ERR get_message_queue_id_prc1 %d\n", status)
75 75 }
76 76
77 77 while(1){
78 78 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
79 79
80 80 //****************************************
81 81 // initialize the mesage for the MATR task
82 82 msgForMATR.norm = current_ring_node_asm_norm_f1;
83 83 msgForMATR.burst_sbm = current_ring_node_asm_burst_sbm_f1;
84 84 msgForMATR.event = 0x00; // this composite event will be sent to the PRC1 task
85 85 //
86 86 //****************************************
87 87
88 88 nodeForAveraging = getRingNodeForAveraging( 1 );
89 89
90 90 ring_node_tab[NB_SM_BEFORE_AVF1-1] = nodeForAveraging;
91 91 for ( i = 2; i < (NB_SM_BEFORE_AVF1+1); i++ )
92 92 {
93 93 nodeForAveraging = nodeForAveraging->previous;
94 94 ring_node_tab[NB_SM_BEFORE_AVF1-i] = nodeForAveraging;
95 95 }
96 96
97 97 // compute the average and store it in the averaged_sm_f1 buffer
98 98 SM_average( current_ring_node_asm_norm_f1->matrix,
99 99 current_ring_node_asm_burst_sbm_f1->matrix,
100 100 ring_node_tab,
101 101 nb_norm_bp1, nb_sbm_bp1,
102 102 &msgForMATR );
103 103
104 104 // update nb_average
105 105 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF1;
106 106 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF1;
107 107 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF1;
108 108 nb_sbm_bp1 = nb_sbm_bp1 + NB_SM_BEFORE_AVF1;
109 109 nb_sbm_bp2 = nb_sbm_bp2 + NB_SM_BEFORE_AVF1;
110 110
111 111 if (nb_sbm_bp1 == nb_sm_before_f1.burst_sbm_bp1)
112 112 {
113 113 nb_sbm_bp1 = 0;
114 114 // set another ring for the ASM storage
115 115 current_ring_node_asm_burst_sbm_f1 = current_ring_node_asm_burst_sbm_f1->next;
116 116 if ( lfrCurrentMode == LFR_MODE_BURST )
117 117 {
118 118 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_BP1_F1;
119 119 }
120 120 else if ( lfrCurrentMode == LFR_MODE_SBM2 )
121 121 {
122 122 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_SBM_BP1_F1;
123 123 }
124 124 }
125 125
126 126 if (nb_sbm_bp2 == nb_sm_before_f1.burst_sbm_bp2)
127 127 {
128 128 nb_sbm_bp2 = 0;
129 129 if ( lfrCurrentMode == LFR_MODE_BURST )
130 130 {
131 131 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_BP2_F1;
132 132 }
133 133 else if ( lfrCurrentMode == LFR_MODE_SBM2 )
134 134 {
135 135 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_SBM_BP2_F1;
136 136 }
137 137 }
138 138
139 139 if (nb_norm_bp1 == nb_sm_before_f1.norm_bp1)
140 140 {
141 141 nb_norm_bp1 = 0;
142 142 // set another ring for the ASM storage
143 143 current_ring_node_asm_norm_f1 = current_ring_node_asm_norm_f1->next;
144 144 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
145 145 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
146 146 {
147 147 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP1_F1;
148 148 }
149 149 }
150 150
151 151 if (nb_norm_bp2 == nb_sm_before_f1.norm_bp2)
152 152 {
153 153 nb_norm_bp2 = 0;
154 154 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
155 155 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
156 156 {
157 157 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP2_F1;
158 158 }
159 159 }
160 160
161 161 if (nb_norm_asm == nb_sm_before_f1.norm_asm)
162 162 {
163 163 nb_norm_asm = 0;
164 164 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
165 165 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
166 166 {
167 167 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_ASM_F1;
168 168 }
169 169 }
170 170
171 171 //*************************
172 172 // send the message to MATR
173 173 if (msgForMATR.event != 0x00)
174 174 {
175 175 status = rtems_message_queue_send( queue_id_prc1, (char *) &msgForMATR, MSG_QUEUE_SIZE_PRC1);
176 176 }
177 177
178 178 if (status != RTEMS_SUCCESSFUL) {
179 179 PRINTF1("in AVF1 *** Error sending message to PRC1, code %d\n", status)
180 180 }
181 181 }
182 182 }
183 183
184 184 rtems_task prc1_task( rtems_task_argument lfrRequestedMode )
185 185 {
186 186 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
187 187 size_t size; // size of the incoming TC packet
188 188 asm_msg *incomingMsg;
189 189 //
190 190 unsigned char sid;
191 191 rtems_status_code status;
192 192 rtems_id queue_id_send;
193 193 rtems_id queue_id_q_p1;
194 194 bp_packet_with_spare packet_norm_bp1;
195 195 bp_packet packet_norm_bp2;
196 196 bp_packet packet_sbm_bp1;
197 197 bp_packet packet_sbm_bp2;
198 198 ring_node *current_ring_node_to_send_asm_f1;
199 199
200 200 unsigned long long int localTime;
201 201
202 202 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
203 203 init_ring( ring_to_send_asm_f1, NB_RING_NODES_ASM_F1, (volatile int*) buffer_asm_f1, TOTAL_SIZE_SM );
204 204 current_ring_node_to_send_asm_f1 = ring_to_send_asm_f1;
205 205
206 206 //*************
207 207 // NORM headers
208 208 BP_init_header_with_spare( &packet_norm_bp1,
209 209 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F1,
210 210 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F1, NB_BINS_COMPRESSED_SM_F1 );
211 211 BP_init_header( &packet_norm_bp2,
212 212 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F1,
213 213 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F1, NB_BINS_COMPRESSED_SM_F1);
214 214
215 215 //***********************
216 216 // BURST and SBM2 headers
217 217 if ( lfrRequestedMode == LFR_MODE_BURST )
218 218 {
219 219 BP_init_header( &packet_sbm_bp1,
220 220 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP1_F1,
221 221 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
222 222 BP_init_header( &packet_sbm_bp2,
223 223 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP2_F1,
224 224 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
225 225 }
226 226 else if ( lfrRequestedMode == LFR_MODE_SBM2 )
227 227 {
228 228 BP_init_header( &packet_sbm_bp1,
229 229 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP1_F1,
230 230 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
231 231 BP_init_header( &packet_sbm_bp2,
232 232 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP2_F1,
233 233 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
234 234 }
235 235 else
236 236 {
237 237 PRINTF1("in PRC1 *** lfrRequestedMode is %d, several headers not initialized\n", (unsigned int) lfrRequestedMode)
238 238 }
239 239
240 240 status = get_message_queue_id_send( &queue_id_send );
241 241 if (status != RTEMS_SUCCESSFUL)
242 242 {
243 243 PRINTF1("in PRC1 *** ERR get_message_queue_id_send %d\n", status)
244 244 }
245 245 status = get_message_queue_id_prc1( &queue_id_q_p1);
246 246 if (status != RTEMS_SUCCESSFUL)
247 247 {
248 248 PRINTF1("in PRC1 *** ERR get_message_queue_id_prc1 %d\n", status)
249 249 }
250 250
251 251 BOOT_PRINTF1("in PRC1 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
252 252
253 253 while(1){
254 254 status = rtems_message_queue_receive( queue_id_q_p1, incomingData, &size, //************************************
255 255 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF0
256 256
257 257 incomingMsg = (asm_msg*) incomingData;
258 258
259 259 ASM_patch( incomingMsg->norm->matrix, asm_f1_patched_norm );
260 260 ASM_patch( incomingMsg->burst_sbm->matrix, asm_f1_patched_burst_sbm );
261 261
262 262 localTime = getTimeAsUnsignedLongLongInt( );
263 263 //***********
264 264 //***********
265 265 // BURST SBM2
266 266 //***********
267 267 //***********
268 268 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP1_F1) || (incomingMsg->event & RTEMS_EVENT_SBM_BP1_F1) )
269 269 {
270 270 sid = getSID( incomingMsg->event );
271 271 // 1) compress the matrix for Basic Parameters calculation
272 272 ASM_compress_reorganize_and_divide_mask( asm_f1_patched_burst_sbm, compressed_sm_sbm_f1,
273 273 nb_sm_before_f1.burst_sbm_bp1,
274 274 NB_BINS_COMPRESSED_SM_SBM_F1, NB_BINS_TO_AVERAGE_ASM_SBM_F1,
275 275 ASM_F1_INDICE_START, CHANNELF1);
276 276 // 2) compute the BP1 set
277 277 BP1_set( compressed_sm_sbm_f1, k_coeff_intercalib_f1_sbm, NB_BINS_COMPRESSED_SM_SBM_F1, packet_sbm_bp1.data );
278 278 // 3) send the BP1 set
279 279 set_time( packet_sbm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
280 280 set_time( packet_sbm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
281 281 packet_sbm_bp1.biaStatusInfo = pa_bia_status_info;
282 282 packet_sbm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
283 BP_send( (char *) &packet_sbm_bp1, queue_id_send,
283 BP_send_s1_s2( (char *) &packet_sbm_bp1, queue_id_send,
284 284 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1 + PACKET_LENGTH_DELTA,
285 285 sid );
286 286 // 4) compute the BP2 set if needed
287 287 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP2_F1) || (incomingMsg->event & RTEMS_EVENT_SBM_BP2_F1) )
288 288 {
289 289 // 1) compute the BP2 set
290 290 BP2_set( compressed_sm_sbm_f1, NB_BINS_COMPRESSED_SM_SBM_F1, packet_sbm_bp2.data );
291 291 // 2) send the BP2 set
292 292 set_time( packet_sbm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
293 293 set_time( packet_sbm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
294 294 packet_sbm_bp2.biaStatusInfo = pa_bia_status_info;
295 295 packet_sbm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
296 BP_send( (char *) &packet_sbm_bp2, queue_id_send,
296 BP_send_s1_s2( (char *) &packet_sbm_bp2, queue_id_send,
297 297 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1 + PACKET_LENGTH_DELTA,
298 298 sid );
299 299 }
300 300 }
301 301
302 302 //*****
303 303 //*****
304 304 // NORM
305 305 //*****
306 306 //*****
307 307 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F1)
308 308 {
309 309 // 1) compress the matrix for Basic Parameters calculation
310 310 ASM_compress_reorganize_and_divide_mask( asm_f1_patched_norm, compressed_sm_norm_f1,
311 311 nb_sm_before_f1.norm_bp1,
312 312 NB_BINS_COMPRESSED_SM_F1, NB_BINS_TO_AVERAGE_ASM_F1,
313 313 ASM_F1_INDICE_START, CHANNELF1 );
314 314 // 2) compute the BP1 set
315 315 BP1_set( compressed_sm_norm_f1, k_coeff_intercalib_f1_norm, NB_BINS_COMPRESSED_SM_F1, packet_norm_bp1.data );
316 316 // 3) send the BP1 set
317 317 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
318 318 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
319 319 packet_norm_bp1.biaStatusInfo = pa_bia_status_info;
320 320 packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
321 321 BP_send( (char *) &packet_norm_bp1, queue_id_send,
322 322 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F1 + PACKET_LENGTH_DELTA,
323 323 SID_NORM_BP1_F1 );
324 324 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F1)
325 325 {
326 326 // 1) compute the BP2 set
327 327 BP2_set( compressed_sm_norm_f1, NB_BINS_COMPRESSED_SM_F1, packet_norm_bp2.data );
328 328 // 2) send the BP2 set
329 329 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
330 330 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
331 331 packet_norm_bp2.biaStatusInfo = pa_bia_status_info;
332 332 packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
333 333 BP_send( (char *) &packet_norm_bp2, queue_id_send,
334 334 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F1 + PACKET_LENGTH_DELTA,
335 335 SID_NORM_BP2_F1 );
336 336 }
337 337 }
338 338
339 339 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F1)
340 340 {
341 341 // 1) reorganize the ASM and divide
342 342 ASM_reorganize_and_divide( asm_f1_patched_norm,
343 343 (float*) current_ring_node_to_send_asm_f1->buffer_address,
344 344 nb_sm_before_f1.norm_bp1 );
345 345 current_ring_node_to_send_asm_f1->coarseTime = incomingMsg->coarseTimeNORM;
346 346 current_ring_node_to_send_asm_f1->fineTime = incomingMsg->fineTimeNORM;
347 347 current_ring_node_to_send_asm_f1->sid = SID_NORM_ASM_F1;
348 348 // 3) send the spectral matrix packets
349 349 status = rtems_message_queue_send( queue_id_send, &current_ring_node_to_send_asm_f1, sizeof( ring_node* ) );
350 350 // change asm ring node
351 351 current_ring_node_to_send_asm_f1 = current_ring_node_to_send_asm_f1->next;
352 352 }
353 353
354 354 update_queue_max_count( queue_id_q_p1, &hk_lfr_q_p1_fifo_size_max );
355 355
356 356 }
357 357 }
358 358
359 359 //**********
360 360 // FUNCTIONS
361 361
362 362 void reset_nb_sm_f1( unsigned char lfrMode )
363 363 {
364 364 nb_sm_before_f1.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0 * 16;
365 365 nb_sm_before_f1.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1 * 16;
366 366 nb_sm_before_f1.norm_asm = (parameter_dump_packet.sy_lfr_n_asm_p[0] * 256 + parameter_dump_packet.sy_lfr_n_asm_p[1]) * 16;
367 367 nb_sm_before_f1.sbm2_bp1 = parameter_dump_packet.sy_lfr_s2_bp_p0 * 16;
368 368 nb_sm_before_f1.sbm2_bp2 = parameter_dump_packet.sy_lfr_s2_bp_p1 * 16;
369 369 nb_sm_before_f1.burst_bp1 = parameter_dump_packet.sy_lfr_b_bp_p0 * 16;
370 370 nb_sm_before_f1.burst_bp2 = parameter_dump_packet.sy_lfr_b_bp_p1 * 16;
371 371
372 372 if (lfrMode == LFR_MODE_SBM2)
373 373 {
374 374 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.sbm2_bp1;
375 375 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.sbm2_bp2;
376 376 }
377 377 else if (lfrMode == LFR_MODE_BURST)
378 378 {
379 379 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.burst_bp1;
380 380 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.burst_bp2;
381 381 }
382 382 else
383 383 {
384 384 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.burst_bp1;
385 385 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.burst_bp2;
386 386 }
387 387 }
388 388
389 389 void init_k_coefficients_prc1( void )
390 390 {
391 391 init_k_coefficients( k_coeff_intercalib_f1_norm, NB_BINS_COMPRESSED_SM_F1 );
392 392
393 393 init_kcoeff_sbm_from_kcoeff_norm( k_coeff_intercalib_f1_norm, k_coeff_intercalib_f1_sbm, NB_BINS_COMPRESSED_SM_F1);
394 394 }
Show file before | Show file after | Hide comments
@@ -1,640 +1,668
1 1 /** Functions related to data processing.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 7 *
8 8 */
9 9
10 10 #include "fsw_processing.h"
11 11 #include "fsw_processing_globals.c"
12 12 #include "fsw_init.h"
13 13
14 14 unsigned int nb_sm_f0;
15 15 unsigned int nb_sm_f0_aux_f1;
16 16 unsigned int nb_sm_f1;
17 17 unsigned int nb_sm_f0_aux_f2;
18 18
19 19 //************************
20 20 // spectral matrices rings
21 21 ring_node sm_ring_f0[ NB_RING_NODES_SM_F0 ];
22 22 ring_node sm_ring_f1[ NB_RING_NODES_SM_F1 ];
23 23 ring_node sm_ring_f2[ NB_RING_NODES_SM_F2 ];
24 24 ring_node *current_ring_node_sm_f0;
25 25 ring_node *current_ring_node_sm_f1;
26 26 ring_node *current_ring_node_sm_f2;
27 27 ring_node *ring_node_for_averaging_sm_f0;
28 28 ring_node *ring_node_for_averaging_sm_f1;
29 29 ring_node *ring_node_for_averaging_sm_f2;
30 30
31 31 //
32 32 ring_node * getRingNodeForAveraging( unsigned char frequencyChannel)
33 33 {
34 34 ring_node *node;
35 35
36 36 node = NULL;
37 37 switch ( frequencyChannel ) {
38 38 case 0:
39 39 node = ring_node_for_averaging_sm_f0;
40 40 break;
41 41 case 1:
42 42 node = ring_node_for_averaging_sm_f1;
43 43 break;
44 44 case 2:
45 45 node = ring_node_for_averaging_sm_f2;
46 46 break;
47 47 default:
48 48 break;
49 49 }
50 50
51 51 return node;
52 52 }
53 53
54 54 //***********************************************************
55 55 // Interrupt Service Routine for spectral matrices processing
56 56
57 57 void spectral_matrices_isr_f0( unsigned char statusReg )
58 58 {
59 59 unsigned char status;
60 60 rtems_status_code status_code;
61 61 ring_node *full_ring_node;
62 62
63 63 status = statusReg & 0x03; // [0011] get the status_ready_matrix_f0_x bits
64 64
65 65 switch(status)
66 66 {
67 67 case 0:
68 68 break;
69 69 case 3:
70 70 // UNEXPECTED VALUE
71 71 spectral_matrix_regs->status = 0x03; // [0011]
72 72 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
73 73 break;
74 74 case 1:
75 75 full_ring_node = current_ring_node_sm_f0->previous;
76 76 full_ring_node->coarseTime = spectral_matrix_regs->f0_0_coarse_time;
77 77 full_ring_node->fineTime = spectral_matrix_regs->f0_0_fine_time;
78 78 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
79 79 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->buffer_address;
80 80 // if there are enough ring nodes ready, wake up an AVFx task
81 81 nb_sm_f0 = nb_sm_f0 + 1;
82 82 if (nb_sm_f0 == NB_SM_BEFORE_AVF0)
83 83 {
84 84 ring_node_for_averaging_sm_f0 = full_ring_node;
85 85 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
86 86 {
87 87 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
88 88 }
89 89 nb_sm_f0 = 0;
90 90 }
91 91 spectral_matrix_regs->status = 0x01; // [0000 0001]
92 92 break;
93 93 case 2:
94 94 full_ring_node = current_ring_node_sm_f0->previous;
95 95 full_ring_node->coarseTime = spectral_matrix_regs->f0_1_coarse_time;
96 96 full_ring_node->fineTime = spectral_matrix_regs->f0_1_fine_time;
97 97 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
98 98 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
99 99 // if there are enough ring nodes ready, wake up an AVFx task
100 100 nb_sm_f0 = nb_sm_f0 + 1;
101 101 if (nb_sm_f0 == NB_SM_BEFORE_AVF0)
102 102 {
103 103 ring_node_for_averaging_sm_f0 = full_ring_node;
104 104 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
105 105 {
106 106 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
107 107 }
108 108 nb_sm_f0 = 0;
109 109 }
110 110 spectral_matrix_regs->status = 0x02; // [0000 0010]
111 111 break;
112 112 }
113 113 }
114 114
115 115 void spectral_matrices_isr_f1( unsigned char statusReg )
116 116 {
117 117 rtems_status_code status_code;
118 118 unsigned char status;
119 119 ring_node *full_ring_node;
120 120
121 121 status = (statusReg & 0x0c) >> 2; // [1100] get the status_ready_matrix_f0_x bits
122 122
123 123 switch(status)
124 124 {
125 125 case 0:
126 126 break;
127 127 case 3:
128 128 // UNEXPECTED VALUE
129 129 spectral_matrix_regs->status = 0xc0; // [1100]
130 130 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
131 131 break;
132 132 case 1:
133 133 full_ring_node = current_ring_node_sm_f1->previous;
134 134 full_ring_node->coarseTime = spectral_matrix_regs->f1_0_coarse_time;
135 135 full_ring_node->fineTime = spectral_matrix_regs->f1_0_fine_time;
136 136 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
137 137 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->buffer_address;
138 138 // if there are enough ring nodes ready, wake up an AVFx task
139 139 nb_sm_f1 = nb_sm_f1 + 1;
140 140 if (nb_sm_f1 == NB_SM_BEFORE_AVF1)
141 141 {
142 142 ring_node_for_averaging_sm_f1 = full_ring_node;
143 143 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
144 144 {
145 145 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
146 146 }
147 147 nb_sm_f1 = 0;
148 148 }
149 149 spectral_matrix_regs->status = 0x04; // [0000 0100]
150 150 break;
151 151 case 2:
152 152 full_ring_node = current_ring_node_sm_f1->previous;
153 153 full_ring_node->coarseTime = spectral_matrix_regs->f1_1_coarse_time;
154 154 full_ring_node->fineTime = spectral_matrix_regs->f1_1_fine_time;
155 155 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
156 156 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
157 157 // if there are enough ring nodes ready, wake up an AVFx task
158 158 nb_sm_f1 = nb_sm_f1 + 1;
159 159 if (nb_sm_f1 == NB_SM_BEFORE_AVF1)
160 160 {
161 161 ring_node_for_averaging_sm_f1 = full_ring_node;
162 162 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
163 163 {
164 164 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
165 165 }
166 166 nb_sm_f1 = 0;
167 167 }
168 168 spectral_matrix_regs->status = 0x08; // [1000 0000]
169 169 break;
170 170 }
171 171 }
172 172
173 173 void spectral_matrices_isr_f2( unsigned char statusReg )
174 174 {
175 175 unsigned char status;
176 176 rtems_status_code status_code;
177 177
178 178 status = (statusReg & 0x30) >> 4; // [0011 0000] get the status_ready_matrix_f0_x bits
179 179
180 180 switch(status)
181 181 {
182 182 case 0:
183 183 break;
184 184 case 3:
185 185 // UNEXPECTED VALUE
186 186 spectral_matrix_regs->status = 0x30; // [0011 0000]
187 187 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
188 188 break;
189 189 case 1:
190 190 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
191 191 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
192 192 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_0_coarse_time;
193 193 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_0_fine_time;
194 194 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->buffer_address;
195 195 spectral_matrix_regs->status = 0x10; // [0001 0000]
196 196 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
197 197 {
198 198 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
199 199 }
200 200 break;
201 201 case 2:
202 202 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
203 203 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
204 204 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_1_coarse_time;
205 205 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_1_fine_time;
206 206 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
207 207 spectral_matrix_regs->status = 0x20; // [0010 0000]
208 208 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
209 209 {
210 210 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
211 211 }
212 212 break;
213 213 }
214 214 }
215 215
216 216 void spectral_matrix_isr_error_handler( unsigned char statusReg )
217 217 {
218 218 rtems_status_code status_code;
219 219
220 220 if (statusReg & 0x7c0) // [0111 1100 0000]
221 221 {
222 222 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_8 );
223 223 }
224 224
225 225 spectral_matrix_regs->status = spectral_matrix_regs->status & 0x7c0;
226 226 }
227 227
228 228 rtems_isr spectral_matrices_isr( rtems_vector_number vector )
229 229 {
230 230 // STATUS REGISTER
231 231 // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
232 232 // 10 9 8
233 233 // buffer_full ** bad_component_err ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
234 234 // 7 6 5 4 3 2 1 0
235 235
236 236 unsigned char statusReg;
237 237
238 238 statusReg = spectral_matrix_regs->status;
239 239
240 240 spectral_matrices_isr_f0( statusReg );
241 241
242 242 spectral_matrices_isr_f1( statusReg );
243 243
244 244 spectral_matrices_isr_f2( statusReg );
245 245
246 246 spectral_matrix_isr_error_handler( statusReg );
247 247 }
248 248
249 249 //******************
250 250 // Spectral Matrices
251 251
252 252 void reset_nb_sm( void )
253 253 {
254 254 nb_sm_f0 = 0;
255 255 nb_sm_f0_aux_f1 = 0;
256 256 nb_sm_f0_aux_f2 = 0;
257 257
258 258 nb_sm_f1 = 0;
259 259 }
260 260
261 261 void SM_init_rings( void )
262 262 {
263 263 init_ring( sm_ring_f0, NB_RING_NODES_SM_F0, sm_f0, TOTAL_SIZE_SM );
264 264 init_ring( sm_ring_f1, NB_RING_NODES_SM_F1, sm_f1, TOTAL_SIZE_SM );
265 265 init_ring( sm_ring_f2, NB_RING_NODES_SM_F2, sm_f2, TOTAL_SIZE_SM );
266 266
267 267 DEBUG_PRINTF1("sm_ring_f0 @%x\n", (unsigned int) sm_ring_f0)
268 268 DEBUG_PRINTF1("sm_ring_f1 @%x\n", (unsigned int) sm_ring_f1)
269 269 DEBUG_PRINTF1("sm_ring_f2 @%x\n", (unsigned int) sm_ring_f2)
270 270 DEBUG_PRINTF1("sm_f0 @%x\n", (unsigned int) sm_f0)
271 271 DEBUG_PRINTF1("sm_f1 @%x\n", (unsigned int) sm_f1)
272 272 DEBUG_PRINTF1("sm_f2 @%x\n", (unsigned int) sm_f2)
273 273 }
274 274
275 275 void ASM_generic_init_ring( ring_node_asm *ring, unsigned char nbNodes )
276 276 {
277 277 unsigned char i;
278 278
279 279 ring[ nbNodes - 1 ].next
280 280 = (ring_node_asm*) &ring[ 0 ];
281 281
282 282 for(i=0; i<nbNodes-1; i++)
283 283 {
284 284 ring[ i ].next = (ring_node_asm*) &ring[ i + 1 ];
285 285 }
286 286 }
287 287
288 288 void SM_reset_current_ring_nodes( void )
289 289 {
290 290 current_ring_node_sm_f0 = sm_ring_f0[0].next;
291 291 current_ring_node_sm_f1 = sm_ring_f1[0].next;
292 292 current_ring_node_sm_f2 = sm_ring_f2[0].next;
293 293
294 294 ring_node_for_averaging_sm_f0 = NULL;
295 295 ring_node_for_averaging_sm_f1 = NULL;
296 296 ring_node_for_averaging_sm_f2 = NULL;
297 297 }
298 298
299 299 //*****************
300 300 // Basic Parameters
301 301
302 302 void BP_init_header( bp_packet *packet,
303 303 unsigned int apid, unsigned char sid,
304 304 unsigned int packetLength, unsigned char blkNr )
305 305 {
306 306 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
307 307 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
308 308 packet->reserved = 0x00;
309 309 packet->userApplication = CCSDS_USER_APP;
310 310 packet->packetID[0] = (unsigned char) (apid >> 8);
311 311 packet->packetID[1] = (unsigned char) (apid);
312 312 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
313 313 packet->packetSequenceControl[1] = 0x00;
314 314 packet->packetLength[0] = (unsigned char) (packetLength >> 8);
315 315 packet->packetLength[1] = (unsigned char) (packetLength);
316 316 // DATA FIELD HEADER
317 317 packet->spare1_pusVersion_spare2 = 0x10;
318 318 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
319 319 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
320 320 packet->destinationID = TM_DESTINATION_ID_GROUND;
321 321 packet->time[0] = 0x00;
322 322 packet->time[1] = 0x00;
323 323 packet->time[2] = 0x00;
324 324 packet->time[3] = 0x00;
325 325 packet->time[4] = 0x00;
326 326 packet->time[5] = 0x00;
327 327 // AUXILIARY DATA HEADER
328 328 packet->sid = sid;
329 329 packet->biaStatusInfo = 0x00;
330 330 packet->sy_lfr_common_parameters_spare = 0x00;
331 331 packet->sy_lfr_common_parameters = 0x00;
332 332 packet->acquisitionTime[0] = 0x00;
333 333 packet->acquisitionTime[1] = 0x00;
334 334 packet->acquisitionTime[2] = 0x00;
335 335 packet->acquisitionTime[3] = 0x00;
336 336 packet->acquisitionTime[4] = 0x00;
337 337 packet->acquisitionTime[5] = 0x00;
338 338 packet->pa_lfr_bp_blk_nr[0] = 0x00; // BLK_NR MSB
339 339 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
340 340 }
341 341
342 342 void BP_init_header_with_spare( bp_packet_with_spare *packet,
343 343 unsigned int apid, unsigned char sid,
344 344 unsigned int packetLength , unsigned char blkNr)
345 345 {
346 346 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
347 347 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
348 348 packet->reserved = 0x00;
349 349 packet->userApplication = CCSDS_USER_APP;
350 350 packet->packetID[0] = (unsigned char) (apid >> 8);
351 351 packet->packetID[1] = (unsigned char) (apid);
352 352 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
353 353 packet->packetSequenceControl[1] = 0x00;
354 354 packet->packetLength[0] = (unsigned char) (packetLength >> 8);
355 355 packet->packetLength[1] = (unsigned char) (packetLength);
356 356 // DATA FIELD HEADER
357 357 packet->spare1_pusVersion_spare2 = 0x10;
358 358 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
359 359 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
360 360 packet->destinationID = TM_DESTINATION_ID_GROUND;
361 361 // AUXILIARY DATA HEADER
362 362 packet->sid = sid;
363 363 packet->biaStatusInfo = 0x00;
364 364 packet->sy_lfr_common_parameters_spare = 0x00;
365 365 packet->sy_lfr_common_parameters = 0x00;
366 366 packet->time[0] = 0x00;
367 367 packet->time[0] = 0x00;
368 368 packet->time[0] = 0x00;
369 369 packet->time[0] = 0x00;
370 370 packet->time[0] = 0x00;
371 371 packet->time[0] = 0x00;
372 372 packet->source_data_spare = 0x00;
373 373 packet->pa_lfr_bp_blk_nr[0] = 0x00; // BLK_NR MSB
374 374 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
375 375 }
376 376
377 377 void BP_send(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
378 378 {
379 379 rtems_status_code status;
380 380
381 381 // SEND PACKET
382 382 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
383 383 if (status != RTEMS_SUCCESSFUL)
384 384 {
385 385 PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
386 386 }
387 387 }
388 388
389 void BP_send_s1_s2(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
390 {
391 /** This function is used to send the BP paquets when needed.
392 *
393 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
394 *
395 * @return void
396 *
397 * SBM1 and SBM2 paquets are sent depending on the type of the LFR mode transition.
398 * BURST paquets are sent everytime.
399 *
400 */
401
402 rtems_status_code status;
403
404 // SEND PACKET
405 // before lastValidTransitionDate, the data are drops even if they are ready
406 // this guarantees that no SBM packets will be received before the requestion enter mode time
407 if ( time_management_regs->coarse_time >= lastValidEnterModeTime)
408 {
409 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
410 if (status != RTEMS_SUCCESSFUL)
411 {
412 PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
413 }
414 }
415 }
416
389 417 //******************
390 418 // general functions
391 419
392 420 void reset_sm_status( void )
393 421 {
394 422 // error
395 423 // 10 --------------- 9 ---------------- 8 ---------------- 7 ---------
396 424 // input_fif0_write_2 input_fifo_write_1 input_fifo_write_0 buffer_full
397 425 // ---------- 5 -- 4 -- 3 -- 2 -- 1 -- 0 --
398 426 // ready bits f2_1 f2_0 f1_1 f1_1 f0_1 f0_0
399 427
400 428 spectral_matrix_regs->status = 0x7ff; // [0111 1111 1111]
401 429 }
402 430
403 431 void reset_spectral_matrix_regs( void )
404 432 {
405 433 /** This function resets the spectral matrices module registers.
406 434 *
407 435 * The registers affected by this function are located at the following offset addresses:
408 436 *
409 437 * - 0x00 config
410 438 * - 0x04 status
411 439 * - 0x08 matrixF0_Address0
412 440 * - 0x10 matrixFO_Address1
413 441 * - 0x14 matrixF1_Address
414 442 * - 0x18 matrixF2_Address
415 443 *
416 444 */
417 445
418 446 set_sm_irq_onError( 0 );
419 447
420 448 set_sm_irq_onNewMatrix( 0 );
421 449
422 450 reset_sm_status();
423 451
424 452 // F1
425 453 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->previous->buffer_address;
426 454 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
427 455 // F2
428 456 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->previous->buffer_address;
429 457 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
430 458 // F3
431 459 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->previous->buffer_address;
432 460 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
433 461
434 462 spectral_matrix_regs->matrix_length = 0xc8; // 25 * 128 / 16 = 200 = 0xc8
435 463 }
436 464
437 465 void set_time( unsigned char *time, unsigned char * timeInBuffer )
438 466 {
439 467 time[0] = timeInBuffer[0];
440 468 time[1] = timeInBuffer[1];
441 469 time[2] = timeInBuffer[2];
442 470 time[3] = timeInBuffer[3];
443 471 time[4] = timeInBuffer[6];
444 472 time[5] = timeInBuffer[7];
445 473 }
446 474
447 475 unsigned long long int get_acquisition_time( unsigned char *timePtr )
448 476 {
449 477 unsigned long long int acquisitionTimeAslong;
450 478 acquisitionTimeAslong = 0x00;
451 479 acquisitionTimeAslong = ( (unsigned long long int) (timePtr[0] & 0x7f) << 40 ) // [0111 1111] mask the synchronization bit
452 480 + ( (unsigned long long int) timePtr[1] << 32 )
453 481 + ( (unsigned long long int) timePtr[2] << 24 )
454 482 + ( (unsigned long long int) timePtr[3] << 16 )
455 483 + ( (unsigned long long int) timePtr[6] << 8 )
456 484 + ( (unsigned long long int) timePtr[7] );
457 485 return acquisitionTimeAslong;
458 486 }
459 487
460 488 unsigned char getSID( rtems_event_set event )
461 489 {
462 490 unsigned char sid;
463 491
464 492 rtems_event_set eventSetBURST;
465 493 rtems_event_set eventSetSBM;
466 494
467 495 //******
468 496 // BURST
469 497 eventSetBURST = RTEMS_EVENT_BURST_BP1_F0
470 498 | RTEMS_EVENT_BURST_BP1_F1
471 499 | RTEMS_EVENT_BURST_BP2_F0
472 500 | RTEMS_EVENT_BURST_BP2_F1;
473 501
474 502 //****
475 503 // SBM
476 504 eventSetSBM = RTEMS_EVENT_SBM_BP1_F0
477 505 | RTEMS_EVENT_SBM_BP1_F1
478 506 | RTEMS_EVENT_SBM_BP2_F0
479 507 | RTEMS_EVENT_SBM_BP2_F1;
480 508
481 509 if (event & eventSetBURST)
482 510 {
483 511 sid = SID_BURST_BP1_F0;
484 512 }
485 513 else if (event & eventSetSBM)
486 514 {
487 515 sid = SID_SBM1_BP1_F0;
488 516 }
489 517 else
490 518 {
491 519 sid = 0;
492 520 }
493 521
494 522 return sid;
495 523 }
496 524
497 525 void extractReImVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
498 526 {
499 527 unsigned int i;
500 528 float re;
501 529 float im;
502 530
503 531 for (i=0; i<NB_BINS_PER_SM; i++){
504 532 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i * 2 ];
505 533 im = inputASM[ (asmComponent*NB_BINS_PER_SM) + i * 2 + 1];
506 534 outputASM[ (asmComponent *NB_BINS_PER_SM) + i] = re;
507 535 outputASM[ (asmComponent+1)*NB_BINS_PER_SM + i] = im;
508 536 }
509 537 }
510 538
511 539 void copyReVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
512 540 {
513 541 unsigned int i;
514 542 float re;
515 543
516 544 for (i=0; i<NB_BINS_PER_SM; i++){
517 545 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i];
518 546 outputASM[ (asmComponent*NB_BINS_PER_SM) + i] = re;
519 547 }
520 548 }
521 549
522 550 void ASM_patch( float *inputASM, float *outputASM )
523 551 {
524 552 extractReImVectors( inputASM, outputASM, 1); // b1b2
525 553 extractReImVectors( inputASM, outputASM, 3 ); // b1b3
526 554 extractReImVectors( inputASM, outputASM, 5 ); // b1e1
527 555 extractReImVectors( inputASM, outputASM, 7 ); // b1e2
528 556 extractReImVectors( inputASM, outputASM, 10 ); // b2b3
529 557 extractReImVectors( inputASM, outputASM, 12 ); // b2e1
530 558 extractReImVectors( inputASM, outputASM, 14 ); // b2e2
531 559 extractReImVectors( inputASM, outputASM, 17 ); // b3e1
532 560 extractReImVectors( inputASM, outputASM, 19 ); // b3e2
533 561 extractReImVectors( inputASM, outputASM, 22 ); // e1e2
534 562
535 563 copyReVectors(inputASM, outputASM, 0 ); // b1b1
536 564 copyReVectors(inputASM, outputASM, 9 ); // b2b2
537 565 copyReVectors(inputASM, outputASM, 16); // b3b3
538 566 copyReVectors(inputASM, outputASM, 21); // e1e1
539 567 copyReVectors(inputASM, outputASM, 24); // e2e2
540 568 }
541 569
542 570 void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
543 571 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage,
544 572 unsigned char ASMIndexStart,
545 573 unsigned char channel )
546 574 {
547 575 //*************
548 576 // input format
549 577 // component0[0 .. 127] component1[0 .. 127] .. component24[0 .. 127]
550 578 //**************
551 579 // output format
552 580 // matr0[0 .. 24] matr1[0 .. 24] .. matr127[0 .. 24]
553 581 //************
554 582 // compression
555 583 // matr0[0 .. 24] matr1[0 .. 24] .. matr11[0 .. 24] => f0 NORM
556 584 // matr0[0 .. 24] matr1[0 .. 24] .. matr22[0 .. 24] => f0 BURST, SBM
557 585
558 586 int frequencyBin;
559 587 int asmComponent;
560 588 int offsetASM;
561 589 int offsetCompressed;
562 590 int offsetFBin;
563 591 int fBinMask;
564 592 int k;
565 593
566 594 // BUILD DATA
567 595 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
568 596 {
569 597 for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
570 598 {
571 599 offsetCompressed = // NO TIME OFFSET
572 600 frequencyBin * NB_VALUES_PER_SM
573 601 + asmComponent;
574 602 offsetASM = // NO TIME OFFSET
575 603 asmComponent * NB_BINS_PER_SM
576 604 + ASMIndexStart
577 605 + frequencyBin * nbBinsToAverage;
578 606 offsetFBin = ASMIndexStart
579 607 + frequencyBin * nbBinsToAverage;
580 608 compressed_spec_mat[ offsetCompressed ] = 0;
581 609 for ( k = 0; k < nbBinsToAverage; k++ )
582 610 {
583 611 fBinMask = getFBinMask( offsetFBin + k, channel );
584 612 compressed_spec_mat[offsetCompressed ] =
585 613 ( compressed_spec_mat[ offsetCompressed ]
586 614 + averaged_spec_mat[ offsetASM + k ] * fBinMask );
587 615 }
588 616 compressed_spec_mat[ offsetCompressed ] =
589 617 compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
590 618 }
591 619 }
592 620
593 621 }
594 622
595 623 int getFBinMask( int index, unsigned char channel )
596 624 {
597 625 unsigned int indexInChar;
598 626 unsigned int indexInTheChar;
599 627 int fbin;
600 628 unsigned char *sy_lfr_fbins_fx_word1;
601 629
602 630 sy_lfr_fbins_fx_word1 = parameter_dump_packet.sy_lfr_fbins_f0_word1;
603 631
604 632 switch(channel)
605 633 {
606 634 case 0:
607 635 sy_lfr_fbins_fx_word1 = parameter_dump_packet.sy_lfr_fbins_f0_word1;
608 636 break;
609 637 case 1:
610 638 sy_lfr_fbins_fx_word1 = parameter_dump_packet.sy_lfr_fbins_f1_word1;
611 639 break;
612 640 case 2:
613 641 sy_lfr_fbins_fx_word1 = parameter_dump_packet.sy_lfr_fbins_f2_word1;
614 642 break;
615 643 default:
616 644 PRINTF("ERR *** in getFBinMask, wrong frequency channel")
617 645 }
618 646
619 647 indexInChar = index >> 3;
620 648 indexInTheChar = index - indexInChar * 8;
621 649
622 650 fbin = (int) ((sy_lfr_fbins_fx_word1[ NB_BYTES_PER_FREQ_MASK - 1 - indexInChar] >> indexInTheChar) & 0x1);
623 651
624 652 return fbin;
625 653 }
626 654
627 655 void init_kcoeff_sbm_from_kcoeff_norm(float *input_kcoeff, float *output_kcoeff, unsigned char nb_bins_norm)
628 656 {
629 657 unsigned char bin;
630 658 unsigned char kcoeff;
631 659
632 660 for (bin=0; bin<nb_bins_norm; bin++)
633 661 {
634 662 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
635 663 {
636 664 output_kcoeff[ (bin*NB_K_COEFF_PER_BIN + kcoeff)*2 ] = input_kcoeff[ bin*NB_K_COEFF_PER_BIN + kcoeff ];
637 665 output_kcoeff[ (bin*NB_K_COEFF_PER_BIN + kcoeff)*2 + 1 ] = input_kcoeff[ bin*NB_K_COEFF_PER_BIN + kcoeff ];
638 666 }
639 667 }
640 668 }
Show file before | Show file after | Hide comments
@@ -1,1604 +1,1618
1 1 /** Functions and tasks related to TeleCommand handling.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle TeleCommands:\n
7 7 * action launching\n
8 8 * TC parsing\n
9 9 * ...
10 10 *
11 11 */
12 12
13 13 #include "tc_handler.h"
14 14 #include "math.h"
15 15
16 16 //***********
17 17 // RTEMS TASK
18 18
19 19 rtems_task actn_task( rtems_task_argument unused )
20 20 {
21 21 /** This RTEMS task is responsible for launching actions upton the reception of valid TeleCommands.
22 22 *
23 23 * @param unused is the starting argument of the RTEMS task
24 24 *
25 25 * The ACTN task waits for data coming from an RTEMS msesage queue. When data arrives, it launches specific actions depending
26 26 * on the incoming TeleCommand.
27 27 *
28 28 */
29 29
30 30 int result;
31 31 rtems_status_code status; // RTEMS status code
32 32 ccsdsTelecommandPacket_t TC; // TC sent to the ACTN task
33 33 size_t size; // size of the incoming TC packet
34 34 unsigned char subtype; // subtype of the current TC packet
35 35 unsigned char time[6];
36 36 rtems_id queue_rcv_id;
37 37 rtems_id queue_snd_id;
38 38
39 39 status = get_message_queue_id_recv( &queue_rcv_id );
40 40 if (status != RTEMS_SUCCESSFUL)
41 41 {
42 42 PRINTF1("in ACTN *** ERR get_message_queue_id_recv %d\n", status)
43 43 }
44 44
45 45 status = get_message_queue_id_send( &queue_snd_id );
46 46 if (status != RTEMS_SUCCESSFUL)
47 47 {
48 48 PRINTF1("in ACTN *** ERR get_message_queue_id_send %d\n", status)
49 49 }
50 50
51 51 result = LFR_SUCCESSFUL;
52 52 subtype = 0; // subtype of the current TC packet
53 53
54 54 BOOT_PRINTF("in ACTN *** \n")
55 55
56 56 while(1)
57 57 {
58 58 status = rtems_message_queue_receive( queue_rcv_id, (char*) &TC, &size,
59 59 RTEMS_WAIT, RTEMS_NO_TIMEOUT);
60 60 getTime( time ); // set time to the current time
61 61 if (status!=RTEMS_SUCCESSFUL)
62 62 {
63 63 PRINTF1("ERR *** in task ACTN *** error receiving a message, code %d \n", status)
64 64 }
65 65 else
66 66 {
67 67 subtype = TC.serviceSubType;
68 68 switch(subtype)
69 69 {
70 70 case TC_SUBTYPE_RESET:
71 71 result = action_reset( &TC, queue_snd_id, time );
72 72 close_action( &TC, result, queue_snd_id );
73 73 break;
74 74 case TC_SUBTYPE_LOAD_COMM:
75 75 result = action_load_common_par( &TC );
76 76 close_action( &TC, result, queue_snd_id );
77 77 break;
78 78 case TC_SUBTYPE_LOAD_NORM:
79 79 result = action_load_normal_par( &TC, queue_snd_id, time );
80 80 close_action( &TC, result, queue_snd_id );
81 81 break;
82 82 case TC_SUBTYPE_LOAD_BURST:
83 83 result = action_load_burst_par( &TC, queue_snd_id, time );
84 84 close_action( &TC, result, queue_snd_id );
85 85 break;
86 86 case TC_SUBTYPE_LOAD_SBM1:
87 87 result = action_load_sbm1_par( &TC, queue_snd_id, time );
88 88 close_action( &TC, result, queue_snd_id );
89 89 break;
90 90 case TC_SUBTYPE_LOAD_SBM2:
91 91 result = action_load_sbm2_par( &TC, queue_snd_id, time );
92 92 close_action( &TC, result, queue_snd_id );
93 93 break;
94 94 case TC_SUBTYPE_DUMP:
95 95 result = action_dump_par( &TC, queue_snd_id );
96 96 close_action( &TC, result, queue_snd_id );
97 97 break;
98 98 case TC_SUBTYPE_ENTER:
99 99 result = action_enter_mode( &TC, queue_snd_id );
100 100 close_action( &TC, result, queue_snd_id );
101 101 break;
102 102 case TC_SUBTYPE_UPDT_INFO:
103 103 result = action_update_info( &TC, queue_snd_id );
104 104 close_action( &TC, result, queue_snd_id );
105 105 break;
106 106 case TC_SUBTYPE_EN_CAL:
107 107 result = action_enable_calibration( &TC, queue_snd_id, time );
108 108 close_action( &TC, result, queue_snd_id );
109 109 break;
110 110 case TC_SUBTYPE_DIS_CAL:
111 111 result = action_disable_calibration( &TC, queue_snd_id, time );
112 112 close_action( &TC, result, queue_snd_id );
113 113 break;
114 114 case TC_SUBTYPE_LOAD_K:
115 115 result = action_load_kcoefficients( &TC, queue_snd_id, time );
116 116 close_action( &TC, result, queue_snd_id );
117 117 break;
118 118 case TC_SUBTYPE_DUMP_K:
119 119 result = action_dump_kcoefficients( &TC, queue_snd_id, time );
120 120 close_action( &TC, result, queue_snd_id );
121 121 break;
122 122 case TC_SUBTYPE_LOAD_FBINS:
123 123 result = action_load_fbins_mask( &TC, queue_snd_id, time );
124 124 close_action( &TC, result, queue_snd_id );
125 125 break;
126 126 case TC_SUBTYPE_UPDT_TIME:
127 127 result = action_update_time( &TC );
128 128 close_action( &TC, result, queue_snd_id );
129 129 break;
130 130 default:
131 131 break;
132 132 }
133 133 }
134 134 }
135 135 }
136 136
137 137 //***********
138 138 // TC ACTIONS
139 139
140 140 int action_reset(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
141 141 {
142 142 /** This function executes specific actions when a TC_LFR_RESET TeleCommand has been received.
143 143 *
144 144 * @param TC points to the TeleCommand packet that is being processed
145 145 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
146 146 *
147 147 */
148 148
149 149 PRINTF("this is the end!!!\n")
150 150 exit(0);
151 151 send_tm_lfr_tc_exe_not_implemented( TC, queue_id, time );
152 152 return LFR_DEFAULT;
153 153 }
154 154
155 155 int action_enter_mode(ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
156 156 {
157 157 /** This function executes specific actions when a TC_LFR_ENTER_MODE TeleCommand has been received.
158 158 *
159 159 * @param TC points to the TeleCommand packet that is being processed
160 160 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
161 161 *
162 162 */
163 163
164 164 rtems_status_code status;
165 165 unsigned char requestedMode;
166 166 unsigned int *transitionCoarseTime_ptr;
167 167 unsigned int transitionCoarseTime;
168 168 unsigned char * bytePosPtr;
169 169
170 170 bytePosPtr = (unsigned char *) &TC->packetID;
171 171
172 172 requestedMode = bytePosPtr[ BYTE_POS_CP_MODE_LFR_SET ];
173 173 transitionCoarseTime_ptr = (unsigned int *) ( &bytePosPtr[ BYTE_POS_CP_LFR_ENTER_MODE_TIME ] );
174 174 transitionCoarseTime = (*transitionCoarseTime_ptr) & 0x7fffffff;
175 175
176 176 status = check_mode_value( requestedMode );
177 177
178 178 if ( status != LFR_SUCCESSFUL ) // the mode value is inconsistent
179 179 {
180 180 send_tm_lfr_tc_exe_inconsistent( TC, queue_id, BYTE_POS_CP_MODE_LFR_SET, requestedMode );
181 181 }
182 182
183 183 else // the mode value is valid, check the transition
184 184 {
185 185 status = check_mode_transition(requestedMode);
186 186 if (status != LFR_SUCCESSFUL)
187 187 {
188 188 PRINTF("ERR *** in action_enter_mode *** check_mode_transition\n")
189 189 send_tm_lfr_tc_exe_not_executable( TC, queue_id );
190 190 }
191 191 }
192 192
193 193 if ( status == LFR_SUCCESSFUL ) // the transition is valid, check the date
194 194 {
195 195 status = check_transition_date( transitionCoarseTime );
196 196 if (status != LFR_SUCCESSFUL)
197 197 {
198 198 PRINTF("ERR *** in action_enter_mode *** check_transition_date\n")
199 199 send_tm_lfr_tc_exe_inconsistent( TC, queue_id,
200 200 BYTE_POS_CP_LFR_ENTER_MODE_TIME,
201 201 bytePosPtr[ BYTE_POS_CP_LFR_ENTER_MODE_TIME + 3 ] );
202 202 }
203 203 }
204 204
205 205 if ( status == LFR_SUCCESSFUL ) // the date is valid, enter the mode
206 206 {
207 207 PRINTF1("OK *** in action_enter_mode *** enter mode %d\n", requestedMode);
208 208
209
209 update_last_valid_transition_date( transitionCoarseTime );
210 210
211 211 switch(requestedMode)
212 212 {
213 213 case LFR_MODE_STANDBY:
214 214 status = enter_mode_standby();
215 215 break;
216 216 case LFR_MODE_NORMAL:
217 217 status = enter_mode_normal( transitionCoarseTime );
218 218 break;
219 219 case LFR_MODE_BURST:
220 220 status = enter_mode_burst( transitionCoarseTime );
221 221 break;
222 222 case LFR_MODE_SBM1:
223 223 status = enter_mode_sbm1( transitionCoarseTime );
224 224 break;
225 225 case LFR_MODE_SBM2:
226 226 status = enter_mode_sbm2( transitionCoarseTime );
227 227 break;
228 228 default:
229 229 break;
230 230 }
231 231 }
232 232
233 233 return status;
234 234 }
235 235
236 236 int action_update_info(ccsdsTelecommandPacket_t *TC, rtems_id queue_id)
237 237 {
238 238 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
239 239 *
240 240 * @param TC points to the TeleCommand packet that is being processed
241 241 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
242 242 *
243 243 * @return LFR directive status code:
244 244 * - LFR_DEFAULT
245 245 * - LFR_SUCCESSFUL
246 246 *
247 247 */
248 248
249 249 unsigned int val;
250 250 int result;
251 251 unsigned int status;
252 252 unsigned char mode;
253 253 unsigned char * bytePosPtr;
254 254
255 255 bytePosPtr = (unsigned char *) &TC->packetID;
256 256
257 257 // check LFR mode
258 258 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET5 ] & 0x1e) >> 1;
259 259 status = check_update_info_hk_lfr_mode( mode );
260 260 if (status == LFR_SUCCESSFUL) // check TDS mode
261 261 {
262 262 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET6 ] & 0xf0) >> 4;
263 263 status = check_update_info_hk_tds_mode( mode );
264 264 }
265 265 if (status == LFR_SUCCESSFUL) // check THR mode
266 266 {
267 267 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET6 ] & 0x0f);
268 268 status = check_update_info_hk_thr_mode( mode );
269 269 }
270 270 if (status == LFR_SUCCESSFUL) // if the parameter check is successful
271 271 {
272 272 val = housekeeping_packet.hk_lfr_update_info_tc_cnt[0] * 256
273 273 + housekeeping_packet.hk_lfr_update_info_tc_cnt[1];
274 274 val++;
275 275 housekeeping_packet.hk_lfr_update_info_tc_cnt[0] = (unsigned char) (val >> 8);
276 276 housekeeping_packet.hk_lfr_update_info_tc_cnt[1] = (unsigned char) (val);
277 277 }
278 278
279 279 // pa_bia_status_info
280 280 // => pa_bia_mode_mux_set 3 bits
281 281 // => pa_bia_mode_hv_enabled 1 bit
282 282 // => pa_bia_mode_bias1_enabled 1 bit
283 283 // => pa_bia_mode_bias2_enabled 1 bit
284 284 // => pa_bia_mode_bias3_enabled 1 bit
285 285 // => pa_bia_on_off (cp_dpu_bias_on_off)
286 286 pa_bia_status_info = bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET2 ] & 0xfe; // [1111 1110]
287 287 pa_bia_status_info = pa_bia_status_info
288 288 | (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET1 ] & 0x1);
289 289
290 290 result = status;
291 291
292 292 return result;
293 293 }
294 294
295 295 int action_enable_calibration(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
296 296 {
297 297 /** This function executes specific actions when a TC_LFR_ENABLE_CALIBRATION TeleCommand has been received.
298 298 *
299 299 * @param TC points to the TeleCommand packet that is being processed
300 300 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
301 301 *
302 302 */
303 303
304 304 int result;
305 305
306 306 result = LFR_DEFAULT;
307 307
308 308 setCalibration( true );
309 309
310 310 result = LFR_SUCCESSFUL;
311 311
312 312 return result;
313 313 }
314 314
315 315 int action_disable_calibration(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
316 316 {
317 317 /** This function executes specific actions when a TC_LFR_DISABLE_CALIBRATION TeleCommand has been received.
318 318 *
319 319 * @param TC points to the TeleCommand packet that is being processed
320 320 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
321 321 *
322 322 */
323 323
324 324 int result;
325 325
326 326 result = LFR_DEFAULT;
327 327
328 328 setCalibration( false );
329 329
330 330 result = LFR_SUCCESSFUL;
331 331
332 332 return result;
333 333 }
334 334
335 335 int action_update_time(ccsdsTelecommandPacket_t *TC)
336 336 {
337 337 /** This function executes specific actions when a TC_LFR_UPDATE_TIME TeleCommand has been received.
338 338 *
339 339 * @param TC points to the TeleCommand packet that is being processed
340 340 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
341 341 *
342 342 * @return LFR_SUCCESSFUL
343 343 *
344 344 */
345 345
346 346 unsigned int val;
347 347
348 348 time_management_regs->coarse_time_load = (TC->dataAndCRC[0] << 24)
349 349 + (TC->dataAndCRC[1] << 16)
350 350 + (TC->dataAndCRC[2] << 8)
351 351 + TC->dataAndCRC[3];
352 352
353 353 val = housekeeping_packet.hk_lfr_update_time_tc_cnt[0] * 256
354 354 + housekeeping_packet.hk_lfr_update_time_tc_cnt[1];
355 355 val++;
356 356 housekeeping_packet.hk_lfr_update_time_tc_cnt[0] = (unsigned char) (val >> 8);
357 357 housekeeping_packet.hk_lfr_update_time_tc_cnt[1] = (unsigned char) (val);
358 358
359 359 return LFR_SUCCESSFUL;
360 360 }
361 361
362 362 //*******************
363 363 // ENTERING THE MODES
364 364 int check_mode_value( unsigned char requestedMode )
365 365 {
366 366 int status;
367 367
368 368 if ( (requestedMode != LFR_MODE_STANDBY)
369 369 && (requestedMode != LFR_MODE_NORMAL) && (requestedMode != LFR_MODE_BURST)
370 370 && (requestedMode != LFR_MODE_SBM1) && (requestedMode != LFR_MODE_SBM2) )
371 371 {
372 372 status = LFR_DEFAULT;
373 373 }
374 374 else
375 375 {
376 376 status = LFR_SUCCESSFUL;
377 377 }
378 378
379 379 return status;
380 380 }
381 381
382 382 int check_mode_transition( unsigned char requestedMode )
383 383 {
384 384 /** This function checks the validity of the transition requested by the TC_LFR_ENTER_MODE.
385 385 *
386 386 * @param requestedMode is the mode requested by the TC_LFR_ENTER_MODE
387 387 *
388 388 * @return LFR directive status codes:
389 389 * - LFR_SUCCESSFUL - the transition is authorized
390 390 * - LFR_DEFAULT - the transition is not authorized
391 391 *
392 392 */
393 393
394 394 int status;
395 395
396 396 switch (requestedMode)
397 397 {
398 398 case LFR_MODE_STANDBY:
399 399 if ( lfrCurrentMode == LFR_MODE_STANDBY ) {
400 400 status = LFR_DEFAULT;
401 401 }
402 402 else
403 403 {
404 404 status = LFR_SUCCESSFUL;
405 405 }
406 406 break;
407 407 case LFR_MODE_NORMAL:
408 408 if ( lfrCurrentMode == LFR_MODE_NORMAL ) {
409 409 status = LFR_DEFAULT;
410 410 }
411 411 else {
412 412 status = LFR_SUCCESSFUL;
413 413 }
414 414 break;
415 415 case LFR_MODE_BURST:
416 416 if ( lfrCurrentMode == LFR_MODE_BURST ) {
417 417 status = LFR_DEFAULT;
418 418 }
419 419 else {
420 420 status = LFR_SUCCESSFUL;
421 421 }
422 422 break;
423 423 case LFR_MODE_SBM1:
424 424 if ( lfrCurrentMode == LFR_MODE_SBM1 ) {
425 425 status = LFR_DEFAULT;
426 426 }
427 427 else {
428 428 status = LFR_SUCCESSFUL;
429 429 }
430 430 break;
431 431 case LFR_MODE_SBM2:
432 432 if ( lfrCurrentMode == LFR_MODE_SBM2 ) {
433 433 status = LFR_DEFAULT;
434 434 }
435 435 else {
436 436 status = LFR_SUCCESSFUL;
437 437 }
438 438 break;
439 439 default:
440 440 status = LFR_DEFAULT;
441 441 break;
442 442 }
443 443
444 444 return status;
445 445 }
446 446
447 447 void update_last_valid_transition_date(unsigned int transitionCoarseTime)
448 448 {
449 lastValidTransitionDate = transitionCoarseTime;
449 lastValidEnterModeTime = transitionCoarseTime;
450 450 }
451 451
452 452 int check_transition_date( unsigned int transitionCoarseTime )
453 453 {
454 454 int status;
455 455 unsigned int localCoarseTime;
456 456 unsigned int deltaCoarseTime;
457 457
458 458 status = LFR_SUCCESSFUL;
459 459
460 460 if (transitionCoarseTime == 0) // transition time = 0 means an instant transition
461 461 {
462 462 status = LFR_SUCCESSFUL;
463 463 }
464 464 else
465 465 {
466 466 localCoarseTime = time_management_regs->coarse_time & 0x7fffffff;
467 467
468 468 PRINTF2("localTime = %x, transitionTime = %x\n", localCoarseTime, transitionCoarseTime)
469 469
470 470 if ( transitionCoarseTime <= localCoarseTime ) // SSS-CP-EQS-322
471 471 {
472 472 status = LFR_DEFAULT;
473 473 PRINTF("ERR *** in check_transition_date *** transitionCoarseTime <= localCoarseTime\n")
474 474 }
475 475
476 476 if (status == LFR_SUCCESSFUL)
477 477 {
478 478 deltaCoarseTime = transitionCoarseTime - localCoarseTime;
479 479 if ( deltaCoarseTime > 3 ) // SSS-CP-EQS-323
480 480 {
481 481 status = LFR_DEFAULT;
482 482 PRINTF1("ERR *** in check_transition_date *** deltaCoarseTime = %x\n", deltaCoarseTime)
483 483 }
484 484 }
485 485 }
486 486
487 487 return status;
488 488 }
489 489
490 490 int restart_asm_activities( unsigned char lfrRequestedMode )
491 491 {
492 492 rtems_status_code status;
493 493
494 494 status = stop_spectral_matrices();
495 495
496 496 status = restart_asm_tasks( lfrRequestedMode );
497 497
498 498 launch_spectral_matrix();
499 499
500 500 return status;
501 501 }
502 502
503 503 int stop_spectral_matrices( void )
504 504 {
505 505 /** This function stops and restarts the current mode average spectral matrices activities.
506 506 *
507 507 * @return RTEMS directive status codes:
508 508 * - RTEMS_SUCCESSFUL - task restarted successfully
509 509 * - RTEMS_INVALID_ID - task id invalid
510 510 * - RTEMS_ALREADY_SUSPENDED - task already suspended
511 511 *
512 512 */
513 513
514 514 rtems_status_code status;
515 515
516 516 status = RTEMS_SUCCESSFUL;
517 517
518 518 // (1) mask interruptions
519 519 LEON_Mask_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
520 520
521 521 // (2) reset spectral matrices registers
522 522 set_sm_irq_onNewMatrix( 0 ); // stop the spectral matrices
523 523 reset_sm_status();
524 524
525 525 // (3) clear interruptions
526 526 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
527 527
528 528 // suspend several tasks
529 529 if (lfrCurrentMode != LFR_MODE_STANDBY) {
530 530 status = suspend_asm_tasks();
531 531 }
532 532
533 533 if (status != RTEMS_SUCCESSFUL)
534 534 {
535 535 PRINTF1("in stop_current_mode *** in suspend_science_tasks *** ERR code: %d\n", status)
536 536 }
537 537
538 538 return status;
539 539 }
540 540
541 541 int stop_current_mode( void )
542 542 {
543 543 /** This function stops the current mode by masking interrupt lines and suspending science tasks.
544 544 *
545 545 * @return RTEMS directive status codes:
546 546 * - RTEMS_SUCCESSFUL - task restarted successfully
547 547 * - RTEMS_INVALID_ID - task id invalid
548 548 * - RTEMS_ALREADY_SUSPENDED - task already suspended
549 549 *
550 550 */
551 551
552 552 rtems_status_code status;
553 553
554 554 status = RTEMS_SUCCESSFUL;
555 555
556 556 // (1) mask interruptions
557 557 LEON_Mask_interrupt( IRQ_WAVEFORM_PICKER ); // mask waveform picker interrupt
558 558 LEON_Mask_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
559 559
560 560 // (2) reset waveform picker registers
561 561 reset_wfp_burst_enable(); // reset burst and enable bits
562 562 reset_wfp_status(); // reset all the status bits
563 563
564 564 // (3) reset spectral matrices registers
565 565 set_sm_irq_onNewMatrix( 0 ); // stop the spectral matrices
566 566 reset_sm_status();
567 567
568 568 // reset lfr VHDL module
569 569 reset_lfr();
570 570
571 571 reset_extractSWF(); // reset the extractSWF flag to false
572 572
573 573 // (4) clear interruptions
574 574 LEON_Clear_interrupt( IRQ_WAVEFORM_PICKER ); // clear waveform picker interrupt
575 575 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
576 576
577 577 // suspend several tasks
578 578 if (lfrCurrentMode != LFR_MODE_STANDBY) {
579 579 status = suspend_science_tasks();
580 580 }
581 581
582 582 if (status != RTEMS_SUCCESSFUL)
583 583 {
584 584 PRINTF1("in stop_current_mode *** in suspend_science_tasks *** ERR code: %d\n", status)
585 585 }
586 586
587 587 return status;
588 588 }
589 589
590 590 int enter_mode_standby()
591 591 {
592 592 /** This function is used to put LFR in the STANDBY mode.
593 593 *
594 594 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
595 595 *
596 596 * @return RTEMS directive status codes:
597 597 * - RTEMS_SUCCESSFUL - task restarted successfully
598 598 * - RTEMS_INVALID_ID - task id invalid
599 599 * - RTEMS_INCORRECT_STATE - task never started
600 600 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
601 601 *
602 602 * The STANDBY mode does not depends on a specific transition date, the effect of the TC_LFR_ENTER_MODE
603 603 * is immediate.
604 604 *
605 605 */
606 606
607 607 int status;
608 608
609 609 status = stop_current_mode(); // STOP THE CURRENT MODE
610 lfrTransitionType = TRANSITION_NOT_SPECIFIC;
610 611
611 612 #ifdef PRINT_TASK_STATISTICS
612 613 rtems_cpu_usage_report();
613 614 #endif
614 615
615 616 #ifdef PRINT_STACK_REPORT
616 617 PRINTF("stack report selected\n")
617 618 rtems_stack_checker_report_usage();
618 619 #endif
619 620
620 621 return status;
621 622 }
622 623
623 624 int enter_mode_normal( unsigned int transitionCoarseTime )
624 625 {
625 626 /** This function is used to start the NORMAL mode.
626 627 *
627 628 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
628 629 *
629 630 * @return RTEMS directive status codes:
630 631 * - RTEMS_SUCCESSFUL - task restarted successfully
631 632 * - RTEMS_INVALID_ID - task id invalid
632 633 * - RTEMS_INCORRECT_STATE - task never started
633 634 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
634 635 *
635 636 * The way the NORMAL mode is started depends on the LFR current mode. If LFR is in SBM1 or SBM2,
636 637 * the snapshots are not restarted, only ASM, BP and CWF data generation are affected.
637 638 *
638 639 */
639 640
640 641 int status;
641 642
642 643 #ifdef PRINT_TASK_STATISTICS
643 644 rtems_cpu_usage_reset();
644 645 #endif
645 646
646 647 status = RTEMS_UNSATISFIED;
647 648
648 649 switch( lfrCurrentMode )
649 650 {
650 651 case LFR_MODE_STANDBY:
652 lfrTransitionType = TRANSITION_NOT_SPECIFIC;
651 653 status = restart_science_tasks( LFR_MODE_NORMAL ); // restart science tasks
652 654 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
653 655 {
654 656 launch_spectral_matrix( );
655 657 launch_waveform_picker( LFR_MODE_NORMAL, transitionCoarseTime );
656 658 }
657 659 break;
658 660 case LFR_MODE_BURST:
661 lfrTransitionType = TRANSITION_NOT_SPECIFIC;
659 662 status = stop_current_mode(); // stop the current mode
660 663 status = restart_science_tasks( LFR_MODE_NORMAL ); // restart the science tasks
661 664 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
662 665 {
663 666 launch_spectral_matrix( );
664 667 launch_waveform_picker( LFR_MODE_NORMAL, transitionCoarseTime );
665 668 }
666 669 break;
667 670 case LFR_MODE_SBM1:
671 lfrTransitionType = TRANSITION_S1_TO_NORM;
668 672 restart_asm_activities( LFR_MODE_NORMAL ); // this is necessary to restart ASM tasks to update the parameters
669 673 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
670 674 break;
671 675 case LFR_MODE_SBM2:
676 lfrTransitionType = TRANSITION_S2_TO_NORM;
672 677 restart_asm_activities( LFR_MODE_NORMAL ); // this is necessary to restart ASM tasks to update the parameters
673 678 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
674 679 break;
675 680 default:
676 681 break;
677 682 }
678 683
679 684 if (status != RTEMS_SUCCESSFUL)
680 685 {
681 686 PRINTF1("ERR *** in enter_mode_normal *** status = %d\n", status)
682 687 status = RTEMS_UNSATISFIED;
683 688 }
684 689
685 690 return status;
686 691 }
687 692
688 693 int enter_mode_burst( unsigned int transitionCoarseTime )
689 694 {
690 695 /** This function is used to start the BURST mode.
691 696 *
692 697 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
693 698 *
694 699 * @return RTEMS directive status codes:
695 700 * - RTEMS_SUCCESSFUL - task restarted successfully
696 701 * - RTEMS_INVALID_ID - task id invalid
697 702 * - RTEMS_INCORRECT_STATE - task never started
698 703 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
699 704 *
700 705 * The way the BURST mode is started does not depend on the LFR current mode.
701 706 *
702 707 */
703 708
704 709
705 710 int status;
706 711
707 712 #ifdef PRINT_TASK_STATISTICS
708 713 rtems_cpu_usage_reset();
709 714 #endif
710 715
716 lfrTransitionType = TRANSITION_NOT_SPECIFIC;
711 717 status = stop_current_mode(); // stop the current mode
712 718 status = restart_science_tasks( LFR_MODE_BURST ); // restart the science tasks
713 719 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
714 720 {
715 721 launch_spectral_matrix( );
716 722 launch_waveform_picker( LFR_MODE_BURST, transitionCoarseTime );
717 723 }
718 724
719 725 if (status != RTEMS_SUCCESSFUL)
720 726 {
721 727 PRINTF1("ERR *** in enter_mode_burst *** status = %d\n", status)
722 728 status = RTEMS_UNSATISFIED;
723 729 }
724 730
725 731 return status;
726 732 }
727 733
728 734 int enter_mode_sbm1( unsigned int transitionCoarseTime )
729 735 {
730 736 /** This function is used to start the SBM1 mode.
731 737 *
732 738 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
733 739 *
734 740 * @return RTEMS directive status codes:
735 741 * - RTEMS_SUCCESSFUL - task restarted successfully
736 742 * - RTEMS_INVALID_ID - task id invalid
737 743 * - RTEMS_INCORRECT_STATE - task never started
738 744 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
739 745 *
740 746 * The way the SBM1 mode is started depends on the LFR current mode. If LFR is in NORMAL or SBM2,
741 747 * the snapshots are not restarted, only ASM, BP and CWF data generation are affected. In other
742 748 * cases, the acquisition is completely restarted.
743 749 *
744 750 */
745 751
746 752 int status;
747 753
748 754 #ifdef PRINT_TASK_STATISTICS
749 755 rtems_cpu_usage_reset();
750 756 #endif
751 757
752 758 status = RTEMS_UNSATISFIED;
753 759
754 760 switch( lfrCurrentMode )
755 761 {
756 762 case LFR_MODE_STANDBY:
763 lfrTransitionType = TRANSITION_NOT_SPECIFIC;
757 764 status = restart_science_tasks( LFR_MODE_SBM1 ); // restart science tasks
758 765 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
759 766 {
760 767 launch_spectral_matrix( );
761 768 launch_waveform_picker( LFR_MODE_SBM1, transitionCoarseTime );
762 769 }
763 770 break;
764 771 case LFR_MODE_NORMAL: // lfrCurrentMode will be updated after the execution of close_action
772 lfrTransitionType = TRANSITION_NORM_TO_S1;
765 773 restart_asm_activities( LFR_MODE_SBM1 );
766 774 status = LFR_SUCCESSFUL;
767 775 break;
768 776 case LFR_MODE_BURST:
777 lfrTransitionType = TRANSITION_NOT_SPECIFIC;
769 778 status = stop_current_mode(); // stop the current mode
770 779 status = restart_science_tasks( LFR_MODE_SBM1 ); // restart the science tasks
771 780 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
772 781 {
773 782 launch_spectral_matrix( );
774 783 launch_waveform_picker( LFR_MODE_SBM1, transitionCoarseTime );
775 784 }
776 785 break;
777 786 case LFR_MODE_SBM2:
787 lfrTransitionType = TRANSITION_S2_TO_S1;
778 788 restart_asm_activities( LFR_MODE_SBM1 );
779 789 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
780 790 break;
781 791 default:
782 792 break;
783 793 }
784 794
785 795 if (status != RTEMS_SUCCESSFUL)
786 796 {
787 797 PRINTF1("ERR *** in enter_mode_sbm1 *** status = %d\n", status)
788 798 status = RTEMS_UNSATISFIED;
789 799 }
790 800
791 801 return status;
792 802 }
793 803
794 804 int enter_mode_sbm2( unsigned int transitionCoarseTime )
795 805 {
796 806 /** This function is used to start the SBM2 mode.
797 807 *
798 808 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
799 809 *
800 810 * @return RTEMS directive status codes:
801 811 * - RTEMS_SUCCESSFUL - task restarted successfully
802 812 * - RTEMS_INVALID_ID - task id invalid
803 813 * - RTEMS_INCORRECT_STATE - task never started
804 814 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
805 815 *
806 816 * The way the SBM2 mode is started depends on the LFR current mode. If LFR is in NORMAL or SBM1,
807 817 * the snapshots are not restarted, only ASM, BP and CWF data generation are affected. In other
808 818 * cases, the acquisition is completely restarted.
809 819 *
810 820 */
811 821
812 822 int status;
813 823
814 824 #ifdef PRINT_TASK_STATISTICS
815 825 rtems_cpu_usage_reset();
816 826 #endif
817 827
818 828 status = RTEMS_UNSATISFIED;
819 829
820 830 switch( lfrCurrentMode )
821 831 {
822 832 case LFR_MODE_STANDBY:
833 lfrTransitionType = TRANSITION_NOT_SPECIFIC;
823 834 status = restart_science_tasks( LFR_MODE_SBM2 ); // restart science tasks
824 835 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
825 836 {
826 837 launch_spectral_matrix( );
827 838 launch_waveform_picker( LFR_MODE_SBM2, transitionCoarseTime );
828 839 }
829 840 break;
830 841 case LFR_MODE_NORMAL:
842 lfrTransitionType = TRANSITION_NORM_TO_S2;
831 843 restart_asm_activities( LFR_MODE_SBM2 );
832 844 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
833 845 break;
834 846 case LFR_MODE_BURST:
847 lfrTransitionType = TRANSITION_NOT_SPECIFIC;
835 848 status = stop_current_mode(); // stop the current mode
836 849 status = restart_science_tasks( LFR_MODE_SBM2 ); // restart the science tasks
837 850 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
838 851 {
839 852 launch_spectral_matrix( );
840 853 launch_waveform_picker( LFR_MODE_SBM2, transitionCoarseTime );
841 854 }
842 855 break;
843 856 case LFR_MODE_SBM1:
857 lfrTransitionType = TRANSITION_S1_TO_S2;
844 858 restart_asm_activities( LFR_MODE_SBM2 );
845 859 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
846 860 break;
847 861 default:
848 862 break;
849 863 }
850 864
851 865 if (status != RTEMS_SUCCESSFUL)
852 866 {
853 867 PRINTF1("ERR *** in enter_mode_sbm2 *** status = %d\n", status)
854 868 status = RTEMS_UNSATISFIED;
855 869 }
856 870
857 871 return status;
858 872 }
859 873
860 874 int restart_science_tasks( unsigned char lfrRequestedMode )
861 875 {
862 876 /** This function is used to restart all science tasks.
863 877 *
864 878 * @return RTEMS directive status codes:
865 879 * - RTEMS_SUCCESSFUL - task restarted successfully
866 880 * - RTEMS_INVALID_ID - task id invalid
867 881 * - RTEMS_INCORRECT_STATE - task never started
868 882 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
869 883 *
870 884 * Science tasks are AVF0, PRC0, WFRM, CWF3, CW2, CWF1
871 885 *
872 886 */
873 887
874 888 rtems_status_code status[10];
875 889 rtems_status_code ret;
876 890
877 891 ret = RTEMS_SUCCESSFUL;
878 892
879 893 status[0] = rtems_task_restart( Task_id[TASKID_AVF0], lfrRequestedMode );
880 894 if (status[0] != RTEMS_SUCCESSFUL)
881 895 {
882 896 PRINTF1("in restart_science_task *** AVF0 ERR %d\n", status[0])
883 897 }
884 898
885 899 status[1] = rtems_task_restart( Task_id[TASKID_PRC0], lfrRequestedMode );
886 900 if (status[1] != RTEMS_SUCCESSFUL)
887 901 {
888 902 PRINTF1("in restart_science_task *** PRC0 ERR %d\n", status[1])
889 903 }
890 904
891 905 status[2] = rtems_task_restart( Task_id[TASKID_WFRM],1 );
892 906 if (status[2] != RTEMS_SUCCESSFUL)
893 907 {
894 908 PRINTF1("in restart_science_task *** WFRM ERR %d\n", status[2])
895 909 }
896 910
897 911 status[3] = rtems_task_restart( Task_id[TASKID_CWF3],1 );
898 912 if (status[3] != RTEMS_SUCCESSFUL)
899 913 {
900 914 PRINTF1("in restart_science_task *** CWF3 ERR %d\n", status[3])
901 915 }
902 916
903 917 status[4] = rtems_task_restart( Task_id[TASKID_CWF2],1 );
904 918 if (status[4] != RTEMS_SUCCESSFUL)
905 919 {
906 920 PRINTF1("in restart_science_task *** CWF2 ERR %d\n", status[4])
907 921 }
908 922
909 923 status[5] = rtems_task_restart( Task_id[TASKID_CWF1],1 );
910 924 if (status[5] != RTEMS_SUCCESSFUL)
911 925 {
912 926 PRINTF1("in restart_science_task *** CWF1 ERR %d\n", status[5])
913 927 }
914 928
915 929 status[6] = rtems_task_restart( Task_id[TASKID_AVF1], lfrRequestedMode );
916 930 if (status[6] != RTEMS_SUCCESSFUL)
917 931 {
918 932 PRINTF1("in restart_science_task *** AVF1 ERR %d\n", status[6])
919 933 }
920 934
921 935 status[7] = rtems_task_restart( Task_id[TASKID_PRC1],lfrRequestedMode );
922 936 if (status[7] != RTEMS_SUCCESSFUL)
923 937 {
924 938 PRINTF1("in restart_science_task *** PRC1 ERR %d\n", status[7])
925 939 }
926 940
927 941 status[8] = rtems_task_restart( Task_id[TASKID_AVF2], 1 );
928 942 if (status[8] != RTEMS_SUCCESSFUL)
929 943 {
930 944 PRINTF1("in restart_science_task *** AVF2 ERR %d\n", status[8])
931 945 }
932 946
933 947 status[9] = rtems_task_restart( Task_id[TASKID_PRC2], 1 );
934 948 if (status[9] != RTEMS_SUCCESSFUL)
935 949 {
936 950 PRINTF1("in restart_science_task *** PRC2 ERR %d\n", status[9])
937 951 }
938 952
939 953 if ( (status[0] != RTEMS_SUCCESSFUL) || (status[1] != RTEMS_SUCCESSFUL) ||
940 954 (status[2] != RTEMS_SUCCESSFUL) || (status[3] != RTEMS_SUCCESSFUL) ||
941 955 (status[4] != RTEMS_SUCCESSFUL) || (status[5] != RTEMS_SUCCESSFUL) ||
942 956 (status[6] != RTEMS_SUCCESSFUL) || (status[7] != RTEMS_SUCCESSFUL) ||
943 957 (status[8] != RTEMS_SUCCESSFUL) || (status[9] != RTEMS_SUCCESSFUL) )
944 958 {
945 959 ret = RTEMS_UNSATISFIED;
946 960 }
947 961
948 962 return ret;
949 963 }
950 964
951 965 int restart_asm_tasks( unsigned char lfrRequestedMode )
952 966 {
953 967 /** This function is used to restart average spectral matrices tasks.
954 968 *
955 969 * @return RTEMS directive status codes:
956 970 * - RTEMS_SUCCESSFUL - task restarted successfully
957 971 * - RTEMS_INVALID_ID - task id invalid
958 972 * - RTEMS_INCORRECT_STATE - task never started
959 973 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
960 974 *
961 975 * ASM tasks are AVF0, PRC0, AVF1, PRC1, AVF2 and PRC2
962 976 *
963 977 */
964 978
965 979 rtems_status_code status[6];
966 980 rtems_status_code ret;
967 981
968 982 ret = RTEMS_SUCCESSFUL;
969 983
970 984 status[0] = rtems_task_restart( Task_id[TASKID_AVF0], lfrRequestedMode );
971 985 if (status[0] != RTEMS_SUCCESSFUL)
972 986 {
973 987 PRINTF1("in restart_science_task *** AVF0 ERR %d\n", status[0])
974 988 }
975 989
976 990 status[1] = rtems_task_restart( Task_id[TASKID_PRC0], lfrRequestedMode );
977 991 if (status[1] != RTEMS_SUCCESSFUL)
978 992 {
979 993 PRINTF1("in restart_science_task *** PRC0 ERR %d\n", status[1])
980 994 }
981 995
982 996 status[2] = rtems_task_restart( Task_id[TASKID_AVF1], lfrRequestedMode );
983 997 if (status[2] != RTEMS_SUCCESSFUL)
984 998 {
985 999 PRINTF1("in restart_science_task *** AVF1 ERR %d\n", status[2])
986 1000 }
987 1001
988 1002 status[3] = rtems_task_restart( Task_id[TASKID_PRC1],lfrRequestedMode );
989 1003 if (status[3] != RTEMS_SUCCESSFUL)
990 1004 {
991 1005 PRINTF1("in restart_science_task *** PRC1 ERR %d\n", status[3])
992 1006 }
993 1007
994 1008 status[4] = rtems_task_restart( Task_id[TASKID_AVF2], 1 );
995 1009 if (status[4] != RTEMS_SUCCESSFUL)
996 1010 {
997 1011 PRINTF1("in restart_science_task *** AVF2 ERR %d\n", status[4])
998 1012 }
999 1013
1000 1014 status[5] = rtems_task_restart( Task_id[TASKID_PRC2], 1 );
1001 1015 if (status[5] != RTEMS_SUCCESSFUL)
1002 1016 {
1003 1017 PRINTF1("in restart_science_task *** PRC2 ERR %d\n", status[5])
1004 1018 }
1005 1019
1006 1020 if ( (status[0] != RTEMS_SUCCESSFUL) || (status[1] != RTEMS_SUCCESSFUL) ||
1007 1021 (status[2] != RTEMS_SUCCESSFUL) || (status[3] != RTEMS_SUCCESSFUL) ||
1008 1022 (status[4] != RTEMS_SUCCESSFUL) || (status[5] != RTEMS_SUCCESSFUL) )
1009 1023 {
1010 1024 ret = RTEMS_UNSATISFIED;
1011 1025 }
1012 1026
1013 1027 return ret;
1014 1028 }
1015 1029
1016 1030 int suspend_science_tasks( void )
1017 1031 {
1018 1032 /** This function suspends the science tasks.
1019 1033 *
1020 1034 * @return RTEMS directive status codes:
1021 1035 * - RTEMS_SUCCESSFUL - task restarted successfully
1022 1036 * - RTEMS_INVALID_ID - task id invalid
1023 1037 * - RTEMS_ALREADY_SUSPENDED - task already suspended
1024 1038 *
1025 1039 */
1026 1040
1027 1041 rtems_status_code status;
1028 1042
1029 1043 PRINTF("in suspend_science_tasks\n")
1030 1044
1031 1045 status = rtems_task_suspend( Task_id[TASKID_AVF0] ); // suspend AVF0
1032 1046 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1033 1047 {
1034 1048 PRINTF1("in suspend_science_task *** AVF0 ERR %d\n", status)
1035 1049 }
1036 1050 else
1037 1051 {
1038 1052 status = RTEMS_SUCCESSFUL;
1039 1053 }
1040 1054 if (status == RTEMS_SUCCESSFUL) // suspend PRC0
1041 1055 {
1042 1056 status = rtems_task_suspend( Task_id[TASKID_PRC0] );
1043 1057 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1044 1058 {
1045 1059 PRINTF1("in suspend_science_task *** PRC0 ERR %d\n", status)
1046 1060 }
1047 1061 else
1048 1062 {
1049 1063 status = RTEMS_SUCCESSFUL;
1050 1064 }
1051 1065 }
1052 1066 if (status == RTEMS_SUCCESSFUL) // suspend AVF1
1053 1067 {
1054 1068 status = rtems_task_suspend( Task_id[TASKID_AVF1] );
1055 1069 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1056 1070 {
1057 1071 PRINTF1("in suspend_science_task *** AVF1 ERR %d\n", status)
1058 1072 }
1059 1073 else
1060 1074 {
1061 1075 status = RTEMS_SUCCESSFUL;
1062 1076 }
1063 1077 }
1064 1078 if (status == RTEMS_SUCCESSFUL) // suspend PRC1
1065 1079 {
1066 1080 status = rtems_task_suspend( Task_id[TASKID_PRC1] );
1067 1081 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1068 1082 {
1069 1083 PRINTF1("in suspend_science_task *** PRC1 ERR %d\n", status)
1070 1084 }
1071 1085 else
1072 1086 {
1073 1087 status = RTEMS_SUCCESSFUL;
1074 1088 }
1075 1089 }
1076 1090 if (status == RTEMS_SUCCESSFUL) // suspend AVF2
1077 1091 {
1078 1092 status = rtems_task_suspend( Task_id[TASKID_AVF2] );
1079 1093 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1080 1094 {
1081 1095 PRINTF1("in suspend_science_task *** AVF2 ERR %d\n", status)
1082 1096 }
1083 1097 else
1084 1098 {
1085 1099 status = RTEMS_SUCCESSFUL;
1086 1100 }
1087 1101 }
1088 1102 if (status == RTEMS_SUCCESSFUL) // suspend PRC2
1089 1103 {
1090 1104 status = rtems_task_suspend( Task_id[TASKID_PRC2] );
1091 1105 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1092 1106 {
1093 1107 PRINTF1("in suspend_science_task *** PRC2 ERR %d\n", status)
1094 1108 }
1095 1109 else
1096 1110 {
1097 1111 status = RTEMS_SUCCESSFUL;
1098 1112 }
1099 1113 }
1100 1114 if (status == RTEMS_SUCCESSFUL) // suspend WFRM
1101 1115 {
1102 1116 status = rtems_task_suspend( Task_id[TASKID_WFRM] );
1103 1117 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1104 1118 {
1105 1119 PRINTF1("in suspend_science_task *** WFRM ERR %d\n", status)
1106 1120 }
1107 1121 else
1108 1122 {
1109 1123 status = RTEMS_SUCCESSFUL;
1110 1124 }
1111 1125 }
1112 1126 if (status == RTEMS_SUCCESSFUL) // suspend CWF3
1113 1127 {
1114 1128 status = rtems_task_suspend( Task_id[TASKID_CWF3] );
1115 1129 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1116 1130 {
1117 1131 PRINTF1("in suspend_science_task *** CWF3 ERR %d\n", status)
1118 1132 }
1119 1133 else
1120 1134 {
1121 1135 status = RTEMS_SUCCESSFUL;
1122 1136 }
1123 1137 }
1124 1138 if (status == RTEMS_SUCCESSFUL) // suspend CWF2
1125 1139 {
1126 1140 status = rtems_task_suspend( Task_id[TASKID_CWF2] );
1127 1141 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1128 1142 {
1129 1143 PRINTF1("in suspend_science_task *** CWF2 ERR %d\n", status)
1130 1144 }
1131 1145 else
1132 1146 {
1133 1147 status = RTEMS_SUCCESSFUL;
1134 1148 }
1135 1149 }
1136 1150 if (status == RTEMS_SUCCESSFUL) // suspend CWF1
1137 1151 {
1138 1152 status = rtems_task_suspend( Task_id[TASKID_CWF1] );
1139 1153 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1140 1154 {
1141 1155 PRINTF1("in suspend_science_task *** CWF1 ERR %d\n", status)
1142 1156 }
1143 1157 else
1144 1158 {
1145 1159 status = RTEMS_SUCCESSFUL;
1146 1160 }
1147 1161 }
1148 1162
1149 1163 return status;
1150 1164 }
1151 1165
1152 1166 int suspend_asm_tasks( void )
1153 1167 {
1154 1168 /** This function suspends the science tasks.
1155 1169 *
1156 1170 * @return RTEMS directive status codes:
1157 1171 * - RTEMS_SUCCESSFUL - task restarted successfully
1158 1172 * - RTEMS_INVALID_ID - task id invalid
1159 1173 * - RTEMS_ALREADY_SUSPENDED - task already suspended
1160 1174 *
1161 1175 */
1162 1176
1163 1177 rtems_status_code status;
1164 1178
1165 1179 PRINTF("in suspend_science_tasks\n")
1166 1180
1167 1181 status = rtems_task_suspend( Task_id[TASKID_AVF0] ); // suspend AVF0
1168 1182 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1169 1183 {
1170 1184 PRINTF1("in suspend_science_task *** AVF0 ERR %d\n", status)
1171 1185 }
1172 1186 else
1173 1187 {
1174 1188 status = RTEMS_SUCCESSFUL;
1175 1189 }
1176 1190
1177 1191 if (status == RTEMS_SUCCESSFUL) // suspend PRC0
1178 1192 {
1179 1193 status = rtems_task_suspend( Task_id[TASKID_PRC0] );
1180 1194 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1181 1195 {
1182 1196 PRINTF1("in suspend_science_task *** PRC0 ERR %d\n", status)
1183 1197 }
1184 1198 else
1185 1199 {
1186 1200 status = RTEMS_SUCCESSFUL;
1187 1201 }
1188 1202 }
1189 1203
1190 1204 if (status == RTEMS_SUCCESSFUL) // suspend AVF1
1191 1205 {
1192 1206 status = rtems_task_suspend( Task_id[TASKID_AVF1] );
1193 1207 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1194 1208 {
1195 1209 PRINTF1("in suspend_science_task *** AVF1 ERR %d\n", status)
1196 1210 }
1197 1211 else
1198 1212 {
1199 1213 status = RTEMS_SUCCESSFUL;
1200 1214 }
1201 1215 }
1202 1216
1203 1217 if (status == RTEMS_SUCCESSFUL) // suspend PRC1
1204 1218 {
1205 1219 status = rtems_task_suspend( Task_id[TASKID_PRC1] );
1206 1220 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1207 1221 {
1208 1222 PRINTF1("in suspend_science_task *** PRC1 ERR %d\n", status)
1209 1223 }
1210 1224 else
1211 1225 {
1212 1226 status = RTEMS_SUCCESSFUL;
1213 1227 }
1214 1228 }
1215 1229
1216 1230 if (status == RTEMS_SUCCESSFUL) // suspend AVF2
1217 1231 {
1218 1232 status = rtems_task_suspend( Task_id[TASKID_AVF2] );
1219 1233 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1220 1234 {
1221 1235 PRINTF1("in suspend_science_task *** AVF2 ERR %d\n", status)
1222 1236 }
1223 1237 else
1224 1238 {
1225 1239 status = RTEMS_SUCCESSFUL;
1226 1240 }
1227 1241 }
1228 1242
1229 1243 if (status == RTEMS_SUCCESSFUL) // suspend PRC2
1230 1244 {
1231 1245 status = rtems_task_suspend( Task_id[TASKID_PRC2] );
1232 1246 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1233 1247 {
1234 1248 PRINTF1("in suspend_science_task *** PRC2 ERR %d\n", status)
1235 1249 }
1236 1250 else
1237 1251 {
1238 1252 status = RTEMS_SUCCESSFUL;
1239 1253 }
1240 1254 }
1241 1255
1242 1256 return status;
1243 1257 }
1244 1258
1245 1259 void launch_waveform_picker( unsigned char mode, unsigned int transitionCoarseTime )
1246 1260 {
1247 1261 WFP_reset_current_ring_nodes();
1248 1262
1249 1263 reset_waveform_picker_regs();
1250 1264
1251 1265 set_wfp_burst_enable_register( mode );
1252 1266
1253 1267 LEON_Clear_interrupt( IRQ_WAVEFORM_PICKER );
1254 1268 LEON_Unmask_interrupt( IRQ_WAVEFORM_PICKER );
1255 1269
1256 1270 if (transitionCoarseTime == 0)
1257 1271 {
1258 1272 waveform_picker_regs->start_date = time_management_regs->coarse_time;
1259 1273 }
1260 1274 else
1261 1275 {
1262 1276 waveform_picker_regs->start_date = transitionCoarseTime;
1263 1277 }
1264 1278
1265 1279 }
1266 1280
1267 1281 void launch_spectral_matrix( void )
1268 1282 {
1269 1283 SM_reset_current_ring_nodes();
1270 1284
1271 1285 reset_spectral_matrix_regs();
1272 1286
1273 1287 reset_nb_sm();
1274 1288
1275 1289 set_sm_irq_onNewMatrix( 1 );
1276 1290
1277 1291 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX );
1278 1292 LEON_Unmask_interrupt( IRQ_SPECTRAL_MATRIX );
1279 1293
1280 1294 }
1281 1295
1282 1296 void set_sm_irq_onNewMatrix( unsigned char value )
1283 1297 {
1284 1298 if (value == 1)
1285 1299 {
1286 1300 spectral_matrix_regs->config = spectral_matrix_regs->config | 0x01;
1287 1301 }
1288 1302 else
1289 1303 {
1290 1304 spectral_matrix_regs->config = spectral_matrix_regs->config & 0xfffffffe; // 1110
1291 1305 }
1292 1306 }
1293 1307
1294 1308 void set_sm_irq_onError( unsigned char value )
1295 1309 {
1296 1310 if (value == 1)
1297 1311 {
1298 1312 spectral_matrix_regs->config = spectral_matrix_regs->config | 0x02;
1299 1313 }
1300 1314 else
1301 1315 {
1302 1316 spectral_matrix_regs->config = spectral_matrix_regs->config & 0xfffffffd; // 1101
1303 1317 }
1304 1318 }
1305 1319
1306 1320 //*****************************
1307 1321 // CONFIGURE CALIBRATION SIGNAL
1308 1322 void setCalibrationPrescaler( unsigned int prescaler )
1309 1323 {
1310 1324 // prescaling of the master clock (25 MHz)
1311 1325 // master clock is divided by 2^prescaler
1312 1326 time_management_regs->calPrescaler = prescaler;
1313 1327 }
1314 1328
1315 1329 void setCalibrationDivisor( unsigned int divisionFactor )
1316 1330 {
1317 1331 // division of the prescaled clock by the division factor
1318 1332 time_management_regs->calDivisor = divisionFactor;
1319 1333 }
1320 1334
1321 1335 void setCalibrationData( void ){
1322 1336 unsigned int k;
1323 1337 unsigned short data;
1324 1338 float val;
1325 1339 float f0;
1326 1340 float f1;
1327 1341 float fs;
1328 1342 float Ts;
1329 1343 float scaleFactor;
1330 1344
1331 1345 f0 = 625;
1332 1346 f1 = 10000;
1333 1347 fs = 160256.410;
1334 1348 Ts = 1. / fs;
1335 1349 scaleFactor = 0.250 / 0.000654; // 191, 500 mVpp, 2 sinus waves => 500 mVpp each, amplitude = 250 mV
1336 1350
1337 1351 time_management_regs->calDataPtr = 0x00;
1338 1352
1339 1353 // build the signal for the SCM calibration
1340 1354 for (k=0; k<256; k++)
1341 1355 {
1342 1356 val = sin( 2 * pi * f0 * k * Ts )
1343 1357 + sin( 2 * pi * f1 * k * Ts );
1344 1358 data = (unsigned short) ((val * scaleFactor) + 2048);
1345 1359 time_management_regs->calData = data & 0xfff;
1346 1360 }
1347 1361 }
1348 1362
1349 1363 void setCalibrationDataInterleaved( void ){
1350 1364 unsigned int k;
1351 1365 float val;
1352 1366 float f0;
1353 1367 float f1;
1354 1368 float fs;
1355 1369 float Ts;
1356 1370 unsigned short data[384];
1357 1371 unsigned char *dataPtr;
1358 1372
1359 1373 f0 = 625;
1360 1374 f1 = 10000;
1361 1375 fs = 240384.615;
1362 1376 Ts = 1. / fs;
1363 1377
1364 1378 time_management_regs->calDataPtr = 0x00;
1365 1379
1366 1380 // build the signal for the SCM calibration
1367 1381 for (k=0; k<384; k++)
1368 1382 {
1369 1383 val = sin( 2 * pi * f0 * k * Ts )
1370 1384 + sin( 2 * pi * f1 * k * Ts );
1371 1385 data[k] = (unsigned short) (val * 512 + 2048);
1372 1386 }
1373 1387
1374 1388 // write the signal in interleaved mode
1375 1389 for (k=0; k<128; k++)
1376 1390 {
1377 1391 dataPtr = (unsigned char*) &data[k*3 + 2];
1378 1392 time_management_regs->calData = (data[k*3] & 0xfff)
1379 1393 + ( (dataPtr[0] & 0x3f) << 12);
1380 1394 time_management_regs->calData = (data[k*3 + 1] & 0xfff)
1381 1395 + ( (dataPtr[1] & 0x3f) << 12);
1382 1396 }
1383 1397 }
1384 1398
1385 1399 void setCalibrationReload( bool state)
1386 1400 {
1387 1401 if (state == true)
1388 1402 {
1389 1403 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | 0x00000010; // [0001 0000]
1390 1404 }
1391 1405 else
1392 1406 {
1393 1407 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & 0xffffffef; // [1110 1111]
1394 1408 }
1395 1409 }
1396 1410
1397 1411 void setCalibrationEnable( bool state )
1398 1412 {
1399 1413 // this bit drives the multiplexer
1400 1414 if (state == true)
1401 1415 {
1402 1416 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | 0x00000040; // [0100 0000]
1403 1417 }
1404 1418 else
1405 1419 {
1406 1420 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & 0xffffffbf; // [1011 1111]
1407 1421 }
1408 1422 }
1409 1423
1410 1424 void setCalibrationInterleaved( bool state )
1411 1425 {
1412 1426 // this bit drives the multiplexer
1413 1427 if (state == true)
1414 1428 {
1415 1429 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | 0x00000020; // [0010 0000]
1416 1430 }
1417 1431 else
1418 1432 {
1419 1433 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & 0xffffffdf; // [1101 1111]
1420 1434 }
1421 1435 }
1422 1436
1423 1437 void setCalibration( bool state )
1424 1438 {
1425 1439 if (state == true)
1426 1440 {
1427 1441 setCalibrationEnable( true );
1428 1442 setCalibrationReload( false );
1429 1443 set_hk_lfr_calib_enable( true );
1430 1444 }
1431 1445 else
1432 1446 {
1433 1447 setCalibrationEnable( false );
1434 1448 setCalibrationReload( true );
1435 1449 set_hk_lfr_calib_enable( false );
1436 1450 }
1437 1451 }
1438 1452
1439 1453 void configureCalibration( bool interleaved )
1440 1454 {
1441 1455 setCalibration( false );
1442 1456 if ( interleaved == true )
1443 1457 {
1444 1458 setCalibrationInterleaved( true );
1445 1459 setCalibrationPrescaler( 0 ); // 25 MHz => 25 000 000
1446 1460 setCalibrationDivisor( 26 ); // => 240 384
1447 1461 setCalibrationDataInterleaved();
1448 1462 }
1449 1463 else
1450 1464 {
1451 1465 setCalibrationPrescaler( 0 ); // 25 MHz => 25 000 000
1452 1466 setCalibrationDivisor( 38 ); // => 160 256 (39 - 1)
1453 1467 setCalibrationData();
1454 1468 }
1455 1469 }
1456 1470
1457 1471 //****************
1458 1472 // CLOSING ACTIONS
1459 1473 void update_last_TC_exe( ccsdsTelecommandPacket_t *TC, unsigned char * time )
1460 1474 {
1461 1475 /** This function is used to update the HK packets statistics after a successful TC execution.
1462 1476 *
1463 1477 * @param TC points to the TC being processed
1464 1478 * @param time is the time used to date the TC execution
1465 1479 *
1466 1480 */
1467 1481
1468 1482 unsigned int val;
1469 1483
1470 1484 housekeeping_packet.hk_lfr_last_exe_tc_id[0] = TC->packetID[0];
1471 1485 housekeeping_packet.hk_lfr_last_exe_tc_id[1] = TC->packetID[1];
1472 1486 housekeeping_packet.hk_lfr_last_exe_tc_type[0] = 0x00;
1473 1487 housekeeping_packet.hk_lfr_last_exe_tc_type[1] = TC->serviceType;
1474 1488 housekeeping_packet.hk_lfr_last_exe_tc_subtype[0] = 0x00;
1475 1489 housekeeping_packet.hk_lfr_last_exe_tc_subtype[1] = TC->serviceSubType;
1476 1490 housekeeping_packet.hk_lfr_last_exe_tc_time[0] = time[0];
1477 1491 housekeeping_packet.hk_lfr_last_exe_tc_time[1] = time[1];
1478 1492 housekeeping_packet.hk_lfr_last_exe_tc_time[2] = time[2];
1479 1493 housekeeping_packet.hk_lfr_last_exe_tc_time[3] = time[3];
1480 1494 housekeeping_packet.hk_lfr_last_exe_tc_time[4] = time[4];
1481 1495 housekeeping_packet.hk_lfr_last_exe_tc_time[5] = time[5];
1482 1496
1483 1497 val = housekeeping_packet.hk_lfr_exe_tc_cnt[0] * 256 + housekeeping_packet.hk_lfr_exe_tc_cnt[1];
1484 1498 val++;
1485 1499 housekeeping_packet.hk_lfr_exe_tc_cnt[0] = (unsigned char) (val >> 8);
1486 1500 housekeeping_packet.hk_lfr_exe_tc_cnt[1] = (unsigned char) (val);
1487 1501 }
1488 1502
1489 1503 void update_last_TC_rej(ccsdsTelecommandPacket_t *TC, unsigned char * time )
1490 1504 {
1491 1505 /** This function is used to update the HK packets statistics after a TC rejection.
1492 1506 *
1493 1507 * @param TC points to the TC being processed
1494 1508 * @param time is the time used to date the TC rejection
1495 1509 *
1496 1510 */
1497 1511
1498 1512 unsigned int val;
1499 1513
1500 1514 housekeeping_packet.hk_lfr_last_rej_tc_id[0] = TC->packetID[0];
1501 1515 housekeeping_packet.hk_lfr_last_rej_tc_id[1] = TC->packetID[1];
1502 1516 housekeeping_packet.hk_lfr_last_rej_tc_type[0] = 0x00;
1503 1517 housekeeping_packet.hk_lfr_last_rej_tc_type[1] = TC->serviceType;
1504 1518 housekeeping_packet.hk_lfr_last_rej_tc_subtype[0] = 0x00;
1505 1519 housekeeping_packet.hk_lfr_last_rej_tc_subtype[1] = TC->serviceSubType;
1506 1520 housekeeping_packet.hk_lfr_last_rej_tc_time[0] = time[0];
1507 1521 housekeeping_packet.hk_lfr_last_rej_tc_time[1] = time[1];
1508 1522 housekeeping_packet.hk_lfr_last_rej_tc_time[2] = time[2];
1509 1523 housekeeping_packet.hk_lfr_last_rej_tc_time[3] = time[3];
1510 1524 housekeeping_packet.hk_lfr_last_rej_tc_time[4] = time[4];
1511 1525 housekeeping_packet.hk_lfr_last_rej_tc_time[5] = time[5];
1512 1526
1513 1527 val = housekeeping_packet.hk_lfr_rej_tc_cnt[0] * 256 + housekeeping_packet.hk_lfr_rej_tc_cnt[1];
1514 1528 val++;
1515 1529 housekeeping_packet.hk_lfr_rej_tc_cnt[0] = (unsigned char) (val >> 8);
1516 1530 housekeeping_packet.hk_lfr_rej_tc_cnt[1] = (unsigned char) (val);
1517 1531 }
1518 1532
1519 1533 void close_action(ccsdsTelecommandPacket_t *TC, int result, rtems_id queue_id )
1520 1534 {
1521 1535 /** This function is the last step of the TC execution workflow.
1522 1536 *
1523 1537 * @param TC points to the TC being processed
1524 1538 * @param result is the result of the TC execution (LFR_SUCCESSFUL / LFR_DEFAULT)
1525 1539 * @param queue_id is the id of the RTEMS message queue used to send TM packets
1526 1540 * @param time is the time used to date the TC execution
1527 1541 *
1528 1542 */
1529 1543
1530 1544 unsigned char requestedMode;
1531 1545
1532 1546 if (result == LFR_SUCCESSFUL)
1533 1547 {
1534 1548 if ( !( (TC->serviceType==TC_TYPE_TIME) & (TC->serviceSubType==TC_SUBTYPE_UPDT_TIME) )
1535 1549 &
1536 1550 !( (TC->serviceType==TC_TYPE_GEN) & (TC->serviceSubType==TC_SUBTYPE_UPDT_INFO))
1537 1551 )
1538 1552 {
1539 1553 send_tm_lfr_tc_exe_success( TC, queue_id );
1540 1554 }
1541 1555 if ( (TC->serviceType == TC_TYPE_GEN) & (TC->serviceSubType == TC_SUBTYPE_ENTER) )
1542 1556 {
1543 1557 //**********************************
1544 1558 // UPDATE THE LFRMODE LOCAL VARIABLE
1545 1559 requestedMode = TC->dataAndCRC[1];
1546 1560 housekeeping_packet.lfr_status_word[0] = (unsigned char) ((requestedMode << 4) + 0x0d);
1547 1561 updateLFRCurrentMode();
1548 1562 }
1549 1563 }
1550 1564 else if (result == LFR_EXE_ERROR)
1551 1565 {
1552 1566 send_tm_lfr_tc_exe_error( TC, queue_id );
1553 1567 }
1554 1568 }
1555 1569
1556 1570 //***************************
1557 1571 // Interrupt Service Routines
1558 1572 rtems_isr commutation_isr1( rtems_vector_number vector )
1559 1573 {
1560 1574 if (rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
1561 1575 PRINTF("In commutation_isr1 *** Error sending event to DUMB\n")
1562 1576 }
1563 1577 }
1564 1578
1565 1579 rtems_isr commutation_isr2( rtems_vector_number vector )
1566 1580 {
1567 1581 if (rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
1568 1582 PRINTF("In commutation_isr2 *** Error sending event to DUMB\n")
1569 1583 }
1570 1584 }
1571 1585
1572 1586 //****************
1573 1587 // OTHER FUNCTIONS
1574 1588 void updateLFRCurrentMode()
1575 1589 {
1576 1590 /** This function updates the value of the global variable lfrCurrentMode.
1577 1591 *
1578 1592 * lfrCurrentMode is a parameter used by several functions to know in which mode LFR is running.
1579 1593 *
1580 1594 */
1581 1595 // update the local value of lfrCurrentMode with the value contained in the housekeeping_packet structure
1582 1596 lfrCurrentMode = (housekeeping_packet.lfr_status_word[0] & 0xf0) >> 4;
1583 1597 }
1584 1598
1585 1599 void set_lfr_soft_reset( unsigned char value )
1586 1600 {
1587 1601 if (value == 1)
1588 1602 {
1589 1603 time_management_regs->ctrl = time_management_regs->ctrl | 0x00000004; // [0100]
1590 1604 }
1591 1605 else
1592 1606 {
1593 1607 time_management_regs->ctrl = time_management_regs->ctrl & 0xfffffffb; // [1011]
1594 1608 }
1595 1609 }
1596 1610
1597 1611 void reset_lfr( void )
1598 1612 {
1599 1613 set_lfr_soft_reset( 1 );
1600 1614
1601 1615 set_lfr_soft_reset( 0 );
1602 1616
1603 1617 set_hk_lfr_sc_potential_flag( true );
1604 1618 }
General Comments 0
You need to be logged in to leave comments. Login now