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

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

major change in fsw_processing.h to save cpu load
paul -
r325:b06a3a324978 R3_plus draft
parent child
Show More
Show file before | Show file after | Hide comments
@@ -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
Show file before | Show file after | Hide comments
@@ -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
Show file before | Show file after | Hide comments
@@ -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 }
413 413 if (status == RTEMS_SUCCESSFUL) // LINK
414 414 {
415 415 status = rtems_task_create(
416 416 Task_name[TASKID_LINK], TASK_PRIORITY_LINK, RTEMS_MINIMUM_STACK_SIZE,
417 417 RTEMS_DEFAULT_MODES,
418 418 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_LINK]
419 419 );
420 420 }
421 421 if (status == RTEMS_SUCCESSFUL) // ACTN
422 422 {
423 423 status = rtems_task_create(
424 424 Task_name[TASKID_ACTN], TASK_PRIORITY_ACTN, RTEMS_MINIMUM_STACK_SIZE,
425 425 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
426 426 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_ACTN]
427 427 );
428 428 }
429 429 if (status == RTEMS_SUCCESSFUL) // SPIQ
430 430 {
431 431 status = rtems_task_create(
432 432 Task_name[TASKID_SPIQ], TASK_PRIORITY_SPIQ, RTEMS_MINIMUM_STACK_SIZE,
433 433 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
434 434 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_SPIQ]
435 435 );
436 436 }
437 437
438 438 //******************
439 439 // SPECTRAL MATRICES
440 440 if (status == RTEMS_SUCCESSFUL) // AVF0
441 441 {
442 442 status = rtems_task_create(
443 443 Task_name[TASKID_AVF0], TASK_PRIORITY_AVF0, RTEMS_MINIMUM_STACK_SIZE,
444 444 RTEMS_DEFAULT_MODES,
445 445 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF0]
446 446 );
447 447 }
448 448 if (status == RTEMS_SUCCESSFUL) // PRC0
449 449 {
450 450 status = rtems_task_create(
451 451 Task_name[TASKID_PRC0], TASK_PRIORITY_PRC0, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
452 452 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
453 453 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC0]
454 454 );
455 455 }
456 456 if (status == RTEMS_SUCCESSFUL) // AVF1
457 457 {
458 458 status = rtems_task_create(
459 459 Task_name[TASKID_AVF1], TASK_PRIORITY_AVF1, RTEMS_MINIMUM_STACK_SIZE,
460 460 RTEMS_DEFAULT_MODES,
461 461 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF1]
462 462 );
463 463 }
464 464 if (status == RTEMS_SUCCESSFUL) // PRC1
465 465 {
466 466 status = rtems_task_create(
467 467 Task_name[TASKID_PRC1], TASK_PRIORITY_PRC1, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
468 468 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
469 469 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC1]
470 470 );
471 471 }
472 472 if (status == RTEMS_SUCCESSFUL) // AVF2
473 473 {
474 474 status = rtems_task_create(
475 475 Task_name[TASKID_AVF2], TASK_PRIORITY_AVF2, RTEMS_MINIMUM_STACK_SIZE,
476 476 RTEMS_DEFAULT_MODES,
477 477 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF2]
478 478 );
479 479 }
480 480 if (status == RTEMS_SUCCESSFUL) // PRC2
481 481 {
482 482 status = rtems_task_create(
483 483 Task_name[TASKID_PRC2], TASK_PRIORITY_PRC2, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
484 484 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
485 485 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC2]
486 486 );
487 487 }
488 488
489 489 //****************
490 490 // WAVEFORM PICKER
491 491 if (status == RTEMS_SUCCESSFUL) // WFRM
492 492 {
493 493 status = rtems_task_create(
494 494 Task_name[TASKID_WFRM], TASK_PRIORITY_WFRM, RTEMS_MINIMUM_STACK_SIZE,
495 495 RTEMS_DEFAULT_MODES,
496 496 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_WFRM]
497 497 );
498 498 }
499 499 if (status == RTEMS_SUCCESSFUL) // CWF3
500 500 {
501 501 status = rtems_task_create(
502 502 Task_name[TASKID_CWF3], TASK_PRIORITY_CWF3, RTEMS_MINIMUM_STACK_SIZE,
503 503 RTEMS_DEFAULT_MODES,
504 504 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF3]
505 505 );
506 506 }
507 507 if (status == RTEMS_SUCCESSFUL) // CWF2
508 508 {
509 509 status = rtems_task_create(
510 510 Task_name[TASKID_CWF2], TASK_PRIORITY_CWF2, RTEMS_MINIMUM_STACK_SIZE,
511 511 RTEMS_DEFAULT_MODES,
512 512 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF2]
513 513 );
514 514 }
515 515 if (status == RTEMS_SUCCESSFUL) // CWF1
516 516 {
517 517 status = rtems_task_create(
518 518 Task_name[TASKID_CWF1], TASK_PRIORITY_CWF1, RTEMS_MINIMUM_STACK_SIZE,
519 519 RTEMS_DEFAULT_MODES,
520 520 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF1]
521 521 );
522 522 }
523 523 if (status == RTEMS_SUCCESSFUL) // SWBD
524 524 {
525 525 status = rtems_task_create(
526 526 Task_name[TASKID_SWBD], TASK_PRIORITY_SWBD, RTEMS_MINIMUM_STACK_SIZE,
527 527 RTEMS_DEFAULT_MODES,
528 528 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SWBD]
529 529 );
530 530 }
531 531
532 532 //*****
533 533 // MISC
534 534 if (status == RTEMS_SUCCESSFUL) // LOAD
535 535 {
536 536 status = rtems_task_create(
537 537 Task_name[TASKID_LOAD], TASK_PRIORITY_LOAD, RTEMS_MINIMUM_STACK_SIZE,
538 538 RTEMS_DEFAULT_MODES,
539 539 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_LOAD]
540 540 );
541 541 }
542 542 if (status == RTEMS_SUCCESSFUL) // DUMB
543 543 {
544 544 status = rtems_task_create(
545 545 Task_name[TASKID_DUMB], TASK_PRIORITY_DUMB, RTEMS_MINIMUM_STACK_SIZE,
546 546 RTEMS_DEFAULT_MODES,
547 547 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_DUMB]
548 548 );
549 549 }
550 550 if (status == RTEMS_SUCCESSFUL) // HOUS
551 551 {
552 552 status = rtems_task_create(
553 553 Task_name[TASKID_HOUS], TASK_PRIORITY_HOUS, RTEMS_MINIMUM_STACK_SIZE,
554 554 RTEMS_DEFAULT_MODES,
555 555 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_HOUS]
556 556 );
557 557 }
558 558
559 559 return status;
560 560 }
561 561
562 562 int start_recv_send_tasks( void )
563 563 {
564 564 rtems_status_code status;
565 565
566 566 status = rtems_task_start( Task_id[TASKID_RECV], recv_task, 1 );
567 567 if (status!=RTEMS_SUCCESSFUL) {
568 568 BOOT_PRINTF("in INIT *** Error starting TASK_RECV\n")
569 569 }
570 570
571 571 if (status == RTEMS_SUCCESSFUL) // SEND
572 572 {
573 573 status = rtems_task_start( Task_id[TASKID_SEND], send_task, 1 );
574 574 if (status!=RTEMS_SUCCESSFUL) {
575 575 BOOT_PRINTF("in INIT *** Error starting TASK_SEND\n")
576 576 }
577 577 }
578 578
579 579 return status;
580 580 }
581 581
582 582 int start_all_tasks( void ) // start all tasks except SEND RECV and HOUS
583 583 {
584 584 /** This function starts all RTEMS tasks used in the software.
585 585 *
586 586 * @return RTEMS directive status codes:
587 587 * - RTEMS_SUCCESSFUL - ask started successfully
588 588 * - RTEMS_INVALID_ADDRESS - invalid task entry point
589 589 * - RTEMS_INVALID_ID - invalid task id
590 590 * - RTEMS_INCORRECT_STATE - task not in the dormant state
591 591 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot start remote task
592 592 *
593 593 */
594 594 // starts all the tasks fot eh flight software
595 595
596 596 rtems_status_code status;
597 597
598 598 //**********
599 599 // SPACEWIRE
600 600 status = rtems_task_start( Task_id[TASKID_SPIQ], spiq_task, 1 );
601 601 if (status!=RTEMS_SUCCESSFUL) {
602 602 BOOT_PRINTF("in INIT *** Error starting TASK_SPIQ\n")
603 603 }
604 604
605 605 if (status == RTEMS_SUCCESSFUL) // LINK
606 606 {
607 607 status = rtems_task_start( Task_id[TASKID_LINK], link_task, 1 );
608 608 if (status!=RTEMS_SUCCESSFUL) {
609 609 BOOT_PRINTF("in INIT *** Error starting TASK_LINK\n")
610 610 }
611 611 }
612 612
613 613 if (status == RTEMS_SUCCESSFUL) // ACTN
614 614 {
615 615 status = rtems_task_start( Task_id[TASKID_ACTN], actn_task, 1 );
616 616 if (status!=RTEMS_SUCCESSFUL) {
617 617 BOOT_PRINTF("in INIT *** Error starting TASK_ACTN\n")
618 618 }
619 619 }
620 620
621 621 //******************
622 622 // SPECTRAL MATRICES
623 623 if (status == RTEMS_SUCCESSFUL) // AVF0
624 624 {
625 625 status = rtems_task_start( Task_id[TASKID_AVF0], avf0_task, LFR_MODE_STANDBY );
626 626 if (status!=RTEMS_SUCCESSFUL) {
627 627 BOOT_PRINTF("in INIT *** Error starting TASK_AVF0\n")
628 628 }
629 629 }
630 630 if (status == RTEMS_SUCCESSFUL) // PRC0
631 631 {
632 632 status = rtems_task_start( Task_id[TASKID_PRC0], prc0_task, LFR_MODE_STANDBY );
633 633 if (status!=RTEMS_SUCCESSFUL) {
634 634 BOOT_PRINTF("in INIT *** Error starting TASK_PRC0\n")
635 635 }
636 636 }
637 637 if (status == RTEMS_SUCCESSFUL) // AVF1
638 638 {
639 639 status = rtems_task_start( Task_id[TASKID_AVF1], avf1_task, LFR_MODE_STANDBY );
640 640 if (status!=RTEMS_SUCCESSFUL) {
641 641 BOOT_PRINTF("in INIT *** Error starting TASK_AVF1\n")
642 642 }
643 643 }
644 644 if (status == RTEMS_SUCCESSFUL) // PRC1
645 645 {
646 646 status = rtems_task_start( Task_id[TASKID_PRC1], prc1_task, LFR_MODE_STANDBY );
647 647 if (status!=RTEMS_SUCCESSFUL) {
648 648 BOOT_PRINTF("in INIT *** Error starting TASK_PRC1\n")
649 649 }
650 650 }
651 651 if (status == RTEMS_SUCCESSFUL) // AVF2
652 652 {
653 653 status = rtems_task_start( Task_id[TASKID_AVF2], avf2_task, 1 );
654 654 if (status!=RTEMS_SUCCESSFUL) {
655 655 BOOT_PRINTF("in INIT *** Error starting TASK_AVF2\n")
656 656 }
657 657 }
658 658 if (status == RTEMS_SUCCESSFUL) // PRC2
659 659 {
660 660 status = rtems_task_start( Task_id[TASKID_PRC2], prc2_task, 1 );
661 661 if (status!=RTEMS_SUCCESSFUL) {
662 662 BOOT_PRINTF("in INIT *** Error starting TASK_PRC2\n")
663 663 }
664 664 }
665 665
666 666 //****************
667 667 // WAVEFORM PICKER
668 668 if (status == RTEMS_SUCCESSFUL) // WFRM
669 669 {
670 670 status = rtems_task_start( Task_id[TASKID_WFRM], wfrm_task, 1 );
671 671 if (status!=RTEMS_SUCCESSFUL) {
672 672 BOOT_PRINTF("in INIT *** Error starting TASK_WFRM\n")
673 673 }
674 674 }
675 675 if (status == RTEMS_SUCCESSFUL) // CWF3
676 676 {
677 677 status = rtems_task_start( Task_id[TASKID_CWF3], cwf3_task, 1 );
678 678 if (status!=RTEMS_SUCCESSFUL) {
679 679 BOOT_PRINTF("in INIT *** Error starting TASK_CWF3\n")
680 680 }
681 681 }
682 682 if (status == RTEMS_SUCCESSFUL) // CWF2
683 683 {
684 684 status = rtems_task_start( Task_id[TASKID_CWF2], cwf2_task, 1 );
685 685 if (status!=RTEMS_SUCCESSFUL) {
686 686 BOOT_PRINTF("in INIT *** Error starting TASK_CWF2\n")
687 687 }
688 688 }
689 689 if (status == RTEMS_SUCCESSFUL) // CWF1
690 690 {
691 691 status = rtems_task_start( Task_id[TASKID_CWF1], cwf1_task, 1 );
692 692 if (status!=RTEMS_SUCCESSFUL) {
693 693 BOOT_PRINTF("in INIT *** Error starting TASK_CWF1\n")
694 694 }
695 695 }
696 696 if (status == RTEMS_SUCCESSFUL) // SWBD
697 697 {
698 698 status = rtems_task_start( Task_id[TASKID_SWBD], swbd_task, 1 );
699 699 if (status!=RTEMS_SUCCESSFUL) {
700 700 BOOT_PRINTF("in INIT *** Error starting TASK_SWBD\n")
701 701 }
702 702 }
703 703
704 704 //*****
705 705 // MISC
706 706 if (status == RTEMS_SUCCESSFUL) // HOUS
707 707 {
708 708 status = rtems_task_start( Task_id[TASKID_HOUS], hous_task, 1 );
709 709 if (status!=RTEMS_SUCCESSFUL) {
710 710 BOOT_PRINTF("in INIT *** Error starting TASK_HOUS\n")
711 711 }
712 712 }
713 713 if (status == RTEMS_SUCCESSFUL) // DUMB
714 714 {
715 715 status = rtems_task_start( Task_id[TASKID_DUMB], dumb_task, 1 );
716 716 if (status!=RTEMS_SUCCESSFUL) {
717 717 BOOT_PRINTF("in INIT *** Error starting TASK_DUMB\n")
718 718 }
719 719 }
720 720 if (status == RTEMS_SUCCESSFUL) // LOAD
721 721 {
722 722 status = rtems_task_start( Task_id[TASKID_LOAD], load_task, 1 );
723 723 if (status!=RTEMS_SUCCESSFUL) {
724 724 BOOT_PRINTF("in INIT *** Error starting TASK_LOAD\n")
725 725 }
726 726 }
727 727
728 728 return status;
729 729 }
730 730
731 731 rtems_status_code create_message_queues( void ) // create the two message queues used in the software
732 732 {
733 733 rtems_status_code status_recv;
734 734 rtems_status_code status_send;
735 735 rtems_status_code status_q_p0;
736 736 rtems_status_code status_q_p1;
737 737 rtems_status_code status_q_p2;
738 738 rtems_status_code ret;
739 739 rtems_id queue_id;
740 740
741 741 ret = RTEMS_SUCCESSFUL;
742 742 queue_id = RTEMS_ID_NONE;
743 743
744 744 //****************************************
745 745 // create the queue for handling valid TCs
746 746 status_recv = rtems_message_queue_create( misc_name[QUEUE_RECV],
747 747 MSG_QUEUE_COUNT_RECV, CCSDS_TC_PKT_MAX_SIZE,
748 748 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
749 749 if ( status_recv != RTEMS_SUCCESSFUL ) {
750 750 PRINTF1("in create_message_queues *** ERR creating QUEU queue, %d\n", status_recv)
751 751 }
752 752
753 753 //************************************************
754 754 // create the queue for handling TM packet sending
755 755 status_send = rtems_message_queue_create( misc_name[QUEUE_SEND],
756 756 MSG_QUEUE_COUNT_SEND, MSG_QUEUE_SIZE_SEND,
757 757 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
758 758 if ( status_send != RTEMS_SUCCESSFUL ) {
759 759 PRINTF1("in create_message_queues *** ERR creating PKTS queue, %d\n", status_send)
760 760 }
761 761
762 762 //*****************************************************************************
763 763 // create the queue for handling averaged spectral matrices for processing @ f0
764 764 status_q_p0 = rtems_message_queue_create( misc_name[QUEUE_PRC0],
765 765 MSG_QUEUE_COUNT_PRC0, MSG_QUEUE_SIZE_PRC0,
766 766 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
767 767 if ( status_q_p0 != RTEMS_SUCCESSFUL ) {
768 768 PRINTF1("in create_message_queues *** ERR creating Q_P0 queue, %d\n", status_q_p0)
769 769 }
770 770
771 771 //*****************************************************************************
772 772 // create the queue for handling averaged spectral matrices for processing @ f1
773 773 status_q_p1 = rtems_message_queue_create( misc_name[QUEUE_PRC1],
774 774 MSG_QUEUE_COUNT_PRC1, MSG_QUEUE_SIZE_PRC1,
775 775 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
776 776 if ( status_q_p1 != RTEMS_SUCCESSFUL ) {
777 777 PRINTF1("in create_message_queues *** ERR creating Q_P1 queue, %d\n", status_q_p1)
778 778 }
779 779
780 780 //*****************************************************************************
781 781 // create the queue for handling averaged spectral matrices for processing @ f2
782 782 status_q_p2 = rtems_message_queue_create( misc_name[QUEUE_PRC2],
783 783 MSG_QUEUE_COUNT_PRC2, MSG_QUEUE_SIZE_PRC2,
784 784 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
785 785 if ( status_q_p2 != RTEMS_SUCCESSFUL ) {
786 786 PRINTF1("in create_message_queues *** ERR creating Q_P2 queue, %d\n", status_q_p2)
787 787 }
788 788
789 789 if ( status_recv != RTEMS_SUCCESSFUL )
790 790 {
791 791 ret = status_recv;
792 792 }
793 793 else if( status_send != RTEMS_SUCCESSFUL )
794 794 {
795 795 ret = status_send;
796 796 }
797 797 else if( status_q_p0 != RTEMS_SUCCESSFUL )
798 798 {
799 799 ret = status_q_p0;
800 800 }
801 801 else if( status_q_p1 != RTEMS_SUCCESSFUL )
802 802 {
803 803 ret = status_q_p1;
804 804 }
805 805 else
806 806 {
807 807 ret = status_q_p2;
808 808 }
809 809
810 810 return ret;
811 811 }
812 812
813 813 rtems_status_code create_timecode_timer( void )
814 814 {
815 815 rtems_status_code status;
816 816
817 817 status = rtems_timer_create( timecode_timer_name, &timecode_timer_id );
818 818
819 819 if ( status != RTEMS_SUCCESSFUL )
820 820 {
821 821 PRINTF1("in create_timer_timecode *** ERR creating SPTC timer, %d\n", status)
822 822 }
823 823 else
824 824 {
825 825 PRINTF("in create_timer_timecode *** OK creating SPTC timer\n")
826 826 }
827 827
828 828 return status;
829 829 }
830 830
831 831 rtems_status_code get_message_queue_id_send( rtems_id *queue_id )
832 832 {
833 833 rtems_status_code status;
834 834 rtems_name queue_name;
835 835
836 836 queue_name = rtems_build_name( 'Q', '_', 'S', 'D' );
837 837
838 838 status = rtems_message_queue_ident( queue_name, 0, queue_id );
839 839
840 840 return status;
841 841 }
842 842
843 843 rtems_status_code get_message_queue_id_recv( rtems_id *queue_id )
844 844 {
845 845 rtems_status_code status;
846 846 rtems_name queue_name;
847 847
848 848 queue_name = rtems_build_name( 'Q', '_', 'R', 'V' );
849 849
850 850 status = rtems_message_queue_ident( queue_name, 0, queue_id );
851 851
852 852 return status;
853 853 }
854 854
855 855 rtems_status_code get_message_queue_id_prc0( rtems_id *queue_id )
856 856 {
857 857 rtems_status_code status;
858 858 rtems_name queue_name;
859 859
860 860 queue_name = rtems_build_name( 'Q', '_', 'P', '0' );
861 861
862 862 status = rtems_message_queue_ident( queue_name, 0, queue_id );
863 863
864 864 return status;
865 865 }
866 866
867 867 rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id )
868 868 {
869 869 rtems_status_code status;
870 870 rtems_name queue_name;
871 871
872 872 queue_name = rtems_build_name( 'Q', '_', 'P', '1' );
873 873
874 874 status = rtems_message_queue_ident( queue_name, 0, queue_id );
875 875
876 876 return status;
877 877 }
878 878
879 879 rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id )
880 880 {
881 881 rtems_status_code status;
882 882 rtems_name queue_name;
883 883
884 884 queue_name = rtems_build_name( 'Q', '_', 'P', '2' );
885 885
886 886 status = rtems_message_queue_ident( queue_name, 0, queue_id );
887 887
888 888 return status;
889 889 }
890 890
891 891 void update_queue_max_count( rtems_id queue_id, unsigned char*fifo_size_max )
892 892 {
893 893 u_int32_t count;
894 894 rtems_status_code status;
895 895
896 896 count = 0;
897 897
898 898 status = rtems_message_queue_get_number_pending( queue_id, &count );
899 899
900 900 count = count + 1;
901 901
902 902 if (status != RTEMS_SUCCESSFUL)
903 903 {
904 904 PRINTF1("in update_queue_max_count *** ERR = %d\n", status)
905 905 }
906 906 else
907 907 {
908 908 if (count > *fifo_size_max)
909 909 {
910 910 *fifo_size_max = count;
911 911 }
912 912 }
913 913 }
914 914
915 915 void init_ring(ring_node ring[], unsigned char nbNodes, volatile int buffer[], unsigned int bufferSize )
916 916 {
917 917 unsigned char i;
918 918
919 919 //***************
920 920 // BUFFER ADDRESS
921 921 for(i=0; i<nbNodes; i++)
922 922 {
923 923 ring[i].coarseTime = INT32_ALL_F;
924 924 ring[i].fineTime = INT32_ALL_F;
925 925 ring[i].sid = INIT_CHAR;
926 926 ring[i].status = INIT_CHAR;
927 927 ring[i].buffer_address = (int) &buffer[ i * bufferSize ];
928 928 }
929 929
930 930 //*****
931 931 // NEXT
932 932 ring[ nbNodes - 1 ].next = (ring_node*) &ring[ 0 ];
933 933 for(i=0; i<nbNodes-1; i++)
934 934 {
935 935 ring[i].next = (ring_node*) &ring[ i + 1 ];
936 936 }
937 937
938 938 //*********
939 939 // PREVIOUS
940 940 ring[ 0 ].previous = (ring_node*) &ring[ nbNodes - 1 ];
941 941 for(i=1; i<nbNodes; i++)
942 942 {
943 943 ring[i].previous = (ring_node*) &ring[ i - 1 ];
944 944 }
945 945 }
General Comments 0
You need to be logged in to leave comments. Login now