##// END OF EJS Templates
HG_REPOSITORIES » LPP » INSTRUMENTATION » USERS » JEANDET » LFR_FSW
RSS

Fork of HG_REPOSITORIES/LPP/INSTRUMENTATION/SOLO_LFR/DEV_PLE

Clone from https://hephaistos.lpp.polytechnique.fr/rhodecode/HG_REPOSITORIES/LPP/INSTRUMENTATION/SOLO_LFR/DEV_PLE

printf removed or replaced by PRINTF macros...
paul -
r227:919e10e56ea1 R3
parent child
Show More
Show file before | Show file after | Hide comments
@@ -1,329 +1,329
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 #include <stdio.h> // printf()
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 //
101 101 ring_node * getRingNodeForAveraging( unsigned char frequencyChannel);
102 102 // ISR
103 103 rtems_isr spectral_matrices_isr( rtems_vector_number vector );
104 104 rtems_isr spectral_matrices_isr_simu( rtems_vector_number vector );
105 105
106 106 //******************
107 107 // Spectral Matrices
108 108 void reset_nb_sm( void );
109 109 // SM
110 110 void SM_init_rings( void );
111 111 void SM_reset_current_ring_nodes( void );
112 112 // ASM
113 113 void ASM_generic_init_ring(ring_node_asm *ring, unsigned char nbNodes );
114 114
115 115 //*****************
116 116 // Basic Parameters
117 117
118 118 void BP_reset_current_ring_nodes( void );
119 119 void BP_init_header(bp_packet *packet,
120 120 unsigned int apid, unsigned char sid,
121 121 unsigned int packetLength , unsigned char blkNr);
122 122 void BP_init_header_with_spare(bp_packet_with_spare *packet,
123 123 unsigned int apid, unsigned char sid,
124 124 unsigned int packetLength, unsigned char blkNr );
125 125 void BP_send( char *data,
126 126 rtems_id queue_id ,
127 127 unsigned int nbBytesToSend , unsigned int sid );
128 128
129 129 //******************
130 130 // general functions
131 131 void reset_sm_status( void );
132 132 void reset_spectral_matrix_regs( void );
133 133 void set_time(unsigned char *time, unsigned char *timeInBuffer );
134 134 unsigned long long int get_acquisition_time( unsigned char *timePtr );
135 135 unsigned char getSID( rtems_event_set event );
136 136
137 137 extern rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id );
138 138 extern rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id );
139 139
140 140 //***************************************
141 141 // DEFINITIONS OF STATIC INLINE FUNCTIONS
142 142 static inline void SM_average(float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
143 143 ring_node *ring_node_tab[],
144 144 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
145 145 asm_msg *msgForMATR );
146 146
147 147 static inline void SM_average_debug(float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
148 148 ring_node *ring_node_tab[],
149 149 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
150 150 asm_msg *msgForMATR );
151 151
152 152 void ASM_patch( float *inputASM, float *outputASM );
153 153
154 154 void extractReImVectors(float *inputASM, float *outputASM, unsigned int asmComponent );
155 155
156 156 static inline void ASM_reorganize_and_divide(float *averaged_spec_mat, float *averaged_spec_mat_reorganized,
157 157 float divider );
158 158
159 159 static inline void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat,
160 160 float divider,
161 161 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart);
162 162
163 163 static inline void ASM_convert(volatile float *input_matrix, char *output_matrix);
164 164
165 165 void SM_average( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
166 166 ring_node *ring_node_tab[],
167 167 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
168 168 asm_msg *msgForMATR )
169 169 {
170 170 float sum;
171 171 unsigned int i;
172 172
173 173 for(i=0; i<TOTAL_SIZE_SM; i++)
174 174 {
175 175 sum = ( (int *) (ring_node_tab[0]->buffer_address) ) [ i ]
176 176 + ( (int *) (ring_node_tab[1]->buffer_address) ) [ i ]
177 177 + ( (int *) (ring_node_tab[2]->buffer_address) ) [ i ]
178 178 + ( (int *) (ring_node_tab[3]->buffer_address) ) [ i ]
179 179 + ( (int *) (ring_node_tab[4]->buffer_address) ) [ i ]
180 180 + ( (int *) (ring_node_tab[5]->buffer_address) ) [ i ]
181 181 + ( (int *) (ring_node_tab[6]->buffer_address) ) [ i ]
182 182 + ( (int *) (ring_node_tab[7]->buffer_address) ) [ i ];
183 183
184 184 if ( (nbAverageNORM == 0) && (nbAverageSBM == 0) )
185 185 {
186 186 averaged_spec_mat_NORM[ i ] = sum;
187 187 averaged_spec_mat_SBM[ i ] = sum;
188 188 msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime;
189 189 msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime;
190 190 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
191 191 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
192 192 }
193 193 else if ( (nbAverageNORM != 0) && (nbAverageSBM != 0) )
194 194 {
195 195 averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum );
196 196 averaged_spec_mat_SBM[ i ] = ( averaged_spec_mat_SBM[ i ] + sum );
197 197 }
198 198 else if ( (nbAverageNORM != 0) && (nbAverageSBM == 0) )
199 199 {
200 200 averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum );
201 201 averaged_spec_mat_SBM[ i ] = sum;
202 202 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
203 203 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
204 204 }
205 205 else
206 206 {
207 207 averaged_spec_mat_NORM[ i ] = sum;
208 208 averaged_spec_mat_SBM[ i ] = ( averaged_spec_mat_SBM[ i ] + sum );
209 209 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
210 210 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
211 211 // PRINTF2("ERR *** in SM_average *** unexpected parameters %d %d\n", nbAverageNORM, nbAverageSBM)
212 212 }
213 213 }
214 214 }
215 215
216 216 void SM_average_debug( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
217 217 ring_node *ring_node_tab[],
218 218 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
219 219 asm_msg *msgForMATR )
220 220 {
221 221 float sum;
222 222 unsigned int i;
223 223
224 224 for(i=0; i<TOTAL_SIZE_SM; i++)
225 225 {
226 226 sum = ( (int *) (ring_node_tab[0]->buffer_address) ) [ i ];
227 227 averaged_spec_mat_NORM[ i ] = sum;
228 228 averaged_spec_mat_SBM[ i ] = sum;
229 229 msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime;
230 230 msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime;
231 231 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
232 232 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
233 233 }
234 234 }
235 235
236 236 void ASM_reorganize_and_divide( float *averaged_spec_mat, float *averaged_spec_mat_reorganized, float divider )
237 237 {
238 238 int frequencyBin;
239 239 int asmComponent;
240 240 unsigned int offsetASM;
241 241 unsigned int offsetASMReorganized;
242 242
243 243 // BUILD DATA
244 244 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
245 245 {
246 246 for( frequencyBin = 0; frequencyBin < NB_BINS_PER_SM; frequencyBin++ )
247 247 {
248 248 offsetASMReorganized =
249 249 frequencyBin * NB_VALUES_PER_SM
250 250 + asmComponent;
251 251 offsetASM =
252 252 asmComponent * NB_BINS_PER_SM
253 253 + frequencyBin;
254 254 averaged_spec_mat_reorganized[offsetASMReorganized ] =
255 255 averaged_spec_mat[ offsetASM ] / divider;
256 256 }
257 257 }
258 258 }
259 259
260 260 void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
261 261 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage, unsigned char ASMIndexStart )
262 262 {
263 263 int frequencyBin;
264 264 int asmComponent;
265 265 int offsetASM;
266 266 int offsetCompressed;
267 267 int k;
268 268
269 269 // BUILD DATA
270 270 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
271 271 {
272 272 for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
273 273 {
274 274 offsetCompressed = // NO TIME OFFSET
275 275 frequencyBin * NB_VALUES_PER_SM
276 276 + asmComponent;
277 277 offsetASM = // NO TIME OFFSET
278 278 asmComponent * NB_BINS_PER_SM
279 279 + ASMIndexStart
280 280 + frequencyBin * nbBinsToAverage;
281 281 compressed_spec_mat[ offsetCompressed ] = 0;
282 282 for ( k = 0; k < nbBinsToAverage; k++ )
283 283 {
284 284 compressed_spec_mat[offsetCompressed ] =
285 285 ( compressed_spec_mat[ offsetCompressed ]
286 286 + averaged_spec_mat[ offsetASM + k ] );
287 287 }
288 288 compressed_spec_mat[ offsetCompressed ] =
289 289 compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
290 290 }
291 291 }
292 292 }
293 293
294 294 void ASM_convert( volatile float *input_matrix, char *output_matrix)
295 295 {
296 296 unsigned int frequencyBin;
297 297 unsigned int asmComponent;
298 298 char * pt_char_input;
299 299 char * pt_char_output;
300 300 unsigned int offsetInput;
301 301 unsigned int offsetOutput;
302 302
303 303 pt_char_input = (char*) &input_matrix;
304 304 pt_char_output = (char*) &output_matrix;
305 305
306 306 // convert all other data
307 307 for( frequencyBin=0; frequencyBin<NB_BINS_PER_SM; frequencyBin++)
308 308 {
309 309 for ( asmComponent=0; asmComponent<NB_VALUES_PER_SM; asmComponent++)
310 310 {
311 311 offsetInput = (frequencyBin*NB_VALUES_PER_SM) + asmComponent ;
312 312 offsetOutput = 2 * ( (frequencyBin*NB_VALUES_PER_SM) + asmComponent ) ;
313 313 pt_char_input = (char*) &input_matrix [ offsetInput ];
314 314 pt_char_output = (char*) &output_matrix[ offsetOutput ];
315 315 pt_char_output[0] = pt_char_input[0]; // bits 31 downto 24 of the float
316 316 pt_char_output[1] = pt_char_input[1]; // bits 23 downto 16 of the float
317 317 }
318 318 }
319 319 }
320 320
321 321 void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat,
322 322 float divider,
323 323 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart);
324 324
325 325 int getFBinMask(int k);
326 326
327 327 void init_kcoeff_sbm_from_kcoeff_norm( float *input_kcoeff, float *output_kcoeff, unsigned char nb_bins_norm);
328 328
329 329 #endif // FSW_PROCESSING_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -1,71 +1,72
1 1 #ifndef TC_LOAD_DUMP_PARAMETERS_H
2 2 #define TC_LOAD_DUMP_PARAMETERS_H
3 3
4 4 #include <rtems.h>
5 5 #include <stdio.h>
6 6
7 7 #include "fsw_params.h"
8 8 #include "wf_handler.h"
9 9 #include "tm_lfr_tc_exe.h"
10 10 #include "fsw_misc.h"
11 11 #include "basic_parameters_params.h"
12 12 #include "avf0_prc0.h"
13 13
14 14 #define FLOAT_EQUAL_ZERO 0.001
15 15
16 16 extern unsigned short sequenceCounterParameterDump;
17 extern unsigned short sequenceCounters_TM_DUMP[];
17 18 extern float k_coeff_intercalib_f0_norm[ ];
18 19 extern float k_coeff_intercalib_f0_sbm[ ];
19 20 extern float k_coeff_intercalib_f1_norm[ ];
20 21 extern float k_coeff_intercalib_f1_sbm[ ];
21 22 extern float k_coeff_intercalib_f2[ ];
22 23
23 24 int action_load_common_par( ccsdsTelecommandPacket_t *TC );
24 25 int action_load_normal_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
25 26 int action_load_burst_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
26 27 int action_load_sbm1_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
27 28 int action_load_sbm2_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
28 29 int action_load_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
29 30 int action_load_fbins_mask(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
30 31 int action_dump_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
31 32 int action_dump_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id );
32 33
33 34 // NORMAL
34 35 int check_common_par_consistency( ccsdsTelecommandPacket_t *TC, rtems_id queue_id );
35 36 int set_sy_lfr_n_swf_l( ccsdsTelecommandPacket_t *TC );
36 37 int set_sy_lfr_n_swf_p( ccsdsTelecommandPacket_t *TC );
37 38 int set_sy_lfr_n_asm_p( ccsdsTelecommandPacket_t *TC );
38 39 int set_sy_lfr_n_bp_p0( ccsdsTelecommandPacket_t *TC );
39 40 int set_sy_lfr_n_bp_p1( ccsdsTelecommandPacket_t *TC );
40 41 int set_sy_lfr_n_cwf_long_f3( ccsdsTelecommandPacket_t *TC );
41 42
42 43 // BURST
43 44 int set_sy_lfr_b_bp_p0( ccsdsTelecommandPacket_t *TC );
44 45 int set_sy_lfr_b_bp_p1( ccsdsTelecommandPacket_t *TC );
45 46
46 47 // SBM1
47 48 int set_sy_lfr_s1_bp_p0( ccsdsTelecommandPacket_t *TC );
48 49 int set_sy_lfr_s1_bp_p1( ccsdsTelecommandPacket_t *TC );
49 50
50 51 // SBM2
51 52 int set_sy_lfr_s2_bp_p0( ccsdsTelecommandPacket_t *TC );
52 53 int set_sy_lfr_s2_bp_p1( ccsdsTelecommandPacket_t *TC );
53 54
54 55 // TC_LFR_UPDATE_INFO
55 56 unsigned int check_update_info_hk_lfr_mode( unsigned char mode );
56 57 unsigned int check_update_info_hk_tds_mode( unsigned char mode );
57 58 unsigned int check_update_info_hk_thr_mode( unsigned char mode );
58 59
59 60 // FBINS_MASK
60 61 int set_sy_lfr_fbins( ccsdsTelecommandPacket_t *TC );
61 62
62 63 // KCOEFFICIENTS
63 64 int set_sy_lfr_kcoeff(ccsdsTelecommandPacket_t *TC , rtems_id queue_id);
64 65 void copyFloatByChar( unsigned char *destination, unsigned char *source );
65 66
66 67 void init_parameter_dump( void );
67 68 void init_kcoefficients_dump( void );
68 69 void init_kcoefficients_dump_packet( Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump, unsigned char pkt_nr, unsigned char blk_nr );
69 void print_k_coeff();
70 void increment_seq_counter_destination_id_dump( unsigned char *packet_sequence_control, unsigned char destination_id );
70 71
71 72 #endif // TC_LOAD_DUMP_PARAMETERS_H
Show file before | Show file after | Hide comments
@@ -1,79 +1,79
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 61
62 62 // HK PACKETS
63 63 Packet_TM_LFR_HK_t housekeeping_packet;
64 64 // message queues occupancy
65 65 unsigned char hk_lfr_q_sd_fifo_size_max;
66 66 unsigned char hk_lfr_q_rv_fifo_size_max;
67 67 unsigned char hk_lfr_q_p0_fifo_size_max;
68 68 unsigned char hk_lfr_q_p1_fifo_size_max;
69 69 unsigned char hk_lfr_q_p2_fifo_size_max;
70 70 // sequence counters are incremented by APID (PID + CAT) and destination ID
71 71 unsigned short sequenceCounters_SCIENCE_NORMAL_BURST;
72 72 unsigned short sequenceCounters_SCIENCE_SBM1_SBM2;
73 73 unsigned short sequenceCounters_TC_EXE[SEQ_CNT_NB_DEST_ID];
74 unsigned short sequenceCounters_TM_DUMP[SEQ_CNT_NB_DEST_ID];
74 75 unsigned short sequenceCounterHK;
75 unsigned short sequenceCounterParameterDump;
76 76 spw_stats spacewire_stats;
77 77 spw_stats spacewire_stats_backup;
78 78
79 79
Show file before | Show file after | Hide comments
@@ -1,872 +1,872
1 1 /** This is the RTEMS initialization module.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * This module contains two very different information:
7 7 * - specific instructions to configure the compilation of the RTEMS executive
8 8 * - functions related to the fligth softwre initialization, especially the INIT RTEMS task
9 9 *
10 10 */
11 11
12 12 //*************************
13 13 // GPL reminder to be added
14 14 //*************************
15 15
16 16 #include <rtems.h>
17 17
18 18 /* configuration information */
19 19
20 20 #define CONFIGURE_INIT
21 21
22 22 #include <bsp.h> /* for device driver prototypes */
23 23
24 24 /* configuration information */
25 25
26 26 #define CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
27 27 #define CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
28 28
29 29 #define CONFIGURE_MAXIMUM_TASKS 20
30 30 #define CONFIGURE_RTEMS_INIT_TASKS_TABLE
31 31 #define CONFIGURE_EXTRA_TASK_STACKS (3 * RTEMS_MINIMUM_STACK_SIZE)
32 32 #define CONFIGURE_LIBIO_MAXIMUM_FILE_DESCRIPTORS 32
33 33 #define CONFIGURE_INIT_TASK_PRIORITY 1 // instead of 100
34 34 #define CONFIGURE_INIT_TASK_MODE (RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT)
35 35 #define CONFIGURE_INIT_TASK_ATTRIBUTES (RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT)
36 36 #define CONFIGURE_MAXIMUM_DRIVERS 16
37 37 #define CONFIGURE_MAXIMUM_PERIODS 5
38 38 #define CONFIGURE_MAXIMUM_TIMERS 5 // STAT (1s), send SWF (0.3s), send CWF3 (1s)
39 39 #define CONFIGURE_MAXIMUM_MESSAGE_QUEUES 5
40 40 #ifdef PRINT_STACK_REPORT
41 41 #define CONFIGURE_STACK_CHECKER_ENABLED
42 42 #endif
43 43
44 44 #include <rtems/confdefs.h>
45 45
46 46 /* If --drvmgr was enabled during the configuration of the RTEMS kernel */
47 47 #ifdef RTEMS_DRVMGR_STARTUP
48 48 #ifdef LEON3
49 49 /* Add Timer and UART Driver */
50 50 #ifdef CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
51 51 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GPTIMER
52 52 #endif
53 53 #ifdef CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
54 54 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_APBUART
55 55 #endif
56 56 #endif
57 57 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GRSPW /* GRSPW Driver */
58 58 #include <drvmgr/drvmgr_confdefs.h>
59 59 #endif
60 60
61 61 #include "fsw_init.h"
62 62 #include "fsw_config.c"
63 63 #include "GscMemoryLPP.hpp"
64 64
65 65 void initCache()
66 66 {
67 67 unsigned int cacheControlRegister;
68 68
69 69 cacheControlRegister = getCacheControlRegister();
70 printf("(0) cacheControlRegister = %x\n", cacheControlRegister);
70 PRINTF1("(0) cacheControlRegister = %x\n", cacheControlRegister)
71 71
72 72 resetCacheControlRegister();
73 73
74 74 enableInstructionCache();
75 75 enableDataCache();
76 76 enableInstructionBurstFetch();
77 77
78 78 cacheControlRegister = getCacheControlRegister();
79 printf("(1) cacheControlRegister = %x\n", cacheControlRegister);
79 PRINTF1("(1) cacheControlRegister = %x\n", cacheControlRegister)
80 80 }
81 81
82 82 rtems_task Init( rtems_task_argument ignored )
83 83 {
84 84 /** This is the RTEMS INIT taks, it is the first task launched by the system.
85 85 *
86 86 * @param unused is the starting argument of the RTEMS task
87 87 *
88 88 * The INIT task create and run all other RTEMS tasks.
89 89 *
90 90 */
91 91
92 92 //***********
93 93 // INIT CACHE
94 94
95 95 unsigned char *vhdlVersion;
96 96
97 97 reset_lfr();
98 98
99 99 reset_local_time();
100 100
101 101 rtems_cpu_usage_reset();
102 102
103 103 rtems_status_code status;
104 104 rtems_status_code status_spw;
105 105 rtems_isr_entry old_isr_handler;
106 106
107 107 // UART settings
108 108 send_console_outputs_on_apbuart_port();
109 109 set_apbuart_scaler_reload_register(REGS_ADDR_APBUART, APBUART_SCALER_RELOAD_VALUE);
110 110 enable_apbuart_transmitter();
111 111
112 112 DEBUG_PRINTF("\n\n\n\n\nIn INIT *** Now the console is on port COM1\n")
113 113
114 114
115 115 PRINTF("\n\n\n\n\n")
116 116
117 117 initCache();
118 118
119 119 PRINTF("*************************\n")
120 120 PRINTF("** LFR Flight Software **\n")
121 121 PRINTF1("** %d.", SW_VERSION_N1)
122 122 PRINTF1("%d." , SW_VERSION_N2)
123 123 PRINTF1("%d." , SW_VERSION_N3)
124 124 PRINTF1("%d **\n", SW_VERSION_N4)
125 125
126 126 vhdlVersion = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
127 127 PRINTF("** VHDL **\n")
128 128 PRINTF1("** %d.", vhdlVersion[1])
129 129 PRINTF1("%d." , vhdlVersion[2])
130 130 PRINTF1("%d **\n", vhdlVersion[3])
131 131 PRINTF("*************************\n")
132 132 PRINTF("\n\n")
133 133
134 134 init_parameter_dump();
135 135 init_kcoefficients_dump();
136 136 init_local_mode_parameters();
137 137 init_housekeeping_parameters();
138 138 init_k_coefficients_prc0();
139 139 init_k_coefficients_prc1();
140 140 init_k_coefficients_prc2();
141 141 pa_bia_status_info = 0x00;
142 142
143 143 // waveform picker initialization
144 144 WFP_init_rings(); // initialize the waveform rings
145 145 WFP_reset_current_ring_nodes();
146 146 reset_waveform_picker_regs();
147 147
148 148 // spectral matrices initialization
149 149 SM_init_rings(); // initialize spectral matrices rings
150 150 SM_reset_current_ring_nodes();
151 151 reset_spectral_matrix_regs();
152 152
153 153 // configure calibration
154 154 configureCalibration( false ); // true means interleaved mode, false is for normal mode
155 155
156 156 updateLFRCurrentMode();
157 157
158 158 BOOT_PRINTF1("in INIT *** lfrCurrentMode is %d\n", lfrCurrentMode)
159 159
160 160 create_names(); // create all names
161 161
162 162 status = create_message_queues(); // create message queues
163 163 if (status != RTEMS_SUCCESSFUL)
164 164 {
165 165 PRINTF1("in INIT *** ERR in create_message_queues, code %d", status)
166 166 }
167 167
168 168 status = create_all_tasks(); // create all tasks
169 169 if (status != RTEMS_SUCCESSFUL)
170 170 {
171 171 PRINTF1("in INIT *** ERR in create_all_tasks, code %d\n", status)
172 172 }
173 173
174 174 // **************************
175 175 // <SPACEWIRE INITIALIZATION>
176 176 grspw_timecode_callback = &timecode_irq_handler;
177 177
178 178 status_spw = spacewire_open_link(); // (1) open the link
179 179 if ( status_spw != RTEMS_SUCCESSFUL )
180 180 {
181 181 PRINTF1("in INIT *** ERR spacewire_open_link code %d\n", status_spw )
182 182 }
183 183
184 184 if ( status_spw == RTEMS_SUCCESSFUL ) // (2) configure the link
185 185 {
186 186 status_spw = spacewire_configure_link( fdSPW );
187 187 if ( status_spw != RTEMS_SUCCESSFUL )
188 188 {
189 189 PRINTF1("in INIT *** ERR spacewire_configure_link code %d\n", status_spw )
190 190 }
191 191 }
192 192
193 193 if ( status_spw == RTEMS_SUCCESSFUL) // (3) start the link
194 194 {
195 195 status_spw = spacewire_start_link( fdSPW );
196 196 if ( status_spw != RTEMS_SUCCESSFUL )
197 197 {
198 198 PRINTF1("in INIT *** ERR spacewire_start_link code %d\n", status_spw )
199 199 }
200 200 }
201 201 // </SPACEWIRE INITIALIZATION>
202 202 // ***************************
203 203
204 204 status = start_all_tasks(); // start all tasks
205 205 if (status != RTEMS_SUCCESSFUL)
206 206 {
207 207 PRINTF1("in INIT *** ERR in start_all_tasks, code %d", status)
208 208 }
209 209
210 210 // start RECV and SEND *AFTER* SpaceWire Initialization, due to the timeout of the start call during the initialization
211 211 status = start_recv_send_tasks();
212 212 if ( status != RTEMS_SUCCESSFUL )
213 213 {
214 214 PRINTF1("in INIT *** ERR start_recv_send_tasks code %d\n", status )
215 215 }
216 216
217 217 // suspend science tasks, they will be restarted later depending on the mode
218 218 status = suspend_science_tasks(); // suspend science tasks (not done in stop_current_mode if current mode = STANDBY)
219 219 if (status != RTEMS_SUCCESSFUL)
220 220 {
221 221 PRINTF1("in INIT *** in suspend_science_tasks *** ERR code: %d\n", status)
222 222 }
223 223
224 224 //******************************
225 225 // <SPECTRAL MATRICES SIMULATOR>
226 226 LEON_Mask_interrupt( IRQ_SM_SIMULATOR );
227 227 configure_timer((gptimer_regs_t*) REGS_ADDR_GPTIMER, TIMER_SM_SIMULATOR, CLKDIV_SM_SIMULATOR,
228 228 IRQ_SPARC_SM_SIMULATOR, spectral_matrices_isr_simu );
229 229 // </SPECTRAL MATRICES SIMULATOR>
230 230 //*******************************
231 231
232 232 // configure IRQ handling for the waveform picker unit
233 233 status = rtems_interrupt_catch( waveforms_isr,
234 234 IRQ_SPARC_WAVEFORM_PICKER,
235 235 &old_isr_handler) ;
236 236 // configure IRQ handling for the spectral matrices unit
237 237 status = rtems_interrupt_catch( spectral_matrices_isr,
238 238 IRQ_SPARC_SPECTRAL_MATRIX,
239 239 &old_isr_handler) ;
240 240
241 241 // if the spacewire link is not up then send an event to the SPIQ task for link recovery
242 242 if ( status_spw != RTEMS_SUCCESSFUL )
243 243 {
244 244 status = rtems_event_send( Task_id[TASKID_SPIQ], SPW_LINKERR_EVENT );
245 245 if ( status != RTEMS_SUCCESSFUL ) {
246 246 PRINTF1("in INIT *** ERR rtems_event_send to SPIQ code %d\n", status )
247 247 }
248 248 }
249 249
250 250 BOOT_PRINTF("delete INIT\n")
251 251
252 252 set_hk_lfr_sc_potential_flag( true );
253 253
254 254 status = rtems_task_delete(RTEMS_SELF);
255 255
256 256 }
257 257
258 258 void init_local_mode_parameters( void )
259 259 {
260 260 /** This function initialize the param_local global variable with default values.
261 261 *
262 262 */
263 263
264 264 unsigned int i;
265 265
266 266 // LOCAL PARAMETERS
267 267
268 268 BOOT_PRINTF1("local_sbm1_nb_cwf_max %d \n", param_local.local_sbm1_nb_cwf_max)
269 269 BOOT_PRINTF1("local_sbm2_nb_cwf_max %d \n", param_local.local_sbm2_nb_cwf_max)
270 270 BOOT_PRINTF1("nb_interrupt_f0_MAX = %d\n", param_local.local_nb_interrupt_f0_MAX)
271 271
272 272 // init sequence counters
273 273
274 274 for(i = 0; i<SEQ_CNT_NB_DEST_ID; i++)
275 275 {
276 276 sequenceCounters_TC_EXE[i] = 0x00;
277 sequenceCounters_TM_DUMP[i] = 0x00;
277 278 }
278 279 sequenceCounters_SCIENCE_NORMAL_BURST = 0x00;
279 280 sequenceCounters_SCIENCE_SBM1_SBM2 = 0x00;
280 281 sequenceCounterHK = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
281 sequenceCounterParameterDump = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
282 282 }
283 283
284 284 void reset_local_time( void )
285 285 {
286 286 time_management_regs->ctrl = time_management_regs->ctrl | 0x02; // [0010] software reset, coarse time = 0x80000000
287 287 }
288 288
289 289 void create_names( void ) // create all names for tasks and queues
290 290 {
291 291 /** This function creates all RTEMS names used in the software for tasks and queues.
292 292 *
293 293 * @return RTEMS directive status codes:
294 294 * - RTEMS_SUCCESSFUL - successful completion
295 295 *
296 296 */
297 297
298 298 // task names
299 299 Task_name[TASKID_RECV] = rtems_build_name( 'R', 'E', 'C', 'V' );
300 300 Task_name[TASKID_ACTN] = rtems_build_name( 'A', 'C', 'T', 'N' );
301 301 Task_name[TASKID_SPIQ] = rtems_build_name( 'S', 'P', 'I', 'Q' );
302 302 Task_name[TASKID_STAT] = rtems_build_name( 'S', 'T', 'A', 'T' );
303 303 Task_name[TASKID_AVF0] = rtems_build_name( 'A', 'V', 'F', '0' );
304 304 Task_name[TASKID_SWBD] = rtems_build_name( 'S', 'W', 'B', 'D' );
305 305 Task_name[TASKID_WFRM] = rtems_build_name( 'W', 'F', 'R', 'M' );
306 306 Task_name[TASKID_DUMB] = rtems_build_name( 'D', 'U', 'M', 'B' );
307 307 Task_name[TASKID_HOUS] = rtems_build_name( 'H', 'O', 'U', 'S' );
308 308 Task_name[TASKID_PRC0] = rtems_build_name( 'P', 'R', 'C', '0' );
309 309 Task_name[TASKID_CWF3] = rtems_build_name( 'C', 'W', 'F', '3' );
310 310 Task_name[TASKID_CWF2] = rtems_build_name( 'C', 'W', 'F', '2' );
311 311 Task_name[TASKID_CWF1] = rtems_build_name( 'C', 'W', 'F', '1' );
312 312 Task_name[TASKID_SEND] = rtems_build_name( 'S', 'E', 'N', 'D' );
313 313 Task_name[TASKID_WTDG] = rtems_build_name( 'W', 'T', 'D', 'G' );
314 314 Task_name[TASKID_AVF1] = rtems_build_name( 'A', 'V', 'F', '1' );
315 315 Task_name[TASKID_PRC1] = rtems_build_name( 'P', 'R', 'C', '1' );
316 316 Task_name[TASKID_AVF2] = rtems_build_name( 'A', 'V', 'F', '2' );
317 317 Task_name[TASKID_PRC2] = rtems_build_name( 'P', 'R', 'C', '2' );
318 318
319 319 // rate monotonic period names
320 320 name_hk_rate_monotonic = rtems_build_name( 'H', 'O', 'U', 'S' );
321 321
322 322 misc_name[QUEUE_RECV] = rtems_build_name( 'Q', '_', 'R', 'V' );
323 323 misc_name[QUEUE_SEND] = rtems_build_name( 'Q', '_', 'S', 'D' );
324 324 misc_name[QUEUE_PRC0] = rtems_build_name( 'Q', '_', 'P', '0' );
325 325 misc_name[QUEUE_PRC1] = rtems_build_name( 'Q', '_', 'P', '1' );
326 326 misc_name[QUEUE_PRC2] = rtems_build_name( 'Q', '_', 'P', '2' );
327 327 }
328 328
329 329 int create_all_tasks( void ) // create all tasks which run in the software
330 330 {
331 331 /** This function creates all RTEMS tasks used in the software.
332 332 *
333 333 * @return RTEMS directive status codes:
334 334 * - RTEMS_SUCCESSFUL - task created successfully
335 335 * - RTEMS_INVALID_ADDRESS - id is NULL
336 336 * - RTEMS_INVALID_NAME - invalid task name
337 337 * - RTEMS_INVALID_PRIORITY - invalid task priority
338 338 * - RTEMS_MP_NOT_CONFIGURED - multiprocessing not configured
339 339 * - RTEMS_TOO_MANY - too many tasks created
340 340 * - RTEMS_UNSATISFIED - not enough memory for stack/FP context
341 341 * - RTEMS_TOO_MANY - too many global objects
342 342 *
343 343 */
344 344
345 345 rtems_status_code status;
346 346
347 347 //**********
348 348 // SPACEWIRE
349 349 // RECV
350 350 status = rtems_task_create(
351 351 Task_name[TASKID_RECV], TASK_PRIORITY_RECV, RTEMS_MINIMUM_STACK_SIZE,
352 352 RTEMS_DEFAULT_MODES,
353 353 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_RECV]
354 354 );
355 355 if (status == RTEMS_SUCCESSFUL) // SEND
356 356 {
357 357 status = rtems_task_create(
358 358 Task_name[TASKID_SEND], TASK_PRIORITY_SEND, RTEMS_MINIMUM_STACK_SIZE * 2,
359 359 RTEMS_DEFAULT_MODES,
360 360 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SEND]
361 361 );
362 362 }
363 363 if (status == RTEMS_SUCCESSFUL) // WTDG
364 364 {
365 365 status = rtems_task_create(
366 366 Task_name[TASKID_WTDG], TASK_PRIORITY_WTDG, RTEMS_MINIMUM_STACK_SIZE,
367 367 RTEMS_DEFAULT_MODES,
368 368 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_WTDG]
369 369 );
370 370 }
371 371 if (status == RTEMS_SUCCESSFUL) // ACTN
372 372 {
373 373 status = rtems_task_create(
374 374 Task_name[TASKID_ACTN], TASK_PRIORITY_ACTN, RTEMS_MINIMUM_STACK_SIZE,
375 375 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
376 376 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_ACTN]
377 377 );
378 378 }
379 379 if (status == RTEMS_SUCCESSFUL) // SPIQ
380 380 {
381 381 status = rtems_task_create(
382 382 Task_name[TASKID_SPIQ], TASK_PRIORITY_SPIQ, RTEMS_MINIMUM_STACK_SIZE,
383 383 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
384 384 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_SPIQ]
385 385 );
386 386 }
387 387
388 388 //******************
389 389 // SPECTRAL MATRICES
390 390 if (status == RTEMS_SUCCESSFUL) // AVF0
391 391 {
392 392 status = rtems_task_create(
393 393 Task_name[TASKID_AVF0], TASK_PRIORITY_AVF0, RTEMS_MINIMUM_STACK_SIZE,
394 394 RTEMS_DEFAULT_MODES,
395 395 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF0]
396 396 );
397 397 }
398 398 if (status == RTEMS_SUCCESSFUL) // PRC0
399 399 {
400 400 status = rtems_task_create(
401 401 Task_name[TASKID_PRC0], TASK_PRIORITY_PRC0, RTEMS_MINIMUM_STACK_SIZE * 2,
402 402 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
403 403 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC0]
404 404 );
405 405 }
406 406 if (status == RTEMS_SUCCESSFUL) // AVF1
407 407 {
408 408 status = rtems_task_create(
409 409 Task_name[TASKID_AVF1], TASK_PRIORITY_AVF1, RTEMS_MINIMUM_STACK_SIZE,
410 410 RTEMS_DEFAULT_MODES,
411 411 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF1]
412 412 );
413 413 }
414 414 if (status == RTEMS_SUCCESSFUL) // PRC1
415 415 {
416 416 status = rtems_task_create(
417 417 Task_name[TASKID_PRC1], TASK_PRIORITY_PRC1, RTEMS_MINIMUM_STACK_SIZE * 2,
418 418 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
419 419 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC1]
420 420 );
421 421 }
422 422 if (status == RTEMS_SUCCESSFUL) // AVF2
423 423 {
424 424 status = rtems_task_create(
425 425 Task_name[TASKID_AVF2], TASK_PRIORITY_AVF2, RTEMS_MINIMUM_STACK_SIZE,
426 426 RTEMS_DEFAULT_MODES,
427 427 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF2]
428 428 );
429 429 }
430 430 if (status == RTEMS_SUCCESSFUL) // PRC2
431 431 {
432 432 status = rtems_task_create(
433 433 Task_name[TASKID_PRC2], TASK_PRIORITY_PRC2, RTEMS_MINIMUM_STACK_SIZE * 2,
434 434 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
435 435 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC2]
436 436 );
437 437 }
438 438
439 439 //****************
440 440 // WAVEFORM PICKER
441 441 if (status == RTEMS_SUCCESSFUL) // WFRM
442 442 {
443 443 status = rtems_task_create(
444 444 Task_name[TASKID_WFRM], TASK_PRIORITY_WFRM, RTEMS_MINIMUM_STACK_SIZE,
445 445 RTEMS_DEFAULT_MODES,
446 446 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_WFRM]
447 447 );
448 448 }
449 449 if (status == RTEMS_SUCCESSFUL) // CWF3
450 450 {
451 451 status = rtems_task_create(
452 452 Task_name[TASKID_CWF3], TASK_PRIORITY_CWF3, RTEMS_MINIMUM_STACK_SIZE,
453 453 RTEMS_DEFAULT_MODES,
454 454 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF3]
455 455 );
456 456 }
457 457 if (status == RTEMS_SUCCESSFUL) // CWF2
458 458 {
459 459 status = rtems_task_create(
460 460 Task_name[TASKID_CWF2], TASK_PRIORITY_CWF2, RTEMS_MINIMUM_STACK_SIZE,
461 461 RTEMS_DEFAULT_MODES,
462 462 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF2]
463 463 );
464 464 }
465 465 if (status == RTEMS_SUCCESSFUL) // CWF1
466 466 {
467 467 status = rtems_task_create(
468 468 Task_name[TASKID_CWF1], TASK_PRIORITY_CWF1, RTEMS_MINIMUM_STACK_SIZE,
469 469 RTEMS_DEFAULT_MODES,
470 470 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF1]
471 471 );
472 472 }
473 473 if (status == RTEMS_SUCCESSFUL) // SWBD
474 474 {
475 475 status = rtems_task_create(
476 476 Task_name[TASKID_SWBD], TASK_PRIORITY_SWBD, RTEMS_MINIMUM_STACK_SIZE,
477 477 RTEMS_DEFAULT_MODES,
478 478 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SWBD]
479 479 );
480 480 }
481 481
482 482 //*****
483 483 // MISC
484 484 if (status == RTEMS_SUCCESSFUL) // STAT
485 485 {
486 486 status = rtems_task_create(
487 487 Task_name[TASKID_STAT], TASK_PRIORITY_STAT, RTEMS_MINIMUM_STACK_SIZE,
488 488 RTEMS_DEFAULT_MODES,
489 489 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_STAT]
490 490 );
491 491 }
492 492 if (status == RTEMS_SUCCESSFUL) // DUMB
493 493 {
494 494 status = rtems_task_create(
495 495 Task_name[TASKID_DUMB], TASK_PRIORITY_DUMB, RTEMS_MINIMUM_STACK_SIZE,
496 496 RTEMS_DEFAULT_MODES,
497 497 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_DUMB]
498 498 );
499 499 }
500 500 if (status == RTEMS_SUCCESSFUL) // HOUS
501 501 {
502 502 status = rtems_task_create(
503 503 Task_name[TASKID_HOUS], TASK_PRIORITY_HOUS, RTEMS_MINIMUM_STACK_SIZE,
504 504 RTEMS_DEFAULT_MODES,
505 505 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_HOUS]
506 506 );
507 507 }
508 508
509 509 return status;
510 510 }
511 511
512 512 int start_recv_send_tasks( void )
513 513 {
514 514 rtems_status_code status;
515 515
516 516 status = rtems_task_start( Task_id[TASKID_RECV], recv_task, 1 );
517 517 if (status!=RTEMS_SUCCESSFUL) {
518 518 BOOT_PRINTF("in INIT *** Error starting TASK_RECV\n")
519 519 }
520 520
521 521 if (status == RTEMS_SUCCESSFUL) // SEND
522 522 {
523 523 status = rtems_task_start( Task_id[TASKID_SEND], send_task, 1 );
524 524 if (status!=RTEMS_SUCCESSFUL) {
525 525 BOOT_PRINTF("in INIT *** Error starting TASK_SEND\n")
526 526 }
527 527 }
528 528
529 529 return status;
530 530 }
531 531
532 532 int start_all_tasks( void ) // start all tasks except SEND RECV and HOUS
533 533 {
534 534 /** This function starts all RTEMS tasks used in the software.
535 535 *
536 536 * @return RTEMS directive status codes:
537 537 * - RTEMS_SUCCESSFUL - ask started successfully
538 538 * - RTEMS_INVALID_ADDRESS - invalid task entry point
539 539 * - RTEMS_INVALID_ID - invalid task id
540 540 * - RTEMS_INCORRECT_STATE - task not in the dormant state
541 541 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot start remote task
542 542 *
543 543 */
544 544 // starts all the tasks fot eh flight software
545 545
546 546 rtems_status_code status;
547 547
548 548 //**********
549 549 // SPACEWIRE
550 550 status = rtems_task_start( Task_id[TASKID_SPIQ], spiq_task, 1 );
551 551 if (status!=RTEMS_SUCCESSFUL) {
552 552 BOOT_PRINTF("in INIT *** Error starting TASK_SPIQ\n")
553 553 }
554 554
555 555 if (status == RTEMS_SUCCESSFUL) // WTDG
556 556 {
557 557 status = rtems_task_start( Task_id[TASKID_WTDG], wtdg_task, 1 );
558 558 if (status!=RTEMS_SUCCESSFUL) {
559 559 BOOT_PRINTF("in INIT *** Error starting TASK_WTDG\n")
560 560 }
561 561 }
562 562
563 563 if (status == RTEMS_SUCCESSFUL) // ACTN
564 564 {
565 565 status = rtems_task_start( Task_id[TASKID_ACTN], actn_task, 1 );
566 566 if (status!=RTEMS_SUCCESSFUL) {
567 567 BOOT_PRINTF("in INIT *** Error starting TASK_ACTN\n")
568 568 }
569 569 }
570 570
571 571 //******************
572 572 // SPECTRAL MATRICES
573 573 if (status == RTEMS_SUCCESSFUL) // AVF0
574 574 {
575 575 status = rtems_task_start( Task_id[TASKID_AVF0], avf0_task, LFR_MODE_STANDBY );
576 576 if (status!=RTEMS_SUCCESSFUL) {
577 577 BOOT_PRINTF("in INIT *** Error starting TASK_AVF0\n")
578 578 }
579 579 }
580 580 if (status == RTEMS_SUCCESSFUL) // PRC0
581 581 {
582 582 status = rtems_task_start( Task_id[TASKID_PRC0], prc0_task, LFR_MODE_STANDBY );
583 583 if (status!=RTEMS_SUCCESSFUL) {
584 584 BOOT_PRINTF("in INIT *** Error starting TASK_PRC0\n")
585 585 }
586 586 }
587 587 if (status == RTEMS_SUCCESSFUL) // AVF1
588 588 {
589 589 status = rtems_task_start( Task_id[TASKID_AVF1], avf1_task, LFR_MODE_STANDBY );
590 590 if (status!=RTEMS_SUCCESSFUL) {
591 591 BOOT_PRINTF("in INIT *** Error starting TASK_AVF1\n")
592 592 }
593 593 }
594 594 if (status == RTEMS_SUCCESSFUL) // PRC1
595 595 {
596 596 status = rtems_task_start( Task_id[TASKID_PRC1], prc1_task, LFR_MODE_STANDBY );
597 597 if (status!=RTEMS_SUCCESSFUL) {
598 598 BOOT_PRINTF("in INIT *** Error starting TASK_PRC1\n")
599 599 }
600 600 }
601 601 if (status == RTEMS_SUCCESSFUL) // AVF2
602 602 {
603 603 status = rtems_task_start( Task_id[TASKID_AVF2], avf2_task, 1 );
604 604 if (status!=RTEMS_SUCCESSFUL) {
605 605 BOOT_PRINTF("in INIT *** Error starting TASK_AVF2\n")
606 606 }
607 607 }
608 608 if (status == RTEMS_SUCCESSFUL) // PRC2
609 609 {
610 610 status = rtems_task_start( Task_id[TASKID_PRC2], prc2_task, 1 );
611 611 if (status!=RTEMS_SUCCESSFUL) {
612 612 BOOT_PRINTF("in INIT *** Error starting TASK_PRC2\n")
613 613 }
614 614 }
615 615
616 616 //****************
617 617 // WAVEFORM PICKER
618 618 if (status == RTEMS_SUCCESSFUL) // WFRM
619 619 {
620 620 status = rtems_task_start( Task_id[TASKID_WFRM], wfrm_task, 1 );
621 621 if (status!=RTEMS_SUCCESSFUL) {
622 622 BOOT_PRINTF("in INIT *** Error starting TASK_WFRM\n")
623 623 }
624 624 }
625 625 if (status == RTEMS_SUCCESSFUL) // CWF3
626 626 {
627 627 status = rtems_task_start( Task_id[TASKID_CWF3], cwf3_task, 1 );
628 628 if (status!=RTEMS_SUCCESSFUL) {
629 629 BOOT_PRINTF("in INIT *** Error starting TASK_CWF3\n")
630 630 }
631 631 }
632 632 if (status == RTEMS_SUCCESSFUL) // CWF2
633 633 {
634 634 status = rtems_task_start( Task_id[TASKID_CWF2], cwf2_task, 1 );
635 635 if (status!=RTEMS_SUCCESSFUL) {
636 636 BOOT_PRINTF("in INIT *** Error starting TASK_CWF2\n")
637 637 }
638 638 }
639 639 if (status == RTEMS_SUCCESSFUL) // CWF1
640 640 {
641 641 status = rtems_task_start( Task_id[TASKID_CWF1], cwf1_task, 1 );
642 642 if (status!=RTEMS_SUCCESSFUL) {
643 643 BOOT_PRINTF("in INIT *** Error starting TASK_CWF1\n")
644 644 }
645 645 }
646 646 if (status == RTEMS_SUCCESSFUL) // SWBD
647 647 {
648 648 status = rtems_task_start( Task_id[TASKID_SWBD], swbd_task, 1 );
649 649 if (status!=RTEMS_SUCCESSFUL) {
650 650 BOOT_PRINTF("in INIT *** Error starting TASK_SWBD\n")
651 651 }
652 652 }
653 653
654 654 //*****
655 655 // MISC
656 656 if (status == RTEMS_SUCCESSFUL) // HOUS
657 657 {
658 658 status = rtems_task_start( Task_id[TASKID_HOUS], hous_task, 1 );
659 659 if (status!=RTEMS_SUCCESSFUL) {
660 660 BOOT_PRINTF("in INIT *** Error starting TASK_HOUS\n")
661 661 }
662 662 }
663 663 if (status == RTEMS_SUCCESSFUL) // DUMB
664 664 {
665 665 status = rtems_task_start( Task_id[TASKID_DUMB], dumb_task, 1 );
666 666 if (status!=RTEMS_SUCCESSFUL) {
667 667 BOOT_PRINTF("in INIT *** Error starting TASK_DUMB\n")
668 668 }
669 669 }
670 670 if (status == RTEMS_SUCCESSFUL) // STAT
671 671 {
672 672 status = rtems_task_start( Task_id[TASKID_STAT], stat_task, 1 );
673 673 if (status!=RTEMS_SUCCESSFUL) {
674 674 BOOT_PRINTF("in INIT *** Error starting TASK_STAT\n")
675 675 }
676 676 }
677 677
678 678 return status;
679 679 }
680 680
681 681 rtems_status_code create_message_queues( void ) // create the two message queues used in the software
682 682 {
683 683 rtems_status_code status_recv;
684 684 rtems_status_code status_send;
685 685 rtems_status_code status_q_p0;
686 686 rtems_status_code status_q_p1;
687 687 rtems_status_code status_q_p2;
688 688 rtems_status_code ret;
689 689 rtems_id queue_id;
690 690
691 691 //****************************************
692 692 // create the queue for handling valid TCs
693 693 status_recv = rtems_message_queue_create( misc_name[QUEUE_RECV],
694 694 MSG_QUEUE_COUNT_RECV, CCSDS_TC_PKT_MAX_SIZE,
695 695 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
696 696 if ( status_recv != RTEMS_SUCCESSFUL ) {
697 697 PRINTF1("in create_message_queues *** ERR creating QUEU queue, %d\n", status_recv)
698 698 }
699 699
700 700 //************************************************
701 701 // create the queue for handling TM packet sending
702 702 status_send = rtems_message_queue_create( misc_name[QUEUE_SEND],
703 703 MSG_QUEUE_COUNT_SEND, MSG_QUEUE_SIZE_SEND,
704 704 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
705 705 if ( status_send != RTEMS_SUCCESSFUL ) {
706 706 PRINTF1("in create_message_queues *** ERR creating PKTS queue, %d\n", status_send)
707 707 }
708 708
709 709 //*****************************************************************************
710 710 // create the queue for handling averaged spectral matrices for processing @ f0
711 711 status_q_p0 = rtems_message_queue_create( misc_name[QUEUE_PRC0],
712 712 MSG_QUEUE_COUNT_PRC0, MSG_QUEUE_SIZE_PRC0,
713 713 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
714 714 if ( status_q_p0 != RTEMS_SUCCESSFUL ) {
715 715 PRINTF1("in create_message_queues *** ERR creating Q_P0 queue, %d\n", status_q_p0)
716 716 }
717 717
718 718 //*****************************************************************************
719 719 // create the queue for handling averaged spectral matrices for processing @ f1
720 720 status_q_p1 = rtems_message_queue_create( misc_name[QUEUE_PRC1],
721 721 MSG_QUEUE_COUNT_PRC1, MSG_QUEUE_SIZE_PRC1,
722 722 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
723 723 if ( status_q_p1 != RTEMS_SUCCESSFUL ) {
724 724 PRINTF1("in create_message_queues *** ERR creating Q_P1 queue, %d\n", status_q_p1)
725 725 }
726 726
727 727 //*****************************************************************************
728 728 // create the queue for handling averaged spectral matrices for processing @ f2
729 729 status_q_p2 = rtems_message_queue_create( misc_name[QUEUE_PRC2],
730 730 MSG_QUEUE_COUNT_PRC2, MSG_QUEUE_SIZE_PRC2,
731 731 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
732 732 if ( status_q_p2 != RTEMS_SUCCESSFUL ) {
733 733 PRINTF1("in create_message_queues *** ERR creating Q_P2 queue, %d\n", status_q_p2)
734 734 }
735 735
736 736 if ( status_recv != RTEMS_SUCCESSFUL )
737 737 {
738 738 ret = status_recv;
739 739 }
740 740 else if( status_send != RTEMS_SUCCESSFUL )
741 741 {
742 742 ret = status_send;
743 743 }
744 744 else if( status_q_p0 != RTEMS_SUCCESSFUL )
745 745 {
746 746 ret = status_q_p0;
747 747 }
748 748 else if( status_q_p1 != RTEMS_SUCCESSFUL )
749 749 {
750 750 ret = status_q_p1;
751 751 }
752 752 else
753 753 {
754 754 ret = status_q_p2;
755 755 }
756 756
757 757 return ret;
758 758 }
759 759
760 760 rtems_status_code get_message_queue_id_send( rtems_id *queue_id )
761 761 {
762 762 rtems_status_code status;
763 763 rtems_name queue_name;
764 764
765 765 queue_name = rtems_build_name( 'Q', '_', 'S', 'D' );
766 766
767 767 status = rtems_message_queue_ident( queue_name, 0, queue_id );
768 768
769 769 return status;
770 770 }
771 771
772 772 rtems_status_code get_message_queue_id_recv( rtems_id *queue_id )
773 773 {
774 774 rtems_status_code status;
775 775 rtems_name queue_name;
776 776
777 777 queue_name = rtems_build_name( 'Q', '_', 'R', 'V' );
778 778
779 779 status = rtems_message_queue_ident( queue_name, 0, queue_id );
780 780
781 781 return status;
782 782 }
783 783
784 784 rtems_status_code get_message_queue_id_prc0( rtems_id *queue_id )
785 785 {
786 786 rtems_status_code status;
787 787 rtems_name queue_name;
788 788
789 789 queue_name = rtems_build_name( 'Q', '_', 'P', '0' );
790 790
791 791 status = rtems_message_queue_ident( queue_name, 0, queue_id );
792 792
793 793 return status;
794 794 }
795 795
796 796 rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id )
797 797 {
798 798 rtems_status_code status;
799 799 rtems_name queue_name;
800 800
801 801 queue_name = rtems_build_name( 'Q', '_', 'P', '1' );
802 802
803 803 status = rtems_message_queue_ident( queue_name, 0, queue_id );
804 804
805 805 return status;
806 806 }
807 807
808 808 rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id )
809 809 {
810 810 rtems_status_code status;
811 811 rtems_name queue_name;
812 812
813 813 queue_name = rtems_build_name( 'Q', '_', 'P', '2' );
814 814
815 815 status = rtems_message_queue_ident( queue_name, 0, queue_id );
816 816
817 817 return status;
818 818 }
819 819
820 820 void update_queue_max_count( rtems_id queue_id, unsigned char*fifo_size_max )
821 821 {
822 822 u_int32_t count;
823 823 rtems_status_code status;
824 824
825 825 status = rtems_message_queue_get_number_pending( queue_id, &count );
826 826
827 827 count = count + 1;
828 828
829 829 if (status != RTEMS_SUCCESSFUL)
830 830 {
831 831 PRINTF1("in update_queue_max_count *** ERR = %d\n", status)
832 832 }
833 833 else
834 834 {
835 835 if (count > *fifo_size_max)
836 836 {
837 837 *fifo_size_max = count;
838 838 }
839 839 }
840 840 }
841 841
842 842 void init_ring(ring_node ring[], unsigned char nbNodes, volatile int buffer[], unsigned int bufferSize )
843 843 {
844 844 unsigned char i;
845 845
846 846 //***************
847 847 // BUFFER ADDRESS
848 848 for(i=0; i<nbNodes; i++)
849 849 {
850 850 ring[i].coarseTime = 0xffffffff;
851 851 ring[i].fineTime = 0xffffffff;
852 852 ring[i].sid = 0x00;
853 853 ring[i].status = 0x00;
854 854 ring[i].buffer_address = (int) &buffer[ i * bufferSize ];
855 855 }
856 856
857 857 //*****
858 858 // NEXT
859 859 ring[ nbNodes - 1 ].next = (ring_node*) &ring[ 0 ];
860 860 for(i=0; i<nbNodes-1; i++)
861 861 {
862 862 ring[i].next = (ring_node*) &ring[ i + 1 ];
863 863 }
864 864
865 865 //*********
866 866 // PREVIOUS
867 867 ring[ 0 ].previous = (ring_node*) &ring[ nbNodes - 1 ];
868 868 for(i=1; i<nbNodes; i++)
869 869 {
870 870 ring[i].previous = (ring_node*) &ring[ i - 1 ];
871 871 }
872 872 }
Show file before | Show file after | Hide comments
@@ -1,571 +1,570
1 1 /** General usage functions and RTEMS tasks.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 */
7 7
8 8 #include "fsw_misc.h"
9 9
10 10 void configure_timer(gptimer_regs_t *gptimer_regs, unsigned char timer, unsigned int clock_divider,
11 11 unsigned char interrupt_level, rtems_isr (*timer_isr)() )
12 12 {
13 13 /** This function configures a GPTIMER timer instantiated in the VHDL design.
14 14 *
15 15 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
16 16 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
17 17 * @param clock_divider is the divider of the 1 MHz clock that will be configured.
18 18 * @param interrupt_level is the interrupt level that the timer drives.
19 19 * @param timer_isr is the interrupt subroutine that will be attached to the IRQ driven by the timer.
20 20 *
21 21 * Interrupt levels are described in the SPARC documentation sparcv8.pdf p.76
22 22 *
23 23 */
24 24
25 25 rtems_status_code status;
26 26 rtems_isr_entry old_isr_handler;
27 27
28 28 gptimer_regs->timer[timer].ctrl = 0x00; // reset the control register
29 29
30 30 status = rtems_interrupt_catch( timer_isr, interrupt_level, &old_isr_handler) ; // see sparcv8.pdf p.76 for interrupt levels
31 31 if (status!=RTEMS_SUCCESSFUL)
32 32 {
33 33 PRINTF("in configure_timer *** ERR rtems_interrupt_catch\n")
34 34 }
35 35
36 36 timer_set_clock_divider( gptimer_regs, timer, clock_divider);
37 37 }
38 38
39 39 void timer_start(gptimer_regs_t *gptimer_regs, unsigned char timer)
40 40 {
41 41 /** This function starts a GPTIMER timer.
42 42 *
43 43 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
44 44 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
45 45 *
46 46 */
47 47
48 48 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000010; // clear pending IRQ if any
49 49 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000004; // LD load value from the reload register
50 50 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000001; // EN enable the timer
51 51 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000002; // RS restart
52 52 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000008; // IE interrupt enable
53 53 }
54 54
55 55 void timer_stop(gptimer_regs_t *gptimer_regs, unsigned char timer)
56 56 {
57 57 /** This function stops a GPTIMER timer.
58 58 *
59 59 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
60 60 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
61 61 *
62 62 */
63 63
64 64 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl & 0xfffffffe; // EN enable the timer
65 65 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl & 0xffffffef; // IE interrupt enable
66 66 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000010; // clear pending IRQ if any
67 67 }
68 68
69 69 void timer_set_clock_divider(gptimer_regs_t *gptimer_regs, unsigned char timer, unsigned int clock_divider)
70 70 {
71 71 /** This function sets the clock divider of a GPTIMER timer.
72 72 *
73 73 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
74 74 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
75 75 * @param clock_divider is the divider of the 1 MHz clock that will be configured.
76 76 *
77 77 */
78 78
79 79 gptimer_regs->timer[timer].reload = clock_divider; // base clock frequency is 1 MHz
80 80 }
81 81
82 82 int send_console_outputs_on_apbuart_port( void ) // Send the console outputs on the apbuart port
83 83 {
84 84 struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) REGS_ADDR_APBUART;
85 85
86 86 apbuart_regs->ctrl = APBUART_CTRL_REG_MASK_TE;
87 87
88 88 return 0;
89 89 }
90 90
91 91 int enable_apbuart_transmitter( void ) // set the bit 1, TE Transmitter Enable to 1 in the APBUART control register
92 92 {
93 93 struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) REGS_ADDR_APBUART;
94 94
95 95 apbuart_regs->ctrl = apbuart_regs->ctrl | APBUART_CTRL_REG_MASK_TE;
96 96
97 97 return 0;
98 98 }
99 99
100 100 void set_apbuart_scaler_reload_register(unsigned int regs, unsigned int value)
101 101 {
102 102 /** This function sets the scaler reload register of the apbuart module
103 103 *
104 104 * @param regs is the address of the apbuart registers in memory
105 105 * @param value is the value that will be stored in the scaler register
106 106 *
107 107 * The value shall be set by the software to get data on the serial interface.
108 108 *
109 109 */
110 110
111 111 struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) regs;
112 112
113 113 apbuart_regs->scaler = value;
114 114 BOOT_PRINTF1("OK *** apbuart port scaler reload register set to 0x%x\n", value)
115 115 }
116 116
117 117 //************
118 118 // RTEMS TASKS
119 119
120 120 rtems_task stat_task(rtems_task_argument argument)
121 121 {
122 122 int i;
123 123 int j;
124 124 i = 0;
125 125 j = 0;
126 126 BOOT_PRINTF("in STAT *** \n")
127 127 while(1){
128 128 rtems_task_wake_after(1000);
129 129 PRINTF1("%d\n", j)
130 130 if (i == CPU_USAGE_REPORT_PERIOD) {
131 131 // #ifdef PRINT_TASK_STATISTICS
132 132 // rtems_cpu_usage_report();
133 133 // rtems_cpu_usage_reset();
134 134 // #endif
135 135 i = 0;
136 136 }
137 137 else i++;
138 138 j++;
139 139 }
140 140 }
141 141
142 142 rtems_task hous_task(rtems_task_argument argument)
143 143 {
144 144 rtems_status_code status;
145 145 rtems_status_code spare_status;
146 146 rtems_id queue_id;
147 147 rtems_rate_monotonic_period_status period_status;
148 148
149 149 status = get_message_queue_id_send( &queue_id );
150 150 if (status != RTEMS_SUCCESSFUL)
151 151 {
152 152 PRINTF1("in HOUS *** ERR get_message_queue_id_send %d\n", status)
153 153 }
154 154
155 155 BOOT_PRINTF("in HOUS ***\n")
156 156
157 157 if (rtems_rate_monotonic_ident( name_hk_rate_monotonic, &HK_id) != RTEMS_SUCCESSFUL) {
158 158 status = rtems_rate_monotonic_create( name_hk_rate_monotonic, &HK_id );
159 159 if( status != RTEMS_SUCCESSFUL ) {
160 160 PRINTF1( "rtems_rate_monotonic_create failed with status of %d\n", status )
161 161 }
162 162 }
163 163
164 164 status = rtems_rate_monotonic_cancel(HK_id);
165 165 if( status != RTEMS_SUCCESSFUL ) {
166 166 PRINTF1( "ERR *** in HOUS *** rtems_rate_monotonic_cancel(HK_id) ***code: %d\n", status )
167 167 }
168 168 else {
169 169 DEBUG_PRINTF("OK *** in HOUS *** rtems_rate_monotonic_cancel(HK_id)\n")
170 170 }
171 171
172 172 // startup phase
173 173 status = rtems_rate_monotonic_period( HK_id, SY_LFR_TIME_SYN_TIMEOUT_in_ticks );
174 174 status = rtems_rate_monotonic_get_status( HK_id, &period_status );
175 175 DEBUG_PRINTF1("startup HK, HK_id status = %d\n", period_status.state)
176 176 while(period_status.state != RATE_MONOTONIC_EXPIRED ) // after SY_LFR_TIME_SYN_TIMEOUT ms, starts HK anyway
177 177 {
178 178 if ((time_management_regs->coarse_time & 0x80000000) == 0x00000000) // check time synchronization
179 179 {
180 180 break; // break if LFR is synchronized
181 181 }
182 182 else
183 183 {
184 184 status = rtems_rate_monotonic_get_status( HK_id, &period_status );
185 185 // sched_yield();
186 186 status = rtems_task_wake_after( 10 ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 100 ms = 10 * 10 ms
187 187 }
188 188 }
189 189 status = rtems_rate_monotonic_cancel(HK_id);
190 190 DEBUG_PRINTF1("startup HK, HK_id status = %d\n", period_status.state)
191 191
192 192 set_hk_lfr_reset_cause( POWER_ON );
193 193
194 194 while(1){ // launch the rate monotonic task
195 195 status = rtems_rate_monotonic_period( HK_id, HK_PERIOD );
196 196 if ( status != RTEMS_SUCCESSFUL ) {
197 197 PRINTF1( "in HOUS *** ERR period: %d\n", status);
198 198 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_6 );
199 199 }
200 200 else {
201 201 housekeeping_packet.packetSequenceControl[0] = (unsigned char) (sequenceCounterHK >> 8);
202 202 housekeeping_packet.packetSequenceControl[1] = (unsigned char) (sequenceCounterHK );
203 203 increment_seq_counter( &sequenceCounterHK );
204 204
205 205 housekeeping_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
206 206 housekeeping_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
207 207 housekeeping_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
208 208 housekeeping_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
209 209 housekeeping_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
210 210 housekeeping_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
211 211
212 212 spacewire_update_statistics();
213 213
214 214 housekeeping_packet.hk_lfr_q_sd_fifo_size_max = hk_lfr_q_sd_fifo_size_max;
215 215 housekeeping_packet.hk_lfr_q_rv_fifo_size_max = hk_lfr_q_rv_fifo_size_max;
216 216 housekeeping_packet.hk_lfr_q_p0_fifo_size_max = hk_lfr_q_p0_fifo_size_max;
217 217 housekeeping_packet.hk_lfr_q_p1_fifo_size_max = hk_lfr_q_p1_fifo_size_max;
218 218 housekeeping_packet.hk_lfr_q_p2_fifo_size_max = hk_lfr_q_p2_fifo_size_max;
219 219
220 220 housekeeping_packet.sy_lfr_common_parameters_spare = parameter_dump_packet.sy_lfr_common_parameters_spare;
221 221 housekeeping_packet.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
222 222 get_temperatures( housekeeping_packet.hk_lfr_temp_scm );
223 223 get_v_e1_e2_f3( housekeeping_packet.hk_lfr_sc_v_f3 );
224 224 get_cpu_load( (unsigned char *) &housekeeping_packet.hk_lfr_cpu_load );
225 225
226 226 // SEND PACKET
227 227 status = rtems_message_queue_send( queue_id, &housekeeping_packet,
228 228 PACKET_LENGTH_HK + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
229 229 if (status != RTEMS_SUCCESSFUL) {
230 230 PRINTF1("in HOUS *** ERR send: %d\n", status)
231 231 }
232 232 }
233 233 }
234 234
235 235 PRINTF("in HOUS *** deleting task\n")
236 236
237 237 status = rtems_task_delete( RTEMS_SELF ); // should not return
238 printf( "rtems_task_delete returned with status of %d.\n", status );
238
239 239 return;
240 240 }
241 241
242 242 rtems_task dumb_task( rtems_task_argument unused )
243 243 {
244 244 /** This RTEMS taks is used to print messages without affecting the general behaviour of the software.
245 245 *
246 246 * @param unused is the starting argument of the RTEMS task
247 247 *
248 248 * The DUMB taks waits for RTEMS events and print messages depending on the incoming events.
249 249 *
250 250 */
251 251
252 252 unsigned int i;
253 253 unsigned int intEventOut;
254 254 unsigned int coarse_time = 0;
255 255 unsigned int fine_time = 0;
256 256 rtems_event_set event_out;
257 257
258 258 char *DumbMessages[12] = {"in DUMB *** default", // RTEMS_EVENT_0
259 259 "in DUMB *** timecode_irq_handler", // RTEMS_EVENT_1
260 260 "in DUMB *** f3 buffer changed", // RTEMS_EVENT_2
261 261 "in DUMB *** in SMIQ *** Error sending event to AVF0", // RTEMS_EVENT_3
262 262 "in DUMB *** spectral_matrices_isr *** Error sending event to SMIQ", // RTEMS_EVENT_4
263 263 "in DUMB *** waveforms_simulator_isr", // RTEMS_EVENT_5
264 264 "VHDL SM *** two buffers f0 ready", // RTEMS_EVENT_6
265 265 "ready for dump", // RTEMS_EVENT_7
266 266 "VHDL ERR *** spectral matrix", // RTEMS_EVENT_8
267 267 "tick", // RTEMS_EVENT_9
268 268 "VHDL ERR *** waveform picker", // RTEMS_EVENT_10
269 269 "VHDL ERR *** unexpected ready matrix values" // RTEMS_EVENT_11
270 270 };
271 271
272 272 BOOT_PRINTF("in DUMB *** \n")
273 273
274 274 while(1){
275 275 rtems_event_receive(RTEMS_EVENT_0 | RTEMS_EVENT_1 | RTEMS_EVENT_2 | RTEMS_EVENT_3
276 276 | RTEMS_EVENT_4 | RTEMS_EVENT_5 | RTEMS_EVENT_6 | RTEMS_EVENT_7
277 277 | RTEMS_EVENT_8 | RTEMS_EVENT_9,
278 278 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT
279 279 intEventOut = (unsigned int) event_out;
280 280 for ( i=0; i<32; i++)
281 281 {
282 282 if ( ((intEventOut >> i) & 0x0001) != 0)
283 283 {
284 284 coarse_time = time_management_regs->coarse_time;
285 285 fine_time = time_management_regs->fine_time;
286 printf("in DUMB *** coarse: %x, fine: %x, %s\n", coarse_time, fine_time, DumbMessages[i]);
287 286 if (i==8)
288 287 {
289 288 }
290 289 if (i==10)
291 290 {
292 291 }
293 292 }
294 293 }
295 294 }
296 295 }
297 296
298 297 //*****************************
299 298 // init housekeeping parameters
300 299
301 300 void init_housekeeping_parameters( void )
302 301 {
303 302 /** This function initialize the housekeeping_packet global variable with default values.
304 303 *
305 304 */
306 305
307 306 unsigned int i = 0;
308 307 unsigned char *parameters;
309 308 unsigned char sizeOfHK;
310 309
311 310 sizeOfHK = sizeof( Packet_TM_LFR_HK_t );
312 311
313 312 parameters = (unsigned char*) &housekeeping_packet;
314 313
315 314 for(i = 0; i< sizeOfHK; i++)
316 315 {
317 316 parameters[i] = 0x00;
318 317 }
319 318
320 319 housekeeping_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
321 320 housekeeping_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
322 321 housekeeping_packet.reserved = DEFAULT_RESERVED;
323 322 housekeeping_packet.userApplication = CCSDS_USER_APP;
324 323 housekeeping_packet.packetID[0] = (unsigned char) (APID_TM_HK >> 8);
325 324 housekeeping_packet.packetID[1] = (unsigned char) (APID_TM_HK);
326 325 housekeeping_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
327 326 housekeeping_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
328 327 housekeeping_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_HK >> 8);
329 328 housekeeping_packet.packetLength[1] = (unsigned char) (PACKET_LENGTH_HK );
330 329 housekeeping_packet.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
331 330 housekeeping_packet.serviceType = TM_TYPE_HK;
332 331 housekeeping_packet.serviceSubType = TM_SUBTYPE_HK;
333 332 housekeeping_packet.destinationID = TM_DESTINATION_ID_GROUND;
334 333 housekeeping_packet.sid = SID_HK;
335 334
336 335 // init status word
337 336 housekeeping_packet.lfr_status_word[0] = DEFAULT_STATUS_WORD_BYTE0;
338 337 housekeeping_packet.lfr_status_word[1] = DEFAULT_STATUS_WORD_BYTE1;
339 338 // init software version
340 339 housekeeping_packet.lfr_sw_version[0] = SW_VERSION_N1;
341 340 housekeeping_packet.lfr_sw_version[1] = SW_VERSION_N2;
342 341 housekeeping_packet.lfr_sw_version[2] = SW_VERSION_N3;
343 342 housekeeping_packet.lfr_sw_version[3] = SW_VERSION_N4;
344 343 // init fpga version
345 344 parameters = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
346 345 housekeeping_packet.lfr_fpga_version[0] = parameters[1]; // n1
347 346 housekeeping_packet.lfr_fpga_version[1] = parameters[2]; // n2
348 347 housekeeping_packet.lfr_fpga_version[2] = parameters[3]; // n3
349 348
350 349 housekeeping_packet.hk_lfr_q_sd_fifo_size = MSG_QUEUE_COUNT_SEND;
351 350 housekeeping_packet.hk_lfr_q_rv_fifo_size = MSG_QUEUE_COUNT_RECV;
352 351 housekeeping_packet.hk_lfr_q_p0_fifo_size = MSG_QUEUE_COUNT_PRC0;
353 352 housekeeping_packet.hk_lfr_q_p1_fifo_size = MSG_QUEUE_COUNT_PRC1;
354 353 housekeeping_packet.hk_lfr_q_p2_fifo_size = MSG_QUEUE_COUNT_PRC2;
355 354 }
356 355
357 356 void increment_seq_counter( unsigned short *packetSequenceControl )
358 357 {
359 358 /** This function increment the sequence counter passes in argument.
360 359 *
361 360 * The increment does not affect the grouping flag. In case of an overflow, the counter is reset to 0.
362 361 *
363 362 */
364 363
365 364 unsigned short segmentation_grouping_flag;
366 365 unsigned short sequence_cnt;
367 366
368 367 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << 8; // keep bits 7 downto 6
369 368 sequence_cnt = (*packetSequenceControl) & 0x3fff; // [0011 1111 1111 1111]
370 369
371 370 if ( sequence_cnt < SEQ_CNT_MAX)
372 371 {
373 372 sequence_cnt = sequence_cnt + 1;
374 373 }
375 374 else
376 375 {
377 376 sequence_cnt = 0;
378 377 }
379 378
380 379 *packetSequenceControl = segmentation_grouping_flag | sequence_cnt ;
381 380 }
382 381
383 382 void getTime( unsigned char *time)
384 383 {
385 384 /** This function write the current local time in the time buffer passed in argument.
386 385 *
387 386 */
388 387
389 388 time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
390 389 time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
391 390 time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
392 391 time[3] = (unsigned char) (time_management_regs->coarse_time);
393 392 time[4] = (unsigned char) (time_management_regs->fine_time>>8);
394 393 time[5] = (unsigned char) (time_management_regs->fine_time);
395 394 }
396 395
397 396 unsigned long long int getTimeAsUnsignedLongLongInt( )
398 397 {
399 398 /** This function write the current local time in the time buffer passed in argument.
400 399 *
401 400 */
402 401 unsigned long long int time;
403 402
404 403 time = ( (unsigned long long int) (time_management_regs->coarse_time & 0x7fffffff) << 16 )
405 404 + time_management_regs->fine_time;
406 405
407 406 return time;
408 407 }
409 408
410 409 void send_dumb_hk( void )
411 410 {
412 411 Packet_TM_LFR_HK_t dummy_hk_packet;
413 412 unsigned char *parameters;
414 413 unsigned int i;
415 414 rtems_id queue_id;
416 415
417 416 dummy_hk_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
418 417 dummy_hk_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
419 418 dummy_hk_packet.reserved = DEFAULT_RESERVED;
420 419 dummy_hk_packet.userApplication = CCSDS_USER_APP;
421 420 dummy_hk_packet.packetID[0] = (unsigned char) (APID_TM_HK >> 8);
422 421 dummy_hk_packet.packetID[1] = (unsigned char) (APID_TM_HK);
423 422 dummy_hk_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
424 423 dummy_hk_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
425 424 dummy_hk_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_HK >> 8);
426 425 dummy_hk_packet.packetLength[1] = (unsigned char) (PACKET_LENGTH_HK );
427 426 dummy_hk_packet.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
428 427 dummy_hk_packet.serviceType = TM_TYPE_HK;
429 428 dummy_hk_packet.serviceSubType = TM_SUBTYPE_HK;
430 429 dummy_hk_packet.destinationID = TM_DESTINATION_ID_GROUND;
431 430 dummy_hk_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
432 431 dummy_hk_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
433 432 dummy_hk_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
434 433 dummy_hk_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
435 434 dummy_hk_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
436 435 dummy_hk_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
437 436 dummy_hk_packet.sid = SID_HK;
438 437
439 438 // init status word
440 439 dummy_hk_packet.lfr_status_word[0] = 0xff;
441 440 dummy_hk_packet.lfr_status_word[1] = 0xff;
442 441 // init software version
443 442 dummy_hk_packet.lfr_sw_version[0] = SW_VERSION_N1;
444 443 dummy_hk_packet.lfr_sw_version[1] = SW_VERSION_N2;
445 444 dummy_hk_packet.lfr_sw_version[2] = SW_VERSION_N3;
446 445 dummy_hk_packet.lfr_sw_version[3] = SW_VERSION_N4;
447 446 // init fpga version
448 447 parameters = (unsigned char *) (REGS_ADDR_WAVEFORM_PICKER + 0xb0);
449 448 dummy_hk_packet.lfr_fpga_version[0] = parameters[1]; // n1
450 449 dummy_hk_packet.lfr_fpga_version[1] = parameters[2]; // n2
451 450 dummy_hk_packet.lfr_fpga_version[2] = parameters[3]; // n3
452 451
453 452 parameters = (unsigned char *) &dummy_hk_packet.hk_lfr_cpu_load;
454 453
455 454 for (i=0; i<100; i++)
456 455 {
457 456 parameters[i] = 0xff;
458 457 }
459 458
460 459 get_message_queue_id_send( &queue_id );
461 460
462 461 rtems_message_queue_send( queue_id, &dummy_hk_packet,
463 462 PACKET_LENGTH_HK + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
464 463 }
465 464
466 465 void get_temperatures( unsigned char *temperatures )
467 466 {
468 467 unsigned char* temp_scm_ptr;
469 468 unsigned char* temp_pcb_ptr;
470 469 unsigned char* temp_fpga_ptr;
471 470
472 471 // SEL1 SEL0
473 472 // 0 0 => PCB
474 473 // 0 1 => FPGA
475 474 // 1 0 => SCM
476 475
477 476 temp_scm_ptr = (unsigned char *) &time_management_regs->temp_scm;
478 477 temp_pcb_ptr = (unsigned char *) &time_management_regs->temp_pcb;
479 478 temp_fpga_ptr = (unsigned char *) &time_management_regs->temp_fpga;
480 479
481 480 temperatures[0] = temp_scm_ptr[2];
482 481 temperatures[1] = temp_scm_ptr[3];
483 482 temperatures[2] = temp_pcb_ptr[2];
484 483 temperatures[3] = temp_pcb_ptr[3];
485 484 temperatures[4] = temp_fpga_ptr[2];
486 485 temperatures[5] = temp_fpga_ptr[3];
487 486 }
488 487
489 488 void get_v_e1_e2_f3( unsigned char *spacecraft_potential )
490 489 {
491 490 unsigned char* v_ptr;
492 491 unsigned char* e1_ptr;
493 492 unsigned char* e2_ptr;
494 493
495 494 v_ptr = (unsigned char *) &waveform_picker_regs->v;
496 495 e1_ptr = (unsigned char *) &waveform_picker_regs->e1;
497 496 e2_ptr = (unsigned char *) &waveform_picker_regs->e2;
498 497
499 498 spacecraft_potential[0] = v_ptr[2];
500 499 spacecraft_potential[1] = v_ptr[3];
501 500 spacecraft_potential[2] = e1_ptr[2];
502 501 spacecraft_potential[3] = e1_ptr[3];
503 502 spacecraft_potential[4] = e2_ptr[2];
504 503 spacecraft_potential[5] = e2_ptr[3];
505 504 }
506 505
507 506 void get_cpu_load( unsigned char *resource_statistics )
508 507 {
509 508 unsigned char cpu_load;
510 509
511 510 cpu_load = lfr_rtems_cpu_usage_report();
512 511
513 512 // HK_LFR_CPU_LOAD
514 513 resource_statistics[0] = cpu_load;
515 514
516 515 // HK_LFR_CPU_LOAD_MAX
517 516 if (cpu_load > resource_statistics[1])
518 517 {
519 518 resource_statistics[1] = cpu_load;
520 519 }
521 520
522 521 // CPU_LOAD_AVE
523 522 resource_statistics[2] = 0;
524 523
525 524 #ifndef PRINT_TASK_STATISTICS
526 525 rtems_cpu_usage_reset();
527 526 #endif
528 527
529 528 }
530 529
531 530 void set_hk_lfr_sc_potential_flag( bool state )
532 531 {
533 532 if (state == true)
534 533 {
535 534 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1] | 0x40; // [0100 0000]
536 535 }
537 536 else
538 537 {
539 538 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1] & 0xbf; // [1011 1111]
540 539 }
541 540 }
542 541
543 542 void set_hk_lfr_mag_fields_flag( bool state )
544 543 {
545 544 if (state == true)
546 545 {
547 546 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1] | 0x20; // [0010 0000]
548 547 }
549 548 else
550 549 {
551 550 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1] & 0xd7; // [1101 1111]
552 551 }
553 552 }
554 553
555 554 void set_hk_lfr_calib_enable( bool state )
556 555 {
557 556 if (state == true)
558 557 {
559 558 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1] | 0x08; // [0000 1000]
560 559 }
561 560 else
562 561 {
563 562 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1] & 0xf7; // [1111 0111]
564 563 }
565 564 }
566 565
567 566 void set_hk_lfr_reset_cause( enum lfr_reset_cause_t lfr_reset_cause )
568 567 {
569 568 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1]
570 569 | (lfr_reset_cause & 0x07 ); // [0000 0111]
571 570 }
Show file before | Show file after | Hide comments
@@ -1,1306 +1,1296
1 1 /** Functions related to the SpaceWire interface.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle SpaceWire transmissions:
7 7 * - configuration of the SpaceWire link
8 8 * - SpaceWire related interruption requests processing
9 9 * - transmission of TeleMetry packets by a dedicated RTEMS task
10 10 * - reception of TeleCommands by a dedicated RTEMS task
11 11 *
12 12 */
13 13
14 14 #include "fsw_spacewire.h"
15 15
16 16 rtems_name semq_name;
17 17 rtems_id semq_id;
18 18
19 19 //*****************
20 20 // waveform headers
21 21 Header_TM_LFR_SCIENCE_CWF_t headerCWF;
22 22 Header_TM_LFR_SCIENCE_SWF_t headerSWF;
23 23 Header_TM_LFR_SCIENCE_ASM_t headerASM;
24 24
25 25 //***********
26 26 // RTEMS TASK
27 27 rtems_task spiq_task(rtems_task_argument unused)
28 28 {
29 29 /** This RTEMS task is awaken by an rtems_event sent by the interruption subroutine of the SpaceWire driver.
30 30 *
31 31 * @param unused is the starting argument of the RTEMS task
32 32 *
33 33 */
34 34
35 35 rtems_event_set event_out;
36 36 rtems_status_code status;
37 37 int linkStatus;
38 38
39 39 BOOT_PRINTF("in SPIQ *** \n")
40 40
41 41 while(true){
42 42 rtems_event_receive(SPW_LINKERR_EVENT, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an SPW_LINKERR_EVENT
43 43 PRINTF("in SPIQ *** got SPW_LINKERR_EVENT\n")
44 44
45 45 // [0] SUSPEND RECV AND SEND TASKS
46 46 status = rtems_task_suspend( Task_id[ TASKID_RECV ] );
47 47 if ( status != RTEMS_SUCCESSFUL ) {
48 48 PRINTF("in SPIQ *** ERR suspending RECV Task\n")
49 49 }
50 50 status = rtems_task_suspend( Task_id[ TASKID_SEND ] );
51 51 if ( status != RTEMS_SUCCESSFUL ) {
52 52 PRINTF("in SPIQ *** ERR suspending SEND Task\n")
53 53 }
54 54
55 55 // [1] CHECK THE LINK
56 56 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (1)
57 57 if ( linkStatus != 5) {
58 58 PRINTF1("in SPIQ *** linkStatus %d, wait...\n", linkStatus)
59 59 status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms
60 60 }
61 61
62 62 // [2] RECHECK THE LINK AFTER SY_LFR_DPU_CONNECT_TIMEOUT
63 63 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (2)
64 64 if ( linkStatus != 5 ) // [2.a] not in run state, reset the link
65 65 {
66 66 spacewire_compute_stats_offsets();
67 67 status = spacewire_reset_link( );
68 68 }
69 69 else // [2.b] in run state, start the link
70 70 {
71 71 status = spacewire_stop_and_start_link( fdSPW ); // start the link
72 72 if ( status != RTEMS_SUCCESSFUL)
73 73 {
74 74 PRINTF1("in SPIQ *** ERR spacewire_stop_and_start_link %d\n", status)
75 75 }
76 76 }
77 77
78 78 // [3] COMPLETE RECOVERY ACTION AFTER SY_LFR_DPU_CONNECT_ATTEMPTS
79 79 if ( status == RTEMS_SUCCESSFUL ) // [3.a] the link is in run state and has been started successfully
80 80 {
81 81 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
82 82 if ( status != RTEMS_SUCCESSFUL ) {
83 83 PRINTF("in SPIQ *** ERR resuming SEND Task\n")
84 84 }
85 85 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
86 86 if ( status != RTEMS_SUCCESSFUL ) {
87 87 PRINTF("in SPIQ *** ERR resuming RECV Task\n")
88 88 }
89 89 }
90 90 else // [3.b] the link is not in run state, go in STANDBY mode
91 91 {
92 92 status = enter_mode( LFR_MODE_STANDBY, 0 );
93 93 if ( status != RTEMS_SUCCESSFUL ) {
94 94 PRINTF1("in SPIQ *** ERR enter_standby_mode *** code %d\n", status)
95 95 }
96 96 // wake the WTDG task up to wait for the link recovery
97 97 status = rtems_event_send ( Task_id[TASKID_WTDG], RTEMS_EVENT_0 );
98 98 status = rtems_task_suspend( RTEMS_SELF );
99 99 }
100 100 }
101 101 }
102 102
103 103 rtems_task recv_task( rtems_task_argument unused )
104 104 {
105 105 /** This RTEMS task is dedicated to the reception of incoming TeleCommands.
106 106 *
107 107 * @param unused is the starting argument of the RTEMS task
108 108 *
109 109 * The RECV task blocks on a call to the read system call, waiting for incoming SpaceWire data. When unblocked:
110 110 * 1. It reads the incoming data.
111 111 * 2. Launches the acceptance procedure.
112 112 * 3. If the Telecommand is valid, sends it to a dedicated RTEMS message queue.
113 113 *
114 114 */
115 115
116 116 int len;
117 117 ccsdsTelecommandPacket_t currentTC;
118 118 unsigned char computed_CRC[ 2 ];
119 119 unsigned char currentTC_LEN_RCV[ 2 ];
120 120 unsigned char destinationID;
121 121 unsigned int estimatedPacketLength;
122 122 unsigned int parserCode;
123 123 rtems_status_code status;
124 124 rtems_id queue_recv_id;
125 125 rtems_id queue_send_id;
126 126
127 127 initLookUpTableForCRC(); // the table is used to compute Cyclic Redundancy Codes
128 128
129 129 status = get_message_queue_id_recv( &queue_recv_id );
130 130 if (status != RTEMS_SUCCESSFUL)
131 131 {
132 132 PRINTF1("in RECV *** ERR get_message_queue_id_recv %d\n", status)
133 133 }
134 134
135 135 status = get_message_queue_id_send( &queue_send_id );
136 136 if (status != RTEMS_SUCCESSFUL)
137 137 {
138 138 PRINTF1("in RECV *** ERR get_message_queue_id_send %d\n", status)
139 139 }
140 140
141 141 BOOT_PRINTF("in RECV *** \n")
142 142
143 143 while(1)
144 144 {
145 145 len = read( fdSPW, (char*) &currentTC, CCSDS_TC_PKT_MAX_SIZE ); // the call to read is blocking
146 146 if (len == -1){ // error during the read call
147 147 PRINTF1("in RECV *** last read call returned -1, ERRNO %d\n", errno)
148 148 }
149 149 else {
150 150 if ( (len+1) < CCSDS_TC_PKT_MIN_SIZE ) {
151 151 PRINTF("in RECV *** packet lenght too short\n")
152 152 }
153 153 else {
154 154 estimatedPacketLength = (unsigned int) (len - CCSDS_TC_TM_PACKET_OFFSET - 3); // => -3 is for Prot ID, Reserved and User App bytes
155 155 currentTC_LEN_RCV[ 0 ] = (unsigned char) (estimatedPacketLength >> 8);
156 156 currentTC_LEN_RCV[ 1 ] = (unsigned char) (estimatedPacketLength );
157 157 // CHECK THE TC
158 158 parserCode = tc_parser( &currentTC, estimatedPacketLength, computed_CRC ) ;
159 159 if ( (parserCode == ILLEGAL_APID) || (parserCode == WRONG_LEN_PKT)
160 160 || (parserCode == INCOR_CHECKSUM) || (parserCode == ILL_TYPE)
161 161 || (parserCode == ILL_SUBTYPE) || (parserCode == WRONG_APP_DATA)
162 162 || (parserCode == WRONG_SRC_ID) )
163 163 { // send TM_LFR_TC_EXE_CORRUPTED
164 164 PRINTF1("TC corrupted received, with code: %d\n", parserCode)
165 165 if ( !( (currentTC.serviceType==TC_TYPE_TIME) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_TIME) )
166 166 &&
167 167 !( (currentTC.serviceType==TC_TYPE_GEN) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_INFO))
168 168 )
169 169 {
170 170 if ( parserCode == WRONG_SRC_ID )
171 171 {
172 172 destinationID = SID_TC_GROUND;
173 173 }
174 174 else
175 175 {
176 176 destinationID = currentTC.sourceID;
177 177 }
178 178 send_tm_lfr_tc_exe_corrupted( &currentTC, queue_send_id,
179 179 computed_CRC, currentTC_LEN_RCV,
180 180 destinationID );
181 181 }
182 182 }
183 183 else
184 184 { // send valid TC to the action launcher
185 185 status = rtems_message_queue_send( queue_recv_id, &currentTC,
186 186 estimatedPacketLength + CCSDS_TC_TM_PACKET_OFFSET + 3);
187 187 }
188 188 }
189 189 }
190 190
191 191 update_queue_max_count( queue_recv_id, &hk_lfr_q_rv_fifo_size_max );
192 192
193 193 }
194 194 }
195 195
196 196 rtems_task send_task( rtems_task_argument argument)
197 197 {
198 198 /** This RTEMS task is dedicated to the transmission of TeleMetry packets.
199 199 *
200 200 * @param unused is the starting argument of the RTEMS task
201 201 *
202 202 * The SEND task waits for a message to become available in the dedicated RTEMS queue. When a message arrives:
203 203 * - if the first byte is equal to CCSDS_DESTINATION_ID, the message is sent as is using the write system call.
204 204 * - if the first byte is not equal to CCSDS_DESTINATION_ID, the message is handled as a spw_ioctl_pkt_send. After
205 205 * analyzis, the packet is sent either using the write system call or using the ioctl call SPACEWIRE_IOCTRL_SEND, depending on the
206 206 * data it contains.
207 207 *
208 208 */
209 209
210 210 rtems_status_code status; // RTEMS status code
211 211 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
212 212 ring_node *incomingRingNodePtr;
213 213 int ring_node_address;
214 214 char *charPtr;
215 215 spw_ioctl_pkt_send *spw_ioctl_send;
216 216 size_t size; // size of the incoming TC packet
217 217 rtems_id queue_send_id;
218 218 unsigned int sid;
219 219
220 220 incomingRingNodePtr = NULL;
221 221 ring_node_address = 0;
222 222 charPtr = (char *) &ring_node_address;
223 223 sid = 0;
224 224
225 225 init_header_cwf( &headerCWF );
226 226 init_header_swf( &headerSWF );
227 227 init_header_asm( &headerASM );
228 228
229 229 status = get_message_queue_id_send( &queue_send_id );
230 230 if (status != RTEMS_SUCCESSFUL)
231 231 {
232 232 PRINTF1("in HOUS *** ERR get_message_queue_id_send %d\n", status)
233 233 }
234 234
235 235 BOOT_PRINTF("in SEND *** \n")
236 236
237 237 while(1)
238 238 {
239 239 status = rtems_message_queue_receive( queue_send_id, incomingData, &size,
240 240 RTEMS_WAIT, RTEMS_NO_TIMEOUT );
241 241
242 242 if (status!=RTEMS_SUCCESSFUL)
243 243 {
244 244 PRINTF1("in SEND *** (1) ERR = %d\n", status)
245 245 }
246 246 else
247 247 {
248 248 if ( size == sizeof(ring_node*) )
249 249 {
250 250 charPtr[0] = incomingData[0];
251 251 charPtr[1] = incomingData[1];
252 252 charPtr[2] = incomingData[2];
253 253 charPtr[3] = incomingData[3];
254 254 incomingRingNodePtr = (ring_node*) ring_node_address;
255 255 sid = incomingRingNodePtr->sid;
256 256 if ( (sid==SID_NORM_CWF_LONG_F3)
257 257 || (sid==SID_BURST_CWF_F2 )
258 258 || (sid==SID_SBM1_CWF_F1 )
259 259 || (sid==SID_SBM2_CWF_F2 ))
260 260 {
261 261 spw_send_waveform_CWF( incomingRingNodePtr, &headerCWF );
262 262 }
263 263 else if ( (sid==SID_NORM_SWF_F0) || (sid== SID_NORM_SWF_F1) || (sid==SID_NORM_SWF_F2) )
264 264 {
265 265 spw_send_waveform_SWF( incomingRingNodePtr, &headerSWF );
266 266 }
267 267 else if ( (sid==SID_NORM_CWF_F3) )
268 268 {
269 269 spw_send_waveform_CWF3_light( incomingRingNodePtr, &headerCWF );
270 270 }
271 271 else if (sid==SID_NORM_ASM_F0)
272 272 {
273 273 spw_send_asm_f0( incomingRingNodePtr, &headerASM );
274 274 }
275 275 else if (sid==SID_NORM_ASM_F1)
276 276 {
277 277 spw_send_asm_f1( incomingRingNodePtr, &headerASM );
278 278 }
279 279 else if (sid==SID_NORM_ASM_F2)
280 280 {
281 281 spw_send_asm_f2( incomingRingNodePtr, &headerASM );
282 282 }
283 283 else if ( sid==TM_CODE_K_DUMP )
284 284 {
285 285 spw_send_k_dump( incomingRingNodePtr );
286 286 }
287 287 else
288 288 {
289 printf("unexpected sid = %d\n", sid);
289 PRINTF1("unexpected sid = %d\n", sid);
290 290 }
291 291 }
292 292 else if ( incomingData[0] == CCSDS_DESTINATION_ID ) // the incoming message is a ccsds packet
293 293 {
294 294 status = write( fdSPW, incomingData, size );
295 295 if (status == -1){
296 296 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
297 297 }
298 298 }
299 299 else // the incoming message is a spw_ioctl_pkt_send structure
300 300 {
301 301 spw_ioctl_send = (spw_ioctl_pkt_send*) incomingData;
302 302 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, spw_ioctl_send );
303 303 if (status == -1){
304 printf("size = %d, %x, %x, %x, %x, %x\n",
305 size,
306 incomingData[0],
307 incomingData[1],
308 incomingData[2],
309 incomingData[3],
310 incomingData[4]);
311 304 PRINTF2("in SEND *** (2.b) ERRNO = %d, RTEMS = %d\n", errno, status)
312 305 }
313 306 }
314 307 }
315 308
316 309 update_queue_max_count( queue_send_id, &hk_lfr_q_sd_fifo_size_max );
317 310
318 311 }
319 312 }
320 313
321 314 rtems_task wtdg_task( rtems_task_argument argument )
322 315 {
323 316 rtems_event_set event_out;
324 317 rtems_status_code status;
325 318 int linkStatus;
326 319
327 320 BOOT_PRINTF("in WTDG ***\n")
328 321
329 322 while(1)
330 323 {
331 324 // wait for an RTEMS_EVENT
332 325 rtems_event_receive( RTEMS_EVENT_0,
333 326 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
334 327 PRINTF("in WTDG *** wait for the link\n")
335 328 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
336 329 while( linkStatus != 5) // wait for the link
337 330 {
338 331 status = rtems_task_wake_after( 10 ); // monitor the link each 100ms
339 332 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
340 333 }
341 334
342 335 status = spacewire_stop_and_start_link( fdSPW );
343 336
344 337 if (status != RTEMS_SUCCESSFUL)
345 338 {
346 339 PRINTF1("in WTDG *** ERR link not started %d\n", status)
347 340 }
348 341 else
349 342 {
350 343 PRINTF("in WTDG *** OK link started\n")
351 344 }
352 345
353 346 // restart the SPIQ task
354 347 status = rtems_task_restart( Task_id[TASKID_SPIQ], 1 );
355 348 if ( status != RTEMS_SUCCESSFUL ) {
356 349 PRINTF("in SPIQ *** ERR restarting SPIQ Task\n")
357 350 }
358 351
359 352 // restart RECV and SEND
360 353 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
361 354 if ( status != RTEMS_SUCCESSFUL ) {
362 355 PRINTF("in SPIQ *** ERR restarting SEND Task\n")
363 356 }
364 357 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
365 358 if ( status != RTEMS_SUCCESSFUL ) {
366 359 PRINTF("in SPIQ *** ERR restarting RECV Task\n")
367 360 }
368 361 }
369 362 }
370 363
371 364 //****************
372 365 // OTHER FUNCTIONS
373 366 int spacewire_open_link( void ) // by default, the driver resets the core: [SPW_CTRL_WRITE(pDev, SPW_CTRL_RESET);]
374 367 {
375 368 /** This function opens the SpaceWire link.
376 369 *
377 370 * @return a valid file descriptor in case of success, -1 in case of a failure
378 371 *
379 372 */
380 373 rtems_status_code status;
381 374
382 375 fdSPW = open(GRSPW_DEVICE_NAME, O_RDWR); // open the device. the open call resets the hardware
383 376 if ( fdSPW < 0 ) {
384 377 PRINTF1("ERR *** in configure_spw_link *** error opening "GRSPW_DEVICE_NAME" with ERR %d\n", errno)
385 378 }
386 379 else
387 380 {
388 381 status = RTEMS_SUCCESSFUL;
389 382 }
390 383
391 384 return status;
392 385 }
393 386
394 387 int spacewire_start_link( int fd )
395 388 {
396 389 rtems_status_code status;
397 390
398 391 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
399 392 // -1 default hardcoded driver timeout
400 393
401 394 return status;
402 395 }
403 396
404 397 int spacewire_stop_and_start_link( int fd )
405 398 {
406 399 rtems_status_code status;
407 400
408 401 status = ioctl( fd, SPACEWIRE_IOCTRL_STOP); // start fails if link pDev->running != 0
409 402 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
410 403 // -1 default hardcoded driver timeout
411 404
412 405 return status;
413 406 }
414 407
415 408 int spacewire_configure_link( int fd )
416 409 {
417 410 /** This function configures the SpaceWire link.
418 411 *
419 412 * @return GR-RTEMS-DRIVER directive status codes:
420 413 * - 22 EINVAL - Null pointer or an out of range value was given as the argument.
421 414 * - 16 EBUSY - Only used for SEND. Returned when no descriptors are avialble in non-blocking mode.
422 415 * - 88 ENOSYS - Returned for SET_DESTKEY if RMAP command handler is not available or if a non-implemented call is used.
423 416 * - 116 ETIMEDOUT - REturned for SET_PACKET_SIZE and START if the link could not be brought up.
424 417 * - 12 ENOMEM - Returned for SET_PACKETSIZE if it was unable to allocate the new buffers.
425 418 * - 5 EIO - Error when writing to grswp hardware registers.
426 419 * - 2 ENOENT - No such file or directory
427 420 */
428 421
429 422 rtems_status_code status;
430 423
431 424 spacewire_set_NP(1, REGS_ADDR_GRSPW); // [N]o [P]ort force
432 425 spacewire_set_RE(1, REGS_ADDR_GRSPW); // [R]MAP [E]nable, the dedicated call seems to break the no port force configuration
433 426
434 427 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_RXBLOCK, 1); // sets the blocking mode for reception
435 428 if (status!=RTEMS_SUCCESSFUL) {
436 429 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_RXBLOCK\n")
437 430 }
438 431 //
439 432 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_EVENT_ID, Task_id[TASKID_SPIQ]); // sets the task ID to which an event is sent when a
440 433 if (status!=RTEMS_SUCCESSFUL) {
441 434 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_EVENT_ID\n") // link-error interrupt occurs
442 435 }
443 436 //
444 437 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_DISABLE_ERR, 0); // automatic link-disabling due to link-error interrupts
445 438 if (status!=RTEMS_SUCCESSFUL) {
446 439 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_DISABLE_ERR\n")
447 440 }
448 441 //
449 442 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ, 1); // sets the link-error interrupt bit
450 443 if (status!=RTEMS_SUCCESSFUL) {
451 444 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ\n")
452 445 }
453 446 //
454 447 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK, 1); // transmission blocks
455 448 if (status!=RTEMS_SUCCESSFUL) {
456 449 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK\n")
457 450 }
458 451 //
459 452 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL, 1); // transmission blocks when no transmission descriptor is available
460 453 if (status!=RTEMS_SUCCESSFUL) {
461 454 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL\n")
462 455 }
463 456 //
464 457 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TCODE_CTRL, 0x0909); // [Time Rx : Time Tx : Link error : Tick-out IRQ]
465 458 if (status!=RTEMS_SUCCESSFUL) {
466 459 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TCODE_CTRL,\n")
467 460 }
468 461
469 462 return status;
470 463 }
471 464
472 465 int spacewire_reset_link( void )
473 466 {
474 467 /** This function is executed by the SPIQ rtems_task wehn it has been awaken by an interruption raised by the SpaceWire driver.
475 468 *
476 469 * @return RTEMS directive status code:
477 470 * - RTEMS_UNSATISFIED is returned is the link is not in the running state after 10 s.
478 471 * - RTEMS_SUCCESSFUL is returned if the link is up before the timeout.
479 472 *
480 473 */
481 474
482 475 rtems_status_code status_spw;
483 476 rtems_status_code status;
484 477 int i;
485 478
486 479 for ( i=0; i<SY_LFR_DPU_CONNECT_ATTEMPT; i++ )
487 480 {
488 481 PRINTF1("in spacewire_reset_link *** link recovery, try %d\n", i);
489 482
490 483 // CLOSING THE DRIVER AT THIS POINT WILL MAKE THE SEND TASK BLOCK THE SYSTEM
491 484
492 485 status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms
493 486
494 487 status_spw = spacewire_stop_and_start_link( fdSPW );
495 488 if ( status_spw != RTEMS_SUCCESSFUL )
496 489 {
497 490 PRINTF1("in spacewire_reset_link *** ERR spacewire_start_link code %d\n", status_spw)
498 491 }
499 492
500 493 if ( status_spw == RTEMS_SUCCESSFUL)
501 494 {
502 495 break;
503 496 }
504 497 }
505 498
506 499 return status_spw;
507 500 }
508 501
509 502 void spacewire_set_NP( unsigned char val, unsigned int regAddr ) // [N]o [P]ort force
510 503 {
511 504 /** This function sets the [N]o [P]ort force bit of the GRSPW control register.
512 505 *
513 506 * @param val is the value, 0 or 1, used to set the value of the NP bit.
514 507 * @param regAddr is the address of the GRSPW control register.
515 508 *
516 509 * NP is the bit 20 of the GRSPW control register.
517 510 *
518 511 */
519 512
520 513 unsigned int *spwptr = (unsigned int*) regAddr;
521 514
522 515 if (val == 1) {
523 516 *spwptr = *spwptr | 0x00100000; // [NP] set the No port force bit
524 517 }
525 518 if (val== 0) {
526 519 *spwptr = *spwptr & 0xffdfffff;
527 520 }
528 521 }
529 522
530 523 void spacewire_set_RE( unsigned char val, unsigned int regAddr ) // [R]MAP [E]nable
531 524 {
532 525 /** This function sets the [R]MAP [E]nable bit of the GRSPW control register.
533 526 *
534 527 * @param val is the value, 0 or 1, used to set the value of the RE bit.
535 528 * @param regAddr is the address of the GRSPW control register.
536 529 *
537 530 * RE is the bit 16 of the GRSPW control register.
538 531 *
539 532 */
540 533
541 534 unsigned int *spwptr = (unsigned int*) regAddr;
542 535
543 536 if (val == 1)
544 537 {
545 538 *spwptr = *spwptr | 0x00010000; // [RE] set the RMAP Enable bit
546 539 }
547 540 if (val== 0)
548 541 {
549 542 *spwptr = *spwptr & 0xfffdffff;
550 543 }
551 544 }
552 545
553 546 void spacewire_compute_stats_offsets( void )
554 547 {
555 548 /** This function computes the SpaceWire statistics offsets in case of a SpaceWire related interruption raising.
556 549 *
557 550 * The offsets keep a record of the statistics in case of a reset of the statistics. They are added to the current statistics
558 551 * to keep the counters consistent even after a reset of the SpaceWire driver (the counter are set to zero by the driver when it
559 552 * during the open systel call).
560 553 *
561 554 */
562 555
563 556 spw_stats spacewire_stats_grspw;
564 557 rtems_status_code status;
565 558
566 559 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &spacewire_stats_grspw );
567 560
568 561 spacewire_stats_backup.packets_received = spacewire_stats_grspw.packets_received
569 562 + spacewire_stats.packets_received;
570 563 spacewire_stats_backup.packets_sent = spacewire_stats_grspw.packets_sent
571 564 + spacewire_stats.packets_sent;
572 565 spacewire_stats_backup.parity_err = spacewire_stats_grspw.parity_err
573 566 + spacewire_stats.parity_err;
574 567 spacewire_stats_backup.disconnect_err = spacewire_stats_grspw.disconnect_err
575 568 + spacewire_stats.disconnect_err;
576 569 spacewire_stats_backup.escape_err = spacewire_stats_grspw.escape_err
577 570 + spacewire_stats.escape_err;
578 571 spacewire_stats_backup.credit_err = spacewire_stats_grspw.credit_err
579 572 + spacewire_stats.credit_err;
580 573 spacewire_stats_backup.write_sync_err = spacewire_stats_grspw.write_sync_err
581 574 + spacewire_stats.write_sync_err;
582 575 spacewire_stats_backup.rx_rmap_header_crc_err = spacewire_stats_grspw.rx_rmap_header_crc_err
583 576 + spacewire_stats.rx_rmap_header_crc_err;
584 577 spacewire_stats_backup.rx_rmap_data_crc_err = spacewire_stats_grspw.rx_rmap_data_crc_err
585 578 + spacewire_stats.rx_rmap_data_crc_err;
586 579 spacewire_stats_backup.early_ep = spacewire_stats_grspw.early_ep
587 580 + spacewire_stats.early_ep;
588 581 spacewire_stats_backup.invalid_address = spacewire_stats_grspw.invalid_address
589 582 + spacewire_stats.invalid_address;
590 583 spacewire_stats_backup.rx_eep_err = spacewire_stats_grspw.rx_eep_err
591 584 + spacewire_stats.rx_eep_err;
592 585 spacewire_stats_backup.rx_truncated = spacewire_stats_grspw.rx_truncated
593 586 + spacewire_stats.rx_truncated;
594 587 }
595 588
596 589 void spacewire_update_statistics( void )
597 590 {
598 591 rtems_status_code status;
599 592 spw_stats spacewire_stats_grspw;
600 593
601 594 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &spacewire_stats_grspw );
602 595
603 596 spacewire_stats.packets_received = spacewire_stats_backup.packets_received
604 597 + spacewire_stats_grspw.packets_received;
605 598 spacewire_stats.packets_sent = spacewire_stats_backup.packets_sent
606 599 + spacewire_stats_grspw.packets_sent;
607 600 spacewire_stats.parity_err = spacewire_stats_backup.parity_err
608 601 + spacewire_stats_grspw.parity_err;
609 602 spacewire_stats.disconnect_err = spacewire_stats_backup.disconnect_err
610 603 + spacewire_stats_grspw.disconnect_err;
611 604 spacewire_stats.escape_err = spacewire_stats_backup.escape_err
612 605 + spacewire_stats_grspw.escape_err;
613 606 spacewire_stats.credit_err = spacewire_stats_backup.credit_err
614 607 + spacewire_stats_grspw.credit_err;
615 608 spacewire_stats.write_sync_err = spacewire_stats_backup.write_sync_err
616 609 + spacewire_stats_grspw.write_sync_err;
617 610 spacewire_stats.rx_rmap_header_crc_err = spacewire_stats_backup.rx_rmap_header_crc_err
618 611 + spacewire_stats_grspw.rx_rmap_header_crc_err;
619 612 spacewire_stats.rx_rmap_data_crc_err = spacewire_stats_backup.rx_rmap_data_crc_err
620 613 + spacewire_stats_grspw.rx_rmap_data_crc_err;
621 614 spacewire_stats.early_ep = spacewire_stats_backup.early_ep
622 615 + spacewire_stats_grspw.early_ep;
623 616 spacewire_stats.invalid_address = spacewire_stats_backup.invalid_address
624 617 + spacewire_stats_grspw.invalid_address;
625 618 spacewire_stats.rx_eep_err = spacewire_stats_backup.rx_eep_err
626 619 + spacewire_stats_grspw.rx_eep_err;
627 620 spacewire_stats.rx_truncated = spacewire_stats_backup.rx_truncated
628 621 + spacewire_stats_grspw.rx_truncated;
629 622 //spacewire_stats.tx_link_err;
630 623
631 624 //****************************
632 625 // DPU_SPACEWIRE_IF_STATISTICS
633 626 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[0] = (unsigned char) (spacewire_stats.packets_received >> 8);
634 627 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[1] = (unsigned char) (spacewire_stats.packets_received);
635 628 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[0] = (unsigned char) (spacewire_stats.packets_sent >> 8);
636 629 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[1] = (unsigned char) (spacewire_stats.packets_sent);
637 630 //housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt;
638 631 //housekeeping_packet.hk_lfr_dpu_spw_last_timc;
639 632
640 633 //******************************************
641 634 // ERROR COUNTERS / SPACEWIRE / LOW SEVERITY
642 635 housekeeping_packet.hk_lfr_dpu_spw_parity = (unsigned char) spacewire_stats.parity_err;
643 636 housekeeping_packet.hk_lfr_dpu_spw_disconnect = (unsigned char) spacewire_stats.disconnect_err;
644 637 housekeeping_packet.hk_lfr_dpu_spw_escape = (unsigned char) spacewire_stats.escape_err;
645 638 housekeeping_packet.hk_lfr_dpu_spw_credit = (unsigned char) spacewire_stats.credit_err;
646 639 housekeeping_packet.hk_lfr_dpu_spw_write_sync = (unsigned char) spacewire_stats.write_sync_err;
647 640
648 641 //*********************************************
649 642 // ERROR COUNTERS / SPACEWIRE / MEDIUM SEVERITY
650 643 housekeeping_packet.hk_lfr_dpu_spw_early_eop = (unsigned char) spacewire_stats.early_ep;
651 644 housekeeping_packet.hk_lfr_dpu_spw_invalid_addr = (unsigned char) spacewire_stats.invalid_address;
652 645 housekeeping_packet.hk_lfr_dpu_spw_eep = (unsigned char) spacewire_stats.rx_eep_err;
653 646 housekeeping_packet.hk_lfr_dpu_spw_rx_too_big = (unsigned char) spacewire_stats.rx_truncated;
654 647 }
655 648
656 649 void timecode_irq_handler( void *pDev, void *regs, int minor, unsigned int tc )
657 650 {
658 651 // a valid timecode has been received, write it in the HK report
659 652 unsigned int *grspwPtr;
660 653 unsigned char timecodeCtr;
661 654 unsigned char updateTimeCtr;
662 655
663 656 grspwPtr = (unsigned int *) (REGS_ADDR_GRSPW + APB_OFFSET_GRSPW_TIME_REGISTER);
664 657
665 658 housekeeping_packet.hk_lfr_dpu_spw_last_timc = (unsigned char) (grspwPtr[0] & 0xff); // [1111 1111]
666 659 timecodeCtr = (unsigned char) (grspwPtr[0] & 0x3f); // [0011 1111]
667 660 updateTimeCtr = time_management_regs->coarse_time_load & 0x3f; // [0011 1111]
668 661
669 662 // update the number of valid timecodes that have been received
670 663 if (housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt == 255)
671 664 {
672 665 housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt = 0;
673 666 }
674 667 else
675 668 {
676 669 housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt = housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt + 1;
677 670 }
678 671
679 672 // check the value of the timecode with respect to the last TC_LFR_UPDATE_TIME => SSS-CP-FS-370
680 673 if (timecodeCtr != updateTimeCtr)
681 674 {
682 675 if (housekeeping_packet.hk_lfr_time_timecode_ctr == 255)
683 676 {
684 677 housekeeping_packet.hk_lfr_time_timecode_ctr = 0;
685 678 }
686 679 else
687 680 {
688 681 housekeeping_packet.hk_lfr_time_timecode_ctr = housekeeping_packet.hk_lfr_time_timecode_ctr + 1;
689 682 }
690 683 }
691 684 }
692 685
693 686 rtems_timer_service_routine user_routine( rtems_id timer_id, void *user_data )
694 687 {
695 688 int linkStatus;
696 689 rtems_status_code status;
697 690
698 691 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
699 692
700 693 if ( linkStatus == 5) {
701 694 PRINTF("in spacewire_reset_link *** link is running\n")
702 695 status = RTEMS_SUCCESSFUL;
703 696 }
704 697 }
705 698
706 699 void init_header_cwf( Header_TM_LFR_SCIENCE_CWF_t *header )
707 700 {
708 701 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
709 702 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
710 703 header->reserved = DEFAULT_RESERVED;
711 704 header->userApplication = CCSDS_USER_APP;
712 705 header->packetSequenceControl[0]= TM_PACKET_SEQ_CTRL_STANDALONE;
713 706 header->packetSequenceControl[1]= TM_PACKET_SEQ_CNT_DEFAULT;
714 707 header->packetLength[0] = 0x00;
715 708 header->packetLength[1] = 0x00;
716 709 // DATA FIELD HEADER
717 710 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
718 711 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
719 712 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; // service subtype
720 713 header->destinationID = TM_DESTINATION_ID_GROUND;
721 714 header->time[0] = 0x00;
722 715 header->time[0] = 0x00;
723 716 header->time[0] = 0x00;
724 717 header->time[0] = 0x00;
725 718 header->time[0] = 0x00;
726 719 header->time[0] = 0x00;
727 720 // AUXILIARY DATA HEADER
728 721 header->sid = 0x00;
729 722 header->hkBIA = DEFAULT_HKBIA;
730 723 header->blkNr[0] = 0x00;
731 724 header->blkNr[1] = 0x00;
732 725 }
733 726
734 727 void init_header_swf( Header_TM_LFR_SCIENCE_SWF_t *header )
735 728 {
736 729 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
737 730 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
738 731 header->reserved = DEFAULT_RESERVED;
739 732 header->userApplication = CCSDS_USER_APP;
740 733 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
741 734 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
742 735 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
743 736 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
744 737 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8);
745 738 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
746 739 // DATA FIELD HEADER
747 740 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
748 741 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
749 742 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; // service subtype
750 743 header->destinationID = TM_DESTINATION_ID_GROUND;
751 744 header->time[0] = 0x00;
752 745 header->time[0] = 0x00;
753 746 header->time[0] = 0x00;
754 747 header->time[0] = 0x00;
755 748 header->time[0] = 0x00;
756 749 header->time[0] = 0x00;
757 750 // AUXILIARY DATA HEADER
758 751 header->sid = 0x00;
759 752 header->hkBIA = DEFAULT_HKBIA;
760 753 header->pktCnt = DEFAULT_PKTCNT; // PKT_CNT
761 754 header->pktNr = 0x00;
762 755 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8);
763 756 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
764 757 }
765 758
766 759 void init_header_asm( Header_TM_LFR_SCIENCE_ASM_t *header )
767 760 {
768 761 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
769 762 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
770 763 header->reserved = DEFAULT_RESERVED;
771 764 header->userApplication = CCSDS_USER_APP;
772 765 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
773 766 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
774 767 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
775 768 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
776 769 header->packetLength[0] = 0x00;
777 770 header->packetLength[1] = 0x00;
778 771 // DATA FIELD HEADER
779 772 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
780 773 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
781 774 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
782 775 header->destinationID = TM_DESTINATION_ID_GROUND;
783 776 header->time[0] = 0x00;
784 777 header->time[0] = 0x00;
785 778 header->time[0] = 0x00;
786 779 header->time[0] = 0x00;
787 780 header->time[0] = 0x00;
788 781 header->time[0] = 0x00;
789 782 // AUXILIARY DATA HEADER
790 783 header->sid = 0x00;
791 784 header->biaStatusInfo = 0x00;
792 785 header->pa_lfr_pkt_cnt_asm = 0x00;
793 786 header->pa_lfr_pkt_nr_asm = 0x00;
794 787 header->pa_lfr_asm_blk_nr[0] = 0x00;
795 788 header->pa_lfr_asm_blk_nr[1] = 0x00;
796 789 }
797 790
798 791 int spw_send_waveform_CWF( ring_node *ring_node_to_send,
799 792 Header_TM_LFR_SCIENCE_CWF_t *header )
800 793 {
801 794 /** This function sends CWF CCSDS packets (F2, F1 or F0).
802 795 *
803 796 * @param waveform points to the buffer containing the data that will be send.
804 797 * @param sid is the source identifier of the data that will be sent.
805 798 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
806 799 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
807 800 * contain information to setup the transmission of the data packets.
808 801 *
809 802 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
810 803 *
811 804 */
812 805
813 806 unsigned int i;
814 807 int ret;
815 808 unsigned int coarseTime;
816 809 unsigned int fineTime;
817 810 rtems_status_code status;
818 811 spw_ioctl_pkt_send spw_ioctl_send_CWF;
819 812 int *dataPtr;
820 813 unsigned char sid;
821 814
822 815 spw_ioctl_send_CWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_CWF;
823 816 spw_ioctl_send_CWF.options = 0;
824 817
825 818 ret = LFR_DEFAULT;
826 819 sid = (unsigned char) ring_node_to_send->sid;
827 820
828 821 coarseTime = ring_node_to_send->coarseTime;
829 822 fineTime = ring_node_to_send->fineTime;
830 823 dataPtr = (int*) ring_node_to_send->buffer_address;
831 824
832 825 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8);
833 826 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
834 827 header->hkBIA = pa_bia_status_info;
835 828 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
836 829 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8);
837 830 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
838 831
839 832 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF; i++) // send waveform
840 833 {
841 834 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF * NB_WORDS_SWF_BLK) ];
842 835 spw_ioctl_send_CWF.hdr = (char*) header;
843 836 // BUILD THE DATA
844 837 spw_ioctl_send_CWF.dlen = BLK_NR_CWF * NB_BYTES_SWF_BLK;
845 838
846 839 // SET PACKET SEQUENCE CONTROL
847 840 increment_seq_counter_source_id( header->packetSequenceControl, sid );
848 841
849 842 // SET SID
850 843 header->sid = sid;
851 844
852 845 // SET PACKET TIME
853 846 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime);
854 847 //
855 848 header->time[0] = header->acquisitionTime[0];
856 849 header->time[1] = header->acquisitionTime[1];
857 850 header->time[2] = header->acquisitionTime[2];
858 851 header->time[3] = header->acquisitionTime[3];
859 852 header->time[4] = header->acquisitionTime[4];
860 853 header->time[5] = header->acquisitionTime[5];
861 854
862 855 // SET PACKET ID
863 856 if ( (sid == SID_SBM1_CWF_F1) || (sid == SID_SBM2_CWF_F2) )
864 857 {
865 858 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2 >> 8);
866 859 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2);
867 860 }
868 861 else
869 862 {
870 863 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
871 864 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
872 865 }
873 866
874 867 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
875 868 if (status != RTEMS_SUCCESSFUL) {
876 printf("%d-%d, ERR %d\n", sid, i, (int) status);
877 869 ret = LFR_DEFAULT;
878 870 }
879 871 }
880 872
881 873 return ret;
882 874 }
883 875
884 876 int spw_send_waveform_SWF( ring_node *ring_node_to_send,
885 877 Header_TM_LFR_SCIENCE_SWF_t *header )
886 878 {
887 879 /** This function sends SWF CCSDS packets (F2, F1 or F0).
888 880 *
889 881 * @param waveform points to the buffer containing the data that will be send.
890 882 * @param sid is the source identifier of the data that will be sent.
891 883 * @param headerSWF points to a table of headers that have been prepared for the data transmission.
892 884 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
893 885 * contain information to setup the transmission of the data packets.
894 886 *
895 887 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
896 888 *
897 889 */
898 890
899 891 unsigned int i;
900 892 int ret;
901 893 unsigned int coarseTime;
902 894 unsigned int fineTime;
903 895 rtems_status_code status;
904 896 spw_ioctl_pkt_send spw_ioctl_send_SWF;
905 897 int *dataPtr;
906 898 unsigned char sid;
907 899
908 900 spw_ioctl_send_SWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_SWF;
909 901 spw_ioctl_send_SWF.options = 0;
910 902
911 903 ret = LFR_DEFAULT;
912 904
913 905 coarseTime = ring_node_to_send->coarseTime;
914 906 fineTime = ring_node_to_send->fineTime;
915 907 dataPtr = (int*) ring_node_to_send->buffer_address;
916 908 sid = ring_node_to_send->sid;
917 909
918 910 header->hkBIA = pa_bia_status_info;
919 911 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
920 912
921 913 for (i=0; i<7; i++) // send waveform
922 914 {
923 915 spw_ioctl_send_SWF.data = (char*) &dataPtr[ (i * BLK_NR_304 * NB_WORDS_SWF_BLK) ];
924 916 spw_ioctl_send_SWF.hdr = (char*) header;
925 917
926 918 // SET PACKET SEQUENCE CONTROL
927 919 increment_seq_counter_source_id( header->packetSequenceControl, sid );
928 920
929 921 // SET PACKET LENGTH AND BLKNR
930 922 if (i == 6)
931 923 {
932 924 spw_ioctl_send_SWF.dlen = BLK_NR_224 * NB_BYTES_SWF_BLK;
933 925 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_224 >> 8);
934 926 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_224 );
935 927 header->blkNr[0] = (unsigned char) (BLK_NR_224 >> 8);
936 928 header->blkNr[1] = (unsigned char) (BLK_NR_224 );
937 929 }
938 930 else
939 931 {
940 932 spw_ioctl_send_SWF.dlen = BLK_NR_304 * NB_BYTES_SWF_BLK;
941 933 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_304 >> 8);
942 934 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_304 );
943 935 header->blkNr[0] = (unsigned char) (BLK_NR_304 >> 8);
944 936 header->blkNr[1] = (unsigned char) (BLK_NR_304 );
945 937 }
946 938
947 939 // SET PACKET TIME
948 940 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime );
949 941 //
950 942 header->time[0] = header->acquisitionTime[0];
951 943 header->time[1] = header->acquisitionTime[1];
952 944 header->time[2] = header->acquisitionTime[2];
953 945 header->time[3] = header->acquisitionTime[3];
954 946 header->time[4] = header->acquisitionTime[4];
955 947 header->time[5] = header->acquisitionTime[5];
956 948
957 949 // SET SID
958 950 header->sid = sid;
959 951
960 952 // SET PKTNR
961 953 header->pktNr = i+1; // PKT_NR
962 954
963 955 // SEND PACKET
964 956 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_SWF );
965 957 if (status != RTEMS_SUCCESSFUL) {
966 printf("%d-%d, ERR %d\n", sid, i, (int) status);
967 958 ret = LFR_DEFAULT;
968 959 }
969 960 }
970 961
971 962 return ret;
972 963 }
973 964
974 965 int spw_send_waveform_CWF3_light( ring_node *ring_node_to_send,
975 966 Header_TM_LFR_SCIENCE_CWF_t *header )
976 967 {
977 968 /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data.
978 969 *
979 970 * @param waveform points to the buffer containing the data that will be send.
980 971 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
981 972 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
982 973 * contain information to setup the transmission of the data packets.
983 974 *
984 975 * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer
985 976 * from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks.
986 977 *
987 978 */
988 979
989 980 unsigned int i;
990 981 int ret;
991 982 unsigned int coarseTime;
992 983 unsigned int fineTime;
993 984 rtems_status_code status;
994 985 spw_ioctl_pkt_send spw_ioctl_send_CWF;
995 986 char *dataPtr;
996 987 unsigned char sid;
997 988
998 989 spw_ioctl_send_CWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_CWF;
999 990 spw_ioctl_send_CWF.options = 0;
1000 991
1001 992 ret = LFR_DEFAULT;
1002 993 sid = ring_node_to_send->sid;
1003 994
1004 995 coarseTime = ring_node_to_send->coarseTime;
1005 996 fineTime = ring_node_to_send->fineTime;
1006 997 dataPtr = (char*) ring_node_to_send->buffer_address;
1007 998
1008 999 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_672 >> 8);
1009 1000 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_672 );
1010 1001 header->hkBIA = pa_bia_status_info;
1011 1002 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1012 1003 header->blkNr[0] = (unsigned char) (BLK_NR_CWF_SHORT_F3 >> 8);
1013 1004 header->blkNr[1] = (unsigned char) (BLK_NR_CWF_SHORT_F3 );
1014 1005
1015 1006 //*********************
1016 1007 // SEND CWF3_light DATA
1017 1008 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF_LIGHT; i++) // send waveform
1018 1009 {
1019 1010 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK) ];
1020 1011 spw_ioctl_send_CWF.hdr = (char*) header;
1021 1012 // BUILD THE DATA
1022 1013 spw_ioctl_send_CWF.dlen = BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK;
1023 1014
1024 1015 // SET PACKET SEQUENCE COUNTER
1025 1016 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1026 1017
1027 1018 // SET SID
1028 1019 header->sid = sid;
1029 1020
1030 1021 // SET PACKET TIME
1031 1022 compute_acquisition_time( coarseTime, fineTime, SID_NORM_CWF_F3, i, header->acquisitionTime );
1032 1023 //
1033 1024 header->time[0] = header->acquisitionTime[0];
1034 1025 header->time[1] = header->acquisitionTime[1];
1035 1026 header->time[2] = header->acquisitionTime[2];
1036 1027 header->time[3] = header->acquisitionTime[3];
1037 1028 header->time[4] = header->acquisitionTime[4];
1038 1029 header->time[5] = header->acquisitionTime[5];
1039 1030
1040 1031 // SET PACKET ID
1041 1032 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
1042 1033 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1043 1034
1044 1035 // SEND PACKET
1045 1036 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
1046 1037 if (status != RTEMS_SUCCESSFUL) {
1047 printf("%d-%d, ERR %d\n", sid, i, (int) status);
1048 1038 ret = LFR_DEFAULT;
1049 1039 }
1050 1040 }
1051 1041
1052 1042 return ret;
1053 1043 }
1054 1044
1055 1045 void spw_send_asm_f0( ring_node *ring_node_to_send,
1056 1046 Header_TM_LFR_SCIENCE_ASM_t *header )
1057 1047 {
1058 1048 unsigned int i;
1059 1049 unsigned int length = 0;
1060 1050 rtems_status_code status;
1061 1051 unsigned int sid;
1062 1052 float *spectral_matrix;
1063 1053 int coarseTime;
1064 1054 int fineTime;
1065 1055 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1066 1056
1067 1057 sid = ring_node_to_send->sid;
1068 1058 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1069 1059 coarseTime = ring_node_to_send->coarseTime;
1070 1060 fineTime = ring_node_to_send->fineTime;
1071 1061
1072 1062 header->biaStatusInfo = pa_bia_status_info;
1073 1063 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1074 1064
1075 1065 for (i=0; i<3; i++)
1076 1066 {
1077 1067 if ((i==0) || (i==1))
1078 1068 {
1079 1069 spw_ioctl_send_ASM.dlen = DLEN_ASM_F0_PKT_1;
1080 1070 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1081 1071 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0_1) ) * NB_VALUES_PER_SM )
1082 1072 ];
1083 1073 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0_1;
1084 1074 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1085 1075 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_1) >> 8 ); // BLK_NR MSB
1086 1076 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0_1); // BLK_NR LSB
1087 1077 }
1088 1078 else
1089 1079 {
1090 1080 spw_ioctl_send_ASM.dlen = DLEN_ASM_F0_PKT_2;
1091 1081 spw_ioctl_send_ASM.data = (char*) &spectral_matrix[
1092 1082 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0_1) ) * NB_VALUES_PER_SM )
1093 1083 ];
1094 1084 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0_2;
1095 1085 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1096 1086 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_2) >> 8 ); // BLK_NR MSB
1097 1087 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0_2); // BLK_NR LSB
1098 1088 }
1099 1089
1100 1090 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1101 1091 spw_ioctl_send_ASM.hdr = (char *) header;
1102 1092 spw_ioctl_send_ASM.options = 0;
1103 1093
1104 1094 // (2) BUILD THE HEADER
1105 1095 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1106 1096 header->packetLength[0] = (unsigned char) (length>>8);
1107 1097 header->packetLength[1] = (unsigned char) (length);
1108 1098 header->sid = (unsigned char) sid; // SID
1109 1099 header->pa_lfr_pkt_cnt_asm = 3;
1110 1100 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1111 1101
1112 1102 // (3) SET PACKET TIME
1113 1103 header->time[0] = (unsigned char) (coarseTime>>24);
1114 1104 header->time[1] = (unsigned char) (coarseTime>>16);
1115 1105 header->time[2] = (unsigned char) (coarseTime>>8);
1116 1106 header->time[3] = (unsigned char) (coarseTime);
1117 1107 header->time[4] = (unsigned char) (fineTime>>8);
1118 1108 header->time[5] = (unsigned char) (fineTime);
1119 1109 //
1120 1110 header->acquisitionTime[0] = header->time[0];
1121 1111 header->acquisitionTime[1] = header->time[1];
1122 1112 header->acquisitionTime[2] = header->time[2];
1123 1113 header->acquisitionTime[3] = header->time[3];
1124 1114 header->acquisitionTime[4] = header->time[4];
1125 1115 header->acquisitionTime[5] = header->time[5];
1126 1116
1127 1117 // (4) SEND PACKET
1128 1118 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1129 1119 if (status != RTEMS_SUCCESSFUL) {
1130 printf("in ASM_send *** ERR %d\n", (int) status);
1120 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1131 1121 }
1132 1122 }
1133 1123 }
1134 1124
1135 1125 void spw_send_asm_f1( ring_node *ring_node_to_send,
1136 1126 Header_TM_LFR_SCIENCE_ASM_t *header )
1137 1127 {
1138 1128 unsigned int i;
1139 1129 unsigned int length = 0;
1140 1130 rtems_status_code status;
1141 1131 unsigned int sid;
1142 1132 float *spectral_matrix;
1143 1133 int coarseTime;
1144 1134 int fineTime;
1145 1135 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1146 1136
1147 1137 sid = ring_node_to_send->sid;
1148 1138 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1149 1139 coarseTime = ring_node_to_send->coarseTime;
1150 1140 fineTime = ring_node_to_send->fineTime;
1151 1141
1152 1142 header->biaStatusInfo = pa_bia_status_info;
1153 1143 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1154 1144
1155 1145 for (i=0; i<3; i++)
1156 1146 {
1157 1147 if ((i==0) || (i==1))
1158 1148 {
1159 1149 spw_ioctl_send_ASM.dlen = DLEN_ASM_F1_PKT_1;
1160 1150 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1161 1151 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1_1) ) * NB_VALUES_PER_SM )
1162 1152 ];
1163 1153 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1_1;
1164 1154 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1165 1155 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_1) >> 8 ); // BLK_NR MSB
1166 1156 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1_1); // BLK_NR LSB
1167 1157 }
1168 1158 else
1169 1159 {
1170 1160 spw_ioctl_send_ASM.dlen = DLEN_ASM_F1_PKT_2;
1171 1161 spw_ioctl_send_ASM.data = (char*) &spectral_matrix[
1172 1162 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1_1) ) * NB_VALUES_PER_SM )
1173 1163 ];
1174 1164 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1_2;
1175 1165 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1176 1166 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_2) >> 8 ); // BLK_NR MSB
1177 1167 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1_2); // BLK_NR LSB
1178 1168 }
1179 1169
1180 1170 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1181 1171 spw_ioctl_send_ASM.hdr = (char *) header;
1182 1172 spw_ioctl_send_ASM.options = 0;
1183 1173
1184 1174 // (2) BUILD THE HEADER
1185 1175 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1186 1176 header->packetLength[0] = (unsigned char) (length>>8);
1187 1177 header->packetLength[1] = (unsigned char) (length);
1188 1178 header->sid = (unsigned char) sid; // SID
1189 1179 header->pa_lfr_pkt_cnt_asm = 3;
1190 1180 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1191 1181
1192 1182 // (3) SET PACKET TIME
1193 1183 header->time[0] = (unsigned char) (coarseTime>>24);
1194 1184 header->time[1] = (unsigned char) (coarseTime>>16);
1195 1185 header->time[2] = (unsigned char) (coarseTime>>8);
1196 1186 header->time[3] = (unsigned char) (coarseTime);
1197 1187 header->time[4] = (unsigned char) (fineTime>>8);
1198 1188 header->time[5] = (unsigned char) (fineTime);
1199 1189 //
1200 1190 header->acquisitionTime[0] = header->time[0];
1201 1191 header->acquisitionTime[1] = header->time[1];
1202 1192 header->acquisitionTime[2] = header->time[2];
1203 1193 header->acquisitionTime[3] = header->time[3];
1204 1194 header->acquisitionTime[4] = header->time[4];
1205 1195 header->acquisitionTime[5] = header->time[5];
1206 1196
1207 1197 // (4) SEND PACKET
1208 1198 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1209 1199 if (status != RTEMS_SUCCESSFUL) {
1210 printf("in ASM_send *** ERR %d\n", (int) status);
1200 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1211 1201 }
1212 1202 }
1213 1203 }
1214 1204
1215 1205 void spw_send_asm_f2( ring_node *ring_node_to_send,
1216 1206 Header_TM_LFR_SCIENCE_ASM_t *header )
1217 1207 {
1218 1208 unsigned int i;
1219 1209 unsigned int length = 0;
1220 1210 rtems_status_code status;
1221 1211 unsigned int sid;
1222 1212 float *spectral_matrix;
1223 1213 int coarseTime;
1224 1214 int fineTime;
1225 1215 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1226 1216
1227 1217 sid = ring_node_to_send->sid;
1228 1218 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1229 1219 coarseTime = ring_node_to_send->coarseTime;
1230 1220 fineTime = ring_node_to_send->fineTime;
1231 1221
1232 1222 header->biaStatusInfo = pa_bia_status_info;
1233 1223 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1234 1224
1235 1225 for (i=0; i<3; i++)
1236 1226 {
1237 1227
1238 1228 spw_ioctl_send_ASM.dlen = DLEN_ASM_F2_PKT;
1239 1229 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1240 1230 ( (ASM_F2_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F2) ) * NB_VALUES_PER_SM )
1241 1231 ];
1242 1232 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F2;
1243 1233 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3;
1244 1234 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F2) >> 8 ); // BLK_NR MSB
1245 1235 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F2); // BLK_NR LSB
1246 1236
1247 1237 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1248 1238 spw_ioctl_send_ASM.hdr = (char *) header;
1249 1239 spw_ioctl_send_ASM.options = 0;
1250 1240
1251 1241 // (2) BUILD THE HEADER
1252 1242 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1253 1243 header->packetLength[0] = (unsigned char) (length>>8);
1254 1244 header->packetLength[1] = (unsigned char) (length);
1255 1245 header->sid = (unsigned char) sid; // SID
1256 1246 header->pa_lfr_pkt_cnt_asm = 3;
1257 1247 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1258 1248
1259 1249 // (3) SET PACKET TIME
1260 1250 header->time[0] = (unsigned char) (coarseTime>>24);
1261 1251 header->time[1] = (unsigned char) (coarseTime>>16);
1262 1252 header->time[2] = (unsigned char) (coarseTime>>8);
1263 1253 header->time[3] = (unsigned char) (coarseTime);
1264 1254 header->time[4] = (unsigned char) (fineTime>>8);
1265 1255 header->time[5] = (unsigned char) (fineTime);
1266 1256 //
1267 1257 header->acquisitionTime[0] = header->time[0];
1268 1258 header->acquisitionTime[1] = header->time[1];
1269 1259 header->acquisitionTime[2] = header->time[2];
1270 1260 header->acquisitionTime[3] = header->time[3];
1271 1261 header->acquisitionTime[4] = header->time[4];
1272 1262 header->acquisitionTime[5] = header->time[5];
1273 1263
1274 1264 // (4) SEND PACKET
1275 1265 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1276 1266 if (status != RTEMS_SUCCESSFUL) {
1277 printf("in ASM_send *** ERR %d\n", (int) status);
1267 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1278 1268 }
1279 1269 }
1280 1270 }
1281 1271
1282 1272 void spw_send_k_dump( ring_node *ring_node_to_send )
1283 1273 {
1284 1274 rtems_status_code status;
1285 1275 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump;
1286 1276 unsigned int packetLength;
1287 1277 unsigned int size;
1288 1278
1289 printf("spw_send_k_dump\n");
1279 PRINTF("spw_send_k_dump\n")
1290 1280
1291 1281 kcoefficients_dump = (Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *) ring_node_to_send->buffer_address;
1292 1282
1293 1283 packetLength = kcoefficients_dump->packetLength[0] * 256 + kcoefficients_dump->packetLength[1];
1294 1284
1295 1285 size = packetLength + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
1296 1286
1297 printf("packetLength %d, size %d\n", packetLength, size );
1287 PRINTF2("packetLength %d, size %d\n", packetLength, size )
1298 1288
1299 1289 status = write( fdSPW, (char *) ring_node_to_send->buffer_address, size );
1300 1290
1301 1291 if (status == -1){
1302 1292 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
1303 1293 }
1304 1294
1305 1295 ring_node_to_send->status = 0x00;
1306 1296 }
Show file before | Show file after | Hide comments
@@ -1,404 +1,404
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 printf("in AVF0 *** Error sending message to MATR, code %d\n", status);
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( 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);
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.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
287 287 BP_send( (char *) &packet_sbm_bp1, queue_id,
288 288 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0 + PACKET_LENGTH_DELTA,
289 289 sid);
290 290 // 4) compute the BP2 set if needed
291 291 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP2_F0) || (incomingMsg->event & RTEMS_EVENT_SBM_BP2_F0) )
292 292 {
293 293 // 1) compute the BP2 set
294 294 BP2_set( compressed_sm_sbm_f0, NB_BINS_COMPRESSED_SM_SBM_F0, packet_sbm_bp2.data );
295 295 // 2) send the BP2 set
296 296 set_time( packet_sbm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
297 297 set_time( packet_sbm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
298 298 packet_sbm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
299 299 BP_send( (char *) &packet_sbm_bp2, queue_id,
300 300 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0 + PACKET_LENGTH_DELTA,
301 301 sid);
302 302 }
303 303 }
304 304
305 305 //*****
306 306 //*****
307 307 // NORM
308 308 //*****
309 309 //*****
310 310 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F0)
311 311 {
312 312 // 1) compress the matrix for Basic Parameters calculation
313 313 ASM_compress_reorganize_and_divide( asm_f0_patched_norm, compressed_sm_norm_f0,
314 314 nb_sm_before_f0.norm_bp1,
315 315 NB_BINS_COMPRESSED_SM_F0, NB_BINS_TO_AVERAGE_ASM_F0,
316 316 ASM_F0_INDICE_START );
317 317 // 2) compute the BP1 set
318 318 BP1_set( compressed_sm_norm_f0, k_coeff_intercalib_f0_norm, NB_BINS_COMPRESSED_SM_F0, packet_norm_bp1.data );
319 319 // 3) send the BP1 set
320 320 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
321 321 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
322 322 packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
323 323 BP_send( (char *) &packet_norm_bp1, queue_id,
324 324 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F0 + PACKET_LENGTH_DELTA,
325 325 SID_NORM_BP1_F0 );
326 326 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F0)
327 327 {
328 328 // 1) compute the BP2 set using the same ASM as the one used for BP1
329 329 BP2_set( compressed_sm_norm_f0, NB_BINS_COMPRESSED_SM_F0, packet_norm_bp2.data );
330 330 // 2) send the BP2 set
331 331 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
332 332 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
333 333 packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
334 334 BP_send( (char *) &packet_norm_bp2, queue_id,
335 335 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F0 + PACKET_LENGTH_DELTA,
336 336 SID_NORM_BP2_F0);
337 337 }
338 338 }
339 339
340 340 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F0)
341 341 {
342 342 // 1) reorganize the ASM and divide
343 343 ASM_reorganize_and_divide( asm_f0_patched_norm,
344 344 (float*) current_ring_node_to_send_asm_f0->buffer_address,
345 345 nb_sm_before_f0.norm_bp1 );
346 346 current_ring_node_to_send_asm_f0->coarseTime = incomingMsg->coarseTimeNORM;
347 347 current_ring_node_to_send_asm_f0->fineTime = incomingMsg->fineTimeNORM;
348 348 current_ring_node_to_send_asm_f0->sid = SID_NORM_ASM_F0;
349 349
350 350 // 3) send the spectral matrix packets
351 351 status = rtems_message_queue_send( queue_id, &current_ring_node_to_send_asm_f0, sizeof( ring_node* ) );
352 352 // change asm ring node
353 353 current_ring_node_to_send_asm_f0 = current_ring_node_to_send_asm_f0->next;
354 354 }
355 355
356 356 update_queue_max_count( queue_id_q_p0, &hk_lfr_q_p0_fifo_size_max );
357 357
358 358 }
359 359 }
360 360
361 361 //**********
362 362 // FUNCTIONS
363 363
364 364 void reset_nb_sm_f0( unsigned char lfrMode )
365 365 {
366 366 nb_sm_before_f0.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0 * 96;
367 367 nb_sm_before_f0.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1 * 96;
368 368 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;
369 369 nb_sm_before_f0.sbm1_bp1 = parameter_dump_packet.sy_lfr_s1_bp_p0 * 24; // 0.25 s per digit
370 370 nb_sm_before_f0.sbm1_bp2 = parameter_dump_packet.sy_lfr_s1_bp_p1 * 96;
371 371 nb_sm_before_f0.sbm2_bp1 = parameter_dump_packet.sy_lfr_s2_bp_p0 * 96;
372 372 nb_sm_before_f0.sbm2_bp2 = parameter_dump_packet.sy_lfr_s2_bp_p1 * 96;
373 373 nb_sm_before_f0.burst_bp1 = parameter_dump_packet.sy_lfr_b_bp_p0 * 96;
374 374 nb_sm_before_f0.burst_bp2 = parameter_dump_packet.sy_lfr_b_bp_p1 * 96;
375 375
376 376 if (lfrMode == LFR_MODE_SBM1)
377 377 {
378 378 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.sbm1_bp1;
379 379 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.sbm1_bp2;
380 380 }
381 381 else if (lfrMode == LFR_MODE_SBM2)
382 382 {
383 383 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.sbm2_bp1;
384 384 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.sbm2_bp2;
385 385 }
386 386 else if (lfrMode == LFR_MODE_BURST)
387 387 {
388 388 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.burst_bp1;
389 389 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.burst_bp2;
390 390 }
391 391 else
392 392 {
393 393 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.burst_bp1;
394 394 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.burst_bp2;
395 395 }
396 396 }
397 397
398 398 void init_k_coefficients_prc0( void )
399 399 {
400 400 init_k_coefficients( k_coeff_intercalib_f0_norm, NB_BINS_COMPRESSED_SM_F0 );
401 401
402 402 init_kcoeff_sbm_from_kcoeff_norm( k_coeff_intercalib_f0_norm, k_coeff_intercalib_f0_sbm, NB_BINS_COMPRESSED_SM_F0);
403 403 }
404 404
Show file before | Show file after | Hide comments
@@ -1,390 +1,390
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 printf("in AVF1 *** Error sending message to PRC1, code %d\n", status);
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( 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);
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.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
282 282 BP_send( (char *) &packet_sbm_bp1, queue_id_send,
283 283 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1 + PACKET_LENGTH_DELTA,
284 284 sid );
285 285 // 4) compute the BP2 set if needed
286 286 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP2_F1) || (incomingMsg->event & RTEMS_EVENT_SBM_BP2_F1) )
287 287 {
288 288 // 1) compute the BP2 set
289 289 BP2_set( compressed_sm_sbm_f1, NB_BINS_COMPRESSED_SM_SBM_F1, packet_sbm_bp2.data );
290 290 // 2) send the BP2 set
291 291 set_time( packet_sbm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
292 292 set_time( packet_sbm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
293 293 packet_sbm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
294 294 BP_send( (char *) &packet_sbm_bp2, queue_id_send,
295 295 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1 + PACKET_LENGTH_DELTA,
296 296 sid );
297 297 }
298 298 }
299 299
300 300 //*****
301 301 //*****
302 302 // NORM
303 303 //*****
304 304 //*****
305 305 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F1)
306 306 {
307 307 // 1) compress the matrix for Basic Parameters calculation
308 308 ASM_compress_reorganize_and_divide( asm_f1_patched_norm, compressed_sm_norm_f1,
309 309 nb_sm_before_f1.norm_bp1,
310 310 NB_BINS_COMPRESSED_SM_F1, NB_BINS_TO_AVERAGE_ASM_F1,
311 311 ASM_F1_INDICE_START );
312 312 // 2) compute the BP1 set
313 313 BP1_set( compressed_sm_norm_f1, k_coeff_intercalib_f1_norm, NB_BINS_COMPRESSED_SM_F1, packet_norm_bp1.data );
314 314 // 3) send the BP1 set
315 315 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
316 316 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
317 317 packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
318 318 BP_send( (char *) &packet_norm_bp1, queue_id_send,
319 319 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F1 + PACKET_LENGTH_DELTA,
320 320 SID_NORM_BP1_F1 );
321 321 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F1)
322 322 {
323 323 // 1) compute the BP2 set
324 324 BP2_set( compressed_sm_norm_f1, NB_BINS_COMPRESSED_SM_F1, packet_norm_bp2.data );
325 325 // 2) send the BP2 set
326 326 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
327 327 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
328 328 packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
329 329 BP_send( (char *) &packet_norm_bp2, queue_id_send,
330 330 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F1 + PACKET_LENGTH_DELTA,
331 331 SID_NORM_BP2_F1 );
332 332 }
333 333 }
334 334
335 335 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F1)
336 336 {
337 337 // 1) reorganize the ASM and divide
338 338 ASM_reorganize_and_divide( asm_f1_patched_norm,
339 339 (float*) current_ring_node_to_send_asm_f1->buffer_address,
340 340 nb_sm_before_f1.norm_bp1 );
341 341 current_ring_node_to_send_asm_f1->coarseTime = incomingMsg->coarseTimeNORM;
342 342 current_ring_node_to_send_asm_f1->fineTime = incomingMsg->fineTimeNORM;
343 343 current_ring_node_to_send_asm_f1->sid = SID_NORM_ASM_F1;
344 344 // 3) send the spectral matrix packets
345 345 status = rtems_message_queue_send( queue_id_send, &current_ring_node_to_send_asm_f1, sizeof( ring_node* ) );
346 346 // change asm ring node
347 347 current_ring_node_to_send_asm_f1 = current_ring_node_to_send_asm_f1->next;
348 348 }
349 349
350 350 update_queue_max_count( queue_id_q_p1, &hk_lfr_q_p1_fifo_size_max );
351 351
352 352 }
353 353 }
354 354
355 355 //**********
356 356 // FUNCTIONS
357 357
358 358 void reset_nb_sm_f1( unsigned char lfrMode )
359 359 {
360 360 nb_sm_before_f1.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0 * 16;
361 361 nb_sm_before_f1.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1 * 16;
362 362 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;
363 363 nb_sm_before_f1.sbm2_bp1 = parameter_dump_packet.sy_lfr_s2_bp_p0 * 16;
364 364 nb_sm_before_f1.sbm2_bp2 = parameter_dump_packet.sy_lfr_s2_bp_p1 * 16;
365 365 nb_sm_before_f1.burst_bp1 = parameter_dump_packet.sy_lfr_b_bp_p0 * 16;
366 366 nb_sm_before_f1.burst_bp2 = parameter_dump_packet.sy_lfr_b_bp_p1 * 16;
367 367
368 368 if (lfrMode == LFR_MODE_SBM2)
369 369 {
370 370 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.sbm2_bp1;
371 371 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.sbm2_bp2;
372 372 }
373 373 else if (lfrMode == LFR_MODE_BURST)
374 374 {
375 375 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.burst_bp1;
376 376 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.burst_bp2;
377 377 }
378 378 else
379 379 {
380 380 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.burst_bp1;
381 381 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.burst_bp2;
382 382 }
383 383 }
384 384
385 385 void init_k_coefficients_prc1( void )
386 386 {
387 387 init_k_coefficients( k_coeff_intercalib_f1_norm, NB_BINS_COMPRESSED_SM_F1 );
388 388
389 389 init_kcoeff_sbm_from_kcoeff_norm( k_coeff_intercalib_f1_norm, k_coeff_intercalib_f1_sbm, NB_BINS_COMPRESSED_SM_F1);
390 390 }
Show file before | Show file after | Hide comments
@@ -1,291 +1,279
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 "avf2_prc2.h"
11 11
12 12 nb_sm_before_bp_asm_f2 nb_sm_before_f2;
13 13
14 14 extern ring_node sm_ring_f2[ ];
15 15
16 16 //***
17 17 // F2
18 18 ring_node_asm asm_ring_norm_f2 [ NB_RING_NODES_ASM_NORM_F2 ];
19 19
20 20 ring_node ring_to_send_asm_f2 [ NB_RING_NODES_ASM_F2 ];
21 21 int buffer_asm_f2 [ NB_RING_NODES_ASM_F2 * TOTAL_SIZE_SM ];
22 22
23 23 float asm_f2_patched_norm [ TOTAL_SIZE_SM ];
24 24 float asm_f2_reorganized [ TOTAL_SIZE_SM ];
25 25
26 26 char asm_f2_char [ TOTAL_SIZE_SM * 2 ];
27 27 float compressed_sm_norm_f2[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F2];
28 28
29 29 float k_coeff_intercalib_f2[ NB_BINS_COMPRESSED_SM_F2 * NB_K_COEFF_PER_BIN ]; // 12 * 32 = 384
30 30
31 31 //************
32 32 // RTEMS TASKS
33 33
34 34 //***
35 35 // F2
36 36 rtems_task avf2_task( rtems_task_argument argument )
37 37 {
38 38 rtems_event_set event_out;
39 39 rtems_status_code status;
40 40 rtems_id queue_id_prc2;
41 41 asm_msg msgForMATR;
42 42 ring_node *nodeForAveraging;
43 43 ring_node_asm *current_ring_node_asm_norm_f2;
44 44
45 45 unsigned int nb_norm_bp1;
46 46 unsigned int nb_norm_bp2;
47 47 unsigned int nb_norm_asm;
48 48
49 49 nb_norm_bp1 = 0;
50 50 nb_norm_bp2 = 0;
51 51 nb_norm_asm = 0;
52 52
53 53 reset_nb_sm_f2( ); // reset the sm counters that drive the BP and ASM computations / transmissions
54 54 ASM_generic_init_ring( asm_ring_norm_f2, NB_RING_NODES_ASM_NORM_F2 );
55 55 current_ring_node_asm_norm_f2 = asm_ring_norm_f2;
56 56
57 57 BOOT_PRINTF("in AVF2 ***\n")
58 58
59 59 status = get_message_queue_id_prc2( &queue_id_prc2 );
60 60 if (status != RTEMS_SUCCESSFUL)
61 61 {
62 62 PRINTF1("in AVF2 *** ERR get_message_queue_id_prc2 %d\n", status)
63 63 }
64 64
65 65 while(1){
66 66 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
67 67
68 68 //****************************************
69 69 // initialize the mesage for the MATR task
70 70 msgForMATR.norm = current_ring_node_asm_norm_f2;
71 71 msgForMATR.burst_sbm = NULL;
72 72 msgForMATR.event = 0x00; // this composite event will be sent to the PRC2 task
73 73 //
74 74 //****************************************
75 75
76 76 nodeForAveraging = getRingNodeForAveraging( 2 );
77 77
78 // printf(" **0** %x . %x", sm_ring_f2[0].coarseTime, sm_ring_f2[0].fineTime);
79 // printf(" **1** %x . %x", sm_ring_f2[1].coarseTime, sm_ring_f2[1].fineTime);
80 // printf(" **2** %x . %x", sm_ring_f2[2].coarseTime, sm_ring_f2[2].fineTime);
81 // printf(" **3** %x . %x", sm_ring_f2[3].coarseTime, sm_ring_f2[3].fineTime);
82 // printf(" **4** %x . %x", sm_ring_f2[4].coarseTime, sm_ring_f2[4].fineTime);
83 // printf(" **5** %x . %x", sm_ring_f2[5].coarseTime, sm_ring_f2[5].fineTime);
84 // printf(" **6** %x . %x", sm_ring_f2[6].coarseTime, sm_ring_f2[6].fineTime);
85 // printf(" **7** %x . %x", sm_ring_f2[7].coarseTime, sm_ring_f2[7].fineTime);
86 // printf(" **8** %x . %x", sm_ring_f2[8].coarseTime, sm_ring_f2[8].fineTime);
87 // printf(" **9** %x . %x", sm_ring_f2[9].coarseTime, sm_ring_f2[9].fineTime);
88 // printf(" **10** %x . %x\n", sm_ring_f2[10].coarseTime, sm_ring_f2[10].fineTime);
89
90 78 // compute the average and store it in the averaged_sm_f2 buffer
91 79 SM_average_f2( current_ring_node_asm_norm_f2->matrix,
92 80 nodeForAveraging,
93 81 nb_norm_bp1,
94 82 &msgForMATR );
95 83
96 84 // update nb_average
97 85 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF2;
98 86 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF2;
99 87 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF2;
100 88
101 89 if (nb_norm_bp1 == nb_sm_before_f2.norm_bp1)
102 90 {
103 91 nb_norm_bp1 = 0;
104 92 // set another ring for the ASM storage
105 93 current_ring_node_asm_norm_f2 = current_ring_node_asm_norm_f2->next;
106 94 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
107 95 || (lfrCurrentMode == LFR_MODE_SBM2) )
108 96 {
109 97 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP1_F2;
110 98 }
111 99 }
112 100
113 101 if (nb_norm_bp2 == nb_sm_before_f2.norm_bp2)
114 102 {
115 103 nb_norm_bp2 = 0;
116 104 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
117 105 || (lfrCurrentMode == LFR_MODE_SBM2) )
118 106 {
119 107 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP2_F2;
120 108 }
121 109 }
122 110
123 111 if (nb_norm_asm == nb_sm_before_f2.norm_asm)
124 112 {
125 113 nb_norm_asm = 0;
126 114 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
127 115 || (lfrCurrentMode == LFR_MODE_SBM2) )
128 116 {
129 117 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_ASM_F2;
130 118 }
131 119 }
132 120
133 121 //*************************
134 122 // send the message to MATR
135 123 if (msgForMATR.event != 0x00)
136 124 {
137 125 status = rtems_message_queue_send( queue_id_prc2, (char *) &msgForMATR, MSG_QUEUE_SIZE_PRC2);
138 126 }
139 127
140 128 if (status != RTEMS_SUCCESSFUL) {
141 printf("in AVF2 *** Error sending message to MATR, code %d\n", status);
129 PRINTF1("in AVF2 *** Error sending message to MATR, code %d\n", status)
142 130 }
143 131 }
144 132 }
145 133
146 134 rtems_task prc2_task( rtems_task_argument argument )
147 135 {
148 136 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
149 137 size_t size; // size of the incoming TC packet
150 138 asm_msg *incomingMsg;
151 139 //
152 140 rtems_status_code status;
153 141 rtems_id queue_id_send;
154 142 rtems_id queue_id_q_p2;
155 143 bp_packet packet_norm_bp1;
156 144 bp_packet packet_norm_bp2;
157 145 ring_node *current_ring_node_to_send_asm_f2;
158 146
159 147 unsigned long long int localTime;
160 148
161 149 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
162 150 init_ring( ring_to_send_asm_f2, NB_RING_NODES_ASM_F2, (volatile int*) buffer_asm_f2, TOTAL_SIZE_SM );
163 151 current_ring_node_to_send_asm_f2 = ring_to_send_asm_f2;
164 152
165 153 //*************
166 154 // NORM headers
167 155 BP_init_header( &packet_norm_bp1,
168 156 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F2,
169 157 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F2, NB_BINS_COMPRESSED_SM_F2 );
170 158 BP_init_header( &packet_norm_bp2,
171 159 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F2,
172 160 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F2, NB_BINS_COMPRESSED_SM_F2 );
173 161
174 162 status = get_message_queue_id_send( &queue_id_send );
175 163 if (status != RTEMS_SUCCESSFUL)
176 164 {
177 165 PRINTF1("in PRC2 *** ERR get_message_queue_id_send %d\n", status)
178 166 }
179 167 status = get_message_queue_id_prc2( &queue_id_q_p2);
180 168 if (status != RTEMS_SUCCESSFUL)
181 169 {
182 170 PRINTF1("in PRC2 *** ERR get_message_queue_id_prc2 %d\n", status)
183 171 }
184 172
185 173 BOOT_PRINTF("in PRC2 ***\n")
186 174
187 175 while(1){
188 176 status = rtems_message_queue_receive( queue_id_q_p2, incomingData, &size, //************************************
189 177 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF2
190 178
191 179 incomingMsg = (asm_msg*) incomingData;
192 180
193 181 ASM_patch( incomingMsg->norm->matrix, asm_f2_patched_norm );
194 182
195 183 localTime = getTimeAsUnsignedLongLongInt( );
196 184
197 185 //*****
198 186 //*****
199 187 // NORM
200 188 //*****
201 189 //*****
202 190 // 1) compress the matrix for Basic Parameters calculation
203 191 ASM_compress_reorganize_and_divide( asm_f2_patched_norm, compressed_sm_norm_f2,
204 192 nb_sm_before_f2.norm_bp1,
205 193 NB_BINS_COMPRESSED_SM_F2, NB_BINS_TO_AVERAGE_ASM_F2,
206 194 ASM_F2_INDICE_START );
207 195 // BP1_F2
208 196 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F2)
209 197 {
210 198 // 1) compute the BP1 set
211 199 BP1_set( compressed_sm_norm_f2, k_coeff_intercalib_f2, NB_BINS_COMPRESSED_SM_F2, packet_norm_bp1.data );
212 200 // 2) send the BP1 set
213 201 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
214 202 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
215 203 packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
216 204 BP_send( (char *) &packet_norm_bp1, queue_id_send,
217 205 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F2 + PACKET_LENGTH_DELTA,
218 206 SID_NORM_BP1_F2 );
219 207 }
220 208 // BP2_F2
221 209 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F2)
222 210 {
223 211 // 1) compute the BP2 set
224 212 BP2_set( compressed_sm_norm_f2, NB_BINS_COMPRESSED_SM_F2, packet_norm_bp2.data );
225 213 // 2) send the BP2 set
226 214 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
227 215 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
228 216 packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
229 217 BP_send( (char *) &packet_norm_bp2, queue_id_send,
230 218 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F2 + PACKET_LENGTH_DELTA,
231 219 SID_NORM_BP2_F2 );
232 220 }
233 221
234 222 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F2)
235 223 {
236 224 // 1) reorganize the ASM and divide
237 225 ASM_reorganize_and_divide( asm_f2_patched_norm,
238 226 (float*) current_ring_node_to_send_asm_f2->buffer_address,
239 227 nb_sm_before_f2.norm_bp1 );
240 228 current_ring_node_to_send_asm_f2->coarseTime = incomingMsg->coarseTimeNORM;
241 229 current_ring_node_to_send_asm_f2->fineTime = incomingMsg->fineTimeNORM;
242 230 current_ring_node_to_send_asm_f2->sid = SID_NORM_ASM_F2;
243 231 // 3) send the spectral matrix packets
244 232 status = rtems_message_queue_send( queue_id_send, &current_ring_node_to_send_asm_f2, sizeof( ring_node* ) );
245 233 // change asm ring node
246 234 current_ring_node_to_send_asm_f2 = current_ring_node_to_send_asm_f2->next;
247 235 }
248 236
249 237 update_queue_max_count( queue_id_q_p2, &hk_lfr_q_p2_fifo_size_max );
250 238
251 239 }
252 240 }
253 241
254 242 //**********
255 243 // FUNCTIONS
256 244
257 245 void reset_nb_sm_f2( void )
258 246 {
259 247 nb_sm_before_f2.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0;
260 248 nb_sm_before_f2.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1;
261 249 nb_sm_before_f2.norm_asm = parameter_dump_packet.sy_lfr_n_asm_p[0] * 256 + parameter_dump_packet.sy_lfr_n_asm_p[1];
262 250 }
263 251
264 252 void SM_average_f2( float *averaged_spec_mat_f2,
265 253 ring_node *ring_node,
266 254 unsigned int nbAverageNormF2,
267 255 asm_msg *msgForMATR )
268 256 {
269 257 float sum;
270 258 unsigned int i;
271 259
272 260 for(i=0; i<TOTAL_SIZE_SM; i++)
273 261 {
274 262 sum = ( (int *) (ring_node->buffer_address) ) [ i ];
275 263 if ( (nbAverageNormF2 == 0) )
276 264 {
277 265 averaged_spec_mat_f2[ i ] = sum;
278 266 msgForMATR->coarseTimeNORM = ring_node->coarseTime;
279 267 msgForMATR->fineTimeNORM = ring_node->fineTime;
280 268 }
281 269 else
282 270 {
283 271 averaged_spec_mat_f2[ i ] = ( averaged_spec_mat_f2[ i ] + sum );
284 272 }
285 273 }
286 274 }
287 275
288 276 void init_k_coefficients_prc2( void )
289 277 {
290 278 init_k_coefficients( k_coeff_intercalib_f2, NB_BINS_COMPRESSED_SM_F2);
291 279 }
Show file before | Show file after | Hide comments
@@ -1,671 +1,671
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 rtems_isr spectral_matrices_isr_simu( rtems_vector_number vector )
250 250 {
251 251 rtems_status_code status_code;
252 252
253 253 //***
254 254 // F0
255 255 nb_sm_f0 = nb_sm_f0 + 1;
256 256 if (nb_sm_f0 == NB_SM_BEFORE_AVF0 )
257 257 {
258 258 ring_node_for_averaging_sm_f0 = current_ring_node_sm_f0;
259 259 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
260 260 {
261 261 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
262 262 }
263 263 nb_sm_f0 = 0;
264 264 }
265 265
266 266 //***
267 267 // F1
268 268 nb_sm_f0_aux_f1 = nb_sm_f0_aux_f1 + 1;
269 269 if (nb_sm_f0_aux_f1 == 6)
270 270 {
271 271 nb_sm_f0_aux_f1 = 0;
272 272 nb_sm_f1 = nb_sm_f1 + 1;
273 273 }
274 274 if (nb_sm_f1 == NB_SM_BEFORE_AVF1 )
275 275 {
276 276 ring_node_for_averaging_sm_f1 = current_ring_node_sm_f1;
277 277 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
278 278 {
279 279 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
280 280 }
281 281 nb_sm_f1 = 0;
282 282 }
283 283
284 284 //***
285 285 // F2
286 286 nb_sm_f0_aux_f2 = nb_sm_f0_aux_f2 + 1;
287 287 if (nb_sm_f0_aux_f2 == 96)
288 288 {
289 289 nb_sm_f0_aux_f2 = 0;
290 290 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2;
291 291 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
292 292 {
293 293 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
294 294 }
295 295 }
296 296 }
297 297
298 298 //******************
299 299 // Spectral Matrices
300 300
301 301 void reset_nb_sm( void )
302 302 {
303 303 nb_sm_f0 = 0;
304 304 nb_sm_f0_aux_f1 = 0;
305 305 nb_sm_f0_aux_f2 = 0;
306 306
307 307 nb_sm_f1 = 0;
308 308 }
309 309
310 310 void SM_init_rings( void )
311 311 {
312 312 init_ring( sm_ring_f0, NB_RING_NODES_SM_F0, sm_f0, TOTAL_SIZE_SM );
313 313 init_ring( sm_ring_f1, NB_RING_NODES_SM_F1, sm_f1, TOTAL_SIZE_SM );
314 314 init_ring( sm_ring_f2, NB_RING_NODES_SM_F2, sm_f2, TOTAL_SIZE_SM );
315 315
316 316 DEBUG_PRINTF1("sm_ring_f0 @%x\n", (unsigned int) sm_ring_f0)
317 317 DEBUG_PRINTF1("sm_ring_f1 @%x\n", (unsigned int) sm_ring_f1)
318 318 DEBUG_PRINTF1("sm_ring_f2 @%x\n", (unsigned int) sm_ring_f2)
319 319 DEBUG_PRINTF1("sm_f0 @%x\n", (unsigned int) sm_f0)
320 320 DEBUG_PRINTF1("sm_f1 @%x\n", (unsigned int) sm_f1)
321 321 DEBUG_PRINTF1("sm_f2 @%x\n", (unsigned int) sm_f2)
322 322 }
323 323
324 324 void ASM_generic_init_ring( ring_node_asm *ring, unsigned char nbNodes )
325 325 {
326 326 unsigned char i;
327 327
328 328 ring[ nbNodes - 1 ].next
329 329 = (ring_node_asm*) &ring[ 0 ];
330 330
331 331 for(i=0; i<nbNodes-1; i++)
332 332 {
333 333 ring[ i ].next = (ring_node_asm*) &ring[ i + 1 ];
334 334 }
335 335 }
336 336
337 337 void SM_reset_current_ring_nodes( void )
338 338 {
339 339 current_ring_node_sm_f0 = sm_ring_f0[0].next;
340 340 current_ring_node_sm_f1 = sm_ring_f1[0].next;
341 341 current_ring_node_sm_f2 = sm_ring_f2[0].next;
342 342
343 343 ring_node_for_averaging_sm_f0 = NULL;
344 344 ring_node_for_averaging_sm_f1 = NULL;
345 345 ring_node_for_averaging_sm_f2 = NULL;
346 346 }
347 347
348 348 //*****************
349 349 // Basic Parameters
350 350
351 351 void BP_init_header( bp_packet *packet,
352 352 unsigned int apid, unsigned char sid,
353 353 unsigned int packetLength, unsigned char blkNr )
354 354 {
355 355 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
356 356 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
357 357 packet->reserved = 0x00;
358 358 packet->userApplication = CCSDS_USER_APP;
359 359 packet->packetID[0] = (unsigned char) (apid >> 8);
360 360 packet->packetID[1] = (unsigned char) (apid);
361 361 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
362 362 packet->packetSequenceControl[1] = 0x00;
363 363 packet->packetLength[0] = (unsigned char) (packetLength >> 8);
364 364 packet->packetLength[1] = (unsigned char) (packetLength);
365 365 // DATA FIELD HEADER
366 366 packet->spare1_pusVersion_spare2 = 0x10;
367 367 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
368 368 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
369 369 packet->destinationID = TM_DESTINATION_ID_GROUND;
370 370 packet->time[0] = 0x00;
371 371 packet->time[1] = 0x00;
372 372 packet->time[2] = 0x00;
373 373 packet->time[3] = 0x00;
374 374 packet->time[4] = 0x00;
375 375 packet->time[5] = 0x00;
376 376 // AUXILIARY DATA HEADER
377 377 packet->sid = sid;
378 378 packet->biaStatusInfo = 0x00;
379 379 packet->sy_lfr_common_parameters_spare = 0x00;
380 380 packet->sy_lfr_common_parameters = 0x00;
381 381 packet->acquisitionTime[0] = 0x00;
382 382 packet->acquisitionTime[1] = 0x00;
383 383 packet->acquisitionTime[2] = 0x00;
384 384 packet->acquisitionTime[3] = 0x00;
385 385 packet->acquisitionTime[4] = 0x00;
386 386 packet->acquisitionTime[5] = 0x00;
387 387 packet->pa_lfr_bp_blk_nr[0] = 0x00; // BLK_NR MSB
388 388 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
389 389 }
390 390
391 391 void BP_init_header_with_spare( bp_packet_with_spare *packet,
392 392 unsigned int apid, unsigned char sid,
393 393 unsigned int packetLength , unsigned char blkNr)
394 394 {
395 395 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
396 396 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
397 397 packet->reserved = 0x00;
398 398 packet->userApplication = CCSDS_USER_APP;
399 399 packet->packetID[0] = (unsigned char) (apid >> 8);
400 400 packet->packetID[1] = (unsigned char) (apid);
401 401 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
402 402 packet->packetSequenceControl[1] = 0x00;
403 403 packet->packetLength[0] = (unsigned char) (packetLength >> 8);
404 404 packet->packetLength[1] = (unsigned char) (packetLength);
405 405 // DATA FIELD HEADER
406 406 packet->spare1_pusVersion_spare2 = 0x10;
407 407 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
408 408 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
409 409 packet->destinationID = TM_DESTINATION_ID_GROUND;
410 410 // AUXILIARY DATA HEADER
411 411 packet->sid = sid;
412 412 packet->biaStatusInfo = 0x00;
413 413 packet->sy_lfr_common_parameters_spare = 0x00;
414 414 packet->sy_lfr_common_parameters = 0x00;
415 415 packet->time[0] = 0x00;
416 416 packet->time[0] = 0x00;
417 417 packet->time[0] = 0x00;
418 418 packet->time[0] = 0x00;
419 419 packet->time[0] = 0x00;
420 420 packet->time[0] = 0x00;
421 421 packet->source_data_spare = 0x00;
422 422 packet->pa_lfr_bp_blk_nr[0] = 0x00; // BLK_NR MSB
423 423 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
424 424 }
425 425
426 426 void BP_send(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
427 427 {
428 428 rtems_status_code status;
429 429
430 430 // SET THE SEQUENCE_CNT PARAMETER
431 431 increment_seq_counter_source_id( (unsigned char*) &data[ PACKET_POS_SEQUENCE_CNT ], sid );
432 432 // SEND PACKET
433 433 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
434 434 if (status != RTEMS_SUCCESSFUL)
435 435 {
436 printf("ERR *** in BP_send *** ERR %d\n", (int) status);
436 PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
437 437 }
438 438 }
439 439
440 440 //******************
441 441 // general functions
442 442
443 443 void reset_sm_status( void )
444 444 {
445 445 // error
446 446 // 10 --------------- 9 ---------------- 8 ---------------- 7 ---------
447 447 // input_fif0_write_2 input_fifo_write_1 input_fifo_write_0 buffer_full
448 448 // ---------- 5 -- 4 -- 3 -- 2 -- 1 -- 0 --
449 449 // ready bits f2_1 f2_0 f1_1 f1_1 f0_1 f0_0
450 450
451 451 spectral_matrix_regs->status = 0x7ff; // [0111 1111 1111]
452 452 }
453 453
454 454 void reset_spectral_matrix_regs( void )
455 455 {
456 456 /** This function resets the spectral matrices module registers.
457 457 *
458 458 * The registers affected by this function are located at the following offset addresses:
459 459 *
460 460 * - 0x00 config
461 461 * - 0x04 status
462 462 * - 0x08 matrixF0_Address0
463 463 * - 0x10 matrixFO_Address1
464 464 * - 0x14 matrixF1_Address
465 465 * - 0x18 matrixF2_Address
466 466 *
467 467 */
468 468
469 469 set_sm_irq_onError( 0 );
470 470
471 471 set_sm_irq_onNewMatrix( 0 );
472 472
473 473 reset_sm_status();
474 474
475 475 // F1
476 476 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->previous->buffer_address;
477 477 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
478 478 // F2
479 479 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->previous->buffer_address;
480 480 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
481 481 // F3
482 482 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->previous->buffer_address;
483 483 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
484 484
485 485 spectral_matrix_regs->matrix_length = 0xc8; // 25 * 128 / 16 = 200 = 0xc8
486 486 }
487 487
488 488 void set_time( unsigned char *time, unsigned char * timeInBuffer )
489 489 {
490 490 time[0] = timeInBuffer[0];
491 491 time[1] = timeInBuffer[1];
492 492 time[2] = timeInBuffer[2];
493 493 time[3] = timeInBuffer[3];
494 494 time[4] = timeInBuffer[6];
495 495 time[5] = timeInBuffer[7];
496 496 }
497 497
498 498 unsigned long long int get_acquisition_time( unsigned char *timePtr )
499 499 {
500 500 unsigned long long int acquisitionTimeAslong;
501 501 acquisitionTimeAslong = 0x00;
502 502 acquisitionTimeAslong = ( (unsigned long long int) (timePtr[0] & 0x7f) << 40 ) // [0111 1111] mask the synchronization bit
503 503 + ( (unsigned long long int) timePtr[1] << 32 )
504 504 + ( (unsigned long long int) timePtr[2] << 24 )
505 505 + ( (unsigned long long int) timePtr[3] << 16 )
506 506 + ( (unsigned long long int) timePtr[6] << 8 )
507 507 + ( (unsigned long long int) timePtr[7] );
508 508 return acquisitionTimeAslong;
509 509 }
510 510
511 511 unsigned char getSID( rtems_event_set event )
512 512 {
513 513 unsigned char sid;
514 514
515 515 rtems_event_set eventSetBURST;
516 516 rtems_event_set eventSetSBM;
517 517
518 518 //******
519 519 // BURST
520 520 eventSetBURST = RTEMS_EVENT_BURST_BP1_F0
521 521 | RTEMS_EVENT_BURST_BP1_F1
522 522 | RTEMS_EVENT_BURST_BP2_F0
523 523 | RTEMS_EVENT_BURST_BP2_F1;
524 524
525 525 //****
526 526 // SBM
527 527 eventSetSBM = RTEMS_EVENT_SBM_BP1_F0
528 528 | RTEMS_EVENT_SBM_BP1_F1
529 529 | RTEMS_EVENT_SBM_BP2_F0
530 530 | RTEMS_EVENT_SBM_BP2_F1;
531 531
532 532 if (event & eventSetBURST)
533 533 {
534 534 sid = SID_BURST_BP1_F0;
535 535 }
536 536 else if (event & eventSetSBM)
537 537 {
538 538 sid = SID_SBM1_BP1_F0;
539 539 }
540 540 else
541 541 {
542 542 sid = 0;
543 543 }
544 544
545 545 return sid;
546 546 }
547 547
548 548 void extractReImVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
549 549 {
550 550 unsigned int i;
551 551 float re;
552 552 float im;
553 553
554 554 for (i=0; i<NB_BINS_PER_SM; i++){
555 555 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i * 2 ];
556 556 im = inputASM[ (asmComponent*NB_BINS_PER_SM) + i * 2 + 1];
557 557 outputASM[ (asmComponent *NB_BINS_PER_SM) + i] = re;
558 558 outputASM[ (asmComponent+1)*NB_BINS_PER_SM + i] = im;
559 559 }
560 560 }
561 561
562 562 void copyReVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
563 563 {
564 564 unsigned int i;
565 565 float re;
566 566
567 567 for (i=0; i<NB_BINS_PER_SM; i++){
568 568 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i];
569 569 outputASM[ (asmComponent*NB_BINS_PER_SM) + i] = re;
570 570 }
571 571 }
572 572
573 573 void ASM_patch( float *inputASM, float *outputASM )
574 574 {
575 575 extractReImVectors( inputASM, outputASM, 1); // b1b2
576 576 extractReImVectors( inputASM, outputASM, 3 ); // b1b3
577 577 extractReImVectors( inputASM, outputASM, 5 ); // b1e1
578 578 extractReImVectors( inputASM, outputASM, 7 ); // b1e2
579 579 extractReImVectors( inputASM, outputASM, 10 ); // b2b3
580 580 extractReImVectors( inputASM, outputASM, 12 ); // b2e1
581 581 extractReImVectors( inputASM, outputASM, 14 ); // b2e2
582 582 extractReImVectors( inputASM, outputASM, 17 ); // b3e1
583 583 extractReImVectors( inputASM, outputASM, 19 ); // b3e2
584 584 extractReImVectors( inputASM, outputASM, 22 ); // e1e2
585 585
586 586 copyReVectors(inputASM, outputASM, 0 ); // b1b1
587 587 copyReVectors(inputASM, outputASM, 9 ); // b2b2
588 588 copyReVectors(inputASM, outputASM, 16); // b3b3
589 589 copyReVectors(inputASM, outputASM, 21); // e1e1
590 590 copyReVectors(inputASM, outputASM, 24); // e2e2
591 591 }
592 592
593 593 void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
594 594 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage, unsigned char ASMIndexStart )
595 595 {
596 596 //*************
597 597 // input format
598 598 // component0[0 .. 127] component1[0 .. 127] .. component24[0 .. 127]
599 599 //**************
600 600 // output format
601 601 // matr0[0 .. 24] matr1[0 .. 24] .. matr127[0 .. 24]
602 602 //************
603 603 // compression
604 604 // matr0[0 .. 24] matr1[0 .. 24] .. matr11[0 .. 24] => f0 NORM
605 605 // matr0[0 .. 24] matr1[0 .. 24] .. matr22[0 .. 24] => f0 BURST, SBM
606 606
607 607 int frequencyBin;
608 608 int asmComponent;
609 609 int offsetASM;
610 610 int offsetCompressed;
611 611 int offsetFBin;
612 612 int fBinMask;
613 613 int k;
614 614
615 615 // BUILD DATA
616 616 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
617 617 {
618 618 for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
619 619 {
620 620 offsetCompressed = // NO TIME OFFSET
621 621 frequencyBin * NB_VALUES_PER_SM
622 622 + asmComponent;
623 623 offsetASM = // NO TIME OFFSET
624 624 asmComponent * NB_BINS_PER_SM
625 625 + ASMIndexStart
626 626 + frequencyBin * nbBinsToAverage;
627 627 offsetFBin = ASMIndexStart
628 628 + frequencyBin * nbBinsToAverage;
629 629 compressed_spec_mat[ offsetCompressed ] = 0;
630 630 for ( k = 0; k < nbBinsToAverage; k++ )
631 631 {
632 632 fBinMask = getFBinMask( offsetFBin + k );
633 633 compressed_spec_mat[offsetCompressed ] =
634 634 ( compressed_spec_mat[ offsetCompressed ]
635 635 + averaged_spec_mat[ offsetASM + k ] * fBinMask );
636 636 }
637 637 compressed_spec_mat[ offsetCompressed ] =
638 638 compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
639 639 }
640 640 }
641 641
642 642 }
643 643
644 644 int getFBinMask( int index )
645 645 {
646 646 unsigned int indexInChar;
647 647 unsigned int indexInTheChar;
648 648 int fbin;
649 649
650 650 indexInChar = index >> 3;
651 651 indexInTheChar = index - indexInChar * 8;
652 652
653 653 fbin = (int) ((parameter_dump_packet.sy_lfr_fbins_f0_word1[ NB_BYTES_PER_FREQ_MASK - 1 - indexInChar] >> indexInTheChar) & 0x1);
654 654
655 655 return fbin;
656 656 }
657 657
658 658 void init_kcoeff_sbm_from_kcoeff_norm(float *input_kcoeff, float *output_kcoeff, unsigned char nb_bins_norm)
659 659 {
660 660 unsigned char bin;
661 661 unsigned char kcoeff;
662 662
663 663 for (bin=0; bin<nb_bins_norm; bin++)
664 664 {
665 665 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
666 666 {
667 667 output_kcoeff[ (bin*NB_K_COEFF_PER_BIN + kcoeff)*2 ] = input_kcoeff[ bin*NB_K_COEFF_PER_BIN + kcoeff ];
668 668 output_kcoeff[ (bin*NB_K_COEFF_PER_BIN + kcoeff)*2 + 1 ] = input_kcoeff[ bin*NB_K_COEFF_PER_BIN + kcoeff ];
669 669 }
670 670 }
671 671 }
Show file before | Show file after | Hide comments
@@ -1,1180 +1,1180
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 printf("this is the end!!!\n");
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 else // the mode value is valid, check the transition
183 183 {
184 184 status = check_mode_transition(requestedMode);
185 185 if (status != LFR_SUCCESSFUL)
186 186 {
187 187 PRINTF("ERR *** in action_enter_mode *** check_mode_transition\n")
188 188 send_tm_lfr_tc_exe_not_executable( TC, queue_id );
189 189 }
190 190 }
191 191
192 192 if ( status == LFR_SUCCESSFUL ) // the transition is valid, check the date
193 193 {
194 194 status = check_transition_date( transitionCoarseTime );
195 195 if (status != LFR_SUCCESSFUL)
196 196 {
197 197 PRINTF("ERR *** in action_enter_mode *** check_transition_date\n")
198 198 send_tm_lfr_tc_exe_inconsistent( TC, queue_id,
199 199 BYTE_POS_CP_LFR_ENTER_MODE_TIME,
200 200 bytePosPtr[ BYTE_POS_CP_LFR_ENTER_MODE_TIME + 3 ] );
201 201 }
202 202 }
203 203
204 204 if ( status == LFR_SUCCESSFUL ) // the date is valid, enter the mode
205 205 {
206 206 PRINTF1("OK *** in action_enter_mode *** enter mode %d\n", requestedMode);
207 207 status = enter_mode( requestedMode, transitionCoarseTime );
208 208 }
209 209
210 210 return status;
211 211 }
212 212
213 213 int action_update_info(ccsdsTelecommandPacket_t *TC, rtems_id queue_id)
214 214 {
215 215 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
216 216 *
217 217 * @param TC points to the TeleCommand packet that is being processed
218 218 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
219 219 *
220 220 * @return LFR directive status code:
221 221 * - LFR_DEFAULT
222 222 * - LFR_SUCCESSFUL
223 223 *
224 224 */
225 225
226 226 unsigned int val;
227 227 int result;
228 228 unsigned int status;
229 229 unsigned char mode;
230 230 unsigned char * bytePosPtr;
231 231
232 232 bytePosPtr = (unsigned char *) &TC->packetID;
233 233
234 234 // check LFR mode
235 235 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET5 ] & 0x1e) >> 1;
236 236 status = check_update_info_hk_lfr_mode( mode );
237 237 if (status == LFR_SUCCESSFUL) // check TDS mode
238 238 {
239 239 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET6 ] & 0xf0) >> 4;
240 240 status = check_update_info_hk_tds_mode( mode );
241 241 }
242 242 if (status == LFR_SUCCESSFUL) // check THR mode
243 243 {
244 244 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET6 ] & 0x0f);
245 245 status = check_update_info_hk_thr_mode( mode );
246 246 }
247 247 if (status == LFR_SUCCESSFUL) // if the parameter check is successful
248 248 {
249 249 val = housekeeping_packet.hk_lfr_update_info_tc_cnt[0] * 256
250 250 + housekeeping_packet.hk_lfr_update_info_tc_cnt[1];
251 251 val++;
252 252 housekeeping_packet.hk_lfr_update_info_tc_cnt[0] = (unsigned char) (val >> 8);
253 253 housekeeping_packet.hk_lfr_update_info_tc_cnt[1] = (unsigned char) (val);
254 254 }
255 255
256 256 // pa_bia_status_info
257 257 // => pa_bia_mode_mux_set 3 bits
258 258 // => pa_bia_mode_hv_enabled 1 bit
259 259 // => pa_bia_mode_bias1_enabled 1 bit
260 260 // => pa_bia_mode_bias2_enabled 1 bit
261 261 // => pa_bia_mode_bias3_enabled 1 bit
262 262 // => pa_bia_on_off (cp_dpu_bias_on_off)
263 263 pa_bia_status_info = bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET2 ] & 0xfe; // [1111 1110]
264 264 pa_bia_status_info = pa_bia_status_info
265 265 | (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET1 ] & 0x1);
266 266
267 267 result = status;
268 268
269 269 return result;
270 270 }
271 271
272 272 int action_enable_calibration(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
273 273 {
274 274 /** This function executes specific actions when a TC_LFR_ENABLE_CALIBRATION TeleCommand has been received.
275 275 *
276 276 * @param TC points to the TeleCommand packet that is being processed
277 277 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
278 278 *
279 279 */
280 280
281 281 int result;
282 282
283 283 result = LFR_DEFAULT;
284 284
285 285 setCalibration( true );
286 286
287 287 result = LFR_SUCCESSFUL;
288 288
289 289 return result;
290 290 }
291 291
292 292 int action_disable_calibration(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
293 293 {
294 294 /** This function executes specific actions when a TC_LFR_DISABLE_CALIBRATION TeleCommand has been received.
295 295 *
296 296 * @param TC points to the TeleCommand packet that is being processed
297 297 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
298 298 *
299 299 */
300 300
301 301 int result;
302 302
303 303 result = LFR_DEFAULT;
304 304
305 305 setCalibration( false );
306 306
307 307 result = LFR_SUCCESSFUL;
308 308
309 309 return result;
310 310 }
311 311
312 312 int action_update_time(ccsdsTelecommandPacket_t *TC)
313 313 {
314 314 /** This function executes specific actions when a TC_LFR_UPDATE_TIME TeleCommand has been received.
315 315 *
316 316 * @param TC points to the TeleCommand packet that is being processed
317 317 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
318 318 *
319 319 * @return LFR_SUCCESSFUL
320 320 *
321 321 */
322 322
323 323 unsigned int val;
324 324
325 325 time_management_regs->coarse_time_load = (TC->dataAndCRC[0] << 24)
326 326 + (TC->dataAndCRC[1] << 16)
327 327 + (TC->dataAndCRC[2] << 8)
328 328 + TC->dataAndCRC[3];
329 329
330 330 val = housekeeping_packet.hk_lfr_update_time_tc_cnt[0] * 256
331 331 + housekeeping_packet.hk_lfr_update_time_tc_cnt[1];
332 332 val++;
333 333 housekeeping_packet.hk_lfr_update_time_tc_cnt[0] = (unsigned char) (val >> 8);
334 334 housekeeping_packet.hk_lfr_update_time_tc_cnt[1] = (unsigned char) (val);
335 335
336 336 return LFR_SUCCESSFUL;
337 337 }
338 338
339 339 //*******************
340 340 // ENTERING THE MODES
341 341 int check_mode_value( unsigned char requestedMode )
342 342 {
343 343 int status;
344 344
345 345 if ( (requestedMode != LFR_MODE_STANDBY)
346 346 && (requestedMode != LFR_MODE_NORMAL) && (requestedMode != LFR_MODE_BURST)
347 347 && (requestedMode != LFR_MODE_SBM1) && (requestedMode != LFR_MODE_SBM2) )
348 348 {
349 349 status = LFR_DEFAULT;
350 350 }
351 351 else
352 352 {
353 353 status = LFR_SUCCESSFUL;
354 354 }
355 355
356 356 return status;
357 357 }
358 358
359 359 int check_mode_transition( unsigned char requestedMode )
360 360 {
361 361 /** This function checks the validity of the transition requested by the TC_LFR_ENTER_MODE.
362 362 *
363 363 * @param requestedMode is the mode requested by the TC_LFR_ENTER_MODE
364 364 *
365 365 * @return LFR directive status codes:
366 366 * - LFR_SUCCESSFUL - the transition is authorized
367 367 * - LFR_DEFAULT - the transition is not authorized
368 368 *
369 369 */
370 370
371 371 int status;
372 372
373 373 switch (requestedMode)
374 374 {
375 375 case LFR_MODE_STANDBY:
376 376 if ( lfrCurrentMode == LFR_MODE_STANDBY ) {
377 377 status = LFR_DEFAULT;
378 378 }
379 379 else
380 380 {
381 381 status = LFR_SUCCESSFUL;
382 382 }
383 383 break;
384 384 case LFR_MODE_NORMAL:
385 385 if ( lfrCurrentMode == LFR_MODE_NORMAL ) {
386 386 status = LFR_DEFAULT;
387 387 }
388 388 else {
389 389 status = LFR_SUCCESSFUL;
390 390 }
391 391 break;
392 392 case LFR_MODE_BURST:
393 393 if ( lfrCurrentMode == LFR_MODE_BURST ) {
394 394 status = LFR_DEFAULT;
395 395 }
396 396 else {
397 397 status = LFR_SUCCESSFUL;
398 398 }
399 399 break;
400 400 case LFR_MODE_SBM1:
401 401 if ( lfrCurrentMode == LFR_MODE_SBM1 ) {
402 402 status = LFR_DEFAULT;
403 403 }
404 404 else {
405 405 status = LFR_SUCCESSFUL;
406 406 }
407 407 break;
408 408 case LFR_MODE_SBM2:
409 409 if ( lfrCurrentMode == LFR_MODE_SBM2 ) {
410 410 status = LFR_DEFAULT;
411 411 }
412 412 else {
413 413 status = LFR_SUCCESSFUL;
414 414 }
415 415 break;
416 416 default:
417 417 status = LFR_DEFAULT;
418 418 break;
419 419 }
420 420
421 421 return status;
422 422 }
423 423
424 424 int check_transition_date( unsigned int transitionCoarseTime )
425 425 {
426 426 int status;
427 427 unsigned int localCoarseTime;
428 428 unsigned int deltaCoarseTime;
429 429
430 430 status = LFR_SUCCESSFUL;
431 431
432 432 if (transitionCoarseTime == 0) // transition time = 0 means an instant transition
433 433 {
434 434 status = LFR_SUCCESSFUL;
435 435 }
436 436 else
437 437 {
438 438 localCoarseTime = time_management_regs->coarse_time & 0x7fffffff;
439 439
440 440 PRINTF2("localTime = %x, transitionTime = %x\n", localCoarseTime, transitionCoarseTime)
441 441
442 442 if ( transitionCoarseTime <= localCoarseTime ) // SSS-CP-EQS-322
443 443 {
444 444 status = LFR_DEFAULT;
445 445 PRINTF("ERR *** in check_transition_date *** transitionCoarseTime <= localCoarseTime\n")
446 446 }
447 447
448 448 if (status == LFR_SUCCESSFUL)
449 449 {
450 450 deltaCoarseTime = transitionCoarseTime - localCoarseTime;
451 451 if ( deltaCoarseTime > 3 ) // SSS-CP-EQS-323
452 452 {
453 453 status = LFR_DEFAULT;
454 454 PRINTF1("ERR *** in check_transition_date *** deltaCoarseTime = %x\n", deltaCoarseTime)
455 455 }
456 456 }
457 457 }
458 458
459 459 return status;
460 460 }
461 461
462 462 int stop_current_mode( void )
463 463 {
464 464 /** This function stops the current mode by masking interrupt lines and suspending science tasks.
465 465 *
466 466 * @return RTEMS directive status codes:
467 467 * - RTEMS_SUCCESSFUL - task restarted successfully
468 468 * - RTEMS_INVALID_ID - task id invalid
469 469 * - RTEMS_ALREADY_SUSPENDED - task already suspended
470 470 *
471 471 */
472 472
473 473 rtems_status_code status;
474 474
475 475 status = RTEMS_SUCCESSFUL;
476 476
477 477 // (1) mask interruptions
478 478 LEON_Mask_interrupt( IRQ_WAVEFORM_PICKER ); // mask waveform picker interrupt
479 479 LEON_Mask_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
480 480
481 481 // (2) reset waveform picker registers
482 482 reset_wfp_burst_enable(); // reset burst and enable bits
483 483 reset_wfp_status(); // reset all the status bits
484 484
485 485 // (3) reset spectral matrices registers
486 486 set_sm_irq_onNewMatrix( 0 ); // stop the spectral matrices
487 487 reset_sm_status();
488 488
489 489 // reset lfr VHDL module
490 490 reset_lfr();
491 491
492 492 reset_extractSWF(); // reset the extractSWF flag to false
493 493
494 494 // (4) clear interruptions
495 495 LEON_Clear_interrupt( IRQ_WAVEFORM_PICKER ); // clear waveform picker interrupt
496 496 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
497 497
498 498 // <Spectral Matrices simulator>
499 499 LEON_Mask_interrupt( IRQ_SM_SIMULATOR ); // mask spectral matrix interrupt simulator
500 500 timer_stop( (gptimer_regs_t*) REGS_ADDR_GPTIMER, TIMER_SM_SIMULATOR );
501 501 LEON_Clear_interrupt( IRQ_SM_SIMULATOR ); // clear spectral matrix interrupt simulator
502 502 // </Spectral Matrices simulator>
503 503
504 504 // suspend several tasks
505 505 if (lfrCurrentMode != LFR_MODE_STANDBY) {
506 506 status = suspend_science_tasks();
507 507 }
508 508
509 509 if (status != RTEMS_SUCCESSFUL)
510 510 {
511 511 PRINTF1("in stop_current_mode *** in suspend_science_tasks *** ERR code: %d\n", status)
512 512 }
513 513
514 514 return status;
515 515 }
516 516
517 517 int enter_mode( unsigned char mode, unsigned int transitionCoarseTime )
518 518 {
519 519 /** This function is launched after a mode transition validation.
520 520 *
521 521 * @param mode is the mode in which LFR will be put.
522 522 *
523 523 * @return RTEMS directive status codes:
524 524 * - RTEMS_SUCCESSFUL - the mode has been entered successfully
525 525 * - RTEMS_NOT_SATISFIED - the mode has not been entered successfully
526 526 *
527 527 */
528 528
529 529 rtems_status_code status;
530 530
531 531 //**********************
532 532 // STOP THE CURRENT MODE
533 533 status = stop_current_mode();
534 534 if (status != RTEMS_SUCCESSFUL)
535 535 {
536 536 PRINTF1("ERR *** in enter_mode *** stop_current_mode with mode = %d\n", mode)
537 537 }
538 538
539 539 //*************************
540 540 // ENTER THE REQUESTED MODE
541 541 if (status == RTEMS_SUCCESSFUL) // if the current mode has been successfully stopped
542 542 {
543 543 if ( (mode == LFR_MODE_NORMAL) || (mode == LFR_MODE_BURST)
544 544 || (mode == LFR_MODE_SBM1) || (mode == LFR_MODE_SBM2) )
545 545 {
546 546 #ifdef PRINT_TASK_STATISTICS
547 547 rtems_cpu_usage_reset();
548 548 #endif
549 549 status = restart_science_tasks( mode );
550 550 if (status == RTEMS_SUCCESSFUL)
551 551 {
552 552 launch_spectral_matrix( );
553 553 launch_waveform_picker( mode, transitionCoarseTime );
554 554 }
555 555 }
556 556 else if ( mode == LFR_MODE_STANDBY )
557 557 {
558 558 #ifdef PRINT_TASK_STATISTICS
559 559 rtems_cpu_usage_report();
560 560 #endif
561 561
562 562 #ifdef PRINT_STACK_REPORT
563 563 PRINTF("stack report selected\n")
564 564 rtems_stack_checker_report_usage();
565 565 #endif
566 566 }
567 567 else
568 568 {
569 569 status = RTEMS_UNSATISFIED;
570 570 }
571 571 }
572 572
573 573 if (status != RTEMS_SUCCESSFUL)
574 574 {
575 575 PRINTF1("ERR *** in enter_mode *** status = %d\n", status)
576 576 status = RTEMS_UNSATISFIED;
577 577 }
578 578
579 579 return status;
580 580 }
581 581
582 582 int restart_science_tasks(unsigned char lfrRequestedMode )
583 583 {
584 584 /** This function is used to restart all science tasks.
585 585 *
586 586 * @return RTEMS directive status codes:
587 587 * - RTEMS_SUCCESSFUL - task restarted successfully
588 588 * - RTEMS_INVALID_ID - task id invalid
589 589 * - RTEMS_INCORRECT_STATE - task never started
590 590 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
591 591 *
592 592 * Science tasks are AVF0, PRC0, WFRM, CWF3, CW2, CWF1
593 593 *
594 594 */
595 595
596 596 rtems_status_code status[10];
597 597 rtems_status_code ret;
598 598
599 599 ret = RTEMS_SUCCESSFUL;
600 600
601 601 status[0] = rtems_task_restart( Task_id[TASKID_AVF0], lfrRequestedMode );
602 602 if (status[0] != RTEMS_SUCCESSFUL)
603 603 {
604 604 PRINTF1("in restart_science_task *** AVF0 ERR %d\n", status[0])
605 605 }
606 606
607 607 status[1] = rtems_task_restart( Task_id[TASKID_PRC0], lfrRequestedMode );
608 608 if (status[1] != RTEMS_SUCCESSFUL)
609 609 {
610 610 PRINTF1("in restart_science_task *** PRC0 ERR %d\n", status[1])
611 611 }
612 612
613 613 status[2] = rtems_task_restart( Task_id[TASKID_WFRM],1 );
614 614 if (status[2] != RTEMS_SUCCESSFUL)
615 615 {
616 616 PRINTF1("in restart_science_task *** WFRM ERR %d\n", status[2])
617 617 }
618 618
619 619 status[3] = rtems_task_restart( Task_id[TASKID_CWF3],1 );
620 620 if (status[3] != RTEMS_SUCCESSFUL)
621 621 {
622 622 PRINTF1("in restart_science_task *** CWF3 ERR %d\n", status[3])
623 623 }
624 624
625 625 status[4] = rtems_task_restart( Task_id[TASKID_CWF2],1 );
626 626 if (status[4] != RTEMS_SUCCESSFUL)
627 627 {
628 628 PRINTF1("in restart_science_task *** CWF2 ERR %d\n", status[4])
629 629 }
630 630
631 631 status[5] = rtems_task_restart( Task_id[TASKID_CWF1],1 );
632 632 if (status[5] != RTEMS_SUCCESSFUL)
633 633 {
634 634 PRINTF1("in restart_science_task *** CWF1 ERR %d\n", status[5])
635 635 }
636 636
637 637 status[6] = rtems_task_restart( Task_id[TASKID_AVF1], lfrRequestedMode );
638 638 if (status[6] != RTEMS_SUCCESSFUL)
639 639 {
640 640 PRINTF1("in restart_science_task *** AVF1 ERR %d\n", status[6])
641 641 }
642 642
643 643 status[7] = rtems_task_restart( Task_id[TASKID_PRC1],lfrRequestedMode );
644 644 if (status[7] != RTEMS_SUCCESSFUL)
645 645 {
646 646 PRINTF1("in restart_science_task *** PRC1 ERR %d\n", status[7])
647 647 }
648 648
649 649 status[8] = rtems_task_restart( Task_id[TASKID_AVF2], 1 );
650 650 if (status[8] != RTEMS_SUCCESSFUL)
651 651 {
652 652 PRINTF1("in restart_science_task *** AVF2 ERR %d\n", status[8])
653 653 }
654 654
655 655 status[9] = rtems_task_restart( Task_id[TASKID_PRC2], 1 );
656 656 if (status[9] != RTEMS_SUCCESSFUL)
657 657 {
658 658 PRINTF1("in restart_science_task *** PRC2 ERR %d\n", status[9])
659 659 }
660 660
661 661 if ( (status[0] != RTEMS_SUCCESSFUL) || (status[1] != RTEMS_SUCCESSFUL) ||
662 662 (status[2] != RTEMS_SUCCESSFUL) || (status[3] != RTEMS_SUCCESSFUL) ||
663 663 (status[4] != RTEMS_SUCCESSFUL) || (status[5] != RTEMS_SUCCESSFUL) ||
664 664 (status[6] != RTEMS_SUCCESSFUL) || (status[7] != RTEMS_SUCCESSFUL) ||
665 665 (status[8] != RTEMS_SUCCESSFUL) || (status[9] != RTEMS_SUCCESSFUL) )
666 666 {
667 667 ret = RTEMS_UNSATISFIED;
668 668 }
669 669
670 670 return ret;
671 671 }
672 672
673 673 int suspend_science_tasks()
674 674 {
675 675 /** This function suspends the science tasks.
676 676 *
677 677 * @return RTEMS directive status codes:
678 678 * - RTEMS_SUCCESSFUL - task restarted successfully
679 679 * - RTEMS_INVALID_ID - task id invalid
680 680 * - RTEMS_ALREADY_SUSPENDED - task already suspended
681 681 *
682 682 */
683 683
684 684 rtems_status_code status;
685 685
686 printf("in suspend_science_tasks\n");
686 PRINTF("in suspend_science_tasks\n")
687 687
688 688 status = rtems_task_suspend( Task_id[TASKID_AVF0] ); // suspend AVF0
689 689 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
690 690 {
691 691 PRINTF1("in suspend_science_task *** AVF0 ERR %d\n", status)
692 692 }
693 693 else
694 694 {
695 695 status = RTEMS_SUCCESSFUL;
696 696 }
697 697 if (status == RTEMS_SUCCESSFUL) // suspend PRC0
698 698 {
699 699 status = rtems_task_suspend( Task_id[TASKID_PRC0] );
700 700 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
701 701 {
702 702 PRINTF1("in suspend_science_task *** PRC0 ERR %d\n", status)
703 703 }
704 704 else
705 705 {
706 706 status = RTEMS_SUCCESSFUL;
707 707 }
708 708 }
709 709 if (status == RTEMS_SUCCESSFUL) // suspend AVF1
710 710 {
711 711 status = rtems_task_suspend( Task_id[TASKID_AVF1] );
712 712 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
713 713 {
714 714 PRINTF1("in suspend_science_task *** AVF1 ERR %d\n", status)
715 715 }
716 716 else
717 717 {
718 718 status = RTEMS_SUCCESSFUL;
719 719 }
720 720 }
721 721 if (status == RTEMS_SUCCESSFUL) // suspend PRC1
722 722 {
723 723 status = rtems_task_suspend( Task_id[TASKID_PRC1] );
724 724 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
725 725 {
726 726 PRINTF1("in suspend_science_task *** PRC1 ERR %d\n", status)
727 727 }
728 728 else
729 729 {
730 730 status = RTEMS_SUCCESSFUL;
731 731 }
732 732 }
733 733 if (status == RTEMS_SUCCESSFUL) // suspend AVF2
734 734 {
735 735 status = rtems_task_suspend( Task_id[TASKID_AVF2] );
736 736 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
737 737 {
738 738 PRINTF1("in suspend_science_task *** AVF2 ERR %d\n", status)
739 739 }
740 740 else
741 741 {
742 742 status = RTEMS_SUCCESSFUL;
743 743 }
744 744 }
745 745 if (status == RTEMS_SUCCESSFUL) // suspend PRC2
746 746 {
747 747 status = rtems_task_suspend( Task_id[TASKID_PRC2] );
748 748 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
749 749 {
750 750 PRINTF1("in suspend_science_task *** PRC2 ERR %d\n", status)
751 751 }
752 752 else
753 753 {
754 754 status = RTEMS_SUCCESSFUL;
755 755 }
756 756 }
757 757 if (status == RTEMS_SUCCESSFUL) // suspend WFRM
758 758 {
759 759 status = rtems_task_suspend( Task_id[TASKID_WFRM] );
760 760 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
761 761 {
762 762 PRINTF1("in suspend_science_task *** WFRM ERR %d\n", status)
763 763 }
764 764 else
765 765 {
766 766 status = RTEMS_SUCCESSFUL;
767 767 }
768 768 }
769 769 if (status == RTEMS_SUCCESSFUL) // suspend CWF3
770 770 {
771 771 status = rtems_task_suspend( Task_id[TASKID_CWF3] );
772 772 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
773 773 {
774 774 PRINTF1("in suspend_science_task *** CWF3 ERR %d\n", status)
775 775 }
776 776 else
777 777 {
778 778 status = RTEMS_SUCCESSFUL;
779 779 }
780 780 }
781 781 if (status == RTEMS_SUCCESSFUL) // suspend CWF2
782 782 {
783 783 status = rtems_task_suspend( Task_id[TASKID_CWF2] );
784 784 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
785 785 {
786 786 PRINTF1("in suspend_science_task *** CWF2 ERR %d\n", status)
787 787 }
788 788 else
789 789 {
790 790 status = RTEMS_SUCCESSFUL;
791 791 }
792 792 }
793 793 if (status == RTEMS_SUCCESSFUL) // suspend CWF1
794 794 {
795 795 status = rtems_task_suspend( Task_id[TASKID_CWF1] );
796 796 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
797 797 {
798 798 PRINTF1("in suspend_science_task *** CWF1 ERR %d\n", status)
799 799 }
800 800 else
801 801 {
802 802 status = RTEMS_SUCCESSFUL;
803 803 }
804 804 }
805 805
806 806 return status;
807 807 }
808 808
809 809 void launch_waveform_picker( unsigned char mode, unsigned int transitionCoarseTime )
810 810 {
811 811 WFP_reset_current_ring_nodes();
812 812
813 813 reset_waveform_picker_regs();
814 814
815 815 set_wfp_burst_enable_register( mode );
816 816
817 817 LEON_Clear_interrupt( IRQ_WAVEFORM_PICKER );
818 818 LEON_Unmask_interrupt( IRQ_WAVEFORM_PICKER );
819 819
820 820 if (transitionCoarseTime == 0)
821 821 {
822 822 waveform_picker_regs->start_date = time_management_regs->coarse_time;
823 823 }
824 824 else
825 825 {
826 826 waveform_picker_regs->start_date = transitionCoarseTime;
827 827 }
828 828
829 829 }
830 830
831 831 void launch_spectral_matrix( void )
832 832 {
833 833 SM_reset_current_ring_nodes();
834 834
835 835 reset_spectral_matrix_regs();
836 836
837 837 reset_nb_sm();
838 838
839 839 set_sm_irq_onNewMatrix( 1 );
840 840
841 841 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX );
842 842 LEON_Unmask_interrupt( IRQ_SPECTRAL_MATRIX );
843 843
844 844 }
845 845
846 846 void launch_spectral_matrix_simu( void )
847 847 {
848 848 SM_reset_current_ring_nodes();
849 849 reset_spectral_matrix_regs();
850 850 reset_nb_sm();
851 851
852 852 // Spectral Matrices simulator
853 853 timer_start( (gptimer_regs_t*) REGS_ADDR_GPTIMER, TIMER_SM_SIMULATOR );
854 854 LEON_Clear_interrupt( IRQ_SM_SIMULATOR );
855 855 LEON_Unmask_interrupt( IRQ_SM_SIMULATOR );
856 856 }
857 857
858 858 void set_sm_irq_onNewMatrix( unsigned char value )
859 859 {
860 860 if (value == 1)
861 861 {
862 862 spectral_matrix_regs->config = spectral_matrix_regs->config | 0x01;
863 863 }
864 864 else
865 865 {
866 866 spectral_matrix_regs->config = spectral_matrix_regs->config & 0xfffffffe; // 1110
867 867 }
868 868 }
869 869
870 870 void set_sm_irq_onError( unsigned char value )
871 871 {
872 872 if (value == 1)
873 873 {
874 874 spectral_matrix_regs->config = spectral_matrix_regs->config | 0x02;
875 875 }
876 876 else
877 877 {
878 878 spectral_matrix_regs->config = spectral_matrix_regs->config & 0xfffffffd; // 1101
879 879 }
880 880 }
881 881
882 882 //*****************************
883 883 // CONFIGURE CALIBRATION SIGNAL
884 884 void setCalibrationPrescaler( unsigned int prescaler )
885 885 {
886 886 // prescaling of the master clock (25 MHz)
887 887 // master clock is divided by 2^prescaler
888 888 time_management_regs->calPrescaler = prescaler;
889 889 }
890 890
891 891 void setCalibrationDivisor( unsigned int divisionFactor )
892 892 {
893 893 // division of the prescaled clock by the division factor
894 894 time_management_regs->calDivisor = divisionFactor;
895 895 }
896 896
897 897 void setCalibrationData( void ){
898 898 unsigned int k;
899 899 unsigned short data;
900 900 float val;
901 901 float f0;
902 902 float f1;
903 903 float fs;
904 904 float Ts;
905 905 float scaleFactor;
906 906
907 907 f0 = 625;
908 908 f1 = 10000;
909 909 fs = 160256.410;
910 910 Ts = 1. / fs;
911 911 scaleFactor = 0.250 / 0.000654; // 191, 500 mVpp, 2 sinus waves => 500 mVpp each, amplitude = 250 mV
912 912
913 913 time_management_regs->calDataPtr = 0x00;
914 914
915 915 // build the signal for the SCM calibration
916 916 for (k=0; k<256; k++)
917 917 {
918 918 val = sin( 2 * pi * f0 * k * Ts )
919 919 + sin( 2 * pi * f1 * k * Ts );
920 920 data = (unsigned short) ((val * scaleFactor) + 2048);
921 921 time_management_regs->calData = data & 0xfff;
922 922 }
923 923 }
924 924
925 925 void setCalibrationDataInterleaved( void ){
926 926 unsigned int k;
927 927 float val;
928 928 float f0;
929 929 float f1;
930 930 float fs;
931 931 float Ts;
932 932 unsigned short data[384];
933 933 unsigned char *dataPtr;
934 934
935 935 f0 = 625;
936 936 f1 = 10000;
937 937 fs = 240384.615;
938 938 Ts = 1. / fs;
939 939
940 940 time_management_regs->calDataPtr = 0x00;
941 941
942 942 // build the signal for the SCM calibration
943 943 for (k=0; k<384; k++)
944 944 {
945 945 val = sin( 2 * pi * f0 * k * Ts )
946 946 + sin( 2 * pi * f1 * k * Ts );
947 947 data[k] = (unsigned short) (val * 512 + 2048);
948 948 }
949 949
950 950 // write the signal in interleaved mode
951 951 for (k=0; k<128; k++)
952 952 {
953 953 dataPtr = (unsigned char*) &data[k*3 + 2];
954 954 time_management_regs->calData = (data[k*3] & 0xfff)
955 955 + ( (dataPtr[0] & 0x3f) << 12);
956 956 time_management_regs->calData = (data[k*3 + 1] & 0xfff)
957 957 + ( (dataPtr[1] & 0x3f) << 12);
958 958 }
959 959 }
960 960
961 961 void setCalibrationReload( bool state)
962 962 {
963 963 if (state == true)
964 964 {
965 965 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | 0x00000010; // [0001 0000]
966 966 }
967 967 else
968 968 {
969 969 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & 0xffffffef; // [1110 1111]
970 970 }
971 971 }
972 972
973 973 void setCalibrationEnable( bool state )
974 974 {
975 975 // this bit drives the multiplexer
976 976 if (state == true)
977 977 {
978 978 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | 0x00000040; // [0100 0000]
979 979 }
980 980 else
981 981 {
982 982 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & 0xffffffbf; // [1011 1111]
983 983 }
984 984 }
985 985
986 986 void setCalibrationInterleaved( bool state )
987 987 {
988 988 // this bit drives the multiplexer
989 989 if (state == true)
990 990 {
991 991 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | 0x00000020; // [0010 0000]
992 992 }
993 993 else
994 994 {
995 995 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & 0xffffffdf; // [1101 1111]
996 996 }
997 997 }
998 998
999 999 void setCalibration( bool state )
1000 1000 {
1001 1001 if (state == true)
1002 1002 {
1003 1003 setCalibrationEnable( true );
1004 1004 setCalibrationReload( false );
1005 1005 set_hk_lfr_calib_enable( true );
1006 1006 }
1007 1007 else
1008 1008 {
1009 1009 setCalibrationEnable( false );
1010 1010 setCalibrationReload( true );
1011 1011 set_hk_lfr_calib_enable( false );
1012 1012 }
1013 1013 }
1014 1014
1015 1015 void configureCalibration( bool interleaved )
1016 1016 {
1017 1017 setCalibration( false );
1018 1018 if ( interleaved == true )
1019 1019 {
1020 1020 setCalibrationInterleaved( true );
1021 1021 setCalibrationPrescaler( 0 ); // 25 MHz => 25 000 000
1022 1022 setCalibrationDivisor( 26 ); // => 240 384
1023 1023 setCalibrationDataInterleaved();
1024 1024 }
1025 1025 else
1026 1026 {
1027 1027 setCalibrationPrescaler( 0 ); // 25 MHz => 25 000 000
1028 1028 setCalibrationDivisor( 38 ); // => 160 256 (39 - 1)
1029 1029 setCalibrationData();
1030 1030 }
1031 1031 }
1032 1032
1033 1033 //****************
1034 1034 // CLOSING ACTIONS
1035 1035 void update_last_TC_exe( ccsdsTelecommandPacket_t *TC, unsigned char * time )
1036 1036 {
1037 1037 /** This function is used to update the HK packets statistics after a successful TC execution.
1038 1038 *
1039 1039 * @param TC points to the TC being processed
1040 1040 * @param time is the time used to date the TC execution
1041 1041 *
1042 1042 */
1043 1043
1044 1044 unsigned int val;
1045 1045
1046 1046 housekeeping_packet.hk_lfr_last_exe_tc_id[0] = TC->packetID[0];
1047 1047 housekeeping_packet.hk_lfr_last_exe_tc_id[1] = TC->packetID[1];
1048 1048 housekeeping_packet.hk_lfr_last_exe_tc_type[0] = 0x00;
1049 1049 housekeeping_packet.hk_lfr_last_exe_tc_type[1] = TC->serviceType;
1050 1050 housekeeping_packet.hk_lfr_last_exe_tc_subtype[0] = 0x00;
1051 1051 housekeeping_packet.hk_lfr_last_exe_tc_subtype[1] = TC->serviceSubType;
1052 1052 housekeeping_packet.hk_lfr_last_exe_tc_time[0] = time[0];
1053 1053 housekeeping_packet.hk_lfr_last_exe_tc_time[1] = time[1];
1054 1054 housekeeping_packet.hk_lfr_last_exe_tc_time[2] = time[2];
1055 1055 housekeeping_packet.hk_lfr_last_exe_tc_time[3] = time[3];
1056 1056 housekeeping_packet.hk_lfr_last_exe_tc_time[4] = time[4];
1057 1057 housekeeping_packet.hk_lfr_last_exe_tc_time[5] = time[5];
1058 1058
1059 1059 val = housekeeping_packet.hk_lfr_exe_tc_cnt[0] * 256 + housekeeping_packet.hk_lfr_exe_tc_cnt[1];
1060 1060 val++;
1061 1061 housekeeping_packet.hk_lfr_exe_tc_cnt[0] = (unsigned char) (val >> 8);
1062 1062 housekeeping_packet.hk_lfr_exe_tc_cnt[1] = (unsigned char) (val);
1063 1063 }
1064 1064
1065 1065 void update_last_TC_rej(ccsdsTelecommandPacket_t *TC, unsigned char * time )
1066 1066 {
1067 1067 /** This function is used to update the HK packets statistics after a TC rejection.
1068 1068 *
1069 1069 * @param TC points to the TC being processed
1070 1070 * @param time is the time used to date the TC rejection
1071 1071 *
1072 1072 */
1073 1073
1074 1074 unsigned int val;
1075 1075
1076 1076 housekeeping_packet.hk_lfr_last_rej_tc_id[0] = TC->packetID[0];
1077 1077 housekeeping_packet.hk_lfr_last_rej_tc_id[1] = TC->packetID[1];
1078 1078 housekeeping_packet.hk_lfr_last_rej_tc_type[0] = 0x00;
1079 1079 housekeeping_packet.hk_lfr_last_rej_tc_type[1] = TC->serviceType;
1080 1080 housekeeping_packet.hk_lfr_last_rej_tc_subtype[0] = 0x00;
1081 1081 housekeeping_packet.hk_lfr_last_rej_tc_subtype[1] = TC->serviceSubType;
1082 1082 housekeeping_packet.hk_lfr_last_rej_tc_time[0] = time[0];
1083 1083 housekeeping_packet.hk_lfr_last_rej_tc_time[1] = time[1];
1084 1084 housekeeping_packet.hk_lfr_last_rej_tc_time[2] = time[2];
1085 1085 housekeeping_packet.hk_lfr_last_rej_tc_time[3] = time[3];
1086 1086 housekeeping_packet.hk_lfr_last_rej_tc_time[4] = time[4];
1087 1087 housekeeping_packet.hk_lfr_last_rej_tc_time[5] = time[5];
1088 1088
1089 1089 val = housekeeping_packet.hk_lfr_rej_tc_cnt[0] * 256 + housekeeping_packet.hk_lfr_rej_tc_cnt[1];
1090 1090 val++;
1091 1091 housekeeping_packet.hk_lfr_rej_tc_cnt[0] = (unsigned char) (val >> 8);
1092 1092 housekeeping_packet.hk_lfr_rej_tc_cnt[1] = (unsigned char) (val);
1093 1093 }
1094 1094
1095 1095 void close_action(ccsdsTelecommandPacket_t *TC, int result, rtems_id queue_id )
1096 1096 {
1097 1097 /** This function is the last step of the TC execution workflow.
1098 1098 *
1099 1099 * @param TC points to the TC being processed
1100 1100 * @param result is the result of the TC execution (LFR_SUCCESSFUL / LFR_DEFAULT)
1101 1101 * @param queue_id is the id of the RTEMS message queue used to send TM packets
1102 1102 * @param time is the time used to date the TC execution
1103 1103 *
1104 1104 */
1105 1105
1106 1106 unsigned char requestedMode;
1107 1107
1108 1108 if (result == LFR_SUCCESSFUL)
1109 1109 {
1110 1110 if ( !( (TC->serviceType==TC_TYPE_TIME) & (TC->serviceSubType==TC_SUBTYPE_UPDT_TIME) )
1111 1111 &
1112 1112 !( (TC->serviceType==TC_TYPE_GEN) & (TC->serviceSubType==TC_SUBTYPE_UPDT_INFO))
1113 1113 )
1114 1114 {
1115 1115 send_tm_lfr_tc_exe_success( TC, queue_id );
1116 1116 }
1117 1117 if ( (TC->serviceType == TC_TYPE_GEN) & (TC->serviceSubType == TC_SUBTYPE_ENTER) )
1118 1118 {
1119 1119 //**********************************
1120 1120 // UPDATE THE LFRMODE LOCAL VARIABLE
1121 1121 requestedMode = TC->dataAndCRC[1];
1122 1122 housekeeping_packet.lfr_status_word[0] = (unsigned char) ((requestedMode << 4) + 0x0d);
1123 1123 updateLFRCurrentMode();
1124 1124 }
1125 1125 }
1126 1126 else if (result == LFR_EXE_ERROR)
1127 1127 {
1128 1128 send_tm_lfr_tc_exe_error( TC, queue_id );
1129 1129 }
1130 1130 }
1131 1131
1132 1132 //***************************
1133 1133 // Interrupt Service Routines
1134 1134 rtems_isr commutation_isr1( rtems_vector_number vector )
1135 1135 {
1136 1136 if (rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
1137 printf("In commutation_isr1 *** Error sending event to DUMB\n");
1137 PRINTF("In commutation_isr1 *** Error sending event to DUMB\n")
1138 1138 }
1139 1139 }
1140 1140
1141 1141 rtems_isr commutation_isr2( rtems_vector_number vector )
1142 1142 {
1143 1143 if (rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
1144 printf("In commutation_isr2 *** Error sending event to DUMB\n");
1144 PRINTF("In commutation_isr2 *** Error sending event to DUMB\n")
1145 1145 }
1146 1146 }
1147 1147
1148 1148 //****************
1149 1149 // OTHER FUNCTIONS
1150 1150 void updateLFRCurrentMode()
1151 1151 {
1152 1152 /** This function updates the value of the global variable lfrCurrentMode.
1153 1153 *
1154 1154 * lfrCurrentMode is a parameter used by several functions to know in which mode LFR is running.
1155 1155 *
1156 1156 */
1157 1157 // update the local value of lfrCurrentMode with the value contained in the housekeeping_packet structure
1158 1158 lfrCurrentMode = (housekeeping_packet.lfr_status_word[0] & 0xf0) >> 4;
1159 1159 }
1160 1160
1161 1161 void set_lfr_soft_reset( unsigned char value )
1162 1162 {
1163 1163 if (value == 1)
1164 1164 {
1165 1165 time_management_regs->ctrl = time_management_regs->ctrl | 0x00000004; // [0100]
1166 1166 }
1167 1167 else
1168 1168 {
1169 1169 time_management_regs->ctrl = time_management_regs->ctrl & 0xfffffffb; // [1011]
1170 1170 }
1171 1171 }
1172 1172
1173 1173 void reset_lfr( void )
1174 1174 {
1175 1175 set_lfr_soft_reset( 1 );
1176 1176
1177 1177 set_lfr_soft_reset( 0 );
1178 1178
1179 1179 set_hk_lfr_sc_potential_flag( true );
1180 1180 }
Show file before | Show file after | Hide comments
@@ -1,1175 +1,1201
1 1 /** Functions to load and dump parameters in the LFR registers.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle TC related to parameter loading and dumping.\n
7 7 * TC_LFR_LOAD_COMMON_PAR\n
8 8 * TC_LFR_LOAD_NORMAL_PAR\n
9 9 * TC_LFR_LOAD_BURST_PAR\n
10 10 * TC_LFR_LOAD_SBM1_PAR\n
11 11 * TC_LFR_LOAD_SBM2_PAR\n
12 12 *
13 13 */
14 14
15 15 #include "tc_load_dump_parameters.h"
16 16
17 17 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t kcoefficients_dump_1;
18 18 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t kcoefficients_dump_2;
19 19 ring_node kcoefficient_node_1;
20 20 ring_node kcoefficient_node_2;
21 21
22 22 int action_load_common_par(ccsdsTelecommandPacket_t *TC)
23 23 {
24 24 /** This function updates the LFR registers with the incoming common parameters.
25 25 *
26 26 * @param TC points to the TeleCommand packet that is being processed
27 27 *
28 28 *
29 29 */
30 30
31 31 parameter_dump_packet.sy_lfr_common_parameters_spare = TC->dataAndCRC[0];
32 32 parameter_dump_packet.sy_lfr_common_parameters = TC->dataAndCRC[1];
33 33 set_wfp_data_shaping( );
34 34 return LFR_SUCCESSFUL;
35 35 }
36 36
37 37 int action_load_normal_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
38 38 {
39 39 /** This function updates the LFR registers with the incoming normal parameters.
40 40 *
41 41 * @param TC points to the TeleCommand packet that is being processed
42 42 * @param queue_id is the id of the queue which handles TM related to this execution step
43 43 *
44 44 */
45 45
46 46 int result;
47 47 int flag;
48 48 rtems_status_code status;
49 49
50 50 flag = LFR_SUCCESSFUL;
51 51
52 52 if ( (lfrCurrentMode == LFR_MODE_NORMAL) ||
53 53 (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) ) {
54 54 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
55 55 flag = LFR_DEFAULT;
56 56 }
57 57
58 58 // CHECK THE PARAMETERS SET CONSISTENCY
59 59 if (flag == LFR_SUCCESSFUL)
60 60 {
61 61 flag = check_common_par_consistency( TC, queue_id );
62 62 }
63 63
64 64 // SET THE PARAMETERS IF THEY ARE CONSISTENT
65 65 if (flag == LFR_SUCCESSFUL)
66 66 {
67 67 result = set_sy_lfr_n_swf_l( TC );
68 68 result = set_sy_lfr_n_swf_p( TC );
69 69 result = set_sy_lfr_n_bp_p0( TC );
70 70 result = set_sy_lfr_n_bp_p1( TC );
71 71 result = set_sy_lfr_n_asm_p( TC );
72 72 result = set_sy_lfr_n_cwf_long_f3( TC );
73 73 }
74 74
75 75 return flag;
76 76 }
77 77
78 78 int action_load_burst_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
79 79 {
80 80 /** This function updates the LFR registers with the incoming burst parameters.
81 81 *
82 82 * @param TC points to the TeleCommand packet that is being processed
83 83 * @param queue_id is the id of the queue which handles TM related to this execution step
84 84 *
85 85 */
86 86
87 87 int flag;
88 88 rtems_status_code status;
89 89 unsigned char sy_lfr_b_bp_p0;
90 90 unsigned char sy_lfr_b_bp_p1;
91 91 float aux;
92 92
93 93 flag = LFR_SUCCESSFUL;
94 94
95 95 if ( lfrCurrentMode == LFR_MODE_BURST ) {
96 96 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
97 97 flag = LFR_DEFAULT;
98 98 }
99 99
100 100 sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
101 101 sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
102 102
103 103 // sy_lfr_b_bp_p0 shall not be lower than its default value
104 104 if (flag == LFR_SUCCESSFUL)
105 105 {
106 106 if (sy_lfr_b_bp_p0 < DEFAULT_SY_LFR_B_BP_P0 )
107 107 {
108 108 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P0+10, sy_lfr_b_bp_p0 );
109 109 flag = WRONG_APP_DATA;
110 110 }
111 111 }
112 112 // sy_lfr_b_bp_p1 shall not be lower than its default value
113 113 if (flag == LFR_SUCCESSFUL)
114 114 {
115 115 if (sy_lfr_b_bp_p1 < DEFAULT_SY_LFR_B_BP_P1 )
116 116 {
117 117 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P1+10, sy_lfr_b_bp_p1 );
118 118 flag = WRONG_APP_DATA;
119 119 }
120 120 }
121 121 //****************************************************************
122 122 // check the consistency between sy_lfr_b_bp_p0 and sy_lfr_b_bp_p1
123 123 if (flag == LFR_SUCCESSFUL)
124 124 {
125 125 sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
126 126 sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
127 127 aux = ( (float ) sy_lfr_b_bp_p1 / sy_lfr_b_bp_p0 ) - floor(sy_lfr_b_bp_p1 / sy_lfr_b_bp_p0);
128 128 if (aux > FLOAT_EQUAL_ZERO)
129 129 {
130 130 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P0+10, sy_lfr_b_bp_p0 );
131 131 flag = LFR_DEFAULT;
132 132 }
133 133 }
134 134
135 135 // SET THE PARAMETERS
136 136 if (flag == LFR_SUCCESSFUL)
137 137 {
138 138 flag = set_sy_lfr_b_bp_p0( TC );
139 139 flag = set_sy_lfr_b_bp_p1( TC );
140 140 }
141 141
142 142 return flag;
143 143 }
144 144
145 145 int action_load_sbm1_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
146 146 {
147 147 /** This function updates the LFR registers with the incoming sbm1 parameters.
148 148 *
149 149 * @param TC points to the TeleCommand packet that is being processed
150 150 * @param queue_id is the id of the queue which handles TM related to this execution step
151 151 *
152 152 */
153 153
154 154 int flag;
155 155 rtems_status_code status;
156 156 unsigned char sy_lfr_s1_bp_p0;
157 157 unsigned char sy_lfr_s1_bp_p1;
158 158 float aux;
159 159
160 160 flag = LFR_SUCCESSFUL;
161 161
162 162 if ( lfrCurrentMode == LFR_MODE_SBM1 ) {
163 163 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
164 164 flag = LFR_DEFAULT;
165 165 }
166 166
167 167 sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
168 168 sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
169 169
170 170 // sy_lfr_s1_bp_p0
171 171 if (flag == LFR_SUCCESSFUL)
172 172 {
173 173 if (sy_lfr_s1_bp_p0 < DEFAULT_SY_LFR_S1_BP_P0 )
174 174 {
175 175 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P0+10, sy_lfr_s1_bp_p0 );
176 176 flag = WRONG_APP_DATA;
177 177 }
178 178 }
179 179 // sy_lfr_s1_bp_p1
180 180 if (flag == LFR_SUCCESSFUL)
181 181 {
182 182 if (sy_lfr_s1_bp_p1 < DEFAULT_SY_LFR_S1_BP_P1 )
183 183 {
184 184 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P1+10, sy_lfr_s1_bp_p1 );
185 185 flag = WRONG_APP_DATA;
186 186 }
187 187 }
188 188 //******************************************************************
189 189 // check the consistency between sy_lfr_s1_bp_p0 and sy_lfr_s1_bp_p1
190 190 if (flag == LFR_SUCCESSFUL)
191 191 {
192 192 aux = ( (float ) sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0*0.25) ) - floor(sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0*0.25));
193 193 if (aux > FLOAT_EQUAL_ZERO)
194 194 {
195 195 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P0+10, sy_lfr_s1_bp_p0 );
196 196 flag = LFR_DEFAULT;
197 197 }
198 198 }
199 199
200 200 // SET THE PARAMETERS
201 201 if (flag == LFR_SUCCESSFUL)
202 202 {
203 203 flag = set_sy_lfr_s1_bp_p0( TC );
204 204 flag = set_sy_lfr_s1_bp_p1( TC );
205 205 }
206 206
207 207 return flag;
208 208 }
209 209
210 210 int action_load_sbm2_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
211 211 {
212 212 /** This function updates the LFR registers with the incoming sbm2 parameters.
213 213 *
214 214 * @param TC points to the TeleCommand packet that is being processed
215 215 * @param queue_id is the id of the queue which handles TM related to this execution step
216 216 *
217 217 */
218 218
219 219 int flag;
220 220 rtems_status_code status;
221 221 unsigned char sy_lfr_s2_bp_p0;
222 222 unsigned char sy_lfr_s2_bp_p1;
223 223 float aux;
224 224
225 225 flag = LFR_SUCCESSFUL;
226 226
227 227 if ( lfrCurrentMode == LFR_MODE_SBM2 ) {
228 228 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
229 229 flag = LFR_DEFAULT;
230 230 }
231 231
232 232 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
233 233 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
234 234
235 235 // sy_lfr_s2_bp_p0
236 236 if (flag == LFR_SUCCESSFUL)
237 237 {
238 238 if (sy_lfr_s2_bp_p0 < DEFAULT_SY_LFR_S2_BP_P0 )
239 239 {
240 240 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0+10, sy_lfr_s2_bp_p0 );
241 241 flag = WRONG_APP_DATA;
242 242 }
243 243 }
244 244 // sy_lfr_s2_bp_p1
245 245 if (flag == LFR_SUCCESSFUL)
246 246 {
247 247 if (sy_lfr_s2_bp_p1 < DEFAULT_SY_LFR_S2_BP_P1 )
248 248 {
249 249 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P1+10, sy_lfr_s2_bp_p1 );
250 250 flag = WRONG_APP_DATA;
251 251 }
252 252 }
253 253 //******************************************************************
254 254 // check the consistency between sy_lfr_s2_bp_p0 and sy_lfr_s2_bp_p1
255 255 if (flag == LFR_SUCCESSFUL)
256 256 {
257 257 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
258 258 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
259 259 aux = ( (float ) sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0 ) - floor(sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0);
260 260 if (aux > FLOAT_EQUAL_ZERO)
261 261 {
262 262 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0+10, sy_lfr_s2_bp_p0 );
263 263 flag = LFR_DEFAULT;
264 264 }
265 265 }
266 266
267 267 // SET THE PARAMETERS
268 268 if (flag == LFR_SUCCESSFUL)
269 269 {
270 270 flag = set_sy_lfr_s2_bp_p0( TC );
271 271 flag = set_sy_lfr_s2_bp_p1( TC );
272 272 }
273 273
274 274 return flag;
275 275 }
276 276
277 277 int action_load_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
278 278 {
279 279 /** This function updates the LFR registers with the incoming sbm2 parameters.
280 280 *
281 281 * @param TC points to the TeleCommand packet that is being processed
282 282 * @param queue_id is the id of the queue which handles TM related to this execution step
283 283 *
284 284 */
285 285
286 286 int flag;
287 287
288 288 flag = LFR_DEFAULT;
289 289
290 290 flag = set_sy_lfr_kcoeff( TC, queue_id );
291 291
292 292 return flag;
293 293 }
294 294
295 295 int action_load_fbins_mask(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
296 296 {
297 297 /** This function updates the LFR registers with the incoming sbm2 parameters.
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 related to this execution step
301 301 *
302 302 */
303 303
304 304 int flag;
305 305
306 306 flag = LFR_DEFAULT;
307 307
308 308 flag = set_sy_lfr_fbins( TC );
309 309
310 310 return flag;
311 311 }
312 312
313 void printKCoefficients(unsigned int freq, unsigned int bin, float *k_coeff)
314 {
315 printf("freq = %d *** bin = %d *** (0) %f *** (1) %f *** (2) %f *** (3) %f *** (4) %f\n",
316 freq,
317 bin,
318 k_coeff[ (bin*NB_K_COEFF_PER_BIN) + 0 ],
319 k_coeff[ (bin*NB_K_COEFF_PER_BIN) + 1 ],
320 k_coeff[ (bin*NB_K_COEFF_PER_BIN) + 2 ],
321 k_coeff[ (bin*NB_K_COEFF_PER_BIN) + 3 ],
322 k_coeff[ (bin*NB_K_COEFF_PER_BIN) + 4 ]);
323 }
324
325 313 int action_dump_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
326 314 {
327 315 /** This function updates the LFR registers with the incoming sbm2 parameters.
328 316 *
329 317 * @param TC points to the TeleCommand packet that is being processed
330 318 * @param queue_id is the id of the queue which handles TM related to this execution step
331 319 *
332 320 */
333 321
334 322 unsigned int address;
335 323 rtems_status_code status;
336 324 unsigned int freq;
337 325 unsigned int bin;
338 326 unsigned int coeff;
339 327 unsigned char *kCoeffPtr;
340 328 unsigned char *kCoeffDumpPtr;
341 329
342 330 // for each sy_lfr_kcoeff_frequency there is 32 kcoeff
343 331 // F0 => 11 bins
344 332 // F1 => 13 bins
345 333 // F2 => 12 bins
346 334 // 36 bins to dump in two packets (30 bins max per packet)
347 335
348 336 //*********
349 337 // PACKET 1
350 338 // 11 F0 bins, 13 F1 bins and 6 F2 bins
351 kcoefficients_dump_1.packetSequenceControl[0] = (unsigned char) (sequenceCounterParameterDump >> 8);
352 kcoefficients_dump_1.packetSequenceControl[1] = (unsigned char) (sequenceCounterParameterDump );
353 339 kcoefficients_dump_1.destinationID = TC->sourceID;
354 increment_seq_counter( &sequenceCounterParameterDump );
340 increment_seq_counter_destination_id_dump( kcoefficients_dump_1.packetSequenceControl, TC->sourceID );
355 341 for( freq=0;
356 342 freq<NB_BINS_COMPRESSED_SM_F0;
357 343 freq++ )
358 344 {
359 345 kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1] = freq;
360 346 bin = freq;
361 347 // printKCoefficients( freq, bin, k_coeff_intercalib_f0_norm);
362 348 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
363 349 {
364 350 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
365 351 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f0_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
366 352 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
367 353 }
368 354 }
369 355 for( freq=NB_BINS_COMPRESSED_SM_F0;
370 356 freq<(NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1);
371 357 freq++ )
372 358 {
373 359 kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1 ] = freq;
374 360 bin = freq - NB_BINS_COMPRESSED_SM_F0;
375 361 // printKCoefficients( freq, bin, k_coeff_intercalib_f1_norm);
376 362 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
377 363 {
378 364 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
379 365 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f1_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
380 366 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
381 367 }
382 368 }
383 369 for( freq=(NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1);
384 370 freq<(NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1+6);
385 371 freq++ )
386 372 {
387 373 kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1 ] = freq;
388 374 bin = freq - (NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1);
389 375 // printKCoefficients( freq, bin, k_coeff_intercalib_f2);
390 376 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
391 377 {
392 378 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
393 379 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
394 380 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
395 381 }
396 382 }
397 383 kcoefficients_dump_1.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
398 384 kcoefficients_dump_1.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
399 385 kcoefficients_dump_1.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
400 386 kcoefficients_dump_1.time[3] = (unsigned char) (time_management_regs->coarse_time);
401 387 kcoefficients_dump_1.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
402 388 kcoefficients_dump_1.time[5] = (unsigned char) (time_management_regs->fine_time);
403 389 // SEND DATA
404 390 kcoefficient_node_1.status = 1;
405 391 address = (unsigned int) &kcoefficient_node_1;
406 392 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
407 393 if (status != RTEMS_SUCCESSFUL) {
408 394 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 1 , code %d", status)
409 395 }
410 396
411 397 //********
412 398 // PACKET 2
413 399 // 6 F2 bins
414 kcoefficients_dump_2.packetSequenceControl[0] = (unsigned char) (sequenceCounterParameterDump >> 8);
415 kcoefficients_dump_2.packetSequenceControl[1] = (unsigned char) (sequenceCounterParameterDump );
416 400 kcoefficients_dump_2.destinationID = TC->sourceID;
417 increment_seq_counter( &sequenceCounterParameterDump );
401 increment_seq_counter_destination_id_dump( kcoefficients_dump_2.packetSequenceControl, TC->sourceID );
418 402 for( freq=0; freq<6; freq++ )
419 403 {
420 404 kcoefficients_dump_2.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1 ] = NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 + 6 + freq;
421 405 bin = freq + 6;
422 406 // printKCoefficients( freq, bin, k_coeff_intercalib_f2);
423 407 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
424 408 {
425 409 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_2.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
426 410 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
427 411 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
428 412 }
429 413 }
430 414 kcoefficients_dump_2.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
431 415 kcoefficients_dump_2.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
432 416 kcoefficients_dump_2.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
433 417 kcoefficients_dump_2.time[3] = (unsigned char) (time_management_regs->coarse_time);
434 418 kcoefficients_dump_2.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
435 419 kcoefficients_dump_2.time[5] = (unsigned char) (time_management_regs->fine_time);
436 420 // SEND DATA
437 421 kcoefficient_node_2.status = 1;
438 422 address = (unsigned int) &kcoefficient_node_2;
439 423 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
440 424 if (status != RTEMS_SUCCESSFUL) {
441 425 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 2, code %d", status)
442 426 }
443 427
444 428 return status;
445 429 }
446 430
447 431 int action_dump_par( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
448 432 {
449 433 /** This function dumps the LFR parameters by sending the appropriate TM packet to the dedicated RTEMS message queue.
450 434 *
451 435 * @param queue_id is the id of the queue which handles TM related to this execution step.
452 436 *
453 437 * @return RTEMS directive status codes:
454 438 * - RTEMS_SUCCESSFUL - message sent successfully
455 439 * - RTEMS_INVALID_ID - invalid queue id
456 440 * - RTEMS_INVALID_SIZE - invalid message size
457 441 * - RTEMS_INVALID_ADDRESS - buffer is NULL
458 442 * - RTEMS_UNSATISFIED - out of message buffers
459 443 * - RTEMS_TOO_MANY - queue s limit has been reached
460 444 *
461 445 */
462 446
463 447 int status;
464 448
465 // UPDATE TIME
466 parameter_dump_packet.packetSequenceControl[0] = (unsigned char) (sequenceCounterParameterDump >> 8);
467 parameter_dump_packet.packetSequenceControl[1] = (unsigned char) (sequenceCounterParameterDump );
468 increment_seq_counter( &sequenceCounterParameterDump );
449 increment_seq_counter_destination_id_dump( parameter_dump_packet.packetSequenceControl, TC->sourceID );
469 450 parameter_dump_packet.destinationID = TC->sourceID;
470 451
452 // UPDATE TIME
471 453 parameter_dump_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
472 454 parameter_dump_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
473 455 parameter_dump_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
474 456 parameter_dump_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
475 457 parameter_dump_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
476 458 parameter_dump_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
477 459 // SEND DATA
478 460 status = rtems_message_queue_send( queue_id, &parameter_dump_packet,
479 461 PACKET_LENGTH_PARAMETER_DUMP + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
480 462 if (status != RTEMS_SUCCESSFUL) {
481 463 PRINTF1("in action_dump *** ERR sending packet, code %d", status)
482 464 }
483 465
484 466 return status;
485 467 }
486 468
487 469 //***********************
488 470 // NORMAL MODE PARAMETERS
489 471
490 472 int check_common_par_consistency( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
491 473 {
492 474 unsigned char msb;
493 475 unsigned char lsb;
494 476 int flag;
495 477 float aux;
496 478 rtems_status_code status;
497 479
498 480 unsigned int sy_lfr_n_swf_l;
499 481 unsigned int sy_lfr_n_swf_p;
500 482 unsigned int sy_lfr_n_asm_p;
501 483 unsigned char sy_lfr_n_bp_p0;
502 484 unsigned char sy_lfr_n_bp_p1;
503 485 unsigned char sy_lfr_n_cwf_long_f3;
504 486
505 487 flag = LFR_SUCCESSFUL;
506 488
507 489 //***************
508 490 // get parameters
509 491 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
510 492 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
511 493 sy_lfr_n_swf_l = msb * 256 + lsb;
512 494
513 495 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
514 496 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
515 497 sy_lfr_n_swf_p = msb * 256 + lsb;
516 498
517 499 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
518 500 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
519 501 sy_lfr_n_asm_p = msb * 256 + lsb;
520 502
521 503 sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
522 504
523 505 sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
524 506
525 507 sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
526 508
527 509 //******************
528 510 // check consistency
529 511 // sy_lfr_n_swf_l
530 512 if (sy_lfr_n_swf_l != 2048)
531 513 {
532 514 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_L+10, sy_lfr_n_swf_l );
533 515 flag = WRONG_APP_DATA;
534 516 }
535 517 // sy_lfr_n_swf_p
536 518 if (flag == LFR_SUCCESSFUL)
537 519 {
538 520 if ( sy_lfr_n_swf_p < 16 )
539 521 {
540 522 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_P+10, sy_lfr_n_swf_p );
541 523 flag = WRONG_APP_DATA;
542 524 }
543 525 }
544 526 // sy_lfr_n_bp_p0
545 527 if (flag == LFR_SUCCESSFUL)
546 528 {
547 529 if (sy_lfr_n_bp_p0 < DFLT_SY_LFR_N_BP_P0)
548 530 {
549 531 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P0+10, sy_lfr_n_bp_p0 );
550 532 flag = WRONG_APP_DATA;
551 533 }
552 534 }
553 535 // sy_lfr_n_asm_p
554 536 if (flag == LFR_SUCCESSFUL)
555 537 {
556 538 if (sy_lfr_n_asm_p == 0)
557 539 {
558 540 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P+10, sy_lfr_n_asm_p );
559 541 flag = WRONG_APP_DATA;
560 542 }
561 543 }
562 544 // sy_lfr_n_asm_p shall be a whole multiple of sy_lfr_n_bp_p0
563 545 if (flag == LFR_SUCCESSFUL)
564 546 {
565 547 aux = ( (float ) sy_lfr_n_asm_p / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_asm_p / sy_lfr_n_bp_p0);
566 548 if (aux > FLOAT_EQUAL_ZERO)
567 549 {
568 550 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P+10, sy_lfr_n_asm_p );
569 551 flag = WRONG_APP_DATA;
570 552 }
571 553 }
572 554 // sy_lfr_n_bp_p1
573 555 if (flag == LFR_SUCCESSFUL)
574 556 {
575 557 if (sy_lfr_n_bp_p1 < DFLT_SY_LFR_N_BP_P1)
576 558 {
577 559 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1+10, sy_lfr_n_bp_p1 );
578 560 flag = WRONG_APP_DATA;
579 561 }
580 562 }
581 563 // sy_lfr_n_bp_p1 shall be a whole multiple of sy_lfr_n_bp_p0
582 564 if (flag == LFR_SUCCESSFUL)
583 565 {
584 566 aux = ( (float ) sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0);
585 567 if (aux > FLOAT_EQUAL_ZERO)
586 568 {
587 569 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1+10, sy_lfr_n_bp_p1 );
588 570 flag = LFR_DEFAULT;
589 571 }
590 572 }
591 573 // sy_lfr_n_cwf_long_f3
592 574
593 575 return flag;
594 576 }
595 577
596 578 int set_sy_lfr_n_swf_l( ccsdsTelecommandPacket_t *TC )
597 579 {
598 580 /** This function sets the number of points of a snapshot (sy_lfr_n_swf_l).
599 581 *
600 582 * @param TC points to the TeleCommand packet that is being processed
601 583 * @param queue_id is the id of the queue which handles TM related to this execution step
602 584 *
603 585 */
604 586
605 587 int result;
606 588
607 589 result = LFR_SUCCESSFUL;
608 590
609 591 parameter_dump_packet.sy_lfr_n_swf_l[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
610 592 parameter_dump_packet.sy_lfr_n_swf_l[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
611 593
612 594 return result;
613 595 }
614 596
615 597 int set_sy_lfr_n_swf_p(ccsdsTelecommandPacket_t *TC )
616 598 {
617 599 /** This function sets the time between two snapshots, in s (sy_lfr_n_swf_p).
618 600 *
619 601 * @param TC points to the TeleCommand packet that is being processed
620 602 * @param queue_id is the id of the queue which handles TM related to this execution step
621 603 *
622 604 */
623 605
624 606 int result;
625 607
626 608 result = LFR_SUCCESSFUL;
627 609
628 610 parameter_dump_packet.sy_lfr_n_swf_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
629 611 parameter_dump_packet.sy_lfr_n_swf_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
630 612
631 613 return result;
632 614 }
633 615
634 616 int set_sy_lfr_n_asm_p( ccsdsTelecommandPacket_t *TC )
635 617 {
636 618 /** This function sets the time between two full spectral matrices transmission, in s (SY_LFR_N_ASM_P).
637 619 *
638 620 * @param TC points to the TeleCommand packet that is being processed
639 621 * @param queue_id is the id of the queue which handles TM related to this execution step
640 622 *
641 623 */
642 624
643 625 int result;
644 626
645 627 result = LFR_SUCCESSFUL;
646 628
647 629 parameter_dump_packet.sy_lfr_n_asm_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
648 630 parameter_dump_packet.sy_lfr_n_asm_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
649 631
650 632 return result;
651 633 }
652 634
653 635 int set_sy_lfr_n_bp_p0( ccsdsTelecommandPacket_t *TC )
654 636 {
655 637 /** This function sets the time between two basic parameter sets, in s (DFLT_SY_LFR_N_BP_P0).
656 638 *
657 639 * @param TC points to the TeleCommand packet that is being processed
658 640 * @param queue_id is the id of the queue which handles TM related to this execution step
659 641 *
660 642 */
661 643
662 644 int status;
663 645
664 646 status = LFR_SUCCESSFUL;
665 647
666 648 parameter_dump_packet.sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
667 649
668 650 return status;
669 651 }
670 652
671 653 int set_sy_lfr_n_bp_p1(ccsdsTelecommandPacket_t *TC )
672 654 {
673 655 /** This function sets the time between two basic parameter sets (autocorrelation + crosscorrelation), in s (sy_lfr_n_bp_p1).
674 656 *
675 657 * @param TC points to the TeleCommand packet that is being processed
676 658 * @param queue_id is the id of the queue which handles TM related to this execution step
677 659 *
678 660 */
679 661
680 662 int status;
681 663
682 664 status = LFR_SUCCESSFUL;
683 665
684 666 parameter_dump_packet.sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
685 667
686 668 return status;
687 669 }
688 670
689 671 int set_sy_lfr_n_cwf_long_f3(ccsdsTelecommandPacket_t *TC )
690 672 {
691 673 /** This function allows to switch from CWF_F3 packets to CWF_LONG_F3 packets.
692 674 *
693 675 * @param TC points to the TeleCommand packet that is being processed
694 676 * @param queue_id is the id of the queue which handles TM related to this execution step
695 677 *
696 678 */
697 679
698 680 int status;
699 681
700 682 status = LFR_SUCCESSFUL;
701 683
702 684 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
703 685
704 686 return status;
705 687 }
706 688
707 689 //**********************
708 690 // BURST MODE PARAMETERS
709 691 int set_sy_lfr_b_bp_p0(ccsdsTelecommandPacket_t *TC)
710 692 {
711 693 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P0).
712 694 *
713 695 * @param TC points to the TeleCommand packet that is being processed
714 696 * @param queue_id is the id of the queue which handles TM related to this execution step
715 697 *
716 698 */
717 699
718 700 int status;
719 701
720 702 status = LFR_SUCCESSFUL;
721 703
722 704 parameter_dump_packet.sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
723 705
724 706 return status;
725 707 }
726 708
727 709 int set_sy_lfr_b_bp_p1( ccsdsTelecommandPacket_t *TC )
728 710 {
729 711 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P1).
730 712 *
731 713 * @param TC points to the TeleCommand packet that is being processed
732 714 * @param queue_id is the id of the queue which handles TM related to this execution step
733 715 *
734 716 */
735 717
736 718 int status;
737 719
738 720 status = LFR_SUCCESSFUL;
739 721
740 722 parameter_dump_packet.sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
741 723
742 724 return status;
743 725 }
744 726
745 727 //*********************
746 728 // SBM1 MODE PARAMETERS
747 729 int set_sy_lfr_s1_bp_p0( ccsdsTelecommandPacket_t *TC )
748 730 {
749 731 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P0).
750 732 *
751 733 * @param TC points to the TeleCommand packet that is being processed
752 734 * @param queue_id is the id of the queue which handles TM related to this execution step
753 735 *
754 736 */
755 737
756 738 int status;
757 739
758 740 status = LFR_SUCCESSFUL;
759 741
760 742 parameter_dump_packet.sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
761 743
762 744 return status;
763 745 }
764 746
765 747 int set_sy_lfr_s1_bp_p1( ccsdsTelecommandPacket_t *TC )
766 748 {
767 749 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P1).
768 750 *
769 751 * @param TC points to the TeleCommand packet that is being processed
770 752 * @param queue_id is the id of the queue which handles TM related to this execution step
771 753 *
772 754 */
773 755
774 756 int status;
775 757
776 758 status = LFR_SUCCESSFUL;
777 759
778 760 parameter_dump_packet.sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
779 761
780 762 return status;
781 763 }
782 764
783 765 //*********************
784 766 // SBM2 MODE PARAMETERS
785 767 int set_sy_lfr_s2_bp_p0(ccsdsTelecommandPacket_t *TC)
786 768 {
787 769 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P0).
788 770 *
789 771 * @param TC points to the TeleCommand packet that is being processed
790 772 * @param queue_id is the id of the queue which handles TM related to this execution step
791 773 *
792 774 */
793 775
794 776 int status;
795 777
796 778 status = LFR_SUCCESSFUL;
797 779
798 780 parameter_dump_packet.sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
799 781
800 782 return status;
801 783 }
802 784
803 785 int set_sy_lfr_s2_bp_p1( ccsdsTelecommandPacket_t *TC )
804 786 {
805 787 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P1).
806 788 *
807 789 * @param TC points to the TeleCommand packet that is being processed
808 790 * @param queue_id is the id of the queue which handles TM related to this execution step
809 791 *
810 792 */
811 793
812 794 int status;
813 795
814 796 status = LFR_SUCCESSFUL;
815 797
816 798 parameter_dump_packet.sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
817 799
818 800 return status;
819 801 }
820 802
821 803 //*******************
822 804 // TC_LFR_UPDATE_INFO
823 805 unsigned int check_update_info_hk_lfr_mode( unsigned char mode )
824 806 {
825 807 unsigned int status;
826 808
827 809 if ( (mode == LFR_MODE_STANDBY) || (mode == LFR_MODE_NORMAL)
828 810 || (mode == LFR_MODE_BURST)
829 811 || (mode == LFR_MODE_SBM1) || (mode == LFR_MODE_SBM2))
830 812 {
831 813 status = LFR_SUCCESSFUL;
832 814 }
833 815 else
834 816 {
835 817 status = LFR_DEFAULT;
836 818 }
837 819
838 820 return status;
839 821 }
840 822
841 823 unsigned int check_update_info_hk_tds_mode( unsigned char mode )
842 824 {
843 825 unsigned int status;
844 826
845 827 if ( (mode == TDS_MODE_STANDBY) || (mode == TDS_MODE_NORMAL)
846 828 || (mode == TDS_MODE_BURST)
847 829 || (mode == TDS_MODE_SBM1) || (mode == TDS_MODE_SBM2)
848 830 || (mode == TDS_MODE_LFM))
849 831 {
850 832 status = LFR_SUCCESSFUL;
851 833 }
852 834 else
853 835 {
854 836 status = LFR_DEFAULT;
855 837 }
856 838
857 839 return status;
858 840 }
859 841
860 842 unsigned int check_update_info_hk_thr_mode( unsigned char mode )
861 843 {
862 844 unsigned int status;
863 845
864 846 if ( (mode == THR_MODE_STANDBY) || (mode == THR_MODE_NORMAL)
865 847 || (mode == THR_MODE_BURST))
866 848 {
867 849 status = LFR_SUCCESSFUL;
868 850 }
869 851 else
870 852 {
871 853 status = LFR_DEFAULT;
872 854 }
873 855
874 856 return status;
875 857 }
876 858
877 859 //***********
878 860 // FBINS MASK
879 861
880 862 int set_sy_lfr_fbins( ccsdsTelecommandPacket_t *TC )
881 863 {
882 864 int status;
883 865 unsigned int k;
884 866 unsigned char *fbins_mask_dump;
885 867 unsigned char *fbins_mask_TC;
886 868
887 869 status = LFR_SUCCESSFUL;
888 870
889 871 fbins_mask_dump = parameter_dump_packet.sy_lfr_fbins_f0_word1;
890 872 fbins_mask_TC = TC->dataAndCRC;
891 873
892 874 for (k=0; k < NB_FBINS_MASKS * NB_BYTES_PER_FBINS_MASK; k++)
893 875 {
894 876 fbins_mask_dump[k] = fbins_mask_TC[k];
895 877 }
896 878 for (k=0; k < NB_FBINS_MASKS; k++)
897 879 {
898 880 unsigned char *auxPtr;
899 881 auxPtr = &parameter_dump_packet.sy_lfr_fbins_f0_word1[k*NB_BYTES_PER_FBINS_MASK];
900 printf("%x %x %x %x\n", auxPtr[0], auxPtr[1], auxPtr[2], auxPtr[3]);
901 882 }
902 883
903 884
904 885 return status;
905 886 }
906 887
907 888 //**************
908 889 // KCOEFFICIENTS
909 890 int set_sy_lfr_kcoeff( ccsdsTelecommandPacket_t *TC,rtems_id queue_id )
910 891 {
911 892 unsigned int kcoeff;
912 893 unsigned short sy_lfr_kcoeff_frequency;
913 894 unsigned short bin;
914 895 unsigned short *freqPtr;
915 896 float *kcoeffPtr_norm;
916 897 float *kcoeffPtr_sbm;
917 898 int status;
918 899 unsigned char *kcoeffLoadPtr;
919 900 unsigned char *kcoeffNormPtr;
920 901 unsigned char *kcoeffSbmPtr_a;
921 902 unsigned char *kcoeffSbmPtr_b;
922 903
923 904 status = LFR_SUCCESSFUL;
924 905
925 906 kcoeffPtr_norm = NULL;
926 907 kcoeffPtr_sbm = NULL;
927 908 bin = 0;
928 909
929 910 freqPtr = (unsigned short *) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY];
930 911 sy_lfr_kcoeff_frequency = *freqPtr;
931 912
932 913 if ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM )
933 914 {
934 915 PRINTF1("ERR *** in set_sy_lfr_kcoeff_frequency *** sy_lfr_kcoeff_frequency = %d\n", sy_lfr_kcoeff_frequency)
935 916 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + 10 + 1,
936 917 TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + 1] ); // +1 to get the LSB instead of the MSB
937 918 status = LFR_DEFAULT;
938 919 }
939 920 else
940 921 {
941 922 if ( ( sy_lfr_kcoeff_frequency >= 0 )
942 923 && ( sy_lfr_kcoeff_frequency < NB_BINS_COMPRESSED_SM_F0 ) )
943 924 {
944 925 kcoeffPtr_norm = k_coeff_intercalib_f0_norm;
945 926 kcoeffPtr_sbm = k_coeff_intercalib_f0_sbm;
946 927 bin = sy_lfr_kcoeff_frequency;
947 928 }
948 929 else if ( ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM_F0 )
949 930 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) ) )
950 931 {
951 932 kcoeffPtr_norm = k_coeff_intercalib_f1_norm;
952 933 kcoeffPtr_sbm = k_coeff_intercalib_f1_sbm;
953 934 bin = sy_lfr_kcoeff_frequency - NB_BINS_COMPRESSED_SM_F0;
954 935 }
955 936 else if ( ( sy_lfr_kcoeff_frequency >= (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) )
956 937 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 + NB_BINS_COMPRESSED_SM_F2) ) )
957 938 {
958 939 kcoeffPtr_norm = k_coeff_intercalib_f2;
959 940 kcoeffPtr_sbm = NULL;
960 941 bin = sy_lfr_kcoeff_frequency - (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1);
961 942 }
962 943 }
963 944
964 printf("in set_sy_lfr_kcoeff *** freq = %d, bin = %d\n", sy_lfr_kcoeff_frequency, bin);
965
966 945 if (kcoeffPtr_norm != NULL ) // update K coefficient for NORMAL data products
967 946 {
968 947 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
969 948 {
970 949 // destination
971 950 kcoeffNormPtr = (unsigned char*) &kcoeffPtr_norm[ (bin * NB_K_COEFF_PER_BIN) + kcoeff ];
972 951 // source
973 952 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + NB_BYTES_PER_FLOAT * kcoeff];
974 953 // copy source to destination
975 954 copyFloatByChar( kcoeffNormPtr, kcoeffLoadPtr );
976 955 }
977 956 }
978 957
979 958 if (kcoeffPtr_sbm != NULL ) // update K coefficient for SBM data products
980 959 {
981 960 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
982 961 {
983 962 // destination
984 963 kcoeffSbmPtr_a= (unsigned char*) &kcoeffPtr_sbm[ ( (bin * NB_K_COEFF_PER_BIN) + kcoeff) * 2 ];
985 964 kcoeffSbmPtr_b= (unsigned char*) &kcoeffPtr_sbm[ ( (bin * NB_K_COEFF_PER_BIN) + kcoeff) * 2 + 1 ];
986 965 // source
987 966 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + NB_BYTES_PER_FLOAT * kcoeff];
988 967 // copy source to destination
989 968 copyFloatByChar( kcoeffSbmPtr_a, kcoeffLoadPtr );
990 969 copyFloatByChar( kcoeffSbmPtr_b, kcoeffLoadPtr );
991 970 }
992 971 }
993 972
994 973 // print_k_coeff();
995 974
996 975 return status;
997 976 }
998 977
999 978 void copyFloatByChar( unsigned char *destination, unsigned char *source )
1000 979 {
1001 980 destination[0] = source[0];
1002 981 destination[1] = source[1];
1003 982 destination[2] = source[2];
1004 983 destination[3] = source[3];
1005 984 }
1006 985
1007 986 //**********
1008 987 // init dump
1009 988
1010 989 void init_parameter_dump( void )
1011 990 {
1012 991 /** This function initialize the parameter_dump_packet global variable with default values.
1013 992 *
1014 993 */
1015 994
1016 995 unsigned int k;
1017 996
1018 997 parameter_dump_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
1019 998 parameter_dump_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
1020 999 parameter_dump_packet.reserved = CCSDS_RESERVED;
1021 1000 parameter_dump_packet.userApplication = CCSDS_USER_APP;
1022 1001 parameter_dump_packet.packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> 8);
1023 1002 parameter_dump_packet.packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;
1024 1003 parameter_dump_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1025 1004 parameter_dump_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1026 1005 parameter_dump_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_PARAMETER_DUMP >> 8);
1027 1006 parameter_dump_packet.packetLength[1] = (unsigned char) PACKET_LENGTH_PARAMETER_DUMP;
1028 1007 // DATA FIELD HEADER
1029 1008 parameter_dump_packet.spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1030 1009 parameter_dump_packet.serviceType = TM_TYPE_PARAMETER_DUMP;
1031 1010 parameter_dump_packet.serviceSubType = TM_SUBTYPE_PARAMETER_DUMP;
1032 1011 parameter_dump_packet.destinationID = TM_DESTINATION_ID_GROUND;
1033 1012 parameter_dump_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
1034 1013 parameter_dump_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
1035 1014 parameter_dump_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
1036 1015 parameter_dump_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
1037 1016 parameter_dump_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
1038 1017 parameter_dump_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
1039 1018 parameter_dump_packet.sid = SID_PARAMETER_DUMP;
1040 1019
1041 1020 //******************
1042 1021 // COMMON PARAMETERS
1043 1022 parameter_dump_packet.sy_lfr_common_parameters_spare = DEFAULT_SY_LFR_COMMON0;
1044 1023 parameter_dump_packet.sy_lfr_common_parameters = DEFAULT_SY_LFR_COMMON1;
1045 1024
1046 1025 //******************
1047 1026 // NORMAL PARAMETERS
1048 1027 parameter_dump_packet.sy_lfr_n_swf_l[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_L >> 8);
1049 1028 parameter_dump_packet.sy_lfr_n_swf_l[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_L );
1050 1029 parameter_dump_packet.sy_lfr_n_swf_p[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_P >> 8);
1051 1030 parameter_dump_packet.sy_lfr_n_swf_p[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_P );
1052 1031 parameter_dump_packet.sy_lfr_n_asm_p[0] = (unsigned char) (DFLT_SY_LFR_N_ASM_P >> 8);
1053 1032 parameter_dump_packet.sy_lfr_n_asm_p[1] = (unsigned char) (DFLT_SY_LFR_N_ASM_P );
1054 1033 parameter_dump_packet.sy_lfr_n_bp_p0 = (unsigned char) DFLT_SY_LFR_N_BP_P0;
1055 1034 parameter_dump_packet.sy_lfr_n_bp_p1 = (unsigned char) DFLT_SY_LFR_N_BP_P1;
1056 1035 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = (unsigned char) DFLT_SY_LFR_N_CWF_LONG_F3;
1057 1036
1058 1037 //*****************
1059 1038 // BURST PARAMETERS
1060 1039 parameter_dump_packet.sy_lfr_b_bp_p0 = (unsigned char) DEFAULT_SY_LFR_B_BP_P0;
1061 1040 parameter_dump_packet.sy_lfr_b_bp_p1 = (unsigned char) DEFAULT_SY_LFR_B_BP_P1;
1062 1041
1063 1042 //****************
1064 1043 // SBM1 PARAMETERS
1065 1044 parameter_dump_packet.sy_lfr_s1_bp_p0 = (unsigned char) DEFAULT_SY_LFR_S1_BP_P0; // min value is 0.25 s for the period
1066 1045 parameter_dump_packet.sy_lfr_s1_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S1_BP_P1;
1067 1046
1068 1047 //****************
1069 1048 // SBM2 PARAMETERS
1070 1049 parameter_dump_packet.sy_lfr_s2_bp_p0 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P0;
1071 1050 parameter_dump_packet.sy_lfr_s2_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P1;
1072 1051
1073 1052 //************
1074 1053 // FBINS MASKS
1075 1054 for (k=0; k < NB_FBINS_MASKS * NB_BYTES_PER_FBINS_MASK; k++)
1076 1055 {
1077 1056 parameter_dump_packet.sy_lfr_fbins_f0_word1[k] = 0xff;
1078 1057 }
1079 1058 }
1080 1059
1081 1060 void init_kcoefficients_dump( void )
1082 1061 {
1083 1062 init_kcoefficients_dump_packet( &kcoefficients_dump_1, 1, 30 );
1084 1063 init_kcoefficients_dump_packet( &kcoefficients_dump_2, 2, 6 );
1085 1064
1086 1065 kcoefficient_node_1.previous = NULL;
1087 1066 kcoefficient_node_1.next = NULL;
1088 1067 kcoefficient_node_1.sid = TM_CODE_K_DUMP;
1089 1068 kcoefficient_node_1.coarseTime = 0x00;
1090 1069 kcoefficient_node_1.fineTime = 0x00;
1091 1070 kcoefficient_node_1.buffer_address = (int) &kcoefficients_dump_1;
1092 1071 kcoefficient_node_1.status = 0x00;
1093 1072
1094 1073 kcoefficient_node_2.previous = NULL;
1095 1074 kcoefficient_node_2.next = NULL;
1096 1075 kcoefficient_node_2.sid = TM_CODE_K_DUMP;
1097 1076 kcoefficient_node_2.coarseTime = 0x00;
1098 1077 kcoefficient_node_2.fineTime = 0x00;
1099 1078 kcoefficient_node_2.buffer_address = (int) &kcoefficients_dump_2;
1100 1079 kcoefficient_node_2.status = 0x00;
1101 1080 }
1102 1081
1103 1082 void init_kcoefficients_dump_packet( Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump, unsigned char pkt_nr, unsigned char blk_nr )
1104 1083 {
1105 1084 unsigned int k;
1106 1085 unsigned int packetLength;
1107 1086
1108 1087 packetLength = blk_nr * 130 + 20 - CCSDS_TC_TM_PACKET_OFFSET; // 4 bytes for the CCSDS header
1109 1088
1110 1089 kcoefficients_dump->targetLogicalAddress = CCSDS_DESTINATION_ID;
1111 1090 kcoefficients_dump->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1112 1091 kcoefficients_dump->reserved = CCSDS_RESERVED;
1113 1092 kcoefficients_dump->userApplication = CCSDS_USER_APP;
1114 1093 kcoefficients_dump->packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> 8);;
1115 1094 kcoefficients_dump->packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;;
1116 1095 kcoefficients_dump->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1117 1096 kcoefficients_dump->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1118 1097 kcoefficients_dump->packetLength[0] = (unsigned char) (packetLength >> 8);
1119 1098 kcoefficients_dump->packetLength[1] = (unsigned char) packetLength;
1120 1099 // DATA FIELD HEADER
1121 1100 kcoefficients_dump->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1122 1101 kcoefficients_dump->serviceType = TM_TYPE_K_DUMP;
1123 1102 kcoefficients_dump->serviceSubType = TM_SUBTYPE_K_DUMP;
1124 1103 kcoefficients_dump->destinationID= TM_DESTINATION_ID_GROUND;
1125 1104 kcoefficients_dump->time[0] = 0x00;
1126 1105 kcoefficients_dump->time[1] = 0x00;
1127 1106 kcoefficients_dump->time[2] = 0x00;
1128 1107 kcoefficients_dump->time[3] = 0x00;
1129 1108 kcoefficients_dump->time[4] = 0x00;
1130 1109 kcoefficients_dump->time[5] = 0x00;
1131 1110 kcoefficients_dump->sid = SID_K_DUMP;
1132 1111
1133 1112 kcoefficients_dump->pkt_cnt = 2;
1134 1113 kcoefficients_dump->pkt_nr = pkt_nr;
1135 1114 kcoefficients_dump->blk_nr = blk_nr;
1136 1115
1137 1116 //******************
1138 1117 // SOURCE DATA repeated N times with N in [0 .. PA_LFR_KCOEFF_BLK_NR]
1139 1118 // one blk is 2 + 4 * 32 = 130 bytes, 30 blks max in one packet (30 * 130 = 3900)
1140 1119 for (k=0; k<3900; k++)
1141 1120 {
1142 1121 kcoefficients_dump->kcoeff_blks[k] = 0x00;
1143 1122 }
1144 1123 }
1145 1124
1146 void print_k_coeff()
1125 void increment_seq_counter_destination_id_dump( unsigned char *packet_sequence_control, unsigned char destination_id )
1147 1126 {
1148 unsigned int kcoeff;
1149 unsigned int bin;
1127 /** This function increment the packet sequence control parameter of a TC, depending on its destination ID.
1128 *
1129 * @param packet_sequence_control points to the packet sequence control which will be incremented
1130 * @param destination_id is the destination ID of the TM, there is one counter by destination ID
1131 *
1132 * If the destination ID is not known, a dedicated counter is incremented.
1133 *
1134 */
1150 1135
1151 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
1136 unsigned short sequence_cnt;
1137 unsigned short segmentation_grouping_flag;
1138 unsigned short new_packet_sequence_control;
1139 unsigned char i;
1140
1141 switch (destination_id)
1152 1142 {
1153 printf("kcoeff = %d *** ", kcoeff);
1154 for (bin=0; bin<NB_BINS_COMPRESSED_SM_F0; bin++)
1155 {
1156 printf( "%f ", k_coeff_intercalib_f0_norm[bin*NB_K_COEFF_PER_BIN+kcoeff] );
1157 }
1158 printf("\n");
1143 case SID_TC_GROUND:
1144 i = GROUND;
1145 break;
1146 case SID_TC_MISSION_TIMELINE:
1147 i = MISSION_TIMELINE;
1148 break;
1149 case SID_TC_TC_SEQUENCES:
1150 i = TC_SEQUENCES;
1151 break;
1152 case SID_TC_RECOVERY_ACTION_CMD:
1153 i = RECOVERY_ACTION_CMD;
1154 break;
1155 case SID_TC_BACKUP_MISSION_TIMELINE:
1156 i = BACKUP_MISSION_TIMELINE;
1157 break;
1158 case SID_TC_DIRECT_CMD:
1159 i = DIRECT_CMD;
1160 break;
1161 case SID_TC_SPARE_GRD_SRC1:
1162 i = SPARE_GRD_SRC1;
1163 break;
1164 case SID_TC_SPARE_GRD_SRC2:
1165 i = SPARE_GRD_SRC2;
1166 break;
1167 case SID_TC_OBCP:
1168 i = OBCP;
1169 break;
1170 case SID_TC_SYSTEM_CONTROL:
1171 i = SYSTEM_CONTROL;
1172 break;
1173 case SID_TC_AOCS:
1174 i = AOCS;
1175 break;
1176 case SID_TC_RPW_INTERNAL:
1177 i = RPW_INTERNAL;
1178 break;
1179 default:
1180 i = GROUND;
1181 break;
1159 1182 }
1160 1183
1161 printf("\n");
1184 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
1185 sequence_cnt = sequenceCounters_TM_DUMP[ i ] & 0x3fff;
1186
1187 new_packet_sequence_control = segmentation_grouping_flag | sequence_cnt ;
1162 1188
1163 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
1189 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> 8);
1190 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
1191
1192 // increment the sequence counter
1193 if ( sequenceCounters_TM_DUMP[ i ] < SEQ_CNT_MAX )
1164 1194 {
1165 printf("kcoeff = %d *** ", kcoeff);
1166 for (bin=0; bin<NB_BINS_COMPRESSED_SM_F0; bin++)
1195 sequenceCounters_TM_DUMP[ i ] = sequenceCounters_TM_DUMP[ i ] + 1;
1196 }
1197 else
1167 1198 {
1168 printf( "[%f, %f] ",
1169 k_coeff_intercalib_f0_sbm[(bin*NB_K_COEFF_PER_BIN )*2 + kcoeff],
1170 k_coeff_intercalib_f0_sbm[(bin*NB_K_COEFF_PER_BIN+1)*2 + kcoeff]);
1171 }
1172 printf("\n");
1199 sequenceCounters_TM_DUMP[ i ] = 0;
1173 1200 }
1174 1201 }
1175
Show file before | Show file after | Hide comments
@@ -1,1372 +1,1373
1 1 /** Functions and tasks related to waveform packet generation.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle waveforms, in snapshot or continuous format.\n
7 7 *
8 8 */
9 9
10 10 #include "wf_handler.h"
11 11
12 12 //***************
13 13 // waveform rings
14 14 // F0
15 15 ring_node waveform_ring_f0[NB_RING_NODES_F0];
16 16 ring_node *current_ring_node_f0;
17 17 ring_node *ring_node_to_send_swf_f0;
18 18 // F1
19 19 ring_node waveform_ring_f1[NB_RING_NODES_F1];
20 20 ring_node *current_ring_node_f1;
21 21 ring_node *ring_node_to_send_swf_f1;
22 22 ring_node *ring_node_to_send_cwf_f1;
23 23 // F2
24 24 ring_node waveform_ring_f2[NB_RING_NODES_F2];
25 25 ring_node *current_ring_node_f2;
26 26 ring_node *ring_node_to_send_swf_f2;
27 27 ring_node *ring_node_to_send_cwf_f2;
28 28 // F3
29 29 ring_node waveform_ring_f3[NB_RING_NODES_F3];
30 30 ring_node *current_ring_node_f3;
31 31 ring_node *ring_node_to_send_cwf_f3;
32 32 char wf_cont_f3_light[ (NB_SAMPLES_PER_SNAPSHOT) * NB_BYTES_CWF3_LIGHT_BLK ];
33 33
34 34 bool extractSWF = false;
35 35 bool swf_f0_ready = false;
36 36 bool swf_f1_ready = false;
37 37 bool swf_f2_ready = false;
38 38
39 39 int wf_snap_extracted[ (NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK) ];
40 40 ring_node ring_node_wf_snap_extracted;
41 41
42 42 //*********************
43 43 // Interrupt SubRoutine
44 44
45 45 ring_node * getRingNodeToSendCWF( unsigned char frequencyChannel)
46 46 {
47 47 ring_node *node;
48 48
49 49 node = NULL;
50 50 switch ( frequencyChannel ) {
51 51 case 1:
52 52 node = ring_node_to_send_cwf_f1;
53 53 break;
54 54 case 2:
55 55 node = ring_node_to_send_cwf_f2;
56 56 break;
57 57 case 3:
58 58 node = ring_node_to_send_cwf_f3;
59 59 break;
60 60 default:
61 61 break;
62 62 }
63 63
64 64 return node;
65 65 }
66 66
67 67 ring_node * getRingNodeToSendSWF( unsigned char frequencyChannel)
68 68 {
69 69 ring_node *node;
70 70
71 71 node = NULL;
72 72 switch ( frequencyChannel ) {
73 73 case 0:
74 74 node = ring_node_to_send_swf_f0;
75 75 break;
76 76 case 1:
77 77 node = ring_node_to_send_swf_f1;
78 78 break;
79 79 case 2:
80 80 node = ring_node_to_send_swf_f2;
81 81 break;
82 82 default:
83 83 break;
84 84 }
85 85
86 86 return node;
87 87 }
88 88
89 89 void reset_extractSWF( void )
90 90 {
91 91 extractSWF = false;
92 92 swf_f0_ready = false;
93 93 swf_f1_ready = false;
94 94 swf_f2_ready = false;
95 95 }
96 96
97 97 inline void waveforms_isr_f3( void )
98 98 {
99 99 rtems_status_code spare_status;
100 100
101 101 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_BURST) // in BURST the data are used to place v, e1 and e2 in the HK packet
102 102 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
103 103 { // in modes other than STANDBY and BURST, send the CWF_F3 data
104 104 //***
105 105 // F3
106 106 if ( (waveform_picker_regs->status & 0xc0) != 0x00 ) { // [1100 0000] check the f3 full bits
107 107 ring_node_to_send_cwf_f3 = current_ring_node_f3->previous;
108 108 current_ring_node_f3 = current_ring_node_f3->next;
109 109 if ((waveform_picker_regs->status & 0x40) == 0x40){ // [0100 0000] f3 buffer 0 is full
110 110 ring_node_to_send_cwf_f3->coarseTime = waveform_picker_regs->f3_0_coarse_time;
111 111 ring_node_to_send_cwf_f3->fineTime = waveform_picker_regs->f3_0_fine_time;
112 112 waveform_picker_regs->addr_data_f3_0 = current_ring_node_f3->buffer_address;
113 113 waveform_picker_regs->status = waveform_picker_regs->status & 0x00008840; // [1000 1000 0100 0000]
114 114 }
115 115 else if ((waveform_picker_regs->status & 0x80) == 0x80){ // [1000 0000] f3 buffer 1 is full
116 116 ring_node_to_send_cwf_f3->coarseTime = waveform_picker_regs->f3_1_coarse_time;
117 117 ring_node_to_send_cwf_f3->fineTime = waveform_picker_regs->f3_1_fine_time;
118 118 waveform_picker_regs->addr_data_f3_1 = current_ring_node_f3->buffer_address;
119 119 waveform_picker_regs->status = waveform_picker_regs->status & 0x00008880; // [1000 1000 1000 0000]
120 120 }
121 121 if (rtems_event_send( Task_id[TASKID_CWF3], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
122 122 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
123 123 }
124 124 }
125 125 }
126 126 }
127 127
128 128 inline void waveforms_isr_normal( void )
129 129 {
130 130 rtems_status_code status;
131 131
132 132 if ( ( (waveform_picker_regs->status & 0x30) != 0x00 ) // [0011 0000] check the f2 full bits
133 133 && ( (waveform_picker_regs->status & 0x0c) != 0x00 ) // [0000 1100] check the f1 full bits
134 134 && ( (waveform_picker_regs->status & 0x03) != 0x00 )) // [0000 0011] check the f0 full bits
135 135 {
136 136 //***
137 137 // F0
138 138 ring_node_to_send_swf_f0 = current_ring_node_f0->previous;
139 139 current_ring_node_f0 = current_ring_node_f0->next;
140 140 if ( (waveform_picker_regs->status & 0x01) == 0x01)
141 141 {
142 142
143 143 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_0_coarse_time;
144 144 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_0_fine_time;
145 145 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->buffer_address;
146 146 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001101; // [0001 0001 0000 0001]
147 147 }
148 148 else if ( (waveform_picker_regs->status & 0x02) == 0x02)
149 149 {
150 150 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_1_coarse_time;
151 151 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_1_fine_time;
152 152 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address;
153 153 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001102; // [0001 0001 0000 0010]
154 154 }
155 155
156 156 //***
157 157 // F1
158 158 ring_node_to_send_swf_f1 = current_ring_node_f1->previous;
159 159 current_ring_node_f1 = current_ring_node_f1->next;
160 160 if ( (waveform_picker_regs->status & 0x04) == 0x04)
161 161 {
162 162 ring_node_to_send_swf_f1->coarseTime = waveform_picker_regs->f1_0_coarse_time;
163 163 ring_node_to_send_swf_f1->fineTime = waveform_picker_regs->f1_0_fine_time;
164 164 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->buffer_address;
165 165 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002204; // [0010 0010 0000 0100] f1 bits = 0
166 166 }
167 167 else if ( (waveform_picker_regs->status & 0x08) == 0x08)
168 168 {
169 169 ring_node_to_send_swf_f1->coarseTime = waveform_picker_regs->f1_1_coarse_time;
170 170 ring_node_to_send_swf_f1->fineTime = waveform_picker_regs->f1_1_fine_time;
171 171 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address;
172 172 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002208; // [0010 0010 0000 1000] f1 bits = 0
173 173 }
174 174
175 175 //***
176 176 // F2
177 177 ring_node_to_send_swf_f2 = current_ring_node_f2->previous;
178 178 current_ring_node_f2 = current_ring_node_f2->next;
179 179 if ( (waveform_picker_regs->status & 0x10) == 0x10)
180 180 {
181 181 ring_node_to_send_swf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
182 182 ring_node_to_send_swf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
183 183 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
184 184 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
185 185 }
186 186 else if ( (waveform_picker_regs->status & 0x20) == 0x20)
187 187 {
188 188 ring_node_to_send_swf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
189 189 ring_node_to_send_swf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
190 190 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
191 191 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
192 192 }
193 193 //
194 194 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_NORMAL );
195 195 if ( status != RTEMS_SUCCESSFUL)
196 196 {
197 197 status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
198 198 }
199 199 }
200 200 }
201 201
202 202 inline void waveforms_isr_burst( void )
203 203 {
204 204 unsigned char status;
205 205 rtems_status_code spare_status;
206 206
207 207 status = (waveform_picker_regs->status & 0x30) >> 4; // [0011 0000] get the status bits for f2
208 208
209 209
210 210 switch(status)
211 211 {
212 212 case 1:
213 213 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
214 214 ring_node_to_send_cwf_f2->sid = SID_BURST_CWF_F2;
215 215 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
216 216 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
217 217 current_ring_node_f2 = current_ring_node_f2->next;
218 218 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
219 219 if (rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_BURST ) != RTEMS_SUCCESSFUL) {
220 220 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
221 221 }
222 222 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
223 223 break;
224 224 case 2:
225 225 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
226 226 ring_node_to_send_cwf_f2->sid = SID_BURST_CWF_F2;
227 227 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
228 228 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
229 229 current_ring_node_f2 = current_ring_node_f2->next;
230 230 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
231 231 if (rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_BURST ) != RTEMS_SUCCESSFUL) {
232 232 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
233 233 }
234 234 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
235 235 break;
236 236 default:
237 237 break;
238 238 }
239 239 }
240 240
241 241 inline void waveforms_isr_sbm1( void )
242 242 {
243 243 rtems_status_code status;
244 244
245 245 //***
246 246 // F1
247 247 if ( (waveform_picker_regs->status & 0x0c) != 0x00 ) { // [0000 1100] check the f1 full bits
248 248 // (1) change the receiving buffer for the waveform picker
249 249 ring_node_to_send_cwf_f1 = current_ring_node_f1->previous;
250 250 current_ring_node_f1 = current_ring_node_f1->next;
251 251 if ( (waveform_picker_regs->status & 0x04) == 0x04)
252 252 {
253 253 ring_node_to_send_cwf_f1->coarseTime = waveform_picker_regs->f1_0_coarse_time;
254 254 ring_node_to_send_cwf_f1->fineTime = waveform_picker_regs->f1_0_fine_time;
255 255 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->buffer_address;
256 256 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002204; // [0010 0010 0000 0100] f1 bits = 0
257 257 }
258 258 else if ( (waveform_picker_regs->status & 0x08) == 0x08)
259 259 {
260 260 ring_node_to_send_cwf_f1->coarseTime = waveform_picker_regs->f1_1_coarse_time;
261 261 ring_node_to_send_cwf_f1->fineTime = waveform_picker_regs->f1_1_fine_time;
262 262 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address;
263 263 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002208; // [0010 0010 0000 1000] f1 bits = 0
264 264 }
265 265 // (2) send an event for the the CWF1 task for transmission (and snapshot extraction if needed)
266 266 status = rtems_event_send( Task_id[TASKID_CWF1], RTEMS_EVENT_MODE_SBM1 );
267 267 }
268 268
269 269 //***
270 270 // F0
271 271 if ( (waveform_picker_regs->status & 0x03) != 0x00 ) { // [0000 0011] check the f0 full bits
272 272 swf_f0_ready = true;
273 273 // change f0 buffer
274 274 ring_node_to_send_swf_f0 = current_ring_node_f0->previous;
275 275 current_ring_node_f0 = current_ring_node_f0->next;
276 276 if ( (waveform_picker_regs->status & 0x01) == 0x01)
277 277 {
278 278
279 279 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_0_coarse_time;
280 280 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_0_fine_time;
281 281 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->buffer_address;
282 282 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001101; // [0001 0001 0000 0001]
283 283 }
284 284 else if ( (waveform_picker_regs->status & 0x02) == 0x02)
285 285 {
286 286 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_1_coarse_time;
287 287 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_1_fine_time;
288 288 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address;
289 289 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001102; // [0001 0001 0000 0010]
290 290 }
291 291 }
292 292
293 293 //***
294 294 // F2
295 295 if ( (waveform_picker_regs->status & 0x30) != 0x00 ) { // [0011 0000] check the f2 full bits
296 296 swf_f2_ready = true;
297 297 // change f2 buffer
298 298 ring_node_to_send_swf_f2 = current_ring_node_f2->previous;
299 299 current_ring_node_f2 = current_ring_node_f2->next;
300 300 if ( (waveform_picker_regs->status & 0x10) == 0x10)
301 301 {
302 302 ring_node_to_send_swf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
303 303 ring_node_to_send_swf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
304 304 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
305 305 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
306 306 }
307 307 else if ( (waveform_picker_regs->status & 0x20) == 0x20)
308 308 {
309 309 ring_node_to_send_swf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
310 310 ring_node_to_send_swf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
311 311 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
312 312 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
313 313 }
314 314 }
315 315 }
316 316
317 317 inline void waveforms_isr_sbm2( void )
318 318 {
319 319 rtems_status_code status;
320 320
321 321 //***
322 322 // F2
323 323 if ( (waveform_picker_regs->status & 0x30) != 0x00 ) { // [0011 0000] check the f2 full bit
324 324 // (1) change the receiving buffer for the waveform picker
325 325 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
326 326 ring_node_to_send_cwf_f2->sid = SID_SBM2_CWF_F2;
327 327 current_ring_node_f2 = current_ring_node_f2->next;
328 328 if ( (waveform_picker_regs->status & 0x10) == 0x10)
329 329 {
330 330 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
331 331 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
332 332 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
333 333 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
334 334 }
335 335 else if ( (waveform_picker_regs->status & 0x20) == 0x20)
336 336 {
337 337 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
338 338 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
339 339 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
340 340 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
341 341 }
342 342 // (2) send an event for the waveforms transmission
343 343 status = rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_SBM2 );
344 344 }
345 345
346 346 //***
347 347 // F0
348 348 if ( (waveform_picker_regs->status & 0x03) != 0x00 ) { // [0000 0011] check the f0 full bit
349 349 swf_f0_ready = true;
350 350 // change f0 buffer
351 351 ring_node_to_send_swf_f0 = current_ring_node_f0->previous;
352 352 current_ring_node_f0 = current_ring_node_f0->next;
353 353 if ( (waveform_picker_regs->status & 0x01) == 0x01)
354 354 {
355 355
356 356 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_0_coarse_time;
357 357 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_0_fine_time;
358 358 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->buffer_address;
359 359 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001101; // [0001 0001 0000 0001]
360 360 }
361 361 else if ( (waveform_picker_regs->status & 0x02) == 0x02)
362 362 {
363 363 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_1_coarse_time;
364 364 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_1_fine_time;
365 365 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address;
366 366 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001102; // [0001 0001 0000 0010]
367 367 }
368 368 }
369 369
370 370 //***
371 371 // F1
372 372 if ( (waveform_picker_regs->status & 0x0c) != 0x00 ) { // [0000 1100] check the f1 full bit
373 373 swf_f1_ready = true;
374 374 ring_node_to_send_swf_f1 = current_ring_node_f1->previous;
375 375 current_ring_node_f1 = current_ring_node_f1->next;
376 376 if ( (waveform_picker_regs->status & 0x04) == 0x04)
377 377 {
378 378 ring_node_to_send_swf_f1->coarseTime = waveform_picker_regs->f1_0_coarse_time;
379 379 ring_node_to_send_swf_f1->fineTime = waveform_picker_regs->f1_0_fine_time;
380 380 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->buffer_address;
381 381 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002204; // [0010 0010 0000 0100] f1 bits = 0
382 382 }
383 383 else if ( (waveform_picker_regs->status & 0x08) == 0x08)
384 384 {
385 385 ring_node_to_send_swf_f1->coarseTime = waveform_picker_regs->f1_1_coarse_time;
386 386 ring_node_to_send_swf_f1->fineTime = waveform_picker_regs->f1_1_fine_time;
387 387 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address;
388 388 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002208; // [0010 0010 0000 1000] f1 bits = 0
389 389 }
390 390 }
391 391 }
392 392
393 393 rtems_isr waveforms_isr( rtems_vector_number vector )
394 394 {
395 395 /** This is the interrupt sub routine called by the waveform picker core.
396 396 *
397 397 * This ISR launch different actions depending mainly on two pieces of information:
398 398 * 1. the values read in the registers of the waveform picker.
399 399 * 2. the current LFR mode.
400 400 *
401 401 */
402 402
403 403 // STATUS
404 404 // new error error buffer full
405 405 // 15 14 13 12 11 10 9 8
406 406 // f3 f2 f1 f0 f3 f2 f1 f0
407 407 //
408 408 // ready buffer
409 409 // 7 6 5 4 3 2 1 0
410 410 // f3_1 f3_0 f2_1 f2_0 f1_1 f1_0 f0_1 f0_0
411 411
412 412 rtems_status_code spare_status;
413 413
414 414 waveforms_isr_f3();
415 415
416 416 if ( (waveform_picker_regs->status & 0xff00) != 0x00) // [1111 1111 0000 0000] check the error bits
417 417 {
418 418 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_10 );
419 419 }
420 420
421 421 switch(lfrCurrentMode)
422 422 {
423 423 //********
424 424 // STANDBY
425 425 case(LFR_MODE_STANDBY):
426 426 break;
427 427
428 428 //******
429 429 // NORMAL
430 430 case(LFR_MODE_NORMAL):
431 431 waveforms_isr_normal();
432 432 break;
433 433
434 434 //******
435 435 // BURST
436 436 case(LFR_MODE_BURST):
437 437 waveforms_isr_burst();
438 438 break;
439 439
440 440 //*****
441 441 // SBM1
442 442 case(LFR_MODE_SBM1):
443 443 waveforms_isr_sbm1();
444 444 break;
445 445
446 446 //*****
447 447 // SBM2
448 448 case(LFR_MODE_SBM2):
449 449 waveforms_isr_sbm2();
450 450 break;
451 451
452 452 //********
453 453 // DEFAULT
454 454 default:
455 455 break;
456 456 }
457 457 }
458 458
459 459 //************
460 460 // RTEMS TASKS
461 461
462 462 rtems_task wfrm_task(rtems_task_argument argument) //used with the waveform picker VHDL IP
463 463 {
464 464 /** This RTEMS task is dedicated to the transmission of snapshots of the NORMAL mode.
465 465 *
466 466 * @param unused is the starting argument of the RTEMS task
467 467 *
468 468 * The following data packets are sent by this task:
469 469 * - TM_LFR_SCIENCE_NORMAL_SWF_F0
470 470 * - TM_LFR_SCIENCE_NORMAL_SWF_F1
471 471 * - TM_LFR_SCIENCE_NORMAL_SWF_F2
472 472 *
473 473 */
474 474
475 475 rtems_event_set event_out;
476 476 rtems_id queue_id;
477 477 rtems_status_code status;
478 478 bool resynchronisationEngaged;
479 479 ring_node *ring_node_wf_snap_extracted_ptr;
480 480
481 481 ring_node_wf_snap_extracted_ptr = (ring_node *) &ring_node_wf_snap_extracted;
482 482
483 483 resynchronisationEngaged = false;
484 484
485 485 status = get_message_queue_id_send( &queue_id );
486 486 if (status != RTEMS_SUCCESSFUL)
487 487 {
488 488 PRINTF1("in WFRM *** ERR get_message_queue_id_send %d\n", status)
489 489 }
490 490
491 491 BOOT_PRINTF("in WFRM ***\n")
492 492
493 493 while(1){
494 494 // wait for an RTEMS_EVENT
495 495 rtems_event_receive(RTEMS_EVENT_MODE_NORMAL | RTEMS_EVENT_MODE_SBM1
496 496 | RTEMS_EVENT_MODE_SBM2 | RTEMS_EVENT_MODE_SBM2_WFRM,
497 497 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
498 498 if(resynchronisationEngaged == false)
499 499 { // engage resynchronisation
500 500 snapshot_resynchronization( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
501 501 resynchronisationEngaged = true;
502 502 }
503 503 else
504 504 { // reset delta_snapshot to the nominal value
505 505 PRINTF("no resynchronisation, reset delta_snapshot to the nominal value\n")
506 506 set_wfp_delta_snapshot();
507 507 resynchronisationEngaged = false;
508 508 }
509 509 //
510 510
511 511 if (event_out == RTEMS_EVENT_MODE_NORMAL)
512 512 {
513 513 DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_NORMAL\n")
514 514 ring_node_to_send_swf_f0->sid = SID_NORM_SWF_F0;
515 515 ring_node_to_send_swf_f1->sid = SID_NORM_SWF_F1;
516 516 ring_node_to_send_swf_f2->sid = SID_NORM_SWF_F2;
517 517 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f0, sizeof( ring_node* ) );
518 518 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f1, sizeof( ring_node* ) );
519 519 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f2, sizeof( ring_node* ) );
520 520 }
521 521 if (event_out == RTEMS_EVENT_MODE_SBM1)
522 522 {
523 523 DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_SBM1\n")
524 524 ring_node_to_send_swf_f0->sid = SID_NORM_SWF_F0;
525 525 ring_node_wf_snap_extracted_ptr->sid = SID_NORM_SWF_F1;
526 526 ring_node_to_send_swf_f2->sid = SID_NORM_SWF_F2;
527 527 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f0, sizeof( ring_node* ) );
528 528 status = rtems_message_queue_send( queue_id, &ring_node_wf_snap_extracted_ptr, sizeof( ring_node* ) );
529 529 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f2, sizeof( ring_node* ) );
530 530 }
531 531 if (event_out == RTEMS_EVENT_MODE_SBM2)
532 532 {
533 533 DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_SBM2\n")
534 534 ring_node_to_send_swf_f0->sid = SID_NORM_SWF_F0;
535 535 ring_node_to_send_swf_f1->sid = SID_NORM_SWF_F1;
536 536 ring_node_wf_snap_extracted_ptr->sid = SID_NORM_SWF_F2;
537 537 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f0, sizeof( ring_node* ) );
538 538 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f1, sizeof( ring_node* ) );
539 539 status = rtems_message_queue_send( queue_id, &ring_node_wf_snap_extracted_ptr, sizeof( ring_node* ) );
540 540 }
541 541 }
542 542 }
543 543
544 544 rtems_task cwf3_task(rtems_task_argument argument) //used with the waveform picker VHDL IP
545 545 {
546 546 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f3.
547 547 *
548 548 * @param unused is the starting argument of the RTEMS task
549 549 *
550 550 * The following data packet is sent by this task:
551 551 * - TM_LFR_SCIENCE_NORMAL_CWF_F3
552 552 *
553 553 */
554 554
555 555 rtems_event_set event_out;
556 556 rtems_id queue_id;
557 557 rtems_status_code status;
558 558 ring_node ring_node_cwf3_light;
559 559 ring_node *ring_node_to_send_cwf;
560 560
561 561 status = get_message_queue_id_send( &queue_id );
562 562 if (status != RTEMS_SUCCESSFUL)
563 563 {
564 564 PRINTF1("in CWF3 *** ERR get_message_queue_id_send %d\n", status)
565 565 }
566 566
567 567 ring_node_to_send_cwf_f3->sid = SID_NORM_CWF_LONG_F3;
568 568
569 569 // init the ring_node_cwf3_light structure
570 570 ring_node_cwf3_light.buffer_address = (int) wf_cont_f3_light;
571 571 ring_node_cwf3_light.coarseTime = 0x00;
572 572 ring_node_cwf3_light.fineTime = 0x00;
573 573 ring_node_cwf3_light.next = NULL;
574 574 ring_node_cwf3_light.previous = NULL;
575 575 ring_node_cwf3_light.sid = SID_NORM_CWF_F3;
576 576 ring_node_cwf3_light.status = 0x00;
577 577
578 578 BOOT_PRINTF("in CWF3 ***\n")
579 579
580 580 while(1){
581 581 // wait for an RTEMS_EVENT
582 582 rtems_event_receive( RTEMS_EVENT_0,
583 583 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
584 584 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
585 585 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode==LFR_MODE_SBM2) )
586 586 {
587 587 ring_node_to_send_cwf = getRingNodeToSendCWF( 3 );
588 588 if ( (parameter_dump_packet.sy_lfr_n_cwf_long_f3 & 0x01) == 0x01)
589 589 {
590 590 PRINTF("send CWF_LONG_F3\n")
591 591 ring_node_to_send_cwf_f3->sid = SID_NORM_CWF_LONG_F3;
592 592 status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf, sizeof( ring_node* ) );
593 593 }
594 594 else
595 595 {
596 596 PRINTF("send CWF_F3 (light)\n")
597 597 send_waveform_CWF3_light( ring_node_to_send_cwf, &ring_node_cwf3_light, queue_id );
598 598 }
599 599
600 600 }
601 601 else
602 602 {
603 603 PRINTF1("in CWF3 *** lfrCurrentMode is %d, no data will be sent\n", lfrCurrentMode)
604 604 }
605 605 }
606 606 }
607 607
608 608 rtems_task cwf2_task(rtems_task_argument argument) // ONLY USED IN BURST AND SBM2
609 609 {
610 610 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f2.
611 611 *
612 612 * @param unused is the starting argument of the RTEMS task
613 613 *
614 614 * The following data packet is sent by this function:
615 615 * - TM_LFR_SCIENCE_BURST_CWF_F2
616 616 * - TM_LFR_SCIENCE_SBM2_CWF_F2
617 617 *
618 618 */
619 619
620 620 rtems_event_set event_out;
621 621 rtems_id queue_id;
622 622 rtems_status_code status;
623 623 ring_node *ring_node_to_send;
624 624 unsigned long long int acquisitionTimeF0_asLong;
625 625
626 626 acquisitionTimeF0_asLong = 0x00;
627 627
628 628 status = get_message_queue_id_send( &queue_id );
629 629 if (status != RTEMS_SUCCESSFUL)
630 630 {
631 631 PRINTF1("in CWF2 *** ERR get_message_queue_id_send %d\n", status)
632 632 }
633 633
634 634 BOOT_PRINTF("in CWF2 ***\n")
635 635
636 636 while(1){
637 637 // wait for an RTEMS_EVENT
638 638 rtems_event_receive( RTEMS_EVENT_MODE_BURST | RTEMS_EVENT_MODE_SBM2,
639 639 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
640 640 ring_node_to_send = getRingNodeToSendCWF( 2 );
641 641 if (event_out == RTEMS_EVENT_MODE_BURST)
642 642 {
643 643 status = rtems_message_queue_send( queue_id, &ring_node_to_send, sizeof( ring_node* ) );
644 644 }
645 645 if (event_out == RTEMS_EVENT_MODE_SBM2)
646 646 {
647 647 status = rtems_message_queue_send( queue_id, &ring_node_to_send, sizeof( ring_node* ) );
648 648 // launch snapshot extraction if needed
649 649 if (extractSWF == true)
650 650 {
651 651 ring_node_to_send_swf_f2 = ring_node_to_send_cwf_f2;
652 652 // extract the snapshot
653 653 build_snapshot_from_ring( ring_node_to_send_swf_f2, 2, acquisitionTimeF0_asLong );
654 654 // send the snapshot when built
655 655 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_SBM2 );
656 656 extractSWF = false;
657 657 }
658 658 if (swf_f0_ready && swf_f1_ready)
659 659 {
660 660 extractSWF = true;
661 661 // record the acquition time of the fΓ  snapshot to use to build the snapshot at f2
662 662 acquisitionTimeF0_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
663 663 swf_f0_ready = false;
664 664 swf_f1_ready = false;
665 665 }
666 666 }
667 667 }
668 668 }
669 669
670 670 rtems_task cwf1_task(rtems_task_argument argument) // ONLY USED IN SBM1
671 671 {
672 672 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f1.
673 673 *
674 674 * @param unused is the starting argument of the RTEMS task
675 675 *
676 676 * The following data packet is sent by this function:
677 677 * - TM_LFR_SCIENCE_SBM1_CWF_F1
678 678 *
679 679 */
680 680
681 681 rtems_event_set event_out;
682 682 rtems_id queue_id;
683 683 rtems_status_code status;
684 684
685 685 ring_node *ring_node_to_send_cwf;
686 686
687 687 status = get_message_queue_id_send( &queue_id );
688 688 if (status != RTEMS_SUCCESSFUL)
689 689 {
690 690 PRINTF1("in CWF1 *** ERR get_message_queue_id_send %d\n", status)
691 691 }
692 692
693 693 BOOT_PRINTF("in CWF1 ***\n")
694 694
695 695 while(1){
696 696 // wait for an RTEMS_EVENT
697 697 rtems_event_receive( RTEMS_EVENT_MODE_SBM1,
698 698 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
699 699 ring_node_to_send_cwf = getRingNodeToSendCWF( 1 );
700 700 ring_node_to_send_cwf_f1->sid = SID_SBM1_CWF_F1;
701 701 status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf, sizeof( ring_node* ) );
702 702 if (status != 0)
703 printf("cwf sending failed\n");
703 {
704 PRINTF("cwf sending failed\n")
705 }
704 706 // launch snapshot extraction if needed
705 707 if (extractSWF == true)
706 708 {
707 709 ring_node_to_send_swf_f1 = ring_node_to_send_cwf;
708 710 // launch the snapshot extraction
709 711 status = rtems_event_send( Task_id[TASKID_SWBD], RTEMS_EVENT_MODE_SBM1 );
710 712 extractSWF = false;
711 713 }
712 714 if (swf_f0_ready == true)
713 715 {
714 716 extractSWF = true;
715 717 swf_f0_ready = false; // this step shall be executed only one time
716 718 }
717 719 if ((swf_f1_ready == true) && (swf_f2_ready == true)) // swf_f1 is ready after the extraction
718 720 {
719 721 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_SBM1 );
720 722 swf_f1_ready = false;
721 723 swf_f2_ready = false;
722 724 }
723 725 }
724 726 }
725 727
726 728 rtems_task swbd_task(rtems_task_argument argument)
727 729 {
728 730 /** This RTEMS task is dedicated to the building of snapshots from different continuous waveforms buffers.
729 731 *
730 732 * @param unused is the starting argument of the RTEMS task
731 733 *
732 734 */
733 735
734 736 rtems_event_set event_out;
735 737 unsigned long long int acquisitionTimeF0_asLong;
736 738
737 739 acquisitionTimeF0_asLong = 0x00;
738 740
739 741 BOOT_PRINTF("in SWBD ***\n")
740 742
741 743 while(1){
742 744 // wait for an RTEMS_EVENT
743 745 rtems_event_receive( RTEMS_EVENT_MODE_SBM1 | RTEMS_EVENT_MODE_SBM2,
744 746 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
745 747 if (event_out == RTEMS_EVENT_MODE_SBM1)
746 748 {
747 749 acquisitionTimeF0_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
748 750 build_snapshot_from_ring( ring_node_to_send_swf_f1, 1, acquisitionTimeF0_asLong );
749 751 swf_f1_ready = true; // the snapshot has been extracted and is ready to be sent
750 752 }
751 753 else
752 754 {
753 755 PRINTF1("in SWBD *** unexpected rtems event received %x\n", (int) event_out)
754 756 }
755 757 }
756 758 }
757 759
758 760 //******************
759 761 // general functions
760 762
761 763 void WFP_init_rings( void )
762 764 {
763 765 // F0 RING
764 766 init_ring( waveform_ring_f0, NB_RING_NODES_F0, wf_buffer_f0, WFRM_BUFFER );
765 767 // F1 RING
766 768 init_ring( waveform_ring_f1, NB_RING_NODES_F1, wf_buffer_f1, WFRM_BUFFER );
767 769 // F2 RING
768 770 init_ring( waveform_ring_f2, NB_RING_NODES_F2, wf_buffer_f2, WFRM_BUFFER );
769 771 // F3 RING
770 772 init_ring( waveform_ring_f3, NB_RING_NODES_F3, wf_buffer_f3, WFRM_BUFFER );
771 773
772 774 ring_node_wf_snap_extracted.buffer_address = (int) wf_snap_extracted;
773 775
774 776 DEBUG_PRINTF1("waveform_ring_f0 @%x\n", (unsigned int) waveform_ring_f0)
775 777 DEBUG_PRINTF1("waveform_ring_f1 @%x\n", (unsigned int) waveform_ring_f1)
776 778 DEBUG_PRINTF1("waveform_ring_f2 @%x\n", (unsigned int) waveform_ring_f2)
777 779 DEBUG_PRINTF1("waveform_ring_f3 @%x\n", (unsigned int) waveform_ring_f3)
778 780 DEBUG_PRINTF1("wf_buffer_f0 @%x\n", (unsigned int) wf_buffer_f0)
779 781 DEBUG_PRINTF1("wf_buffer_f1 @%x\n", (unsigned int) wf_buffer_f1)
780 782 DEBUG_PRINTF1("wf_buffer_f2 @%x\n", (unsigned int) wf_buffer_f2)
781 783 DEBUG_PRINTF1("wf_buffer_f3 @%x\n", (unsigned int) wf_buffer_f3)
782 784
783 785 }
784 786
785 787 void WFP_reset_current_ring_nodes( void )
786 788 {
787 789 current_ring_node_f0 = waveform_ring_f0[0].next;
788 790 current_ring_node_f1 = waveform_ring_f1[0].next;
789 791 current_ring_node_f2 = waveform_ring_f2[0].next;
790 792 current_ring_node_f3 = waveform_ring_f3[0].next;
791 793
792 794 ring_node_to_send_swf_f0 = waveform_ring_f0;
793 795 ring_node_to_send_swf_f1 = waveform_ring_f1;
794 796 ring_node_to_send_swf_f2 = waveform_ring_f2;
795 797
796 798 ring_node_to_send_cwf_f1 = waveform_ring_f1;
797 799 ring_node_to_send_cwf_f2 = waveform_ring_f2;
798 800 ring_node_to_send_cwf_f3 = waveform_ring_f3;
799 801 }
800 802
801 803 int send_waveform_CWF3_light( ring_node *ring_node_to_send, ring_node *ring_node_cwf3_light, rtems_id queue_id )
802 804 {
803 805 /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data.
804 806 *
805 807 * @param waveform points to the buffer containing the data that will be send.
806 808 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
807 809 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
808 810 * contain information to setup the transmission of the data packets.
809 811 *
810 812 * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer
811 813 * from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks.
812 814 *
813 815 */
814 816
815 817 unsigned int i;
816 818 int ret;
817 819 rtems_status_code status;
818 820
819 821 char *sample;
820 822 int *dataPtr;
821 823
822 824 ret = LFR_DEFAULT;
823 825
824 826 dataPtr = (int*) ring_node_to_send->buffer_address;
825 827
826 828 ring_node_cwf3_light->coarseTime = ring_node_to_send->coarseTime;
827 829 ring_node_cwf3_light->fineTime = ring_node_to_send->fineTime;
828 830
829 831 //**********************
830 832 // BUILD CWF3_light DATA
831 833 for ( i=0; i< NB_SAMPLES_PER_SNAPSHOT; i++)
832 834 {
833 835 sample = (char*) &dataPtr[ (i * NB_WORDS_SWF_BLK) ];
834 836 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) ] = sample[ 0 ];
835 837 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 1 ] = sample[ 1 ];
836 838 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 2 ] = sample[ 2 ];
837 839 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 3 ] = sample[ 3 ];
838 840 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 4 ] = sample[ 4 ];
839 841 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 5 ] = sample[ 5 ];
840 842 }
841 843
842 844 // SEND PACKET
843 845 status = rtems_message_queue_send( queue_id, &ring_node_cwf3_light, sizeof( ring_node* ) );
844 846 if (status != RTEMS_SUCCESSFUL) {
845 printf("%d-%d, ERR %d\n", SID_NORM_CWF_F3, i, (int) status);
846 847 ret = LFR_DEFAULT;
847 848 }
848 849
849 850 return ret;
850 851 }
851 852
852 853 void compute_acquisition_time( unsigned int coarseTime, unsigned int fineTime,
853 854 unsigned int sid, unsigned char pa_lfr_pkt_nr, unsigned char * acquisitionTime )
854 855 {
855 856 unsigned long long int acquisitionTimeAsLong;
856 857 unsigned char localAcquisitionTime[6];
857 858 double deltaT;
858 859
859 860 deltaT = 0.;
860 861
861 862 localAcquisitionTime[0] = (unsigned char) ( coarseTime >> 24 );
862 863 localAcquisitionTime[1] = (unsigned char) ( coarseTime >> 16 );
863 864 localAcquisitionTime[2] = (unsigned char) ( coarseTime >> 8 );
864 865 localAcquisitionTime[3] = (unsigned char) ( coarseTime );
865 866 localAcquisitionTime[4] = (unsigned char) ( fineTime >> 8 );
866 867 localAcquisitionTime[5] = (unsigned char) ( fineTime );
867 868
868 869 acquisitionTimeAsLong = ( (unsigned long long int) localAcquisitionTime[0] << 40 )
869 870 + ( (unsigned long long int) localAcquisitionTime[1] << 32 )
870 871 + ( (unsigned long long int) localAcquisitionTime[2] << 24 )
871 872 + ( (unsigned long long int) localAcquisitionTime[3] << 16 )
872 873 + ( (unsigned long long int) localAcquisitionTime[4] << 8 )
873 874 + ( (unsigned long long int) localAcquisitionTime[5] );
874 875
875 876 switch( sid )
876 877 {
877 878 case SID_NORM_SWF_F0:
878 879 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * 65536. / 24576. ;
879 880 break;
880 881
881 882 case SID_NORM_SWF_F1:
882 883 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * 65536. / 4096. ;
883 884 break;
884 885
885 886 case SID_NORM_SWF_F2:
886 887 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * 65536. / 256. ;
887 888 break;
888 889
889 890 case SID_SBM1_CWF_F1:
890 891 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 4096. ;
891 892 break;
892 893
893 894 case SID_SBM2_CWF_F2:
894 895 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 256. ;
895 896 break;
896 897
897 898 case SID_BURST_CWF_F2:
898 899 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 256. ;
899 900 break;
900 901
901 902 case SID_NORM_CWF_F3:
902 903 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF_SHORT_F3 * 65536. / 16. ;
903 904 break;
904 905
905 906 case SID_NORM_CWF_LONG_F3:
906 907 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 16. ;
907 908 break;
908 909
909 910 default:
910 911 PRINTF1("in compute_acquisition_time *** ERR unexpected sid %d\n", sid)
911 912 deltaT = 0.;
912 913 break;
913 914 }
914 915
915 916 acquisitionTimeAsLong = acquisitionTimeAsLong + (unsigned long long int) deltaT;
916 917 //
917 918 acquisitionTime[0] = (unsigned char) (acquisitionTimeAsLong >> 40);
918 919 acquisitionTime[1] = (unsigned char) (acquisitionTimeAsLong >> 32);
919 920 acquisitionTime[2] = (unsigned char) (acquisitionTimeAsLong >> 24);
920 921 acquisitionTime[3] = (unsigned char) (acquisitionTimeAsLong >> 16);
921 922 acquisitionTime[4] = (unsigned char) (acquisitionTimeAsLong >> 8 );
922 923 acquisitionTime[5] = (unsigned char) (acquisitionTimeAsLong );
923 924
924 925 }
925 926
926 927 void build_snapshot_from_ring( ring_node *ring_node_to_send, unsigned char frequencyChannel, unsigned long long int acquisitionTimeF0_asLong )
927 928 {
928 929 unsigned int i;
929 930 unsigned long long int centerTime_asLong;
930 931 unsigned long long int acquisitionTime_asLong;
931 932 unsigned long long int bufferAcquisitionTime_asLong;
932 933 unsigned char *ptr1;
933 934 unsigned char *ptr2;
934 935 unsigned char *timeCharPtr;
935 936 unsigned char nb_ring_nodes;
936 937 unsigned long long int frequency_asLong;
937 938 unsigned long long int nbTicksPerSample_asLong;
938 939 unsigned long long int nbSamplesPart1_asLong;
939 940 unsigned long long int sampleOffset_asLong;
940 941
941 942 unsigned int deltaT_F0;
942 943 unsigned int deltaT_F1;
943 944 unsigned long long int deltaT_F2;
944 945
945 946 deltaT_F0 = 2731; // (2048. / 24576. / 2.) * 65536. = 2730.667;
946 947 deltaT_F1 = 16384; // (2048. / 4096. / 2.) * 65536. = 16384;
947 948 deltaT_F2 = 262144; // (2048. / 256. / 2.) * 65536. = 262144;
948 949 sampleOffset_asLong = 0x00;
949 950
950 951 // (1) get the f0 acquisition time => the value is passed in argument
951 952
952 953 // (2) compute the central reference time
953 954 centerTime_asLong = acquisitionTimeF0_asLong + deltaT_F0;
954 955
955 956 // (3) compute the acquisition time of the current snapshot
956 957 switch(frequencyChannel)
957 958 {
958 959 case 1: // 1 is for F1 = 4096 Hz
959 960 acquisitionTime_asLong = centerTime_asLong - deltaT_F1;
960 961 nb_ring_nodes = NB_RING_NODES_F1;
961 962 frequency_asLong = 4096;
962 963 nbTicksPerSample_asLong = 16; // 65536 / 4096;
963 964 break;
964 965 case 2: // 2 is for F2 = 256 Hz
965 966 acquisitionTime_asLong = centerTime_asLong - deltaT_F2;
966 967 nb_ring_nodes = NB_RING_NODES_F2;
967 968 frequency_asLong = 256;
968 969 nbTicksPerSample_asLong = 256; // 65536 / 256;
969 970 break;
970 971 default:
971 972 acquisitionTime_asLong = centerTime_asLong;
972 973 frequency_asLong = 256;
973 974 nbTicksPerSample_asLong = 256;
974 975 break;
975 976 }
976 977
977 978 //****************************************************************************
978 979 // (4) search the ring_node with the acquisition time <= acquisitionTime_asLong
979 980 for (i=0; i<nb_ring_nodes; i++)
980 981 {
981 982 PRINTF1("%d ... ", i)
982 983 bufferAcquisitionTime_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send->coarseTime );
983 984 if (bufferAcquisitionTime_asLong <= acquisitionTime_asLong)
984 985 {
985 986 PRINTF1("buffer found with acquisition time = %llx\n", bufferAcquisitionTime_asLong)
986 987 break;
987 988 }
988 989 ring_node_to_send = ring_node_to_send->previous;
989 990 }
990 991
991 992 // (5) compute the number of samples to take in the current buffer
992 993 sampleOffset_asLong = ((acquisitionTime_asLong - bufferAcquisitionTime_asLong) * frequency_asLong ) >> 16;
993 994 nbSamplesPart1_asLong = NB_SAMPLES_PER_SNAPSHOT - sampleOffset_asLong;
994 995 PRINTF2("sampleOffset_asLong = %lld, nbSamplesPart1_asLong = %lld\n", sampleOffset_asLong, nbSamplesPart1_asLong)
995 996
996 997 // (6) compute the final acquisition time
997 998 acquisitionTime_asLong = bufferAcquisitionTime_asLong +
998 999 sampleOffset_asLong * nbTicksPerSample_asLong;
999 1000
1000 1001 // (7) copy the acquisition time at the beginning of the extrated snapshot
1001 1002 ptr1 = (unsigned char*) &acquisitionTime_asLong;
1002 1003 // fine time
1003 1004 ptr2 = (unsigned char*) &ring_node_wf_snap_extracted.fineTime;
1004 1005 ptr2[2] = ptr1[ 4 + 2 ];
1005 1006 ptr2[3] = ptr1[ 5 + 2 ];
1006 1007 // coarse time
1007 1008 ptr2 = (unsigned char*) &ring_node_wf_snap_extracted.coarseTime;
1008 1009 ptr2[0] = ptr1[ 0 + 2 ];
1009 1010 ptr2[1] = ptr1[ 1 + 2 ];
1010 1011 ptr2[2] = ptr1[ 2 + 2 ];
1011 1012 ptr2[3] = ptr1[ 3 + 2 ];
1012 1013
1013 1014 // re set the synchronization bit
1014 1015 timeCharPtr = (unsigned char*) &ring_node_to_send->coarseTime;
1015 1016 ptr2[0] = ptr2[0] | (timeCharPtr[0] & 0x80); // [1000 0000]
1016 1017
1017 1018 if ( (nbSamplesPart1_asLong >= NB_SAMPLES_PER_SNAPSHOT) | (nbSamplesPart1_asLong < 0) )
1018 1019 {
1019 1020 nbSamplesPart1_asLong = 0;
1020 1021 }
1021 1022 // copy the part 1 of the snapshot in the extracted buffer
1022 1023 for ( i = 0; i < (nbSamplesPart1_asLong * NB_WORDS_SWF_BLK); i++ )
1023 1024 {
1024 1025 wf_snap_extracted[i] =
1025 1026 ((int*) ring_node_to_send->buffer_address)[ i + (sampleOffset_asLong * NB_WORDS_SWF_BLK) ];
1026 1027 }
1027 1028 // copy the part 2 of the snapshot in the extracted buffer
1028 1029 ring_node_to_send = ring_node_to_send->next;
1029 1030 for ( i = (nbSamplesPart1_asLong * NB_WORDS_SWF_BLK); i < (NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK); i++ )
1030 1031 {
1031 1032 wf_snap_extracted[i] =
1032 1033 ((int*) ring_node_to_send->buffer_address)[ (i-(nbSamplesPart1_asLong * NB_WORDS_SWF_BLK)) ];
1033 1034 }
1034 1035 }
1035 1036
1036 1037 void snapshot_resynchronization( unsigned char *timePtr )
1037 1038 {
1038 1039 unsigned long long int acquisitionTime;
1039 1040 unsigned long long int centerTime;
1040 1041 unsigned long long int previousTick;
1041 1042 unsigned long long int nextTick;
1042 1043 unsigned long long int deltaPreviousTick;
1043 1044 unsigned long long int deltaNextTick;
1044 1045 unsigned int deltaTickInF2;
1045 1046 double deltaPrevious;
1046 1047 double deltaNext;
1047 1048
1048 1049 acquisitionTime = get_acquisition_time( timePtr );
1049 1050
1050 1051 // compute center time
1051 1052 centerTime = acquisitionTime + 2731; // (2048. / 24576. / 2.) * 65536. = 2730.667;
1052 1053 previousTick = centerTime - (centerTime & 0xffff);
1053 1054 nextTick = previousTick + 65536;
1054 1055
1055 1056 deltaPreviousTick = centerTime - previousTick;
1056 1057 deltaNextTick = nextTick - centerTime;
1057 1058
1058 1059 deltaPrevious = ((double) deltaPreviousTick) / 65536. * 1000.;
1059 1060 deltaNext = ((double) deltaNextTick) / 65536. * 1000.;
1060 1061
1061 1062 PRINTF2("delta previous = %f ms, delta next = %f ms\n", deltaPrevious, deltaNext)
1062 1063 PRINTF2("delta previous = %llu, delta next = %llu\n", deltaPreviousTick, deltaNextTick)
1063 1064
1064 1065 // which tick is the closest
1065 1066 if (deltaPreviousTick > deltaNextTick)
1066 1067 {
1067 1068 deltaTickInF2 = floor( (deltaNext * 256. / 1000.) ); // the division by 2 is important here
1068 1069 waveform_picker_regs->delta_snapshot = waveform_picker_regs->delta_snapshot + deltaTickInF2;
1069 printf("correction of = + %u\n", deltaTickInF2);
1070 PRINTF1("correction of = + %u\n", deltaTickInF2)
1070 1071 }
1071 1072 else
1072 1073 {
1073 1074 deltaTickInF2 = floor( (deltaPrevious * 256. / 1000.) ); // the division by 2 is important here
1074 1075 waveform_picker_regs->delta_snapshot = waveform_picker_regs->delta_snapshot - deltaTickInF2;
1075 printf("correction of = - %u\n", deltaTickInF2);
1076 PRINTF1("correction of = - %u\n", deltaTickInF2)
1076 1077 }
1077 1078 }
1078 1079
1079 1080 //**************
1080 1081 // wfp registers
1081 1082 void reset_wfp_burst_enable( void )
1082 1083 {
1083 1084 /** This function resets the waveform picker burst_enable register.
1084 1085 *
1085 1086 * The burst bits [f2 f1 f0] and the enable bits [f3 f2 f1 f0] are set to 0.
1086 1087 *
1087 1088 */
1088 1089
1089 1090 // [1000 000] burst f2, f1, f0 enable f3, f2, f1, f0
1090 1091 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable & 0x80;
1091 1092 }
1092 1093
1093 1094 void reset_wfp_status( void )
1094 1095 {
1095 1096 /** This function resets the waveform picker status register.
1096 1097 *
1097 1098 * All status bits are set to 0 [new_err full_err full].
1098 1099 *
1099 1100 */
1100 1101
1101 1102 waveform_picker_regs->status = 0xffff;
1102 1103 }
1103 1104
1104 1105 void reset_wfp_buffer_addresses( void )
1105 1106 {
1106 1107 // F0
1107 1108 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->previous->buffer_address; // 0x08
1108 1109 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address; // 0x0c
1109 1110 // F1
1110 1111 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->previous->buffer_address; // 0x10
1111 1112 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address; // 0x14
1112 1113 // F2
1113 1114 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->previous->buffer_address; // 0x18
1114 1115 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address; // 0x1c
1115 1116 // F3
1116 1117 waveform_picker_regs->addr_data_f3_0 = current_ring_node_f3->previous->buffer_address; // 0x20
1117 1118 waveform_picker_regs->addr_data_f3_1 = current_ring_node_f3->buffer_address; // 0x24
1118 1119 }
1119 1120
1120 1121 void reset_waveform_picker_regs( void )
1121 1122 {
1122 1123 /** This function resets the waveform picker module registers.
1123 1124 *
1124 1125 * The registers affected by this function are located at the following offset addresses:
1125 1126 * - 0x00 data_shaping
1126 1127 * - 0x04 run_burst_enable
1127 1128 * - 0x08 addr_data_f0
1128 1129 * - 0x0C addr_data_f1
1129 1130 * - 0x10 addr_data_f2
1130 1131 * - 0x14 addr_data_f3
1131 1132 * - 0x18 status
1132 1133 * - 0x1C delta_snapshot
1133 1134 * - 0x20 delta_f0
1134 1135 * - 0x24 delta_f0_2
1135 1136 * - 0x28 delta_f1
1136 1137 * - 0x2c delta_f2
1137 1138 * - 0x30 nb_data_by_buffer
1138 1139 * - 0x34 nb_snapshot_param
1139 1140 * - 0x38 start_date
1140 1141 * - 0x3c nb_word_in_buffer
1141 1142 *
1142 1143 */
1143 1144
1144 1145 set_wfp_data_shaping(); // 0x00 *** R1 R0 SP1 SP0 BW
1145 1146
1146 1147 reset_wfp_burst_enable(); // 0x04 *** [run *** burst f2, f1, f0 *** enable f3, f2, f1, f0 ]
1147 1148
1148 1149 reset_wfp_buffer_addresses();
1149 1150
1150 1151 reset_wfp_status(); // 0x18
1151 1152
1152 1153 set_wfp_delta_snapshot(); // 0x1c *** 300 s => 0x12bff
1153 1154
1154 1155 set_wfp_delta_f0_f0_2(); // 0x20, 0x24
1155 1156
1156 1157 set_wfp_delta_f1(); // 0x28
1157 1158
1158 1159 set_wfp_delta_f2(); // 0x2c
1159 1160
1160 1161 DEBUG_PRINTF1("delta_snapshot %x\n", waveform_picker_regs->delta_snapshot)
1161 1162 DEBUG_PRINTF1("delta_f0 %x\n", waveform_picker_regs->delta_f0)
1162 1163 DEBUG_PRINTF1("delta_f0_2 %x\n", waveform_picker_regs->delta_f0_2)
1163 1164 DEBUG_PRINTF1("delta_f1 %x\n", waveform_picker_regs->delta_f1)
1164 1165 DEBUG_PRINTF1("delta_f2 %x\n", waveform_picker_regs->delta_f2)
1165 1166 // 2688 = 8 * 336
1166 1167 waveform_picker_regs->nb_data_by_buffer = 0xa7f; // 0x30 *** 2688 - 1 => nb samples -1
1167 1168 waveform_picker_regs->snapshot_param = 0xa80; // 0x34 *** 2688 => nb samples
1168 1169 waveform_picker_regs->start_date = 0x7fffffff; // 0x38
1169 1170 //
1170 1171 // coarse time and fine time registers are not initialized, they are volatile
1171 1172 //
1172 1173 waveform_picker_regs->buffer_length = 0x1f8;// buffer length in burst = 3 * 2688 / 16 = 504 = 0x1f8
1173 1174 }
1174 1175
1175 1176 void set_wfp_data_shaping( void )
1176 1177 {
1177 1178 /** This function sets the data_shaping register of the waveform picker module.
1178 1179 *
1179 1180 * The value is read from one field of the parameter_dump_packet structure:\n
1180 1181 * bw_sp0_sp1_r0_r1
1181 1182 *
1182 1183 */
1183 1184
1184 1185 unsigned char data_shaping;
1185 1186
1186 1187 // get the parameters for the data shaping [BW SP0 SP1 R0 R1] in sy_lfr_common1 and configure the register
1187 1188 // waveform picker : [R1 R0 SP1 SP0 BW]
1188 1189
1189 1190 data_shaping = parameter_dump_packet.sy_lfr_common_parameters;
1190 1191
1191 1192 waveform_picker_regs->data_shaping =
1192 1193 ( (data_shaping & 0x20) >> 5 ) // BW
1193 1194 + ( (data_shaping & 0x10) >> 3 ) // SP0
1194 1195 + ( (data_shaping & 0x08) >> 1 ) // SP1
1195 1196 + ( (data_shaping & 0x04) << 1 ) // R0
1196 1197 + ( (data_shaping & 0x02) << 3 ) // R1
1197 1198 + ( (data_shaping & 0x01) << 5 ); // R2
1198 1199 }
1199 1200
1200 1201 void set_wfp_burst_enable_register( unsigned char mode )
1201 1202 {
1202 1203 /** This function sets the waveform picker burst_enable register depending on the mode.
1203 1204 *
1204 1205 * @param mode is the LFR mode to launch.
1205 1206 *
1206 1207 * The burst bits shall be before the enable bits.
1207 1208 *
1208 1209 */
1209 1210
1210 1211 // [0000 0000] burst f2, f1, f0 enable f3 f2 f1 f0
1211 1212 // the burst bits shall be set first, before the enable bits
1212 1213 switch(mode) {
1213 1214 case(LFR_MODE_NORMAL):
1214 1215 waveform_picker_regs->run_burst_enable = 0x00; // [0000 0000] no burst enable
1215 1216 waveform_picker_regs->run_burst_enable = 0x0f; // [0000 1111] enable f3 f2 f1 f0
1216 1217 break;
1217 1218 case(LFR_MODE_BURST):
1218 1219 waveform_picker_regs->run_burst_enable = 0x40; // [0100 0000] f2 burst enabled
1219 1220 // waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x04; // [0100] enable f2
1220 1221 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x0c; // [1100] enable f3 AND f2
1221 1222 break;
1222 1223 case(LFR_MODE_SBM1):
1223 1224 waveform_picker_regs->run_burst_enable = 0x20; // [0010 0000] f1 burst enabled
1224 1225 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x0f; // [1111] enable f3 f2 f1 f0
1225 1226 break;
1226 1227 case(LFR_MODE_SBM2):
1227 1228 waveform_picker_regs->run_burst_enable = 0x40; // [0100 0000] f2 burst enabled
1228 1229 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x0f; // [1111] enable f3 f2 f1 f0
1229 1230 break;
1230 1231 default:
1231 1232 waveform_picker_regs->run_burst_enable = 0x00; // [0000 0000] no burst enabled, no waveform enabled
1232 1233 break;
1233 1234 }
1234 1235 }
1235 1236
1236 1237 void set_wfp_delta_snapshot( void )
1237 1238 {
1238 1239 /** This function sets the delta_snapshot register of the waveform picker module.
1239 1240 *
1240 1241 * The value is read from two (unsigned char) of the parameter_dump_packet structure:
1241 1242 * - sy_lfr_n_swf_p[0]
1242 1243 * - sy_lfr_n_swf_p[1]
1243 1244 *
1244 1245 */
1245 1246
1246 1247 unsigned int delta_snapshot;
1247 1248 unsigned int delta_snapshot_in_T2;
1248 1249
1249 1250 delta_snapshot = parameter_dump_packet.sy_lfr_n_swf_p[0]*256
1250 1251 + parameter_dump_packet.sy_lfr_n_swf_p[1];
1251 1252
1252 1253 delta_snapshot_in_T2 = delta_snapshot * 256;
1253 1254 waveform_picker_regs->delta_snapshot = delta_snapshot_in_T2 - 1; // max 4 bytes
1254 1255 }
1255 1256
1256 1257 void set_wfp_delta_f0_f0_2( void )
1257 1258 {
1258 1259 unsigned int delta_snapshot;
1259 1260 unsigned int nb_samples_per_snapshot;
1260 1261 float delta_f0_in_float;
1261 1262
1262 1263 delta_snapshot = waveform_picker_regs->delta_snapshot;
1263 1264 nb_samples_per_snapshot = parameter_dump_packet.sy_lfr_n_swf_l[0] * 256 + parameter_dump_packet.sy_lfr_n_swf_l[1];
1264 1265 delta_f0_in_float =nb_samples_per_snapshot / 2. * ( 1. / 256. - 1. / 24576.) * 256.;
1265 1266
1266 1267 waveform_picker_regs->delta_f0 = delta_snapshot - floor( delta_f0_in_float );
1267 1268 waveform_picker_regs->delta_f0_2 = 0x30; // 48 = 11 0000, max 7 bits
1268 1269 }
1269 1270
1270 1271 void set_wfp_delta_f1( void )
1271 1272 {
1272 1273 unsigned int delta_snapshot;
1273 1274 unsigned int nb_samples_per_snapshot;
1274 1275 float delta_f1_in_float;
1275 1276
1276 1277 delta_snapshot = waveform_picker_regs->delta_snapshot;
1277 1278 nb_samples_per_snapshot = parameter_dump_packet.sy_lfr_n_swf_l[0] * 256 + parameter_dump_packet.sy_lfr_n_swf_l[1];
1278 1279 delta_f1_in_float = nb_samples_per_snapshot / 2. * ( 1. / 256. - 1. / 4096.) * 256.;
1279 1280
1280 1281 waveform_picker_regs->delta_f1 = delta_snapshot - floor( delta_f1_in_float );
1281 1282 }
1282 1283
1283 1284 void set_wfp_delta_f2()
1284 1285 {
1285 1286 unsigned int delta_snapshot;
1286 1287 unsigned int nb_samples_per_snapshot;
1287 1288
1288 1289 delta_snapshot = waveform_picker_regs->delta_snapshot;
1289 1290 nb_samples_per_snapshot = parameter_dump_packet.sy_lfr_n_swf_l[0] * 256 + parameter_dump_packet.sy_lfr_n_swf_l[1];
1290 1291
1291 1292 waveform_picker_regs->delta_f2 = delta_snapshot - nb_samples_per_snapshot / 2;
1292 1293 }
1293 1294
1294 1295 //*****************
1295 1296 // local parameters
1296 1297
1297 1298 void increment_seq_counter_source_id( unsigned char *packet_sequence_control, unsigned int sid )
1298 1299 {
1299 1300 /** This function increments the parameter "sequence_cnt" depending on the sid passed in argument.
1300 1301 *
1301 1302 * @param packet_sequence_control is a pointer toward the parameter sequence_cnt to update.
1302 1303 * @param sid is the source identifier of the packet being updated.
1303 1304 *
1304 1305 * REQ-LFR-SRS-5240 / SSS-CP-FS-590
1305 1306 * The sequence counters shall wrap around from 2^14 to zero.
1306 1307 * The sequence counter shall start at zero at startup.
1307 1308 *
1308 1309 * REQ-LFR-SRS-5239 / SSS-CP-FS-580
1309 1310 * All TM_LFR_SCIENCE_ packets are sent to ground, i.e. destination id = 0
1310 1311 *
1311 1312 */
1312 1313
1313 1314 unsigned short *sequence_cnt;
1314 1315 unsigned short segmentation_grouping_flag;
1315 1316 unsigned short new_packet_sequence_control;
1316 1317 rtems_mode initial_mode_set;
1317 1318 rtems_mode current_mode_set;
1318 1319 rtems_status_code status;
1319 1320
1320 1321 //******************************************
1321 1322 // CHANGE THE MODE OF THE CALLING RTEMS TASK
1322 1323 status = rtems_task_mode( RTEMS_NO_PREEMPT, RTEMS_PREEMPT_MASK, &initial_mode_set );
1323 1324
1324 1325 if ( (sid == SID_NORM_SWF_F0) || (sid == SID_NORM_SWF_F1) || (sid == SID_NORM_SWF_F2)
1325 1326 || (sid == SID_NORM_CWF_F3) || (sid == SID_NORM_CWF_LONG_F3)
1326 1327 || (sid == SID_BURST_CWF_F2)
1327 1328 || (sid == SID_NORM_ASM_F0) || (sid == SID_NORM_ASM_F1) || (sid == SID_NORM_ASM_F2)
1328 1329 || (sid == SID_NORM_BP1_F0) || (sid == SID_NORM_BP1_F1) || (sid == SID_NORM_BP1_F2)
1329 1330 || (sid == SID_NORM_BP2_F0) || (sid == SID_NORM_BP2_F1) || (sid == SID_NORM_BP2_F2)
1330 1331 || (sid == SID_BURST_BP1_F0) || (sid == SID_BURST_BP2_F0)
1331 1332 || (sid == SID_BURST_BP1_F1) || (sid == SID_BURST_BP2_F1) )
1332 1333 {
1333 1334 sequence_cnt = (unsigned short *) &sequenceCounters_SCIENCE_NORMAL_BURST;
1334 1335 }
1335 1336 else if ( (sid ==SID_SBM1_CWF_F1) || (sid ==SID_SBM2_CWF_F2)
1336 1337 || (sid == SID_SBM1_BP1_F0) || (sid == SID_SBM1_BP2_F0)
1337 1338 || (sid == SID_SBM2_BP1_F0) || (sid == SID_SBM2_BP2_F0)
1338 1339 || (sid == SID_SBM2_BP1_F1) || (sid == SID_SBM2_BP2_F1) )
1339 1340 {
1340 1341 sequence_cnt = (unsigned short *) &sequenceCounters_SCIENCE_SBM1_SBM2;
1341 1342 }
1342 1343 else
1343 1344 {
1344 1345 sequence_cnt = (unsigned short *) NULL;
1345 1346 PRINTF1("in increment_seq_counter_source_id *** ERR apid_destid %d not known\n", sid)
1346 1347 }
1347 1348
1348 1349 if (sequence_cnt != NULL)
1349 1350 {
1350 1351 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
1351 1352 *sequence_cnt = (*sequence_cnt) & 0x3fff;
1352 1353
1353 1354 new_packet_sequence_control = segmentation_grouping_flag | (*sequence_cnt) ;
1354 1355
1355 1356 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> 8);
1356 1357 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
1357 1358
1358 1359 // increment the sequence counter
1359 1360 if ( *sequence_cnt < SEQ_CNT_MAX)
1360 1361 {
1361 1362 *sequence_cnt = *sequence_cnt + 1;
1362 1363 }
1363 1364 else
1364 1365 {
1365 1366 *sequence_cnt = 0;
1366 1367 }
1367 1368 }
1368 1369
1369 1370 //***********************************
1370 1371 // RESET THE MODE OF THE CALLING TASK
1371 1372 status = rtems_task_mode( initial_mode_set, RTEMS_PREEMPT_MASK, &current_mode_set );
1372 1373 }
General Comments 0
You need to be logged in to leave comments. Login now