##// END OF EJS Templates
major change in fsw_processing.h to save cpu load
paul -
r325:b06a3a324978 R3_plus draft
parent child
Show More
@@ -1,380 +1,371
1 1 #ifndef FSW_PROCESSING_H_INCLUDED
2 2 #define FSW_PROCESSING_H_INCLUDED
3 3
4 4 #include <rtems.h>
5 5 #include <grspw.h>
6 6 #include <math.h>
7 7 #include <stdlib.h> // abs() is in the stdlib
8 8 #include <stdio.h>
9 9 #include <math.h>
10 10 #include <grlib_regs.h>
11 11
12 12 #include "fsw_params.h"
13 13
14 14 #define SBM_COEFF_PER_NORM_COEFF 2
15 15 #define MAX_SRC_DATA 780 // MAX size is 26 bins * 30 Bytes [TM_LFR_SCIENCE_BURST_BP2_F1]
16 16 #define MAX_SRC_DATA_WITH_SPARE 143 // 13 bins * 11 Bytes
17 17
18 18 #define NODE_0 0
19 19 #define NODE_1 1
20 20 #define NODE_2 2
21 21 #define NODE_3 3
22 22 #define NODE_4 4
23 23 #define NODE_5 5
24 24 #define NODE_6 6
25 25 #define NODE_7 7
26 26
27 27 typedef struct ring_node_asm
28 28 {
29 29 struct ring_node_asm *next;
30 30 float matrix[ TOTAL_SIZE_SM ];
31 31 unsigned int status;
32 32 } ring_node_asm;
33 33
34 34 typedef struct
35 35 {
36 36 unsigned char targetLogicalAddress;
37 37 unsigned char protocolIdentifier;
38 38 unsigned char reserved;
39 39 unsigned char userApplication;
40 40 unsigned char packetID[BYTES_PER_PACKETID];
41 41 unsigned char packetSequenceControl[BYTES_PER_SEQ_CTRL];
42 42 unsigned char packetLength[BYTES_PER_PKT_LEN];
43 43 // DATA FIELD HEADER
44 44 unsigned char spare1_pusVersion_spare2;
45 45 unsigned char serviceType;
46 46 unsigned char serviceSubType;
47 47 unsigned char destinationID;
48 48 unsigned char time[BYTES_PER_TIME];
49 49 // AUXILIARY HEADER
50 50 unsigned char sid;
51 51 unsigned char pa_bia_status_info;
52 52 unsigned char sy_lfr_common_parameters_spare;
53 53 unsigned char sy_lfr_common_parameters;
54 54 unsigned char acquisitionTime[BYTES_PER_TIME];
55 55 unsigned char pa_lfr_bp_blk_nr[BYTES_PER_BLKNR];
56 56 // SOURCE DATA
57 57 unsigned char data[ MAX_SRC_DATA ]; // MAX size is 26 bins * 30 Bytes [TM_LFR_SCIENCE_BURST_BP2_F1]
58 58 } bp_packet;
59 59
60 60 typedef struct
61 61 {
62 62 unsigned char targetLogicalAddress;
63 63 unsigned char protocolIdentifier;
64 64 unsigned char reserved;
65 65 unsigned char userApplication;
66 66 unsigned char packetID[BYTES_PER_PACKETID];
67 67 unsigned char packetSequenceControl[BYTES_PER_SEQ_CTRL];
68 68 unsigned char packetLength[BYTES_PER_PKT_LEN];
69 69 // DATA FIELD HEADER
70 70 unsigned char spare1_pusVersion_spare2;
71 71 unsigned char serviceType;
72 72 unsigned char serviceSubType;
73 73 unsigned char destinationID;
74 74 unsigned char time[BYTES_PER_TIME];
75 75 // AUXILIARY HEADER
76 76 unsigned char sid;
77 77 unsigned char pa_bia_status_info;
78 78 unsigned char sy_lfr_common_parameters_spare;
79 79 unsigned char sy_lfr_common_parameters;
80 80 unsigned char acquisitionTime[BYTES_PER_TIME];
81 81 unsigned char source_data_spare;
82 82 unsigned char pa_lfr_bp_blk_nr[BYTES_PER_BLKNR];
83 83 // SOURCE DATA
84 84 unsigned char data[ MAX_SRC_DATA_WITH_SPARE ]; // 13 bins * 11 Bytes
85 85 } bp_packet_with_spare; // only for TM_LFR_SCIENCE_NORMAL_BP1_F0 and F1
86 86
87 87 typedef struct asm_msg
88 88 {
89 89 ring_node_asm *norm;
90 90 ring_node_asm *burst_sbm;
91 91 rtems_event_set event;
92 92 unsigned int coarseTimeNORM;
93 93 unsigned int fineTimeNORM;
94 94 unsigned int coarseTimeSBM;
95 95 unsigned int fineTimeSBM;
96 96 unsigned int numberOfSMInASMNORM;
97 97 unsigned int numberOfSMInASMSBM;
98 98 } asm_msg;
99 99
100 100 extern unsigned char thisIsAnASMRestart;
101 101
102 102 extern volatile int sm_f0[ ];
103 103 extern volatile int sm_f1[ ];
104 104 extern volatile int sm_f2[ ];
105 105 extern unsigned int acquisitionDurations[];
106 106
107 107 // parameters
108 108 extern struct param_local_str param_local;
109 109 extern Packet_TM_LFR_PARAMETER_DUMP_t parameter_dump_packet;
110 110
111 111 // registers
112 112 extern time_management_regs_t *time_management_regs;
113 113 extern volatile spectral_matrix_regs_t *spectral_matrix_regs;
114 114
115 115 extern rtems_name misc_name[];
116 116 extern rtems_id Task_id[]; /* array of task ids */
117 117
118 118 ring_node * getRingNodeForAveraging( unsigned char frequencyChannel);
119 119 // ISR
120 120 rtems_isr spectral_matrices_isr( rtems_vector_number vector );
121 121
122 122 //******************
123 123 // Spectral Matrices
124 124 void reset_nb_sm( void );
125 125 // SM
126 126 void SM_init_rings( void );
127 127 void SM_reset_current_ring_nodes( void );
128 128 // ASM
129 129 void ASM_generic_init_ring(ring_node_asm *ring, unsigned char nbNodes );
130 130
131 131 //*****************
132 132 // Basic Parameters
133 133
134 134 void BP_reset_current_ring_nodes( void );
135 135 void BP_init_header(bp_packet *packet,
136 136 unsigned int apid, unsigned char sid,
137 137 unsigned int packetLength , unsigned char blkNr);
138 138 void BP_init_header_with_spare(bp_packet_with_spare *packet,
139 139 unsigned int apid, unsigned char sid,
140 140 unsigned int packetLength, unsigned char blkNr );
141 141 void BP_send( char *data,
142 142 rtems_id queue_id,
143 143 unsigned int nbBytesToSend , unsigned int sid );
144 144 void BP_send_s1_s2(char *data,
145 145 rtems_id queue_id,
146 146 unsigned int nbBytesToSend, unsigned int sid );
147 147
148 148 //******************
149 149 // general functions
150 150 void reset_sm_status( void );
151 151 void reset_spectral_matrix_regs( void );
152 152 void set_time(unsigned char *time, unsigned char *timeInBuffer );
153 153 unsigned long long int get_acquisition_time( unsigned char *timePtr );
154 154 unsigned char getSID( rtems_event_set event );
155 155
156 156 extern rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id );
157 157 extern rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id );
158 158
159 159 //***************************************
160 160 // DEFINITIONS OF STATIC INLINE FUNCTIONS
161 161 static inline void SM_average(float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
162 162 ring_node *ring_node_tab[],
163 163 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
164 164 asm_msg *msgForMATR , unsigned char channel);
165 165
166 166 void ASM_patch( float *inputASM, float *outputASM );
167 167
168 168 void extractReImVectors(float *inputASM, float *outputASM, unsigned int asmComponent );
169 169
170 170 static inline void ASM_reorganize_and_divide(float *averaged_spec_mat, float *averaged_spec_mat_reorganized,
171 171 float divider );
172 172
173 173 static inline void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat,
174 174 float divider,
175 175 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart);
176 176
177 177 static inline void ASM_convert(volatile float *input_matrix, char *output_matrix);
178 178
179 179 unsigned char acquisitionTimeIsValid(unsigned int coarseTime, unsigned int fineTime, unsigned char channel);
180 180
181 181 void SM_average( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
182 182 ring_node *ring_node_tab[],
183 183 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
184 184 asm_msg *msgForMATR, unsigned char channel )
185 185 {
186 186 float sum;
187 187 unsigned int i;
188 188 unsigned int k;
189 189 unsigned char incomingSMIsValid[NB_SM_BEFORE_AVF0_F1];
190 190 unsigned int numberOfValidSM;
191 191 unsigned char isValid;
192 192
193 193 //**************
194 194 // PAS FILTERING
195 195 // check acquisitionTime of the incoming data
196 196 numberOfValidSM = 0;
197 197 for (k=0; k<NB_SM_BEFORE_AVF0_F1; k++)
198 198 {
199 199 isValid = acquisitionTimeIsValid( ring_node_tab[k]->coarseTime, ring_node_tab[k]->fineTime, channel );
200 200 incomingSMIsValid[k] = isValid;
201 201 numberOfValidSM = numberOfValidSM + isValid;
202 202 }
203 203
204 204 //************************
205 205 // AVERAGE SPECTRAL MATRIX
206 206 for(i=0; i<TOTAL_SIZE_SM; i++)
207 207 {
208 // sum = ( (int *) (ring_node_tab[0]->buffer_address) ) [ i ]
209 // + ( (int *) (ring_node_tab[1]->buffer_address) ) [ i ]
210 // + ( (int *) (ring_node_tab[2]->buffer_address) ) [ i ]
211 // + ( (int *) (ring_node_tab[3]->buffer_address) ) [ i ]
212 // + ( (int *) (ring_node_tab[4]->buffer_address) ) [ i ]
213 // + ( (int *) (ring_node_tab[5]->buffer_address) ) [ i ]
214 // + ( (int *) (ring_node_tab[6]->buffer_address) ) [ i ]
215 // + ( (int *) (ring_node_tab[7]->buffer_address) ) [ i ];
216
217 sum = ( incomingSMIsValid[BYTE_0] * ((int *)(ring_node_tab[NODE_0]->buffer_address) )[ i ] )
218 + ( incomingSMIsValid[BYTE_1] * ((int *)(ring_node_tab[NODE_1]->buffer_address) )[ i ] )
219 + ( incomingSMIsValid[BYTE_2] * ((int *)(ring_node_tab[NODE_2]->buffer_address) )[ i ] )
220 + ( incomingSMIsValid[BYTE_3] * ((int *)(ring_node_tab[NODE_3]->buffer_address) )[ i ] )
221 + ( incomingSMIsValid[BYTE_4] * ((int *)(ring_node_tab[NODE_4]->buffer_address) )[ i ] )
222 + ( incomingSMIsValid[BYTE_5] * ((int *)(ring_node_tab[NODE_5]->buffer_address) )[ i ] )
223 + ( incomingSMIsValid[BYTE_6] * ((int *)(ring_node_tab[NODE_6]->buffer_address) )[ i ] )
224 + ( incomingSMIsValid[BYTE_7] * ((int *)(ring_node_tab[NODE_7]->buffer_address) )[ i ] );
208 sum = INIT_FLOAT;
209 for ( k = 0; k < NB_SM_BEFORE_AVF0_F1; k++ )
210 {
211 if (incomingSMIsValid[k] == 1)
212 {
213 sum = sum + ( (int *) (ring_node_tab[0]->buffer_address) ) [ i ] ;
214 }
215 }
225 216
226 217 if ( (nbAverageNORM == 0) && (nbAverageSBM == 0) )
227 218 {
228 219 averaged_spec_mat_NORM[ i ] = sum;
229 220 averaged_spec_mat_SBM[ i ] = sum;
230 221 msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime;
231 222 msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime;
232 223 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
233 224 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
234 225 }
235 226 else if ( (nbAverageNORM != 0) && (nbAverageSBM != 0) )
236 227 {
237 228 averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum );
238 229 averaged_spec_mat_SBM[ i ] = ( averaged_spec_mat_SBM[ i ] + sum );
239 230 }
240 231 else if ( (nbAverageNORM != 0) && (nbAverageSBM == 0) )
241 232 {
242 233 averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum );
243 234 averaged_spec_mat_SBM[ i ] = sum;
244 235 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
245 236 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
246 237 }
247 238 else
248 239 {
249 240 averaged_spec_mat_NORM[ i ] = sum;
250 241 averaged_spec_mat_SBM[ i ] = ( averaged_spec_mat_SBM[ i ] + sum );
251 242 msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime;
252 243 msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime;
253 244 // PRINTF2("ERR *** in SM_average *** unexpected parameters %d %d\n", nbAverageNORM, nbAverageSBM)
254 245 }
255 246 }
256 247
257 248 //*******************
258 249 // UPDATE SM COUNTERS
259 250 if ( (nbAverageNORM == 0) && (nbAverageSBM == 0) )
260 251 {
261 252 msgForMATR->numberOfSMInASMNORM = numberOfValidSM;
262 253 msgForMATR->numberOfSMInASMSBM = numberOfValidSM;
263 254 }
264 255 else if ( (nbAverageNORM != 0) && (nbAverageSBM != 0) )
265 256 {
266 257 msgForMATR->numberOfSMInASMNORM = msgForMATR->numberOfSMInASMNORM + numberOfValidSM;
267 258 msgForMATR->numberOfSMInASMSBM = msgForMATR->numberOfSMInASMSBM + numberOfValidSM;
268 259 }
269 260 else if ( (nbAverageNORM != 0) && (nbAverageSBM == 0) )
270 261 {
271 262 msgForMATR->numberOfSMInASMNORM = msgForMATR->numberOfSMInASMNORM + numberOfValidSM;
272 263 msgForMATR->numberOfSMInASMSBM = numberOfValidSM;
273 264 }
274 265 else
275 266 {
276 267 msgForMATR->numberOfSMInASMNORM = numberOfValidSM;
277 268 msgForMATR->numberOfSMInASMSBM = msgForMATR->numberOfSMInASMSBM + numberOfValidSM;
278 269 }
279 270 }
280 271
281 272 void ASM_reorganize_and_divide( float *averaged_spec_mat, float *averaged_spec_mat_reorganized, float divider )
282 273 {
283 274 int frequencyBin;
284 275 int asmComponent;
285 276 unsigned int offsetASM;
286 277 unsigned int offsetASMReorganized;
287 278
288 279 // BUILD DATA
289 280 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
290 281 {
291 282 for( frequencyBin = 0; frequencyBin < NB_BINS_PER_SM; frequencyBin++ )
292 283 {
293 284 offsetASMReorganized =
294 285 (frequencyBin * NB_VALUES_PER_SM)
295 286 + asmComponent;
296 287 offsetASM =
297 288 (asmComponent * NB_BINS_PER_SM)
298 289 + frequencyBin;
299 290 if ( divider != INIT_FLOAT )
300 291 {
301 292 averaged_spec_mat_reorganized[offsetASMReorganized ] = averaged_spec_mat[ offsetASM ] / divider;
302 293 }
303 294 else
304 295 {
305 296 averaged_spec_mat_reorganized[offsetASMReorganized ] = INIT_FLOAT;
306 297 }
307 298 }
308 299 }
309 300 }
310 301
311 302 void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
312 303 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage, unsigned char ASMIndexStart )
313 304 {
314 305 int frequencyBin;
315 306 int asmComponent;
316 307 int offsetASM;
317 308 int offsetCompressed;
318 309 int k;
319 310
320 311 // BUILD DATA
321 312 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
322 313 {
323 314 for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
324 315 {
325 316 offsetCompressed = // NO TIME OFFSET
326 317 (frequencyBin * NB_VALUES_PER_SM)
327 318 + asmComponent;
328 319 offsetASM = // NO TIME OFFSET
329 320 (asmComponent * NB_BINS_PER_SM)
330 321 + ASMIndexStart
331 322 + (frequencyBin * nbBinsToAverage);
332 323 compressed_spec_mat[ offsetCompressed ] = 0;
333 324 for ( k = 0; k < nbBinsToAverage; k++ )
334 325 {
335 326 compressed_spec_mat[offsetCompressed ] =
336 327 ( compressed_spec_mat[ offsetCompressed ]
337 328 + averaged_spec_mat[ offsetASM + k ] );
338 329 }
339 330 compressed_spec_mat[ offsetCompressed ] =
340 331 compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
341 332 }
342 333 }
343 334 }
344 335
345 336 void ASM_convert( volatile float *input_matrix, char *output_matrix)
346 337 {
347 338 unsigned int frequencyBin;
348 339 unsigned int asmComponent;
349 340 char * pt_char_input;
350 341 char * pt_char_output;
351 342 unsigned int offsetInput;
352 343 unsigned int offsetOutput;
353 344
354 345 pt_char_input = (char*) &input_matrix;
355 346 pt_char_output = (char*) &output_matrix;
356 347
357 348 // convert all other data
358 349 for( frequencyBin=0; frequencyBin<NB_BINS_PER_SM; frequencyBin++)
359 350 {
360 351 for ( asmComponent=0; asmComponent<NB_VALUES_PER_SM; asmComponent++)
361 352 {
362 353 offsetInput = (frequencyBin*NB_VALUES_PER_SM) + asmComponent ;
363 354 offsetOutput = SM_BYTES_PER_VAL * ( (frequencyBin*NB_VALUES_PER_SM) + asmComponent ) ;
364 355 pt_char_input = (char*) &input_matrix [ offsetInput ];
365 356 pt_char_output = (char*) &output_matrix[ offsetOutput ];
366 357 pt_char_output[0] = pt_char_input[0]; // bits 31 downto 24 of the float
367 358 pt_char_output[1] = pt_char_input[1]; // bits 23 downto 16 of the float
368 359 }
369 360 }
370 361 }
371 362
372 363 void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat,
373 364 float divider,
374 365 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart, unsigned char channel);
375 366
376 367 int getFBinMask(int k, unsigned char channel);
377 368
378 369 void init_kcoeff_sbm_from_kcoeff_norm( float *input_kcoeff, float *output_kcoeff, unsigned char nb_bins_norm);
379 370
380 371 #endif // FSW_PROCESSING_H_INCLUDED
@@ -1,108 +1,107
1 1 cmake_minimum_required (VERSION 2.6)
2 2 project (fsw)
3 3
4 4 include(sparc-rtems)
5 5 include(cppcheck)
6 6
7 7 include_directories("../header"
8 8 "../header/lfr_common_headers"
9 9 "../header/processing"
10 10 "../LFR_basic-parameters"
11 11 "../src")
12 12
13 13 set(SOURCES wf_handler.c
14 14 tc_handler.c
15 15 fsw_misc.c
16 16 fsw_init.c
17 17 fsw_globals.c
18 18 fsw_spacewire.c
19 19 tc_load_dump_parameters.c
20 20 tm_lfr_tc_exe.c
21 21 tc_acceptance.c
22 22 processing/fsw_processing.c
23 23 processing/avf0_prc0.c
24 24 processing/avf1_prc1.c
25 25 processing/avf2_prc2.c
26 26 lfr_cpu_usage_report.c
27 27 ${LFR_BP_SRC}
28 28 ../header/wf_handler.h
29 29 ../header/tc_handler.h
30 30 ../header/grlib_regs.h
31 31 ../header/fsw_misc.h
32 32 ../header/fsw_init.h
33 33 ../header/fsw_spacewire.h
34 34 ../header/tc_load_dump_parameters.h
35 35 ../header/tm_lfr_tc_exe.h
36 36 ../header/tc_acceptance.h
37 37 ../header/processing/fsw_processing.h
38 38 ../header/processing/avf0_prc0.h
39 39 ../header/processing/avf1_prc1.h
40 40 ../header/processing/avf2_prc2.h
41 41 ../header/fsw_params_wf_handler.h
42 42 ../header/lfr_cpu_usage_report.h
43 43 ../header/lfr_common_headers/ccsds_types.h
44 44 ../header/lfr_common_headers/fsw_params.h
45 45 ../header/lfr_common_headers/fsw_params_nb_bytes.h
46 46 ../header/lfr_common_headers/fsw_params_processing.h
47 47 ../header/lfr_common_headers/tm_byte_positions.h
48 48 ../LFR_basic-parameters/basic_parameters.h
49 49 ../LFR_basic-parameters/basic_parameters_params.h
50 50 ../header/GscMemoryLPP.hpp
51 51 )
52 52
53 53
54 54 option(FSW_verbose "Enable verbose LFR" ON)
55 55 option(FSW_boot_messages "Enable LFR boot messages" ON)
56 56 option(FSW_debug_messages "Enable LFR debug messages" ON)
57 57 option(FSW_cpu_usage_report "Enable LFR cpu usage report" OFF)
58 58 option(FSW_stack_report "Enable LFR stack report" OFF)
59 59 option(FSW_vhdl_dev "?" OFF)
60 60 option(FSW_lpp_dpu_destid "Set to debug at LPP" ON)
61 61 option(FSW_debug_watchdog "Enable debug watchdog" OFF)
62 62 option(FSW_debug_tch "?" OFF)
63 63
64 64 set(SW_VERSION_N1 "3" CACHE STRING "Choose N1 FSW Version." FORCE)
65 65 set(SW_VERSION_N2 "1" CACHE STRING "Choose N2 FSW Version." FORCE)
66 66 set(SW_VERSION_N3 "0" CACHE STRING "Choose N3 FSW Version." FORCE)
67 set(SW_VERSION_N4 "4" CACHE STRING "Choose N4 FSW Version." FORCE)
68
67 set(SW_VERSION_N4 "5" CACHE STRING "Choose N4 FSW Version." FORCE)
69 68
70 69 if(FSW_verbose)
71 70 add_definitions(-DPRINT_MESSAGES_ON_CONSOLE)
72 71 endif()
73 72 if(FSW_boot_messages)
74 73 add_definitions(-DBOOT_MESSAGES)
75 74 endif()
76 75 if(FSW_debug_messages)
77 76 add_definitions(-DDEBUG_MESSAGES)
78 77 endif()
79 78 if(FSW_cpu_usage_report)
80 79 add_definitions(-DPRINT_TASK_STATISTICS)
81 80 endif()
82 81 if(FSW_stack_report)
83 82 add_definitions(-DPRINT_STACK_REPORT)
84 83 endif()
85 84 if(FSW_vhdl_dev)
86 85 add_definitions(-DVHDL_DEV)
87 86 endif()
88 87 if(FSW_lpp_dpu_destid)
89 88 add_definitions(-DLPP_DPU_DESTID)
90 89 endif()
91 90 if(FSW_debug_watchdog)
92 91 add_definitions(-DDEBUG_WATCHDOG)
93 92 endif()
94 93 if(FSW_debug_tch)
95 94 add_definitions(-DDEBUG_TCH)
96 95 endif()
97 96
98 97 add_definitions(-DMSB_FIRST_TCH)
99 98
100 99 add_definitions(-DSWVERSION=-1-0)
101 100 add_definitions(-DSW_VERSION_N1=${SW_VERSION_N1})
102 101 add_definitions(-DSW_VERSION_N2=${SW_VERSION_N2})
103 102 add_definitions(-DSW_VERSION_N3=${SW_VERSION_N3})
104 103 add_definitions(-DSW_VERSION_N4=${SW_VERSION_N4})
105 104
106 105 add_executable(fsw ${SOURCES})
107 106 add_test_cppcheck(fsw STYLE UNUSED_FUNCTIONS POSSIBLE_ERROR MISSING_INCLUDE)
108 107
@@ -1,945 +1,945
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 // [spiq] [link] [spacewire_reset_link]
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
51 51 #ifdef CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
52 52 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GPTIMER
53 53 #endif
54 54
55 55 #ifdef CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
56 56 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_APBUART
57 57 #endif
58 58
59 59 #endif
60 60 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GRSPW /* GRSPW Driver */
61 61
62 62 #include <drvmgr/drvmgr_confdefs.h>
63 63 #endif
64 64
65 65 #include "fsw_init.h"
66 66 #include "fsw_config.c"
67 67 #include "GscMemoryLPP.hpp"
68 68
69 69 void initCache()
70 70 {
71 71 // ASI 2 contains a few control registers that have not been assigned as ancillary state registers.
72 72 // These should only be read and written using 32-bit LDA/STA instructions.
73 73 // All cache registers are accessed through load/store operations to the alternate address space (LDA/STA), using ASI = 2.
74 74 // The table below shows the register addresses:
75 75 // 0x00 Cache control register
76 76 // 0x04 Reserved
77 77 // 0x08 Instruction cache configuration register
78 78 // 0x0C Data cache configuration register
79 79
80 80 // Cache Control Register Leon3 / Leon3FT
81 81 // 31..30 29 28 27..24 23 22 21 20..19 18 17 16
82 82 // RFT PS TB DS FD FI FT ST IB
83 83 // 15 14 13..12 11..10 9..8 7..6 5 4 3..2 1..0
84 84 // IP DP ITE IDE DTE DDE DF IF DCS ICS
85 85
86 86 unsigned int cacheControlRegister;
87 87
88 88 CCR_resetCacheControlRegister();
89 89 ASR16_resetRegisterProtectionControlRegister();
90 90
91 91 cacheControlRegister = CCR_getValue();
92 92 PRINTF1("(0) CCR - Cache Control Register = %x\n", cacheControlRegister);
93 93 PRINTF1("(0) ASR16 = %x\n", *asr16Ptr);
94 94
95 95 CCR_enableInstructionCache(); // ICS bits
96 96 CCR_enableDataCache(); // DCS bits
97 97 CCR_enableInstructionBurstFetch(); // IB bit
98 98
99 99 faultTolerantScheme();
100 100
101 101 cacheControlRegister = CCR_getValue();
102 102 PRINTF1("(1) CCR - Cache Control Register = %x\n", cacheControlRegister);
103 103 PRINTF1("(1) ASR16 Register protection control register = %x\n", *asr16Ptr);
104 104
105 105 PRINTF("\n");
106 106 }
107 107
108 108 rtems_task Init( rtems_task_argument ignored )
109 109 {
110 110 /** This is the RTEMS INIT taks, it is the first task launched by the system.
111 111 *
112 112 * @param unused is the starting argument of the RTEMS task
113 113 *
114 114 * The INIT task create and run all other RTEMS tasks.
115 115 *
116 116 */
117 117
118 118 //***********
119 119 // INIT CACHE
120 120
121 121 unsigned char *vhdlVersion;
122 122
123 123 reset_lfr();
124 124
125 125 reset_local_time();
126 126
127 127 rtems_cpu_usage_reset();
128 128
129 129 rtems_status_code status;
130 130 rtems_status_code status_spw;
131 131 rtems_isr_entry old_isr_handler;
132 132
133 133 old_isr_handler = NULL;
134 134
135 135 // UART settings
136 136 enable_apbuart_transmitter();
137 137 set_apbuart_scaler_reload_register(REGS_ADDR_APBUART, APBUART_SCALER_RELOAD_VALUE);
138 138
139 139 DEBUG_PRINTF("\n\n\n\n\nIn INIT *** Now the console is on port COM1\n")
140 140
141 141
142 142 PRINTF("\n\n\n\n\n")
143 143
144 144 initCache();
145 145
146 146 PRINTF("*************************\n")
147 147 PRINTF("** LFR Flight Software **\n")
148 148
149 149 PRINTF1("** %d-", SW_VERSION_N1)
150 150 PRINTF1("%d-" , SW_VERSION_N2)
151 151 PRINTF1("%d-" , SW_VERSION_N3)
152 152 PRINTF1("%d **\n", SW_VERSION_N4)
153 153
154 154 vhdlVersion = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
155 155 PRINTF("** VHDL **\n")
156 PRINTF1("** %d.", vhdlVersion[1])
157 PRINTF1("%d." , vhdlVersion[2])
156 PRINTF1("** %d-", vhdlVersion[1])
157 PRINTF1("%d-" , vhdlVersion[2])
158 158 PRINTF1("%d **\n", vhdlVersion[3])
159 159 PRINTF("*************************\n")
160 160 PRINTF("\n\n")
161 161
162 162 init_parameter_dump();
163 163 init_kcoefficients_dump();
164 164 init_local_mode_parameters();
165 165 init_housekeeping_parameters();
166 166 init_k_coefficients_prc0();
167 167 init_k_coefficients_prc1();
168 168 init_k_coefficients_prc2();
169 169 pa_bia_status_info = INIT_CHAR;
170 170 cp_rpw_sc_rw_f_flags = INIT_CHAR;
171 171 cp_rpw_sc_rw1_f1 = INIT_FLOAT;
172 172 cp_rpw_sc_rw1_f2 = INIT_FLOAT;
173 173 cp_rpw_sc_rw2_f1 = INIT_FLOAT;
174 174 cp_rpw_sc_rw2_f2 = INIT_FLOAT;
175 175 cp_rpw_sc_rw3_f1 = INIT_FLOAT;
176 176 cp_rpw_sc_rw3_f2 = INIT_FLOAT;
177 177 cp_rpw_sc_rw4_f1 = INIT_FLOAT;
178 178 cp_rpw_sc_rw4_f2 = INIT_FLOAT;
179 179 // initialize filtering parameters
180 180 filterPar.spare_sy_lfr_pas_filter_enabled = DEFAULT_SY_LFR_PAS_FILTER_ENABLED;
181 181 filterPar.sy_lfr_pas_filter_modulus = DEFAULT_SY_LFR_PAS_FILTER_MODULUS;
182 182 filterPar.sy_lfr_pas_filter_tbad = DEFAULT_SY_LFR_PAS_FILTER_TBAD;
183 183 filterPar.sy_lfr_pas_filter_offset = DEFAULT_SY_LFR_PAS_FILTER_OFFSET;
184 184 filterPar.sy_lfr_pas_filter_shift = DEFAULT_SY_LFR_PAS_FILTER_SHIFT;
185 185 filterPar.sy_lfr_sc_rw_delta_f = DEFAULT_SY_LFR_SC_RW_DELTA_F;
186 186 update_last_valid_transition_date( DEFAULT_LAST_VALID_TRANSITION_DATE );
187 187
188 188 // waveform picker initialization
189 189 WFP_init_rings();
190 190 LEON_Clear_interrupt( IRQ_SPARC_GPTIMER_WATCHDOG ); // initialize the waveform rings
191 191 WFP_reset_current_ring_nodes();
192 192 reset_waveform_picker_regs();
193 193
194 194 // spectral matrices initialization
195 195 SM_init_rings(); // initialize spectral matrices rings
196 196 SM_reset_current_ring_nodes();
197 197 reset_spectral_matrix_regs();
198 198
199 199 // configure calibration
200 200 configureCalibration( false ); // true means interleaved mode, false is for normal mode
201 201
202 202 updateLFRCurrentMode( LFR_MODE_STANDBY );
203 203
204 204 BOOT_PRINTF1("in INIT *** lfrCurrentMode is %d\n", lfrCurrentMode)
205 205
206 206 create_names(); // create all names
207 207
208 208 status = create_timecode_timer(); // create the timer used by timecode_irq_handler
209 209 if (status != RTEMS_SUCCESSFUL)
210 210 {
211 211 PRINTF1("in INIT *** ERR in create_timer_timecode, code %d", status)
212 212 }
213 213
214 214 status = create_message_queues(); // create message queues
215 215 if (status != RTEMS_SUCCESSFUL)
216 216 {
217 217 PRINTF1("in INIT *** ERR in create_message_queues, code %d", status)
218 218 }
219 219
220 220 status = create_all_tasks(); // create all tasks
221 221 if (status != RTEMS_SUCCESSFUL)
222 222 {
223 223 PRINTF1("in INIT *** ERR in create_all_tasks, code %d\n", status)
224 224 }
225 225
226 226 // **************************
227 227 // <SPACEWIRE INITIALIZATION>
228 228 status_spw = spacewire_open_link(); // (1) open the link
229 229 if ( status_spw != RTEMS_SUCCESSFUL )
230 230 {
231 231 PRINTF1("in INIT *** ERR spacewire_open_link code %d\n", status_spw )
232 232 }
233 233
234 234 if ( status_spw == RTEMS_SUCCESSFUL ) // (2) configure the link
235 235 {
236 236 status_spw = spacewire_configure_link( fdSPW );
237 237 if ( status_spw != RTEMS_SUCCESSFUL )
238 238 {
239 239 PRINTF1("in INIT *** ERR spacewire_configure_link code %d\n", status_spw )
240 240 }
241 241 }
242 242
243 243 if ( status_spw == RTEMS_SUCCESSFUL) // (3) start the link
244 244 {
245 245 status_spw = spacewire_start_link( fdSPW );
246 246 if ( status_spw != RTEMS_SUCCESSFUL )
247 247 {
248 248 PRINTF1("in INIT *** ERR spacewire_start_link code %d\n", status_spw )
249 249 }
250 250 }
251 251 // </SPACEWIRE INITIALIZATION>
252 252 // ***************************
253 253
254 254 status = start_all_tasks(); // start all tasks
255 255 if (status != RTEMS_SUCCESSFUL)
256 256 {
257 257 PRINTF1("in INIT *** ERR in start_all_tasks, code %d", status)
258 258 }
259 259
260 260 // start RECV and SEND *AFTER* SpaceWire Initialization, due to the timeout of the start call during the initialization
261 261 status = start_recv_send_tasks();
262 262 if ( status != RTEMS_SUCCESSFUL )
263 263 {
264 264 PRINTF1("in INIT *** ERR start_recv_send_tasks code %d\n", status )
265 265 }
266 266
267 267 // suspend science tasks, they will be restarted later depending on the mode
268 268 status = suspend_science_tasks(); // suspend science tasks (not done in stop_current_mode if current mode = STANDBY)
269 269 if (status != RTEMS_SUCCESSFUL)
270 270 {
271 271 PRINTF1("in INIT *** in suspend_science_tasks *** ERR code: %d\n", status)
272 272 }
273 273
274 274 // configure IRQ handling for the waveform picker unit
275 275 status = rtems_interrupt_catch( waveforms_isr,
276 276 IRQ_SPARC_WAVEFORM_PICKER,
277 277 &old_isr_handler) ;
278 278 // configure IRQ handling for the spectral matrices unit
279 279 status = rtems_interrupt_catch( spectral_matrices_isr,
280 280 IRQ_SPARC_SPECTRAL_MATRIX,
281 281 &old_isr_handler) ;
282 282
283 283 // if the spacewire link is not up then send an event to the SPIQ task for link recovery
284 284 if ( status_spw != RTEMS_SUCCESSFUL )
285 285 {
286 286 status = rtems_event_send( Task_id[TASKID_SPIQ], SPW_LINKERR_EVENT );
287 287 if ( status != RTEMS_SUCCESSFUL ) {
288 288 PRINTF1("in INIT *** ERR rtems_event_send to SPIQ code %d\n", status )
289 289 }
290 290 }
291 291
292 292 BOOT_PRINTF("delete INIT\n")
293 293
294 294 set_hk_lfr_sc_potential_flag( true );
295 295
296 296 // start the timer to detect a missing spacewire timecode
297 297 // the timeout is larger because the spw IP needs to receive several valid timecodes before generating a tickout
298 298 // if a tickout is generated, the timer is restarted
299 299 status = rtems_timer_fire_after( timecode_timer_id, TIMECODE_TIMER_TIMEOUT_INIT, timecode_timer_routine, NULL );
300 300
301 301 grspw_timecode_callback = &timecode_irq_handler;
302 302
303 303 status = rtems_task_delete(RTEMS_SELF);
304 304
305 305 }
306 306
307 307 void init_local_mode_parameters( void )
308 308 {
309 309 /** This function initialize the param_local global variable with default values.
310 310 *
311 311 */
312 312
313 313 unsigned int i;
314 314
315 315 // LOCAL PARAMETERS
316 316
317 317 BOOT_PRINTF1("local_sbm1_nb_cwf_max %d \n", param_local.local_sbm1_nb_cwf_max)
318 318 BOOT_PRINTF1("local_sbm2_nb_cwf_max %d \n", param_local.local_sbm2_nb_cwf_max)
319 319
320 320 // init sequence counters
321 321
322 322 for(i = 0; i<SEQ_CNT_NB_DEST_ID; i++)
323 323 {
324 324 sequenceCounters_TC_EXE[i] = INIT_CHAR;
325 325 sequenceCounters_TM_DUMP[i] = INIT_CHAR;
326 326 }
327 327 sequenceCounters_SCIENCE_NORMAL_BURST = INIT_CHAR;
328 328 sequenceCounters_SCIENCE_SBM1_SBM2 = INIT_CHAR;
329 329 sequenceCounterHK = TM_PACKET_SEQ_CTRL_STANDALONE << TM_PACKET_SEQ_SHIFT;
330 330 }
331 331
332 332 void reset_local_time( void )
333 333 {
334 334 time_management_regs->ctrl = time_management_regs->ctrl | VAL_SOFTWARE_RESET; // [0010] software reset, coarse time = 0x80000000
335 335 }
336 336
337 337 void create_names( void ) // create all names for tasks and queues
338 338 {
339 339 /** This function creates all RTEMS names used in the software for tasks and queues.
340 340 *
341 341 * @return RTEMS directive status codes:
342 342 * - RTEMS_SUCCESSFUL - successful completion
343 343 *
344 344 */
345 345
346 346 // task names
347 347 Task_name[TASKID_RECV] = rtems_build_name( 'R', 'E', 'C', 'V' );
348 348 Task_name[TASKID_ACTN] = rtems_build_name( 'A', 'C', 'T', 'N' );
349 349 Task_name[TASKID_SPIQ] = rtems_build_name( 'S', 'P', 'I', 'Q' );
350 350 Task_name[TASKID_LOAD] = rtems_build_name( 'L', 'O', 'A', 'D' );
351 351 Task_name[TASKID_AVF0] = rtems_build_name( 'A', 'V', 'F', '0' );
352 352 Task_name[TASKID_SWBD] = rtems_build_name( 'S', 'W', 'B', 'D' );
353 353 Task_name[TASKID_WFRM] = rtems_build_name( 'W', 'F', 'R', 'M' );
354 354 Task_name[TASKID_DUMB] = rtems_build_name( 'D', 'U', 'M', 'B' );
355 355 Task_name[TASKID_HOUS] = rtems_build_name( 'H', 'O', 'U', 'S' );
356 356 Task_name[TASKID_PRC0] = rtems_build_name( 'P', 'R', 'C', '0' );
357 357 Task_name[TASKID_CWF3] = rtems_build_name( 'C', 'W', 'F', '3' );
358 358 Task_name[TASKID_CWF2] = rtems_build_name( 'C', 'W', 'F', '2' );
359 359 Task_name[TASKID_CWF1] = rtems_build_name( 'C', 'W', 'F', '1' );
360 360 Task_name[TASKID_SEND] = rtems_build_name( 'S', 'E', 'N', 'D' );
361 361 Task_name[TASKID_LINK] = rtems_build_name( 'L', 'I', 'N', 'K' );
362 362 Task_name[TASKID_AVF1] = rtems_build_name( 'A', 'V', 'F', '1' );
363 363 Task_name[TASKID_PRC1] = rtems_build_name( 'P', 'R', 'C', '1' );
364 364 Task_name[TASKID_AVF2] = rtems_build_name( 'A', 'V', 'F', '2' );
365 365 Task_name[TASKID_PRC2] = rtems_build_name( 'P', 'R', 'C', '2' );
366 366
367 367 // rate monotonic period names
368 368 name_hk_rate_monotonic = rtems_build_name( 'H', 'O', 'U', 'S' );
369 369
370 370 misc_name[QUEUE_RECV] = rtems_build_name( 'Q', '_', 'R', 'V' );
371 371 misc_name[QUEUE_SEND] = rtems_build_name( 'Q', '_', 'S', 'D' );
372 372 misc_name[QUEUE_PRC0] = rtems_build_name( 'Q', '_', 'P', '0' );
373 373 misc_name[QUEUE_PRC1] = rtems_build_name( 'Q', '_', 'P', '1' );
374 374 misc_name[QUEUE_PRC2] = rtems_build_name( 'Q', '_', 'P', '2' );
375 375
376 376 timecode_timer_name = rtems_build_name( 'S', 'P', 'T', 'C' );
377 377 }
378 378
379 379 int create_all_tasks( void ) // create all tasks which run in the software
380 380 {
381 381 /** This function creates all RTEMS tasks used in the software.
382 382 *
383 383 * @return RTEMS directive status codes:
384 384 * - RTEMS_SUCCESSFUL - task created successfully
385 385 * - RTEMS_INVALID_ADDRESS - id is NULL
386 386 * - RTEMS_INVALID_NAME - invalid task name
387 387 * - RTEMS_INVALID_PRIORITY - invalid task priority
388 388 * - RTEMS_MP_NOT_CONFIGURED - multiprocessing not configured
389 389 * - RTEMS_TOO_MANY - too many tasks created
390 390 * - RTEMS_UNSATISFIED - not enough memory for stack/FP context
391 391 * - RTEMS_TOO_MANY - too many global objects
392 392 *
393 393 */
394 394
395 395 rtems_status_code status;
396 396
397 397 //**********
398 398 // SPACEWIRE
399 399 // RECV
400 400 status = rtems_task_create(
401 401 Task_name[TASKID_RECV], TASK_PRIORITY_RECV, RTEMS_MINIMUM_STACK_SIZE,
402 402 RTEMS_DEFAULT_MODES,
403 403 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_RECV]
404 404 );
405 405 if (status == RTEMS_SUCCESSFUL) // SEND
406 406 {
407 407 status = rtems_task_create(
408 408 Task_name[TASKID_SEND], TASK_PRIORITY_SEND, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
409 409 RTEMS_DEFAULT_MODES,
410 410 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SEND]
411 411 );
412 412 }