##// END OF EJS Templates
ICD 4.1 taken into account
paul -
r283:c0251025dc7b R3_plus draft
parent child
Show More
@@ -1,2 +1,2
1 1 3081d1f9bb20b2b64a192585337a292a9804e0c5 LFR_basic-parameters
2 c3197ff831df5057bdd145a4efd94ded0618661f header/lfr_common_headers
2 81c3289ebd2a13e3b3147acdf60e34678378f905 header/lfr_common_headers
@@ -1,124 +1,123
1 1 TEMPLATE = app
2 2 # CONFIG += console v8 sim
3 3 # CONFIG options =
4 4 # verbose
5 5 # boot_messages
6 6 # debug_messages
7 7 # cpu_usage_report
8 8 # stack_report
9 9 # vhdl_dev
10 10 # debug_tch
11 11 # lpp_dpu_destid /!\ REMOVE BEFORE DELIVERY TO LESIA /!\
12 12 # debug_watchdog
13 13 CONFIG += console verbose lpp_dpu_destid
14 14 CONFIG -= qt
15 15
16 16 include(./sparc.pri)
17 17
18 18 # flight software version
19 19 SWVERSION=-1-0
20 20 DEFINES += SW_VERSION_N1=3 # major
21 21 DEFINES += SW_VERSION_N2=1 # minor
22 22 DEFINES += SW_VERSION_N3=0 # patch
23 23 DEFINES += SW_VERSION_N4=0 # internal
24 24
25 25 # <GCOV>
26 26 #QMAKE_CFLAGS_RELEASE += -fprofile-arcs -ftest-coverage
27 27 #LIBS += -lgcov /opt/GCOV/01A/lib/overload.o -lc
28 28 # </GCOV>
29 29
30 30 # <CHANGE BEFORE FLIGHT>
31 31 contains( CONFIG, lpp_dpu_destid ) {
32 32 DEFINES += LPP_DPU_DESTID
33 33 }
34 34 # </CHANGE BEFORE FLIGHT>
35 35
36 36 contains( CONFIG, debug_tch ) {
37 37 DEFINES += DEBUG_TCH
38 38 }
39 39 DEFINES += MSB_FIRST_TCH
40 40
41 41 contains( CONFIG, vhdl_dev ) {
42 42 DEFINES += VHDL_DEV
43 43 }
44 44
45 45 contains( CONFIG, verbose ) {
46 46 DEFINES += PRINT_MESSAGES_ON_CONSOLE
47 47 }
48 48
49 49 contains( CONFIG, debug_messages ) {
50 50 DEFINES += DEBUG_MESSAGES
51 51 }
52 52
53 53 contains( CONFIG, cpu_usage_report ) {
54 54 DEFINES += PRINT_TASK_STATISTICS
55 55 }
56 56
57 57 contains( CONFIG, stack_report ) {
58 58 DEFINES += PRINT_STACK_REPORT
59 59 }
60 60
61 61 contains( CONFIG, boot_messages ) {
62 62 DEFINES += BOOT_MESSAGES
63 63 }
64 64
65 65 contains( CONFIG, debug_watchdog ) {
66 66 DEFINES += DEBUG_WATCHDOG
67 67 }
68 68
69 69 #doxygen.target = doxygen
70 70 #doxygen.commands = doxygen ../doc/Doxyfile
71 71 #QMAKE_EXTRA_TARGETS += doxygen
72 72
73 73 TARGET = fsw
74 74
75 75 INCLUDEPATH += \
76 76 $${PWD}/../src \
77 77 $${PWD}/../header \
78 78 $${PWD}/../header/lfr_common_headers \
79 79 $${PWD}/../header/processing \
80 80 $${PWD}/../LFR_basic-parameters
81 81
82 82 SOURCES += \
83 83 ../src/wf_handler.c \
84 84 ../src/tc_handler.c \
85 85 ../src/fsw_misc.c \
86 86 ../src/fsw_init.c \
87 87 ../src/fsw_globals.c \
88 88 ../src/fsw_spacewire.c \
89 89 ../src/tc_load_dump_parameters.c \
90 90 ../src/tm_lfr_tc_exe.c \
91 91 ../src/tc_acceptance.c \
92 92 ../src/processing/fsw_processing.c \
93 93 ../src/processing/avf0_prc0.c \
94 94 ../src/processing/avf1_prc1.c \
95 95 ../src/processing/avf2_prc2.c \
96 96 ../src/lfr_cpu_usage_report.c \
97 97 ../LFR_basic-parameters/basic_parameters.c
98 98
99 99 HEADERS += \
100 100 ../header/wf_handler.h \
101 101 ../header/tc_handler.h \
102 102 ../header/grlib_regs.h \
103 103 ../header/fsw_misc.h \
104 104 ../header/fsw_init.h \
105 105 ../header/fsw_spacewire.h \
106 106 ../header/tc_load_dump_parameters.h \
107 107 ../header/tm_lfr_tc_exe.h \
108 108 ../header/tc_acceptance.h \
109 109 ../header/processing/fsw_processing.h \
110 110 ../header/processing/avf0_prc0.h \
111 111 ../header/processing/avf1_prc1.h \
112 112 ../header/processing/avf2_prc2.h \
113 113 ../header/fsw_params_wf_handler.h \
114 114 ../header/lfr_cpu_usage_report.h \
115 115 ../header/lfr_common_headers/ccsds_types.h \
116 116 ../header/lfr_common_headers/fsw_params.h \
117 117 ../header/lfr_common_headers/fsw_params_nb_bytes.h \
118 118 ../header/lfr_common_headers/fsw_params_processing.h \
119 ../header/lfr_common_headers/TC_types.h \
120 119 ../header/lfr_common_headers/tm_byte_positions.h \
121 120 ../LFR_basic-parameters/basic_parameters.h \
122 121 ../LFR_basic-parameters/basic_parameters_params.h \
123 122 ../header/GscMemoryLPP.hpp
124 123
@@ -1,54 +1,63
1 1 #ifndef FSW_INIT_H_INCLUDED
2 2 #define FSW_INIT_H_INCLUDED
3 3
4 4 #include <rtems.h>
5 5 #include <leon.h>
6 6
7 7 #include "fsw_params.h"
8 8 #include "fsw_misc.h"
9 9 #include "fsw_processing.h"
10 10
11 11 #include "tc_handler.h"
12 12 #include "wf_handler.h"
13 13 #include "fsw_spacewire.h"
14 14
15 15 #include "avf0_prc0.h"
16 16 #include "avf1_prc1.h"
17 17 #include "avf2_prc2.h"
18 18
19 19 extern rtems_name Task_name[20]; /* array of task names */
20 20 extern rtems_id Task_id[20]; /* array of task ids */
21 21 extern rtems_name timecode_timer_name;
22 22 extern rtems_id timecode_timer_id;
23 23 extern unsigned char pa_bia_status_info;
24 extern unsigned char cp_rpw_sc_rw_f_flags;
25 extern float cp_rpw_sc_rw1_f1;
26 extern float cp_rpw_sc_rw1_f2;
27 extern float cp_rpw_sc_rw2_f1;
28 extern float cp_rpw_sc_rw2_f2;
29 extern float cp_rpw_sc_rw3_f1;
30 extern float cp_rpw_sc_rw3_f2;
31 extern float cp_rpw_sc_rw4_f1;
32 extern float cp_rpw_sc_rw4_f2;
24 33
25 34 // RTEMS TASKS
26 35 rtems_task Init( rtems_task_argument argument);
27 36
28 37 // OTHER functions
29 38 void create_names( void );
30 39 int create_all_tasks( void );
31 40 int start_all_tasks( void );
32 41 //
33 42 rtems_status_code create_message_queues( void );
34 43 rtems_status_code create_timecode_timer( void );
35 44 rtems_status_code get_message_queue_id_send( rtems_id *queue_id );
36 45 rtems_status_code get_message_queue_id_recv( rtems_id *queue_id );
37 46 rtems_status_code get_message_queue_id_prc0( rtems_id *queue_id );
38 47 rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id );
39 48 rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id );
40 49 void update_queue_max_count( rtems_id queue_id, unsigned char*fifo_size_max );
41 50 void init_ring(ring_node ring[], unsigned char nbNodes, volatile int buffer[], unsigned int bufferSize );
42 51 //
43 52 int start_recv_send_tasks( void );
44 53 //
45 54 void init_local_mode_parameters( void );
46 55 void reset_local_time( void );
47 56
48 57 extern void rtems_cpu_usage_report( void );
49 58 extern void rtems_cpu_usage_reset( void );
50 59 extern void rtems_stack_checker_report_usage( void );
51 60
52 61 extern int sched_yield( void );
53 62
54 63 #endif // FSW_INIT_H_INCLUDED
@@ -1,332 +1,335
1 1 #ifndef FSW_PROCESSING_H_INCLUDED
2 2 #define FSW_PROCESSING_H_INCLUDED
3 3
4 4 #include <rtems.h>
5 5 #include <grspw.h>
6 6 #include <math.h>
7 7 #include <stdlib.h> // abs() is in the stdlib
8 8 #include <stdio.h>
9 9 #include <math.h>
10 10 #include <grlib_regs.h>
11 11
12 12 #include "fsw_params.h"
13 13
14 14 typedef struct ring_node_asm
15 15 {
16 16 struct ring_node_asm *next;
17 17 float matrix[ TOTAL_SIZE_SM ];
18 18 unsigned int status;
19 19 } ring_node_asm;
20 20
21 21 typedef struct
22 22 {
23 23 unsigned char targetLogicalAddress;
24 24 unsigned char protocolIdentifier;
25 25 unsigned char reserved;
26 26 unsigned char userApplication;
27 27 unsigned char packetID[2];
28 28 unsigned char packetSequenceControl[2];
29 29 unsigned char packetLength[2];
30 30 // DATA FIELD HEADER
31 31 unsigned char spare1_pusVersion_spare2;
32 32 unsigned char serviceType;
33 33 unsigned char serviceSubType;
34 34 unsigned char destinationID;
35 35 unsigned char time[6];
36 36 // AUXILIARY HEADER
37 37 unsigned char sid;
38 unsigned char biaStatusInfo;
38 unsigned char pa_bia_status_info;
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 unsigned char biaStatusInfo;
64 unsigned char pa_bia_status_info;
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 unsigned char thisIsAnASMRestart;
86 86
87 87 extern volatile int sm_f0[ ];
88 88 extern volatile int sm_f1[ ];
89 89 extern volatile int sm_f2[ ];
90 90
91 91 // parameters
92 92 extern struct param_local_str param_local;
93 93 extern Packet_TM_LFR_PARAMETER_DUMP_t parameter_dump_packet;
94 extern unsigned char rw_fbins_mask_f0[16];
95 extern unsigned char rw_fbins_mask_f1[16];
96 extern unsigned char rw_fbins_mask_f2[16];
94 97
95 98 // registers
96 99 extern time_management_regs_t *time_management_regs;
97 100 extern volatile spectral_matrix_regs_t *spectral_matrix_regs;
98 101
99 102 extern rtems_name misc_name[5];
100 103 extern rtems_id Task_id[20]; /* array of task ids */
101 104
102 105 ring_node * getRingNodeForAveraging( unsigned char frequencyChannel);
103 106 // ISR
104 107 rtems_isr spectral_matrices_isr( rtems_vector_number vector );
105 108
106 109 //******************
107 110 // Spectral Matrices
108 111 void reset_nb_sm( void );
109 112 // SM
110 113 void SM_init_rings( void );
111 114 void SM_reset_current_ring_nodes( void );
112 115 // ASM
113 116 void ASM_generic_init_ring(ring_node_asm *ring, unsigned char nbNodes );
114 117
115 118 //*****************
116 119 // Basic Parameters
117 120
118 121 void BP_reset_current_ring_nodes( void );
119 122 void BP_init_header(bp_packet *packet,
120 123 unsigned int apid, unsigned char sid,
121 124 unsigned int packetLength , unsigned char blkNr);
122 125 void BP_init_header_with_spare(bp_packet_with_spare *packet,
123 126 unsigned int apid, unsigned char sid,
124 127 unsigned int packetLength, unsigned char blkNr );
125 128 void BP_send( char *data,
126 129 rtems_id queue_id,
127 130 unsigned int nbBytesToSend , unsigned int sid );
128 131 void BP_send_s1_s2(char *data,
129 132 rtems_id queue_id,
130 133 unsigned int nbBytesToSend, unsigned int sid );
131 134
132 135 //******************
133 136 // general functions
134 137 void reset_sm_status( void );
135 138 void reset_spectral_matrix_regs( void );
136 139 void set_time(unsigned char *time, unsigned char *timeInBuffer );
137 140 unsigned long long int get_acquisition_time( unsigned char *timePtr );
138 141 unsigned char getSID( rtems_event_set event );
139 142
140 143 extern rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id );
141 144 extern rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id );
142 145
143 146 //***************************************
144 147 // DEFINITIONS OF STATIC INLINE FUNCTIONS
145 148 static inline void SM_average(float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
146 149 ring_node *ring_node_tab[],
147 150 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
148 151 asm_msg *msgForMATR );
149 152
150 153 static inline void SM_average_debug(float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
151 154 ring_node *ring_node_tab[],
152 155 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
153 156 asm_msg *msgForMATR );
154 157
155 158 void ASM_patch( float *inputASM, float *outputASM );
156 159
157 160 void extractReImVectors(float *inputASM, float *outputASM, unsigned int asmComponent );
158 161
159 162 static inline void ASM_reorganize_and_divide(float *averaged_spec_mat, float *averaged_spec_mat_reorganized,
160 163 float divider );
161 164
162 165 static inline void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat,
163 166 float divider,
164 167 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart);
165 168
166 169 static inline void ASM_convert(volatile float *input_matrix, char *output_matrix);
167 170
168 171 void SM_average( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
169 172 ring_node *ring_node_tab[],
170 173 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
171 174 asm_msg *msgForMATR )
172 175 {
173 176 float sum;
174 177 unsigned int i;
175 178
176 179 for(i=0; i<TOTAL_SIZE_SM; i++)
177 180 {
178 181 sum = ( (int *) (ring_node_tab[0]->buffer_address) ) [ i ]
179 182 + ( (int *) (ring_node_tab[1]->buffer_address) ) [ i ]
180 183 + ( (int *) (ring_node_tab[2]->buffer_address) ) [ i ]
181 184 + ( (int *) (ring_node_tab[3]->buffer_address) ) [ i ]
182 185 + ( (int *) (ring_node_tab[4]->buffer_address) ) [ i ]
183 186 + ( (int *) (ring_node_tab[5]->buffer_address) ) [ i ]
184 187 + ( (int *) (ring_node_tab[6]->buffer_address) ) [ i ]
185 188 + ( (int *) (ring_node_tab[7]->buffer_address) ) [ i ];
186 189
187 190 if ( (nbAverageNORM == 0) && (nbAverageSBM == 0) )
188 191 {
189 192 averaged_spec_mat_NORM[ i ] = sum;
190 193 averaged_spec_mat_SBM[ i ] = sum;
191 194 msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime;
192 195 msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime;
193 196 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
194 197 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
195 198 }
196 199 else if ( (nbAverageNORM != 0) && (nbAverageSBM != 0) )
197 200 {
198 201 averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum );
199 202 averaged_spec_mat_SBM[ i ] = ( averaged_spec_mat_SBM[ i ] + sum );
200 203 }
201 204 else if ( (nbAverageNORM != 0) && (nbAverageSBM == 0) )
202 205 {
203 206 averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum );
204 207 averaged_spec_mat_SBM[ i ] = sum;
205 208 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
206 209 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
207 210 }
208 211 else
209 212 {
210 213 averaged_spec_mat_NORM[ i ] = sum;
211 214 averaged_spec_mat_SBM[ i ] = ( averaged_spec_mat_SBM[ i ] + sum );
212 215 msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime;
213 216 msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime;
214 217 // PRINTF2("ERR *** in SM_average *** unexpected parameters %d %d\n", nbAverageNORM, nbAverageSBM)
215 218 }
216 219 }
217 220 }
218 221
219 222 void SM_average_debug( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
220 223 ring_node *ring_node_tab[],
221 224 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
222 225 asm_msg *msgForMATR )
223 226 {
224 227 float sum;
225 228 unsigned int i;
226 229
227 230 for(i=0; i<TOTAL_SIZE_SM; i++)
228 231 {
229 232 sum = ( (int *) (ring_node_tab[0]->buffer_address) ) [ i ];
230 233 averaged_spec_mat_NORM[ i ] = sum;
231 234 averaged_spec_mat_SBM[ i ] = sum;
232 235 msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime;
233 236 msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime;
234 237 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
235 238 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
236 239 }
237 240 }
238 241
239 242 void ASM_reorganize_and_divide( float *averaged_spec_mat, float *averaged_spec_mat_reorganized, float divider )
240 243 {
241 244 int frequencyBin;
242 245 int asmComponent;
243 246 unsigned int offsetASM;
244 247 unsigned int offsetASMReorganized;
245 248
246 249 // BUILD DATA
247 250 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
248 251 {
249 252 for( frequencyBin = 0; frequencyBin < NB_BINS_PER_SM; frequencyBin++ )
250 253 {
251 254 offsetASMReorganized =
252 255 frequencyBin * NB_VALUES_PER_SM
253 256 + asmComponent;
254 257 offsetASM =
255 258 asmComponent * NB_BINS_PER_SM
256 259 + frequencyBin;
257 260 averaged_spec_mat_reorganized[offsetASMReorganized ] =
258 261 averaged_spec_mat[ offsetASM ] / divider;
259 262 }
260 263 }
261 264 }
262 265
263 266 void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
264 267 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage, unsigned char ASMIndexStart )
265 268 {
266 269 int frequencyBin;
267 270 int asmComponent;
268 271 int offsetASM;
269 272 int offsetCompressed;
270 273 int k;
271 274
272 275 // BUILD DATA
273 276 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
274 277 {
275 278 for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
276 279 {
277 280 offsetCompressed = // NO TIME OFFSET
278 281 frequencyBin * NB_VALUES_PER_SM
279 282 + asmComponent;
280 283 offsetASM = // NO TIME OFFSET
281 284 asmComponent * NB_BINS_PER_SM
282 285 + ASMIndexStart
283 286 + frequencyBin * nbBinsToAverage;
284 287 compressed_spec_mat[ offsetCompressed ] = 0;
285 288 for ( k = 0; k < nbBinsToAverage; k++ )
286 289 {
287 290 compressed_spec_mat[offsetCompressed ] =
288 291 ( compressed_spec_mat[ offsetCompressed ]
289 292 + averaged_spec_mat[ offsetASM + k ] );
290 293 }
291 294 compressed_spec_mat[ offsetCompressed ] =
292 295 compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
293 296 }
294 297 }
295 298 }
296 299
297 300 void ASM_convert( volatile float *input_matrix, char *output_matrix)
298 301 {
299 302 unsigned int frequencyBin;
300 303 unsigned int asmComponent;
301 304 char * pt_char_input;
302 305 char * pt_char_output;
303 306 unsigned int offsetInput;
304 307 unsigned int offsetOutput;
305 308
306 309 pt_char_input = (char*) &input_matrix;
307 310 pt_char_output = (char*) &output_matrix;
308 311
309 312 // convert all other data
310 313 for( frequencyBin=0; frequencyBin<NB_BINS_PER_SM; frequencyBin++)
311 314 {
312 315 for ( asmComponent=0; asmComponent<NB_VALUES_PER_SM; asmComponent++)
313 316 {
314 317 offsetInput = (frequencyBin*NB_VALUES_PER_SM) + asmComponent ;
315 318 offsetOutput = 2 * ( (frequencyBin*NB_VALUES_PER_SM) + asmComponent ) ;
316 319 pt_char_input = (char*) &input_matrix [ offsetInput ];
317 320 pt_char_output = (char*) &output_matrix[ offsetOutput ];
318 321 pt_char_output[0] = pt_char_input[0]; // bits 31 downto 24 of the float
319 322 pt_char_output[1] = pt_char_input[1]; // bits 23 downto 16 of the float
320 323 }
321 324 }
322 325 }
323 326
324 327 void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat,
325 328 float divider,
326 329 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart, unsigned char channel);
327 330
328 331 int getFBinMask(int k, unsigned char channel);
329 332
330 333 void init_kcoeff_sbm_from_kcoeff_norm( float *input_kcoeff, float *output_kcoeff, unsigned char nb_bins_norm);
331 334
332 335 #endif // FSW_PROCESSING_H_INCLUDED
@@ -1,76 +1,82
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 17 extern unsigned short sequenceCounters_TM_DUMP[];
18 18 extern float k_coeff_intercalib_f0_norm[ ];
19 19 extern float k_coeff_intercalib_f0_sbm[ ];
20 20 extern float k_coeff_intercalib_f1_norm[ ];
21 21 extern float k_coeff_intercalib_f1_sbm[ ];
22 22 extern float k_coeff_intercalib_f2[ ];
23 extern unsigned char rw_fbins_mask_f0[16];
24 extern unsigned char rw_fbins_mask_f1[16];
25 extern unsigned char rw_fbins_mask_f2[16];
23 26
24 27 int action_load_common_par( ccsdsTelecommandPacket_t *TC );
25 28 int action_load_normal_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
26 29 int action_load_burst_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
27 30 int action_load_sbm1_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
28 31 int action_load_sbm2_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
29 32 int action_load_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
30 33 int action_load_fbins_mask(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
31 int action_load_pas_filter_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
34 int action_load_filter_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
32 35 int action_dump_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
33 36 int action_dump_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id );
34 37
35 38 // NORMAL
36 39 int check_normal_par_consistency( ccsdsTelecommandPacket_t *TC, rtems_id queue_id );
37 40 int set_sy_lfr_n_swf_l( ccsdsTelecommandPacket_t *TC );
38 41 int set_sy_lfr_n_swf_p( ccsdsTelecommandPacket_t *TC );
39 42 int set_sy_lfr_n_asm_p( ccsdsTelecommandPacket_t *TC );
40 43 int set_sy_lfr_n_bp_p0( ccsdsTelecommandPacket_t *TC );
41 44 int set_sy_lfr_n_bp_p1( ccsdsTelecommandPacket_t *TC );
42 45 int set_sy_lfr_n_cwf_long_f3( ccsdsTelecommandPacket_t *TC );
43 46
44 47 // BURST
45 48 int set_sy_lfr_b_bp_p0( ccsdsTelecommandPacket_t *TC );
46 49 int set_sy_lfr_b_bp_p1( ccsdsTelecommandPacket_t *TC );
47 50
48 51 // SBM1
49 52 int set_sy_lfr_s1_bp_p0( ccsdsTelecommandPacket_t *TC );
50 53 int set_sy_lfr_s1_bp_p1( ccsdsTelecommandPacket_t *TC );
51 54
52 55 // SBM2
53 56 int set_sy_lfr_s2_bp_p0( ccsdsTelecommandPacket_t *TC );
54 57 int set_sy_lfr_s2_bp_p1( ccsdsTelecommandPacket_t *TC );
55 58
56 59 // TC_LFR_UPDATE_INFO
57 60 unsigned int check_update_info_hk_lfr_mode( unsigned char mode );
58 61 unsigned int check_update_info_hk_tds_mode( unsigned char mode );
59 62 unsigned int check_update_info_hk_thr_mode( unsigned char mode );
63 void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC );
64 void build_rw_fbins_mask( unsigned int channel );
65 void build_rw_fbins_masks();
60 66
61 67 // FBINS_MASK
62 68 int set_sy_lfr_fbins( ccsdsTelecommandPacket_t *TC );
63 69
64 70 // TC_LFR_LOAD_PARS_FILTER_PAR
65 71 int check_sy_lfr_pas_filter_parameters( ccsdsTelecommandPacket_t *TC, rtems_id queue_id );
66 72
67 73 // KCOEFFICIENTS
68 74 int set_sy_lfr_kcoeff(ccsdsTelecommandPacket_t *TC , rtems_id queue_id);
69 75 void copyFloatByChar( unsigned char *destination, unsigned char *source );
70 76
71 77 void init_parameter_dump( void );
72 78 void init_kcoefficients_dump( void );
73 79 void init_kcoefficients_dump_packet( Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump, unsigned char pkt_nr, unsigned char blk_nr );
74 80 void increment_seq_counter_destination_id_dump( unsigned char *packet_sequence_control, unsigned char destination_id );
75 81
76 82 #endif // TC_LOAD_DUMP_PARAMETERS_H
@@ -1,81 +1,95
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 rtems_name timecode_timer_name;
30 30 rtems_id timecode_timer_id;
31 31 int fdSPW = 0;
32 32 int fdUART = 0;
33 33 unsigned char lfrCurrentMode;
34 34 unsigned char pa_bia_status_info;
35 35 unsigned char thisIsAnASMRestart = 0;
36 36 unsigned char oneTcLfrUpdateTimeReceived = 0;
37 37
38 38 // WAVEFORMS GLOBAL VARIABLES // 2048 * 3 * 4 + 2 * 4 = 24576 + 8 bytes = 24584
39 39 // 97 * 256 = 24832 => delta = 248 bytes = 62 words
40 40 // WAVEFORMS GLOBAL VARIABLES // 2688 * 3 * 4 + 2 * 4 = 32256 + 8 bytes = 32264
41 41 // 127 * 256 = 32512 => delta = 248 bytes = 62 words
42 42 // F0 F1 F2 F3
43 43 volatile int wf_buffer_f0[ NB_RING_NODES_F0 * WFRM_BUFFER ] __attribute__((aligned(0x100)));
44 44 volatile int wf_buffer_f1[ NB_RING_NODES_F1 * WFRM_BUFFER ] __attribute__((aligned(0x100)));
45 45 volatile int wf_buffer_f2[ NB_RING_NODES_F2 * WFRM_BUFFER ] __attribute__((aligned(0x100)));
46 46 volatile int wf_buffer_f3[ NB_RING_NODES_F3 * WFRM_BUFFER ] __attribute__((aligned(0x100)));
47 47
48 48 //***********************************
49 49 // SPECTRAL MATRICES GLOBAL VARIABLES
50 50
51 51 // alignment constraints for the spectral matrices buffers => the first data after the time (8 bytes) shall be aligned on 0x00
52 52 volatile int sm_f0[ NB_RING_NODES_SM_F0 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100)));
53 53 volatile int sm_f1[ NB_RING_NODES_SM_F1 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100)));
54 54 volatile int sm_f2[ NB_RING_NODES_SM_F2 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100)));
55 55
56 56 // APB CONFIGURATION REGISTERS
57 57 time_management_regs_t *time_management_regs = (time_management_regs_t*) REGS_ADDR_TIME_MANAGEMENT;
58 58 gptimer_regs_t *gptimer_regs = (gptimer_regs_t *) REGS_ADDR_GPTIMER;
59 59 waveform_picker_regs_0_1_18_t *waveform_picker_regs = (waveform_picker_regs_0_1_18_t*) REGS_ADDR_WAVEFORM_PICKER;
60 60 spectral_matrix_regs_t *spectral_matrix_regs = (spectral_matrix_regs_t*) REGS_ADDR_SPECTRAL_MATRIX;
61 61
62 62 // MODE PARAMETERS
63 63 Packet_TM_LFR_PARAMETER_DUMP_t parameter_dump_packet;
64 64 struct param_local_str param_local;
65 65 unsigned int lastValidEnterModeTime;
66 66
67 67 // HK PACKETS
68 68 Packet_TM_LFR_HK_t housekeeping_packet;
69 unsigned char cp_rpw_sc_rw_f_flags;
69 70 // message queues occupancy
70 71 unsigned char hk_lfr_q_sd_fifo_size_max;
71 72 unsigned char hk_lfr_q_rv_fifo_size_max;
72 73 unsigned char hk_lfr_q_p0_fifo_size_max;
73 74 unsigned char hk_lfr_q_p1_fifo_size_max;
74 75 unsigned char hk_lfr_q_p2_fifo_size_max;
75 76 // sequence counters are incremented by APID (PID + CAT) and destination ID
76 77 unsigned short sequenceCounters_SCIENCE_NORMAL_BURST;
77 78 unsigned short sequenceCounters_SCIENCE_SBM1_SBM2;
78 79 unsigned short sequenceCounters_TC_EXE[SEQ_CNT_NB_DEST_ID];
79 80 unsigned short sequenceCounters_TM_DUMP[SEQ_CNT_NB_DEST_ID];
80 81 unsigned short sequenceCounterHK;
81 82 spw_stats grspw_stats;
83
84 // TC_LFR_UPDATE_INFO
85 float cp_rpw_sc_rw1_f1;
86 float cp_rpw_sc_rw1_f2;
87 float cp_rpw_sc_rw2_f1;
88 float cp_rpw_sc_rw2_f2;
89 float cp_rpw_sc_rw3_f1;
90 float cp_rpw_sc_rw3_f2;
91 float cp_rpw_sc_rw4_f1;
92 float cp_rpw_sc_rw4_f2;
93 unsigned char rw_fbins_mask_f0[16];
94 unsigned char rw_fbins_mask_f1[16];
95 unsigned char rw_fbins_mask_f2[16];
@@ -1,917 +1,926
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 #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 // ASI 2 contains a few control registers that have not been assigned as ancillary state registers.
68 68 // These should only be read and written using 32-bit LDA/STA instructions.
69 69 // All cache registers are accessed through load/store operations to the alternate address space (LDA/STA), using ASI = 2.
70 70 // The table below shows the register addresses:
71 71 // 0x00 Cache control register
72 72 // 0x04 Reserved
73 73 // 0x08 Instruction cache configuration register
74 74 // 0x0C Data cache configuration register
75 75
76 76 // Cache Control Register Leon3 / Leon3FT
77 77 // 31..30 29 28 27..24 23 22 21 20..19 18 17 16
78 78 // RFT PS TB DS FD FI FT ST IB
79 79 // 15 14 13..12 11..10 9..8 7..6 5 4 3..2 1..0
80 80 // IP DP ITE IDE DTE DDE DF IF DCS ICS
81 81
82 82 unsigned int cacheControlRegister;
83 83
84 84 CCR_resetCacheControlRegister();
85 85 ASR16_resetRegisterProtectionControlRegister();
86 86
87 87 cacheControlRegister = CCR_getValue();
88 88 PRINTF1("(0) CCR - Cache Control Register = %x\n", cacheControlRegister);
89 89 PRINTF1("(0) ASR16 = %x\n", *asr16Ptr);
90 90
91 91 CCR_enableInstructionCache(); // ICS bits
92 92 CCR_enableDataCache(); // DCS bits
93 93 CCR_enableInstructionBurstFetch(); // IB bit
94 94
95 95 faultTolerantScheme();
96 96
97 97 cacheControlRegister = CCR_getValue();
98 98 PRINTF1("(1) CCR - Cache Control Register = %x\n", cacheControlRegister);
99 99 PRINTF1("(1) ASR16 Register protection control register = %x\n", *asr16Ptr);
100 100
101 101 PRINTF("\n");
102 102 }
103 103
104 104 rtems_task Init( rtems_task_argument ignored )
105 105 {
106 106 /** This is the RTEMS INIT taks, it is the first task launched by the system.
107 107 *
108 108 * @param unused is the starting argument of the RTEMS task
109 109 *
110 110 * The INIT task create and run all other RTEMS tasks.
111 111 *
112 112 */
113 113
114 114 //***********
115 115 // INIT CACHE
116 116
117 117 unsigned char *vhdlVersion;
118 118
119 119 reset_lfr();
120 120
121 121 reset_local_time();
122 122
123 123 rtems_cpu_usage_reset();
124 124
125 125 rtems_status_code status;
126 126 rtems_status_code status_spw;
127 127 rtems_isr_entry old_isr_handler;
128 128
129 129 // UART settings
130 130 enable_apbuart_transmitter();
131 131 set_apbuart_scaler_reload_register(REGS_ADDR_APBUART, APBUART_SCALER_RELOAD_VALUE);
132 132
133 133 DEBUG_PRINTF("\n\n\n\n\nIn INIT *** Now the console is on port COM1\n")
134 134
135 135
136 136 PRINTF("\n\n\n\n\n")
137 137
138 138 initCache();
139 139
140 140 PRINTF("*************************\n")
141 141 PRINTF("** LFR Flight Software **\n")
142 142 PRINTF1("** %d.", SW_VERSION_N1)
143 143 PRINTF1("%d." , SW_VERSION_N2)
144 144 PRINTF1("%d." , SW_VERSION_N3)
145 145 PRINTF1("%d **\n", SW_VERSION_N4)
146 146
147 147 vhdlVersion = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
148 148 PRINTF("** VHDL **\n")
149 149 PRINTF1("** %d.", vhdlVersion[1])
150 150 PRINTF1("%d." , vhdlVersion[2])
151 151 PRINTF1("%d **\n", vhdlVersion[3])
152 152 PRINTF("*************************\n")
153 153 PRINTF("\n\n")
154 154
155 155 init_parameter_dump();
156 156 init_kcoefficients_dump();
157 157 init_local_mode_parameters();
158 158 init_housekeeping_parameters();
159 159 init_k_coefficients_prc0();
160 160 init_k_coefficients_prc1();
161 161 init_k_coefficients_prc2();
162 162 pa_bia_status_info = 0x00;
163 cp_rpw_sc_rw_f_flags = 0x00;
164 cp_rpw_sc_rw1_f1 = 0.0;
165 cp_rpw_sc_rw1_f2 = 0.0;
166 cp_rpw_sc_rw2_f1 = 0.0;
167 cp_rpw_sc_rw2_f2 = 0.0;
168 cp_rpw_sc_rw3_f1 = 0.0;
169 cp_rpw_sc_rw3_f2 = 0.0;
170 cp_rpw_sc_rw4_f1 = 0.0;
171 cp_rpw_sc_rw4_f2 = 0.0;
163 172 update_last_valid_transition_date( DEFAULT_LAST_VALID_TRANSITION_DATE );
164 173
165 174 // waveform picker initialization
166 175 WFP_init_rings(); LEON_Clear_interrupt( IRQ_SPARC_GPTIMER_WATCHDOG ); // initialize the waveform rings
167 176 WFP_reset_current_ring_nodes();
168 177 reset_waveform_picker_regs();
169 178
170 179 // spectral matrices initialization
171 180 SM_init_rings(); // initialize spectral matrices rings
172 181 SM_reset_current_ring_nodes();
173 182 reset_spectral_matrix_regs();
174 183
175 184 // configure calibration
176 185 configureCalibration( false ); // true means interleaved mode, false is for normal mode
177 186
178 187 updateLFRCurrentMode( LFR_MODE_STANDBY );
179 188
180 189 BOOT_PRINTF1("in INIT *** lfrCurrentMode is %d\n", lfrCurrentMode)
181 190
182 191 create_names(); // create all names
183 192
184 193 status = create_timecode_timer(); // create the timer used by timecode_irq_handler
185 194 if (status != RTEMS_SUCCESSFUL)
186 195 {
187 196 PRINTF1("in INIT *** ERR in create_timer_timecode, code %d", status)
188 197 }
189 198
190 199 status = create_message_queues(); // create message queues
191 200 if (status != RTEMS_SUCCESSFUL)
192 201 {
193 202 PRINTF1("in INIT *** ERR in create_message_queues, code %d", status)
194 203 }
195 204
196 205 status = create_all_tasks(); // create all tasks
197 206 if (status != RTEMS_SUCCESSFUL)
198 207 {
199 208 PRINTF1("in INIT *** ERR in create_all_tasks, code %d\n", status)
200 209 }
201 210
202 211 // **************************
203 212 // <SPACEWIRE INITIALIZATION>
204 213 status_spw = spacewire_open_link(); // (1) open the link
205 214 if ( status_spw != RTEMS_SUCCESSFUL )
206 215 {
207 216 PRINTF1("in INIT *** ERR spacewire_open_link code %d\n", status_spw )
208 217 }
209 218
210 219 if ( status_spw == RTEMS_SUCCESSFUL ) // (2) configure the link
211 220 {
212 221 status_spw = spacewire_configure_link( fdSPW );
213 222 if ( status_spw != RTEMS_SUCCESSFUL )
214 223 {
215 224 PRINTF1("in INIT *** ERR spacewire_configure_link code %d\n", status_spw )
216 225 }
217 226 }
218 227
219 228 if ( status_spw == RTEMS_SUCCESSFUL) // (3) start the link
220 229 {
221 230 status_spw = spacewire_start_link( fdSPW );
222 231 if ( status_spw != RTEMS_SUCCESSFUL )
223 232 {
224 233 PRINTF1("in INIT *** ERR spacewire_start_link code %d\n", status_spw )
225 234 }
226 235 }
227 236 // </SPACEWIRE INITIALIZATION>
228 237 // ***************************
229 238
230 239 status = start_all_tasks(); // start all tasks
231 240 if (status != RTEMS_SUCCESSFUL)
232 241 {
233 242 PRINTF1("in INIT *** ERR in start_all_tasks, code %d", status)
234 243 }
235 244
236 245 // start RECV and SEND *AFTER* SpaceWire Initialization, due to the timeout of the start call during the initialization
237 246 status = start_recv_send_tasks();
238 247 if ( status != RTEMS_SUCCESSFUL )
239 248 {
240 249 PRINTF1("in INIT *** ERR start_recv_send_tasks code %d\n", status )
241 250 }
242 251
243 252 // suspend science tasks, they will be restarted later depending on the mode
244 253 status = suspend_science_tasks(); // suspend science tasks (not done in stop_current_mode if current mode = STANDBY)
245 254 if (status != RTEMS_SUCCESSFUL)
246 255 {
247 256 PRINTF1("in INIT *** in suspend_science_tasks *** ERR code: %d\n", status)
248 257 }
249 258
250 259 // configure IRQ handling for the waveform picker unit
251 260 status = rtems_interrupt_catch( waveforms_isr,
252 261 IRQ_SPARC_WAVEFORM_PICKER,
253 262 &old_isr_handler) ;
254 263 // configure IRQ handling for the spectral matrices unit
255 264 status = rtems_interrupt_catch( spectral_matrices_isr,
256 265 IRQ_SPARC_SPECTRAL_MATRIX,
257 266 &old_isr_handler) ;
258 267
259 268 // if the spacewire link is not up then send an event to the SPIQ task for link recovery
260 269 if ( status_spw != RTEMS_SUCCESSFUL )
261 270 {
262 271 status = rtems_event_send( Task_id[TASKID_SPIQ], SPW_LINKERR_EVENT );
263 272 if ( status != RTEMS_SUCCESSFUL ) {
264 273 PRINTF1("in INIT *** ERR rtems_event_send to SPIQ code %d\n", status )
265 274 }
266 275 }
267 276
268 277 BOOT_PRINTF("delete INIT\n")
269 278
270 279 set_hk_lfr_sc_potential_flag( true );
271 280
272 281 // start the timer to detect a missing spacewire timecode
273 282 // the timeout is larger because the spw IP needs to receive several valid timecodes before generating a tickout
274 283 // if a tickout is generated, the timer is restarted
275 284 status = rtems_timer_fire_after( timecode_timer_id, TIMECODE_TIMER_TIMEOUT_INIT, timecode_timer_routine, NULL );
276 285
277 286 grspw_timecode_callback = &timecode_irq_handler;
278 287
279 288 status = rtems_task_delete(RTEMS_SELF);
280 289
281 290 }
282 291
283 292 void init_local_mode_parameters( void )
284 293 {
285 294 /** This function initialize the param_local global variable with default values.
286 295 *
287 296 */
288 297
289 298 unsigned int i;
290 299
291 300 // LOCAL PARAMETERS
292 301
293 302 BOOT_PRINTF1("local_sbm1_nb_cwf_max %d \n", param_local.local_sbm1_nb_cwf_max)
294 303 BOOT_PRINTF1("local_sbm2_nb_cwf_max %d \n", param_local.local_sbm2_nb_cwf_max)
295 304 BOOT_PRINTF1("nb_interrupt_f0_MAX = %d\n", param_local.local_nb_interrupt_f0_MAX)
296 305
297 306 // init sequence counters
298 307
299 308 for(i = 0; i<SEQ_CNT_NB_DEST_ID; i++)
300 309 {
301 310 sequenceCounters_TC_EXE[i] = 0x00;
302 311 sequenceCounters_TM_DUMP[i] = 0x00;
303 312 }
304 313 sequenceCounters_SCIENCE_NORMAL_BURST = 0x00;
305 314 sequenceCounters_SCIENCE_SBM1_SBM2 = 0x00;
306 315 sequenceCounterHK = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
307 316 }
308 317
309 318 void reset_local_time( void )
310 319 {
311 320 time_management_regs->ctrl = time_management_regs->ctrl | 0x02; // [0010] software reset, coarse time = 0x80000000
312 321 }
313 322
314 323 void create_names( void ) // create all names for tasks and queues
315 324 {
316 325 /** This function creates all RTEMS names used in the software for tasks and queues.
317 326 *
318 327 * @return RTEMS directive status codes:
319 328 * - RTEMS_SUCCESSFUL - successful completion
320 329 *
321 330 */
322 331
323 332 // task names
324 333 Task_name[TASKID_RECV] = rtems_build_name( 'R', 'E', 'C', 'V' );
325 334 Task_name[TASKID_ACTN] = rtems_build_name( 'A', 'C', 'T', 'N' );
326 335 Task_name[TASKID_SPIQ] = rtems_build_name( 'S', 'P', 'I', 'Q' );
327 336 Task_name[TASKID_LOAD] = rtems_build_name( 'L', 'O', 'A', 'D' );
328 337 Task_name[TASKID_AVF0] = rtems_build_name( 'A', 'V', 'F', '0' );
329 338 Task_name[TASKID_SWBD] = rtems_build_name( 'S', 'W', 'B', 'D' );
330 339 Task_name[TASKID_WFRM] = rtems_build_name( 'W', 'F', 'R', 'M' );
331 340 Task_name[TASKID_DUMB] = rtems_build_name( 'D', 'U', 'M', 'B' );
332 341 Task_name[TASKID_HOUS] = rtems_build_name( 'H', 'O', 'U', 'S' );
333 342 Task_name[TASKID_PRC0] = rtems_build_name( 'P', 'R', 'C', '0' );
334 343 Task_name[TASKID_CWF3] = rtems_build_name( 'C', 'W', 'F', '3' );
335 344 Task_name[TASKID_CWF2] = rtems_build_name( 'C', 'W', 'F', '2' );
336 345 Task_name[TASKID_CWF1] = rtems_build_name( 'C', 'W', 'F', '1' );
337 346 Task_name[TASKID_SEND] = rtems_build_name( 'S', 'E', 'N', 'D' );
338 347 Task_name[TASKID_LINK] = rtems_build_name( 'L', 'I', 'N', 'K' );
339 348 Task_name[TASKID_AVF1] = rtems_build_name( 'A', 'V', 'F', '1' );
340 349 Task_name[TASKID_PRC1] = rtems_build_name( 'P', 'R', 'C', '1' );
341 350 Task_name[TASKID_AVF2] = rtems_build_name( 'A', 'V', 'F', '2' );
342 351 Task_name[TASKID_PRC2] = rtems_build_name( 'P', 'R', 'C', '2' );
343 352
344 353 // rate monotonic period names
345 354 name_hk_rate_monotonic = rtems_build_name( 'H', 'O', 'U', 'S' );
346 355
347 356 misc_name[QUEUE_RECV] = rtems_build_name( 'Q', '_', 'R', 'V' );
348 357 misc_name[QUEUE_SEND] = rtems_build_name( 'Q', '_', 'S', 'D' );
349 358 misc_name[QUEUE_PRC0] = rtems_build_name( 'Q', '_', 'P', '0' );
350 359 misc_name[QUEUE_PRC1] = rtems_build_name( 'Q', '_', 'P', '1' );
351 360 misc_name[QUEUE_PRC2] = rtems_build_name( 'Q', '_', 'P', '2' );
352 361
353 362 timecode_timer_name = rtems_build_name( 'S', 'P', 'T', 'C' );
354 363 }
355 364
356 365 int create_all_tasks( void ) // create all tasks which run in the software
357 366 {
358 367 /** This function creates all RTEMS tasks used in the software.
359 368 *
360 369 * @return RTEMS directive status codes:
361 370 * - RTEMS_SUCCESSFUL - task created successfully
362 371 * - RTEMS_INVALID_ADDRESS - id is NULL
363 372 * - RTEMS_INVALID_NAME - invalid task name
364 373 * - RTEMS_INVALID_PRIORITY - invalid task priority
365 374 * - RTEMS_MP_NOT_CONFIGURED - multiprocessing not configured
366 375 * - RTEMS_TOO_MANY - too many tasks created
367 376 * - RTEMS_UNSATISFIED - not enough memory for stack/FP context
368 377 * - RTEMS_TOO_MANY - too many global objects
369 378 *
370 379 */
371 380
372 381 rtems_status_code status;
373 382
374 383 //**********
375 384 // SPACEWIRE
376 385 // RECV
377 386 status = rtems_task_create(
378 387 Task_name[TASKID_RECV], TASK_PRIORITY_RECV, RTEMS_MINIMUM_STACK_SIZE,
379 388 RTEMS_DEFAULT_MODES,
380 389 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_RECV]
381 390 );
382 391 if (status == RTEMS_SUCCESSFUL) // SEND
383 392 {
384 393 status = rtems_task_create(
385 394 Task_name[TASKID_SEND], TASK_PRIORITY_SEND, RTEMS_MINIMUM_STACK_SIZE * 2,
386 395 RTEMS_DEFAULT_MODES,
387 396 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SEND]
388 397 );
389 398 }
390 399 if (status == RTEMS_SUCCESSFUL) // LINK
391 400 {
392 401 status = rtems_task_create(
393 402 Task_name[TASKID_LINK], TASK_PRIORITY_LINK, RTEMS_MINIMUM_STACK_SIZE,
394 403 RTEMS_DEFAULT_MODES,
395 404 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_LINK]
396 405 );
397 406 }
398 407 if (status == RTEMS_SUCCESSFUL) // ACTN
399 408 {
400 409 status = rtems_task_create(
401 410 Task_name[TASKID_ACTN], TASK_PRIORITY_ACTN, RTEMS_MINIMUM_STACK_SIZE,
402 411 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
403 412 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_ACTN]
404 413 );
405 414 }
406 415 if (status == RTEMS_SUCCESSFUL) // SPIQ
407 416 {
408 417 status = rtems_task_create(
409 418 Task_name[TASKID_SPIQ], TASK_PRIORITY_SPIQ, RTEMS_MINIMUM_STACK_SIZE,
410 419 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
411 420 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_SPIQ]
412 421 );
413 422 }
414 423
415 424 //******************
416 425 // SPECTRAL MATRICES
417 426 if (status == RTEMS_SUCCESSFUL) // AVF0
418 427 {
419 428 status = rtems_task_create(
420 429 Task_name[TASKID_AVF0], TASK_PRIORITY_AVF0, RTEMS_MINIMUM_STACK_SIZE,
421 430 RTEMS_DEFAULT_MODES,
422 431 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF0]
423 432 );
424 433 }
425 434 if (status == RTEMS_SUCCESSFUL) // PRC0
426 435 {
427 436 status = rtems_task_create(
428 437 Task_name[TASKID_PRC0], TASK_PRIORITY_PRC0, RTEMS_MINIMUM_STACK_SIZE * 2,
429 438 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
430 439 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC0]
431 440 );
432 441 }
433 442 if (status == RTEMS_SUCCESSFUL) // AVF1
434 443 {
435 444 status = rtems_task_create(
436 445 Task_name[TASKID_AVF1], TASK_PRIORITY_AVF1, RTEMS_MINIMUM_STACK_SIZE,
437 446 RTEMS_DEFAULT_MODES,
438 447 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF1]
439 448 );
440 449 }
441 450 if (status == RTEMS_SUCCESSFUL) // PRC1
442 451 {
443 452 status = rtems_task_create(
444 453 Task_name[TASKID_PRC1], TASK_PRIORITY_PRC1, RTEMS_MINIMUM_STACK_SIZE * 2,
445 454 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
446 455 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC1]
447 456 );
448 457 }
449 458 if (status == RTEMS_SUCCESSFUL) // AVF2
450 459 {
451 460 status = rtems_task_create(
452 461 Task_name[TASKID_AVF2], TASK_PRIORITY_AVF2, RTEMS_MINIMUM_STACK_SIZE,
453 462 RTEMS_DEFAULT_MODES,
454 463 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF2]
455 464 );
456 465 }
457 466 if (status == RTEMS_SUCCESSFUL) // PRC2
458 467 {
459 468 status = rtems_task_create(
460 469 Task_name[TASKID_PRC2], TASK_PRIORITY_PRC2, RTEMS_MINIMUM_STACK_SIZE * 2,
461 470 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
462 471 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC2]
463 472 );
464 473 }
465 474
466 475 //****************
467 476 // WAVEFORM PICKER
468 477 if (status == RTEMS_SUCCESSFUL) // WFRM
469 478 {
470 479 status = rtems_task_create(
471 480 Task_name[TASKID_WFRM], TASK_PRIORITY_WFRM, RTEMS_MINIMUM_STACK_SIZE,
472 481 RTEMS_DEFAULT_MODES,
473 482 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_WFRM]
474 483 );
475 484 }
476 485 if (status == RTEMS_SUCCESSFUL) // CWF3
477 486 {
478 487 status = rtems_task_create(
479 488 Task_name[TASKID_CWF3], TASK_PRIORITY_CWF3, RTEMS_MINIMUM_STACK_SIZE,
480 489 RTEMS_DEFAULT_MODES,
481 490 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF3]
482 491 );
483 492 }
484 493 if (status == RTEMS_SUCCESSFUL) // CWF2
485 494 {
486 495 status = rtems_task_create(
487 496 Task_name[TASKID_CWF2], TASK_PRIORITY_CWF2, RTEMS_MINIMUM_STACK_SIZE,
488 497 RTEMS_DEFAULT_MODES,
489 498 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF2]
490 499 );
491 500 }
492 501 if (status == RTEMS_SUCCESSFUL) // CWF1
493 502 {
494 503 status = rtems_task_create(
495 504 Task_name[TASKID_CWF1], TASK_PRIORITY_CWF1, RTEMS_MINIMUM_STACK_SIZE,
496 505 RTEMS_DEFAULT_MODES,
497 506 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF1]
498 507 );
499 508 }
500 509 if (status == RTEMS_SUCCESSFUL) // SWBD
501 510 {
502 511 status = rtems_task_create(
503 512 Task_name[TASKID_SWBD], TASK_PRIORITY_SWBD, RTEMS_MINIMUM_STACK_SIZE,
504 513 RTEMS_DEFAULT_MODES,
505 514 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SWBD]
506 515 );
507 516 }
508 517
509 518 //*****
510 519 // MISC
511 520 if (status == RTEMS_SUCCESSFUL) // LOAD
512 521 {
513 522 status = rtems_task_create(
514 523 Task_name[TASKID_LOAD], TASK_PRIORITY_LOAD, RTEMS_MINIMUM_STACK_SIZE,
515 524 RTEMS_DEFAULT_MODES,
516 525 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_LOAD]
517 526 );
518 527 }
519 528 if (status == RTEMS_SUCCESSFUL) // DUMB
520 529 {
521 530 status = rtems_task_create(
522 531 Task_name[TASKID_DUMB], TASK_PRIORITY_DUMB, RTEMS_MINIMUM_STACK_SIZE,
523 532 RTEMS_DEFAULT_MODES,
524 533 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_DUMB]
525 534 );
526 535 }
527 536 if (status == RTEMS_SUCCESSFUL) // HOUS
528 537 {
529 538 status = rtems_task_create(
530 539 Task_name[TASKID_HOUS], TASK_PRIORITY_HOUS, RTEMS_MINIMUM_STACK_SIZE,
531 540 RTEMS_DEFAULT_MODES,
532 541 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_HOUS]
533 542 );
534 543 }
535 544
536 545 return status;
537 546 }
538 547
539 548 int start_recv_send_tasks( void )
540 549 {
541 550 rtems_status_code status;
542 551
543 552 status = rtems_task_start( Task_id[TASKID_RECV], recv_task, 1 );
544 553 if (status!=RTEMS_SUCCESSFUL) {
545 554 BOOT_PRINTF("in INIT *** Error starting TASK_RECV\n")
546 555 }
547 556
548 557 if (status == RTEMS_SUCCESSFUL) // SEND
549 558 {
550 559 status = rtems_task_start( Task_id[TASKID_SEND], send_task, 1 );
551 560 if (status!=RTEMS_SUCCESSFUL) {
552 561 BOOT_PRINTF("in INIT *** Error starting TASK_SEND\n")
553 562 }
554 563 }
555 564
556 565 return status;
557 566 }
558 567
559 568 int start_all_tasks( void ) // start all tasks except SEND RECV and HOUS
560 569 {
561 570 /** This function starts all RTEMS tasks used in the software.
562 571 *
563 572 * @return RTEMS directive status codes:
564 573 * - RTEMS_SUCCESSFUL - ask started successfully
565 574 * - RTEMS_INVALID_ADDRESS - invalid task entry point
566 575 * - RTEMS_INVALID_ID - invalid task id
567 576 * - RTEMS_INCORRECT_STATE - task not in the dormant state
568 577 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot start remote task
569 578 *
570 579 */
571 580 // starts all the tasks fot eh flight software
572 581
573 582 rtems_status_code status;
574 583
575 584 //**********
576 585 // SPACEWIRE
577 586 status = rtems_task_start( Task_id[TASKID_SPIQ], spiq_task, 1 );
578 587 if (status!=RTEMS_SUCCESSFUL) {
579 588 BOOT_PRINTF("in INIT *** Error starting TASK_SPIQ\n")
580 589 }
581 590
582 591 if (status == RTEMS_SUCCESSFUL) // LINK
583 592 {
584 593 status = rtems_task_start( Task_id[TASKID_LINK], link_task, 1 );
585 594 if (status!=RTEMS_SUCCESSFUL) {
586 595 BOOT_PRINTF("in INIT *** Error starting TASK_LINK\n")
587 596 }
588 597 }
589 598
590 599 if (status == RTEMS_SUCCESSFUL) // ACTN
591 600 {
592 601 status = rtems_task_start( Task_id[TASKID_ACTN], actn_task, 1 );
593 602 if (status!=RTEMS_SUCCESSFUL) {
594 603 BOOT_PRINTF("in INIT *** Error starting TASK_ACTN\n")
595 604 }
596 605 }
597 606
598 607 //******************
599 608 // SPECTRAL MATRICES
600 609 if (status == RTEMS_SUCCESSFUL) // AVF0
601 610 {
602 611 status = rtems_task_start( Task_id[TASKID_AVF0], avf0_task, LFR_MODE_STANDBY );
603 612 if (status!=RTEMS_SUCCESSFUL) {
604 613 BOOT_PRINTF("in INIT *** Error starting TASK_AVF0\n")
605 614 }
606 615 }
607 616 if (status == RTEMS_SUCCESSFUL) // PRC0
608 617 {
609 618 status = rtems_task_start( Task_id[TASKID_PRC0], prc0_task, LFR_MODE_STANDBY );
610 619 if (status!=RTEMS_SUCCESSFUL) {
611 620 BOOT_PRINTF("in INIT *** Error starting TASK_PRC0\n")
612 621 }
613 622 }
614 623 if (status == RTEMS_SUCCESSFUL) // AVF1
615 624 {
616 625 status = rtems_task_start( Task_id[TASKID_AVF1], avf1_task, LFR_MODE_STANDBY );
617 626 if (status!=RTEMS_SUCCESSFUL) {
618 627 BOOT_PRINTF("in INIT *** Error starting TASK_AVF1\n")
619 628 }
620 629 }
621 630 if (status == RTEMS_SUCCESSFUL) // PRC1
622 631 {
623 632 status = rtems_task_start( Task_id[TASKID_PRC1], prc1_task, LFR_MODE_STANDBY );
624 633 if (status!=RTEMS_SUCCESSFUL) {
625 634 BOOT_PRINTF("in INIT *** Error starting TASK_PRC1\n")
626 635 }
627 636 }
628 637 if (status == RTEMS_SUCCESSFUL) // AVF2
629 638 {
630 639 status = rtems_task_start( Task_id[TASKID_AVF2], avf2_task, 1 );
631 640 if (status!=RTEMS_SUCCESSFUL) {
632 641 BOOT_PRINTF("in INIT *** Error starting TASK_AVF2\n")
633 642 }
634 643 }
635 644 if (status == RTEMS_SUCCESSFUL) // PRC2
636 645 {
637 646 status = rtems_task_start( Task_id[TASKID_PRC2], prc2_task, 1 );
638 647 if (status!=RTEMS_SUCCESSFUL) {
639 648 BOOT_PRINTF("in INIT *** Error starting TASK_PRC2\n")
640 649 }
641 650 }
642 651
643 652 //****************
644 653 // WAVEFORM PICKER
645 654 if (status == RTEMS_SUCCESSFUL) // WFRM
646 655 {
647 656 status = rtems_task_start( Task_id[TASKID_WFRM], wfrm_task, 1 );
648 657 if (status!=RTEMS_SUCCESSFUL) {
649 658 BOOT_PRINTF("in INIT *** Error starting TASK_WFRM\n")
650 659 }
651 660 }
652 661 if (status == RTEMS_SUCCESSFUL) // CWF3
653 662 {
654 663 status = rtems_task_start( Task_id[TASKID_CWF3], cwf3_task, 1 );
655 664 if (status!=RTEMS_SUCCESSFUL) {
656 665 BOOT_PRINTF("in INIT *** Error starting TASK_CWF3\n")
657 666 }
658 667 }
659 668 if (status == RTEMS_SUCCESSFUL) // CWF2
660 669 {
661 670 status = rtems_task_start( Task_id[TASKID_CWF2], cwf2_task, 1 );
662 671 if (status!=RTEMS_SUCCESSFUL) {
663 672 BOOT_PRINTF("in INIT *** Error starting TASK_CWF2\n")
664 673 }
665 674 }
666 675 if (status == RTEMS_SUCCESSFUL) // CWF1
667 676 {
668 677 status = rtems_task_start( Task_id[TASKID_CWF1], cwf1_task, 1 );
669 678 if (status!=RTEMS_SUCCESSFUL) {
670 679 BOOT_PRINTF("in INIT *** Error starting TASK_CWF1\n")
671 680 }
672 681 }
673 682 if (status == RTEMS_SUCCESSFUL) // SWBD
674 683 {
675 684 status = rtems_task_start( Task_id[TASKID_SWBD], swbd_task, 1 );
676 685 if (status!=RTEMS_SUCCESSFUL) {
677 686 BOOT_PRINTF("in INIT *** Error starting TASK_SWBD\n")
678 687 }
679 688 }
680 689
681 690 //*****
682 691 // MISC
683 692 if (status == RTEMS_SUCCESSFUL) // HOUS
684 693 {
685 694 status = rtems_task_start( Task_id[TASKID_HOUS], hous_task, 1 );
686 695 if (status!=RTEMS_SUCCESSFUL) {
687 696 BOOT_PRINTF("in INIT *** Error starting TASK_HOUS\n")
688 697 }
689 698 }
690 699 if (status == RTEMS_SUCCESSFUL) // DUMB
691 700 {
692 701 status = rtems_task_start( Task_id[TASKID_DUMB], dumb_task, 1 );
693 702 if (status!=RTEMS_SUCCESSFUL) {
694 703 BOOT_PRINTF("in INIT *** Error starting TASK_DUMB\n")
695 704 }
696 705 }
697 706 if (status == RTEMS_SUCCESSFUL) // LOAD
698 707 {
699 708 status = rtems_task_start( Task_id[TASKID_LOAD], load_task, 1 );
700 709 if (status!=RTEMS_SUCCESSFUL) {
701 710 BOOT_PRINTF("in INIT *** Error starting TASK_LOAD\n")
702 711 }
703 712 }
704 713
705 714 return status;
706 715 }
707 716
708 717 rtems_status_code create_message_queues( void ) // create the two message queues used in the software
709 718 {
710 719 rtems_status_code status_recv;
711 720 rtems_status_code status_send;
712 721 rtems_status_code status_q_p0;
713 722 rtems_status_code status_q_p1;
714 723 rtems_status_code status_q_p2;
715 724 rtems_status_code ret;
716 725 rtems_id queue_id;
717 726
718 727 //****************************************
719 728 // create the queue for handling valid TCs
720 729 status_recv = rtems_message_queue_create( misc_name[QUEUE_RECV],
721 730 MSG_QUEUE_COUNT_RECV, CCSDS_TC_PKT_MAX_SIZE,
722 731 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
723 732 if ( status_recv != RTEMS_SUCCESSFUL ) {
724 733 PRINTF1("in create_message_queues *** ERR creating QUEU queue, %d\n", status_recv)
725 734 }
726 735
727 736 //************************************************
728 737 // create the queue for handling TM packet sending
729 738 status_send = rtems_message_queue_create( misc_name[QUEUE_SEND],
730 739 MSG_QUEUE_COUNT_SEND, MSG_QUEUE_SIZE_SEND,
731 740 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
732 741 if ( status_send != RTEMS_SUCCESSFUL ) {
733 742 PRINTF1("in create_message_queues *** ERR creating PKTS queue, %d\n", status_send)
734 743 }
735 744
736 745 //*****************************************************************************
737 746 // create the queue for handling averaged spectral matrices for processing @ f0
738 747 status_q_p0 = rtems_message_queue_create( misc_name[QUEUE_PRC0],
739 748 MSG_QUEUE_COUNT_PRC0, MSG_QUEUE_SIZE_PRC0,
740 749 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
741 750 if ( status_q_p0 != RTEMS_SUCCESSFUL ) {
742 751 PRINTF1("in create_message_queues *** ERR creating Q_P0 queue, %d\n", status_q_p0)
743 752 }
744 753
745 754 //*****************************************************************************
746 755 // create the queue for handling averaged spectral matrices for processing @ f1
747 756 status_q_p1 = rtems_message_queue_create( misc_name[QUEUE_PRC1],
748 757 MSG_QUEUE_COUNT_PRC1, MSG_QUEUE_SIZE_PRC1,
749 758 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
750 759 if ( status_q_p1 != RTEMS_SUCCESSFUL ) {
751 760 PRINTF1("in create_message_queues *** ERR creating Q_P1 queue, %d\n", status_q_p1)
752 761 }
753 762
754 763 //*****************************************************************************
755 764 // create the queue for handling averaged spectral matrices for processing @ f2
756 765 status_q_p2 = rtems_message_queue_create( misc_name[QUEUE_PRC2],
757 766 MSG_QUEUE_COUNT_PRC2, MSG_QUEUE_SIZE_PRC2,
758 767 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
759 768 if ( status_q_p2 != RTEMS_SUCCESSFUL ) {
760 769 PRINTF1("in create_message_queues *** ERR creating Q_P2 queue, %d\n", status_q_p2)
761 770 }
762 771
763 772 if ( status_recv != RTEMS_SUCCESSFUL )
764 773 {
765 774 ret = status_recv;
766 775 }
767 776 else if( status_send != RTEMS_SUCCESSFUL )
768 777 {
769 778 ret = status_send;
770 779 }
771 780 else if( status_q_p0 != RTEMS_SUCCESSFUL )
772 781 {
773 782 ret = status_q_p0;
774 783 }
775 784 else if( status_q_p1 != RTEMS_SUCCESSFUL )
776 785 {
777 786 ret = status_q_p1;
778 787 }
779 788 else
780 789 {
781 790 ret = status_q_p2;
782 791 }
783 792
784 793 return ret;
785 794 }
786 795
787 796 rtems_status_code create_timecode_timer( void )
788 797 {
789 798 rtems_status_code status;
790 799
791 800 status = rtems_timer_create( timecode_timer_name, &timecode_timer_id );
792 801
793 802 if ( status != RTEMS_SUCCESSFUL )
794 803 {
795 804 PRINTF1("in create_timer_timecode *** ERR creating SPTC timer, %d\n", status)
796 805 }
797 806 else
798 807 {
799 808 PRINTF("in create_timer_timecode *** OK creating SPTC timer\n")
800 809 }
801 810
802 811 return status;
803 812 }
804 813
805 814 rtems_status_code get_message_queue_id_send( rtems_id *queue_id )
806 815 {
807 816 rtems_status_code status;
808 817 rtems_name queue_name;
809 818
810 819 queue_name = rtems_build_name( 'Q', '_', 'S', 'D' );
811 820
812 821 status = rtems_message_queue_ident( queue_name, 0, queue_id );
813 822
814 823 return status;
815 824 }
816 825
817 826 rtems_status_code get_message_queue_id_recv( rtems_id *queue_id )
818 827 {
819 828 rtems_status_code status;
820 829 rtems_name queue_name;
821 830
822 831 queue_name = rtems_build_name( 'Q', '_', 'R', 'V' );
823 832
824 833 status = rtems_message_queue_ident( queue_name, 0, queue_id );
825 834
826 835 return status;
827 836 }
828 837
829 838 rtems_status_code get_message_queue_id_prc0( rtems_id *queue_id )
830 839 {
831 840 rtems_status_code status;
832 841 rtems_name queue_name;
833 842
834 843 queue_name = rtems_build_name( 'Q', '_', 'P', '0' );
835 844
836 845 status = rtems_message_queue_ident( queue_name, 0, queue_id );
837 846
838 847 return status;
839 848 }
840 849
841 850 rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id )
842 851 {
843 852 rtems_status_code status;
844 853 rtems_name queue_name;
845 854
846 855 queue_name = rtems_build_name( 'Q', '_', 'P', '1' );
847 856
848 857 status = rtems_message_queue_ident( queue_name, 0, queue_id );
849 858
850 859 return status;
851 860 }
852 861
853 862 rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id )
854 863 {
855 864 rtems_status_code status;
856 865 rtems_name queue_name;
857 866
858 867 queue_name = rtems_build_name( 'Q', '_', 'P', '2' );
859 868
860 869 status = rtems_message_queue_ident( queue_name, 0, queue_id );
861 870
862 871 return status;
863 872 }
864 873
865 874 void update_queue_max_count( rtems_id queue_id, unsigned char*fifo_size_max )
866 875 {
867 876 u_int32_t count;
868 877 rtems_status_code status;
869 878
870 879 status = rtems_message_queue_get_number_pending( queue_id, &count );
871 880
872 881 count = count + 1;
873 882
874 883 if (status != RTEMS_SUCCESSFUL)
875 884 {
876 885 PRINTF1("in update_queue_max_count *** ERR = %d\n", status)
877 886 }
878 887 else
879 888 {
880 889 if (count > *fifo_size_max)
881 890 {
882 891 *fifo_size_max = count;
883 892 }
884 893 }
885 894 }
886 895
887 896 void init_ring(ring_node ring[], unsigned char nbNodes, volatile int buffer[], unsigned int bufferSize )
888 897 {
889 898 unsigned char i;
890 899
891 900 //***************
892 901 // BUFFER ADDRESS
893 902 for(i=0; i<nbNodes; i++)
894 903 {
895 904 ring[i].coarseTime = 0xffffffff;
896 905 ring[i].fineTime = 0xffffffff;
897 906 ring[i].sid = 0x00;
898 907 ring[i].status = 0x00;
899 908 ring[i].buffer_address = (int) &buffer[ i * bufferSize ];
900 909 }
901 910
902 911 //*****
903 912 // NEXT
904 913 ring[ nbNodes - 1 ].next = (ring_node*) &ring[ 0 ];
905 914 for(i=0; i<nbNodes-1; i++)
906 915 {
907 916 ring[i].next = (ring_node*) &ring[ i + 1 ];
908 917 }
909 918
910 919 //*********
911 920 // PREVIOUS
912 921 ring[ 0 ].previous = (ring_node*) &ring[ nbNodes - 1 ];
913 922 for(i=1; i<nbNodes; i++)
914 923 {
915 924 ring[i].previous = (ring_node*) &ring[ i - 1 ];
916 925 }
917 926 }
@@ -1,790 +1,792
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 timer_configure(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( timer, clock_divider);
37 37 }
38 38
39 39 void timer_start(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(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(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 // WATCHDOG
83 83
84 84 rtems_isr watchdog_isr( rtems_vector_number vector )
85 85 {
86 86 rtems_status_code status_code;
87 87
88 88 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_12 );
89 89
90 90 PRINTF("watchdog_isr *** this is the end, exit(0)\n");
91 91
92 92 exit(0);
93 93 }
94 94
95 95 void watchdog_configure(void)
96 96 {
97 97 /** This function configure the watchdog.
98 98 *
99 99 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
100 100 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
101 101 *
102 102 * The watchdog is a timer provided by the GPTIMER IP core of the GRLIB.
103 103 *
104 104 */
105 105
106 106 LEON_Mask_interrupt( IRQ_GPTIMER_WATCHDOG ); // mask gptimer/watchdog interrupt during configuration
107 107
108 108 timer_configure( TIMER_WATCHDOG, CLKDIV_WATCHDOG, IRQ_SPARC_GPTIMER_WATCHDOG, watchdog_isr );
109 109
110 110 LEON_Clear_interrupt( IRQ_GPTIMER_WATCHDOG ); // clear gptimer/watchdog interrupt
111 111 }
112 112
113 113 void watchdog_stop(void)
114 114 {
115 115 LEON_Mask_interrupt( IRQ_GPTIMER_WATCHDOG ); // mask gptimer/watchdog interrupt line
116 116 timer_stop( TIMER_WATCHDOG );
117 117 LEON_Clear_interrupt( IRQ_GPTIMER_WATCHDOG ); // clear gptimer/watchdog interrupt
118 118 }
119 119
120 120 void watchdog_reload(void)
121 121 {
122 122 /** This function reloads the watchdog timer counter with the timer reload value.
123 123 *
124 124 * @param void
125 125 *
126 126 * @return void
127 127 *
128 128 */
129 129
130 130 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | 0x00000004; // LD load value from the reload register
131 131 }
132 132
133 133 void watchdog_start(void)
134 134 {
135 135 /** This function starts the watchdog timer.
136 136 *
137 137 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
138 138 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
139 139 *
140 140 */
141 141
142 142 LEON_Clear_interrupt( IRQ_GPTIMER_WATCHDOG );
143 143
144 144 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | 0x00000010; // clear pending IRQ if any
145 145 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | 0x00000004; // LD load value from the reload register
146 146 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | 0x00000001; // EN enable the timer
147 147 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | 0x00000008; // IE interrupt enable
148 148
149 149 LEON_Unmask_interrupt( IRQ_GPTIMER_WATCHDOG );
150 150
151 151 }
152 152
153 153 int enable_apbuart_transmitter( void ) // set the bit 1, TE Transmitter Enable to 1 in the APBUART control register
154 154 {
155 155 struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) REGS_ADDR_APBUART;
156 156
157 157 apbuart_regs->ctrl = APBUART_CTRL_REG_MASK_TE;
158 158
159 159 return 0;
160 160 }
161 161
162 162 void set_apbuart_scaler_reload_register(unsigned int regs, unsigned int value)
163 163 {
164 164 /** This function sets the scaler reload register of the apbuart module
165 165 *
166 166 * @param regs is the address of the apbuart registers in memory
167 167 * @param value is the value that will be stored in the scaler register
168 168 *
169 169 * The value shall be set by the software to get data on the serial interface.
170 170 *
171 171 */
172 172
173 173 struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) regs;
174 174
175 175 apbuart_regs->scaler = value;
176 176
177 177 BOOT_PRINTF1("OK *** apbuart port scaler reload register set to 0x%x\n", value)
178 178 }
179 179
180 180 //************
181 181 // RTEMS TASKS
182 182
183 183 rtems_task load_task(rtems_task_argument argument)
184 184 {
185 185 BOOT_PRINTF("in LOAD *** \n")
186 186
187 187 rtems_status_code status;
188 188 unsigned int i;
189 189 unsigned int j;
190 190 rtems_name name_watchdog_rate_monotonic; // name of the watchdog rate monotonic
191 191 rtems_id watchdog_period_id; // id of the watchdog rate monotonic period
192 192
193 193 name_watchdog_rate_monotonic = rtems_build_name( 'L', 'O', 'A', 'D' );
194 194
195 195 status = rtems_rate_monotonic_create( name_watchdog_rate_monotonic, &watchdog_period_id );
196 196 if( status != RTEMS_SUCCESSFUL ) {
197 197 PRINTF1( "in LOAD *** rtems_rate_monotonic_create failed with status of %d\n", status )
198 198 }
199 199
200 200 i = 0;
201 201 j = 0;
202 202
203 203 watchdog_configure();
204 204
205 205 watchdog_start();
206 206
207 207 set_sy_lfr_watchdog_enabled( true );
208 208
209 209 while(1){
210 210 status = rtems_rate_monotonic_period( watchdog_period_id, WATCHDOG_PERIOD );
211 211 watchdog_reload();
212 212 i = i + 1;
213 213 if ( i == 10 )
214 214 {
215 215 i = 0;
216 216 j = j + 1;
217 217 PRINTF1("%d\n", j)
218 218 }
219 219 #ifdef DEBUG_WATCHDOG
220 220 if (j == 3 )
221 221 {
222 222 status = rtems_task_delete(RTEMS_SELF);
223 223 }
224 224 #endif
225 225 }
226 226 }
227 227
228 228 rtems_task hous_task(rtems_task_argument argument)
229 229 {
230 230 rtems_status_code status;
231 231 rtems_status_code spare_status;
232 232 rtems_id queue_id;
233 233 rtems_rate_monotonic_period_status period_status;
234 234
235 235 status = get_message_queue_id_send( &queue_id );
236 236 if (status != RTEMS_SUCCESSFUL)
237 237 {
238 238 PRINTF1("in HOUS *** ERR get_message_queue_id_send %d\n", status)
239 239 }
240 240
241 241 BOOT_PRINTF("in HOUS ***\n");
242 242
243 243 if (rtems_rate_monotonic_ident( name_hk_rate_monotonic, &HK_id) != RTEMS_SUCCESSFUL) {
244 244 status = rtems_rate_monotonic_create( name_hk_rate_monotonic, &HK_id );
245 245 if( status != RTEMS_SUCCESSFUL ) {
246 246 PRINTF1( "rtems_rate_monotonic_create failed with status of %d\n", status );
247 247 }
248 248 }
249 249
250 250 status = rtems_rate_monotonic_cancel(HK_id);
251 251 if( status != RTEMS_SUCCESSFUL ) {
252 252 PRINTF1( "ERR *** in HOUS *** rtems_rate_monotonic_cancel(HK_id) ***code: %d\n", status );
253 253 }
254 254 else {
255 255 DEBUG_PRINTF("OK *** in HOUS *** rtems_rate_monotonic_cancel(HK_id)\n");
256 256 }
257 257
258 258 // startup phase
259 259 status = rtems_rate_monotonic_period( HK_id, SY_LFR_TIME_SYN_TIMEOUT_in_ticks );
260 260 status = rtems_rate_monotonic_get_status( HK_id, &period_status );
261 261 DEBUG_PRINTF1("startup HK, HK_id status = %d\n", period_status.state)
262 262 while(period_status.state != RATE_MONOTONIC_EXPIRED ) // after SY_LFR_TIME_SYN_TIMEOUT ms, starts HK anyway
263 263 {
264 264 if ((time_management_regs->coarse_time & 0x80000000) == 0x00000000) // check time synchronization
265 265 {
266 266 break; // break if LFR is synchronized
267 267 }
268 268 else
269 269 {
270 270 status = rtems_rate_monotonic_get_status( HK_id, &period_status );
271 271 // sched_yield();
272 272 status = rtems_task_wake_after( 10 ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 100 ms = 10 * 10 ms
273 273 }
274 274 }
275 275 status = rtems_rate_monotonic_cancel(HK_id);
276 276 DEBUG_PRINTF1("startup HK, HK_id status = %d\n", period_status.state)
277 277
278 278 set_hk_lfr_reset_cause( POWER_ON );
279 279
280 280 while(1){ // launch the rate monotonic task
281 281 status = rtems_rate_monotonic_period( HK_id, HK_PERIOD );
282 282 if ( status != RTEMS_SUCCESSFUL ) {
283 283 PRINTF1( "in HOUS *** ERR period: %d\n", status);
284 284 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_6 );
285 285 }
286 286 else {
287 287 housekeeping_packet.packetSequenceControl[0] = (unsigned char) (sequenceCounterHK >> 8);
288 288 housekeeping_packet.packetSequenceControl[1] = (unsigned char) (sequenceCounterHK );
289 289 increment_seq_counter( &sequenceCounterHK );
290 290
291 291 housekeeping_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
292 292 housekeeping_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
293 293 housekeeping_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
294 294 housekeeping_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
295 295 housekeeping_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
296 296 housekeeping_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
297 297
298 298 spacewire_update_hk_lfr_link_state( &housekeeping_packet.lfr_status_word[0] );
299 299
300 300 spacewire_read_statistics();
301 301
302 302 update_hk_with_grspw_stats();
303 303
304 304 set_hk_lfr_time_not_synchro();
305 305
306 306 housekeeping_packet.hk_lfr_q_sd_fifo_size_max = hk_lfr_q_sd_fifo_size_max;
307 307 housekeeping_packet.hk_lfr_q_rv_fifo_size_max = hk_lfr_q_rv_fifo_size_max;
308 308 housekeeping_packet.hk_lfr_q_p0_fifo_size_max = hk_lfr_q_p0_fifo_size_max;
309 309 housekeeping_packet.hk_lfr_q_p1_fifo_size_max = hk_lfr_q_p1_fifo_size_max;
310 310 housekeeping_packet.hk_lfr_q_p2_fifo_size_max = hk_lfr_q_p2_fifo_size_max;
311 311
312 312 housekeeping_packet.sy_lfr_common_parameters_spare = parameter_dump_packet.sy_lfr_common_parameters_spare;
313 313 housekeeping_packet.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
314 314 get_temperatures( housekeeping_packet.hk_lfr_temp_scm );
315 315 get_v_e1_e2_f3( housekeeping_packet.hk_lfr_sc_v_f3 );
316 316 get_cpu_load( (unsigned char *) &housekeeping_packet.hk_lfr_cpu_load );
317 317
318 318 hk_lfr_le_me_he_update();
319 319
320 housekeeping_packet.hk_lfr_sc_rw_f_flags = cp_rpw_sc_rw_f_flags;
321
320 322 // SEND PACKET
321 323 status = rtems_message_queue_send( queue_id, &housekeeping_packet,
322 324 PACKET_LENGTH_HK + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
323 325 if (status != RTEMS_SUCCESSFUL) {
324 326 PRINTF1("in HOUS *** ERR send: %d\n", status)
325 327 }
326 328 }
327 329 }
328 330
329 331 PRINTF("in HOUS *** deleting task\n")
330 332
331 333 status = rtems_task_delete( RTEMS_SELF ); // should not return
332 334
333 335 return;
334 336 }
335 337
336 338 rtems_task dumb_task( rtems_task_argument unused )
337 339 {
338 340 /** This RTEMS taks is used to print messages without affecting the general behaviour of the software.
339 341 *
340 342 * @param unused is the starting argument of the RTEMS task
341 343 *
342 344 * The DUMB taks waits for RTEMS events and print messages depending on the incoming events.
343 345 *
344 346 */
345 347
346 348 unsigned int i;
347 349 unsigned int intEventOut;
348 350 unsigned int coarse_time = 0;
349 351 unsigned int fine_time = 0;
350 352 rtems_event_set event_out;
351 353
352 354 char *DumbMessages[15] = {"in DUMB *** default", // RTEMS_EVENT_0
353 355 "in DUMB *** timecode_irq_handler", // RTEMS_EVENT_1
354 356 "in DUMB *** f3 buffer changed", // RTEMS_EVENT_2
355 357 "in DUMB *** in SMIQ *** Error sending event to AVF0", // RTEMS_EVENT_3
356 358 "in DUMB *** spectral_matrices_isr *** Error sending event to SMIQ", // RTEMS_EVENT_4
357 359 "in DUMB *** waveforms_simulator_isr", // RTEMS_EVENT_5
358 360 "VHDL SM *** two buffers f0 ready", // RTEMS_EVENT_6
359 361 "ready for dump", // RTEMS_EVENT_7
360 362 "VHDL ERR *** spectral matrix", // RTEMS_EVENT_8
361 363 "tick", // RTEMS_EVENT_9
362 364 "VHDL ERR *** waveform picker", // RTEMS_EVENT_10
363 365 "VHDL ERR *** unexpected ready matrix values", // RTEMS_EVENT_11
364 366 "WATCHDOG timer", // RTEMS_EVENT_12
365 367 "TIMECODE timer", // RTEMS_EVENT_13
366 368 "TIMECODE ISR" // RTEMS_EVENT_14
367 369 };
368 370
369 371 BOOT_PRINTF("in DUMB *** \n")
370 372
371 373 while(1){
372 374 rtems_event_receive(RTEMS_EVENT_0 | RTEMS_EVENT_1 | RTEMS_EVENT_2 | RTEMS_EVENT_3
373 375 | RTEMS_EVENT_4 | RTEMS_EVENT_5 | RTEMS_EVENT_6 | RTEMS_EVENT_7
374 376 | RTEMS_EVENT_8 | RTEMS_EVENT_9 | RTEMS_EVENT_12 | RTEMS_EVENT_13
375 377 | RTEMS_EVENT_14,
376 378 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT
377 379 intEventOut = (unsigned int) event_out;
378 380 for ( i=0; i<32; i++)
379 381 {
380 382 if ( ((intEventOut >> i) & 0x0001) != 0)
381 383 {
382 384 coarse_time = time_management_regs->coarse_time;
383 385 fine_time = time_management_regs->fine_time;
384 386 if (i==12)
385 387 {
386 388 PRINTF1("%s\n", DumbMessages[12])
387 389 }
388 390 if (i==13)
389 391 {
390 392 PRINTF1("%s\n", DumbMessages[13])
391 393 }
392 394 if (i==14)
393 395 {
394 396 PRINTF1("%s\n", DumbMessages[1])
395 397 }
396 398 }
397 399 }
398 400 }
399 401 }
400 402
401 403 //*****************************
402 404 // init housekeeping parameters
403 405
404 406 void init_housekeeping_parameters( void )
405 407 {
406 408 /** This function initialize the housekeeping_packet global variable with default values.
407 409 *
408 410 */
409 411
410 412 unsigned int i = 0;
411 413 unsigned char *parameters;
412 414 unsigned char sizeOfHK;
413 415
414 416 sizeOfHK = sizeof( Packet_TM_LFR_HK_t );
415 417
416 418 parameters = (unsigned char*) &housekeeping_packet;
417 419
418 420 for(i = 0; i< sizeOfHK; i++)
419 421 {
420 422 parameters[i] = 0x00;
421 423 }
422 424
423 425 housekeeping_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
424 426 housekeeping_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
425 427 housekeeping_packet.reserved = DEFAULT_RESERVED;
426 428 housekeeping_packet.userApplication = CCSDS_USER_APP;
427 429 housekeeping_packet.packetID[0] = (unsigned char) (APID_TM_HK >> 8);
428 430 housekeeping_packet.packetID[1] = (unsigned char) (APID_TM_HK);
429 431 housekeeping_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
430 432 housekeeping_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
431 433 housekeeping_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_HK >> 8);
432 434 housekeeping_packet.packetLength[1] = (unsigned char) (PACKET_LENGTH_HK );
433 435 housekeeping_packet.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
434 436 housekeeping_packet.serviceType = TM_TYPE_HK;
435 437 housekeeping_packet.serviceSubType = TM_SUBTYPE_HK;
436 438 housekeeping_packet.destinationID = TM_DESTINATION_ID_GROUND;
437 439 housekeeping_packet.sid = SID_HK;
438 440
439 441 // init status word
440 442 housekeeping_packet.lfr_status_word[0] = DEFAULT_STATUS_WORD_BYTE0;
441 443 housekeeping_packet.lfr_status_word[1] = DEFAULT_STATUS_WORD_BYTE1;
442 444 // init software version
443 445 housekeeping_packet.lfr_sw_version[0] = SW_VERSION_N1;
444 446 housekeeping_packet.lfr_sw_version[1] = SW_VERSION_N2;
445 447 housekeeping_packet.lfr_sw_version[2] = SW_VERSION_N3;
446 448 housekeeping_packet.lfr_sw_version[3] = SW_VERSION_N4;
447 449 // init fpga version
448 450 parameters = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
449 451 housekeeping_packet.lfr_fpga_version[0] = parameters[1]; // n1
450 452 housekeeping_packet.lfr_fpga_version[1] = parameters[2]; // n2
451 453 housekeeping_packet.lfr_fpga_version[2] = parameters[3]; // n3
452 454
453 455 housekeeping_packet.hk_lfr_q_sd_fifo_size = MSG_QUEUE_COUNT_SEND;
454 456 housekeeping_packet.hk_lfr_q_rv_fifo_size = MSG_QUEUE_COUNT_RECV;
455 457 housekeeping_packet.hk_lfr_q_p0_fifo_size = MSG_QUEUE_COUNT_PRC0;
456 458 housekeeping_packet.hk_lfr_q_p1_fifo_size = MSG_QUEUE_COUNT_PRC1;
457 459 housekeeping_packet.hk_lfr_q_p2_fifo_size = MSG_QUEUE_COUNT_PRC2;
458 460 }
459 461
460 462 void increment_seq_counter( unsigned short *packetSequenceControl )
461 463 {
462 464 /** This function increment the sequence counter passes in argument.
463 465 *
464 466 * The increment does not affect the grouping flag. In case of an overflow, the counter is reset to 0.
465 467 *
466 468 */
467 469
468 470 unsigned short segmentation_grouping_flag;
469 471 unsigned short sequence_cnt;
470 472
471 473 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << 8; // keep bits 7 downto 6
472 474 sequence_cnt = (*packetSequenceControl) & 0x3fff; // [0011 1111 1111 1111]
473 475
474 476 if ( sequence_cnt < SEQ_CNT_MAX)
475 477 {
476 478 sequence_cnt = sequence_cnt + 1;
477 479 }
478 480 else
479 481 {
480 482 sequence_cnt = 0;
481 483 }
482 484
483 485 *packetSequenceControl = segmentation_grouping_flag | sequence_cnt ;
484 486 }
485 487
486 488 void getTime( unsigned char *time)
487 489 {
488 490 /** This function write the current local time in the time buffer passed in argument.
489 491 *
490 492 */
491 493
492 494 time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
493 495 time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
494 496 time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
495 497 time[3] = (unsigned char) (time_management_regs->coarse_time);
496 498 time[4] = (unsigned char) (time_management_regs->fine_time>>8);
497 499 time[5] = (unsigned char) (time_management_regs->fine_time);
498 500 }
499 501
500 502 unsigned long long int getTimeAsUnsignedLongLongInt( )
501 503 {
502 504 /** This function write the current local time in the time buffer passed in argument.
503 505 *
504 506 */
505 507 unsigned long long int time;
506 508
507 509 time = ( (unsigned long long int) (time_management_regs->coarse_time & 0x7fffffff) << 16 )
508 510 + time_management_regs->fine_time;
509 511
510 512 return time;
511 513 }
512 514
513 515 void send_dumb_hk( void )
514 516 {
515 517 Packet_TM_LFR_HK_t dummy_hk_packet;
516 518 unsigned char *parameters;
517 519 unsigned int i;
518 520 rtems_id queue_id;
519 521
520 522 dummy_hk_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
521 523 dummy_hk_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
522 524 dummy_hk_packet.reserved = DEFAULT_RESERVED;
523 525 dummy_hk_packet.userApplication = CCSDS_USER_APP;
524 526 dummy_hk_packet.packetID[0] = (unsigned char) (APID_TM_HK >> 8);
525 527 dummy_hk_packet.packetID[1] = (unsigned char) (APID_TM_HK);
526 528 dummy_hk_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
527 529 dummy_hk_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
528 530 dummy_hk_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_HK >> 8);
529 531 dummy_hk_packet.packetLength[1] = (unsigned char) (PACKET_LENGTH_HK );
530 532 dummy_hk_packet.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
531 533 dummy_hk_packet.serviceType = TM_TYPE_HK;
532 534 dummy_hk_packet.serviceSubType = TM_SUBTYPE_HK;
533 535 dummy_hk_packet.destinationID = TM_DESTINATION_ID_GROUND;
534 536 dummy_hk_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
535 537 dummy_hk_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
536 538 dummy_hk_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
537 539 dummy_hk_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
538 540 dummy_hk_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
539 541 dummy_hk_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
540 542 dummy_hk_packet.sid = SID_HK;
541 543
542 544 // init status word
543 545 dummy_hk_packet.lfr_status_word[0] = 0xff;
544 546 dummy_hk_packet.lfr_status_word[1] = 0xff;
545 547 // init software version
546 548 dummy_hk_packet.lfr_sw_version[0] = SW_VERSION_N1;
547 549 dummy_hk_packet.lfr_sw_version[1] = SW_VERSION_N2;
548 550 dummy_hk_packet.lfr_sw_version[2] = SW_VERSION_N3;
549 551 dummy_hk_packet.lfr_sw_version[3] = SW_VERSION_N4;
550 552 // init fpga version
551 553 parameters = (unsigned char *) (REGS_ADDR_WAVEFORM_PICKER + 0xb0);
552 554 dummy_hk_packet.lfr_fpga_version[0] = parameters[1]; // n1
553 555 dummy_hk_packet.lfr_fpga_version[1] = parameters[2]; // n2
554 556 dummy_hk_packet.lfr_fpga_version[2] = parameters[3]; // n3
555 557
556 558 parameters = (unsigned char *) &dummy_hk_packet.hk_lfr_cpu_load;
557 559
558 560 for (i=0; i<100; i++)
559 561 {
560 562 parameters[i] = 0xff;
561 563 }
562 564
563 565 get_message_queue_id_send( &queue_id );
564 566
565 567 rtems_message_queue_send( queue_id, &dummy_hk_packet,
566 568 PACKET_LENGTH_HK + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
567 569 }
568 570
569 571 void get_temperatures( unsigned char *temperatures )
570 572 {
571 573 unsigned char* temp_scm_ptr;
572 574 unsigned char* temp_pcb_ptr;
573 575 unsigned char* temp_fpga_ptr;
574 576
575 577 // SEL1 SEL0
576 578 // 0 0 => PCB
577 579 // 0 1 => FPGA
578 580 // 1 0 => SCM
579 581
580 582 temp_scm_ptr = (unsigned char *) &time_management_regs->temp_scm;
581 583 temp_pcb_ptr = (unsigned char *) &time_management_regs->temp_pcb;
582 584 temp_fpga_ptr = (unsigned char *) &time_management_regs->temp_fpga;
583 585
584 586 temperatures[0] = temp_scm_ptr[2];
585 587 temperatures[1] = temp_scm_ptr[3];
586 588 temperatures[2] = temp_pcb_ptr[2];
587 589 temperatures[3] = temp_pcb_ptr[3];
588 590 temperatures[4] = temp_fpga_ptr[2];
589 591 temperatures[5] = temp_fpga_ptr[3];
590 592 }
591 593
592 594 void get_v_e1_e2_f3( unsigned char *spacecraft_potential )
593 595 {
594 596 unsigned char* v_ptr;
595 597 unsigned char* e1_ptr;
596 598 unsigned char* e2_ptr;
597 599
598 600 v_ptr = (unsigned char *) &waveform_picker_regs->v;
599 601 e1_ptr = (unsigned char *) &waveform_picker_regs->e1;
600 602 e2_ptr = (unsigned char *) &waveform_picker_regs->e2;
601 603
602 604 spacecraft_potential[0] = v_ptr[2];
603 605 spacecraft_potential[1] = v_ptr[3];
604 606 spacecraft_potential[2] = e1_ptr[2];
605 607 spacecraft_potential[3] = e1_ptr[3];
606 608 spacecraft_potential[4] = e2_ptr[2];
607 609 spacecraft_potential[5] = e2_ptr[3];
608 610 }
609 611
610 612 void get_cpu_load( unsigned char *resource_statistics )
611 613 {
612 614 unsigned char cpu_load;
613 615
614 616 cpu_load = lfr_rtems_cpu_usage_report();
615 617
616 618 // HK_LFR_CPU_LOAD
617 619 resource_statistics[0] = cpu_load;
618 620
619 621 // HK_LFR_CPU_LOAD_MAX
620 622 if (cpu_load > resource_statistics[1])
621 623 {
622 624 resource_statistics[1] = cpu_load;
623 625 }
624 626
625 627 // CPU_LOAD_AVE
626 628 resource_statistics[2] = 0;
627 629
628 630 #ifndef PRINT_TASK_STATISTICS
629 631 rtems_cpu_usage_reset();
630 632 #endif
631 633
632 634 }
633 635
634 636 void set_hk_lfr_sc_potential_flag( bool state )
635 637 {
636 638 if (state == true)
637 639 {
638 640 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1] | 0x40; // [0100 0000]
639 641 }
640 642 else
641 643 {
642 644 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1] & 0xbf; // [1011 1111]
643 645 }
644 646 }
645 647
646 648 void set_sy_lfr_watchdog_enabled( bool state )
647 649 {
648 650 if (state == true)
649 651 {
650 652 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1] | 0x10; // [0001 0000]
651 653 }
652 654 else
653 655 {
654 656 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1] & 0xef; // [1110 1111]
655 657 }
656 658 }
657 659
658 660 void set_hk_lfr_calib_enable( bool state )
659 661 {
660 662 if (state == true)
661 663 {
662 664 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1] | 0x08; // [0000 1000]
663 665 }
664 666 else
665 667 {
666 668 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1] & 0xf7; // [1111 0111]
667 669 }
668 670 }
669 671
670 672 void set_hk_lfr_reset_cause( enum lfr_reset_cause_t lfr_reset_cause )
671 673 {
672 674 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1] & 0xf8; // [1111 1000]
673 675
674 676 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1]
675 677 | (lfr_reset_cause & 0x07 ); // [0000 0111]
676 678
677 679 }
678 680
679 681 void hk_lfr_le_me_he_update()
680 682 {
681 683 unsigned int hk_lfr_le_cnt;
682 684 unsigned int hk_lfr_me_cnt;
683 685 unsigned int hk_lfr_he_cnt;
684 686
685 687 hk_lfr_le_cnt = 0;
686 688 hk_lfr_me_cnt = 0;
687 689 hk_lfr_he_cnt = 0;
688 690
689 691 //update the low severity error counter
690 692 hk_lfr_le_cnt =
691 693 housekeeping_packet.hk_lfr_dpu_spw_parity
692 694 + housekeeping_packet.hk_lfr_dpu_spw_disconnect
693 695 + housekeeping_packet.hk_lfr_dpu_spw_escape
694 696 + housekeeping_packet.hk_lfr_dpu_spw_credit
695 697 + housekeeping_packet.hk_lfr_dpu_spw_write_sync
696 698 + housekeeping_packet.hk_lfr_timecode_erroneous
697 699 + housekeeping_packet.hk_lfr_timecode_missing
698 700 + housekeeping_packet.hk_lfr_timecode_invalid
699 701 + housekeeping_packet.hk_lfr_time_timecode_it
700 702 + housekeeping_packet.hk_lfr_time_not_synchro
701 703 + housekeeping_packet.hk_lfr_time_timecode_ctr;
702 704 // housekeeping_packet.hk_lfr_dpu_spw_rx_ahb => not handled by the grspw driver
703 705 // housekeeping_packet.hk_lfr_dpu_spw_tx_ahb => not handled by the grspw driver
704 706
705 707 //update the medium severity error counter
706 708 hk_lfr_me_cnt =
707 709 housekeeping_packet.hk_lfr_dpu_spw_early_eop
708 710 + housekeeping_packet.hk_lfr_dpu_spw_invalid_addr
709 711 + housekeeping_packet.hk_lfr_dpu_spw_eep
710 712 + housekeeping_packet.hk_lfr_dpu_spw_rx_too_big;
711 713
712 714 //update the high severity error counter
713 715 hk_lfr_he_cnt = 0;
714 716
715 717 // update housekeeping packet counters, convert unsigned int numbers in 2 bytes numbers
716 718 // LE
717 719 housekeeping_packet.hk_lfr_le_cnt[0] = (unsigned char) ((hk_lfr_le_cnt & 0xff00) >> 8);
718 720 housekeeping_packet.hk_lfr_le_cnt[1] = (unsigned char) (hk_lfr_le_cnt & 0x00ff);
719 721 // ME
720 722 housekeeping_packet.hk_lfr_me_cnt[0] = (unsigned char) ((hk_lfr_me_cnt & 0xff00) >> 8);
721 723 housekeeping_packet.hk_lfr_me_cnt[1] = (unsigned char) (hk_lfr_me_cnt & 0x00ff);
722 724 // HE
723 725 housekeeping_packet.hk_lfr_he_cnt[0] = (unsigned char) ((hk_lfr_he_cnt & 0xff00) >> 8);
724 726 housekeeping_packet.hk_lfr_he_cnt[1] = (unsigned char) (hk_lfr_he_cnt & 0x00ff);
725 727
726 728 }
727 729
728 730 void set_hk_lfr_time_not_synchro()
729 731 {
730 732 static unsigned char synchroLost = 1;
731 733 int synchronizationBit;
732 734
733 735 // get the synchronization bit
734 736 synchronizationBit = (time_management_regs->coarse_time & 0x80000000) >> 31; // 1000 0000 0000 0000
735 737
736 738 switch (synchronizationBit)
737 739 {
738 740 case 0:
739 741 if (synchroLost == 1)
740 742 {
741 743 synchroLost = 0;
742 744 }
743 745 break;
744 746 case 1:
745 747 if (synchroLost == 0 )
746 748 {
747 749 synchroLost = 1;
748 750 increase_unsigned_char_counter(&housekeeping_packet.hk_lfr_time_not_synchro);
749 751 update_hk_lfr_last_er_fields( RID_LE_LFR_TIME, CODE_NOT_SYNCHRO );
750 752 }
751 753 break;
752 754 default:
753 755 PRINTF1("in hk_lfr_time_not_synchro *** unexpected value for synchronizationBit = %d\n", synchronizationBit);
754 756 break;
755 757 }
756 758
757 759 }
758 760
759 761 void set_hk_lfr_ahb_correctable()
760 762 {
761 763 /** This function builds the error counter hk_lfr_ahb_correctable using the statistics provided
762 764 * by the Cache Control Register (ASI 2, offset 0) and in the Register Protection Control Register (ASR16) on the
763 765 * detected errors in the cache, in the integer unit and in the floating point unit.
764 766 *
765 767 * @param void
766 768 *
767 769 * @return void
768 770 *
769 771 * All errors are summed to set the value of the hk_lfr_ahb_correctable counter.
770 772 *
771 773 */
772 774
773 775 unsigned int ahb_correctable;
774 776 unsigned int instructionErrorCounter;
775 777 unsigned int dataErrorCounter;
776 778 unsigned int fprfErrorCounter;
777 779 unsigned int iurfErrorCounter;
778 780
779 781 CCR_getInstructionAndDataErrorCounters( &instructionErrorCounter, &dataErrorCounter);
780 782 ASR16_get_FPRF_IURF_ErrorCounters( &fprfErrorCounter, &iurfErrorCounter);
781 783
782 784 ahb_correctable = instructionErrorCounter
783 785 + dataErrorCounter
784 786 + fprfErrorCounter
785 787 + iurfErrorCounter
786 788 + housekeeping_packet.hk_lfr_ahb_correctable;
787 789
788 790 housekeeping_packet.hk_lfr_ahb_correctable = (unsigned char) (ahb_correctable & 0xff); // [1111 1111]
789 791
790 792 }
@@ -1,1598 +1,1598
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 unsigned char previousTimecodeCtr = 0;
26 26 unsigned int *grspwPtr = (unsigned int *) (REGS_ADDR_GRSPW + APB_OFFSET_GRSPW_TIME_REGISTER);
27 27
28 28 //***********
29 29 // RTEMS TASK
30 30 rtems_task spiq_task(rtems_task_argument unused)
31 31 {
32 32 /** This RTEMS task is awaken by an rtems_event sent by the interruption subroutine of the SpaceWire driver.
33 33 *
34 34 * @param unused is the starting argument of the RTEMS task
35 35 *
36 36 */
37 37
38 38 rtems_event_set event_out;
39 39 rtems_status_code status;
40 40 int linkStatus;
41 41
42 42 BOOT_PRINTF("in SPIQ *** \n")
43 43
44 44 while(true){
45 45 rtems_event_receive(SPW_LINKERR_EVENT, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an SPW_LINKERR_EVENT
46 46 PRINTF("in SPIQ *** got SPW_LINKERR_EVENT\n")
47 47
48 48 // [0] SUSPEND RECV AND SEND TASKS
49 49 status = rtems_task_suspend( Task_id[ TASKID_RECV ] );
50 50 if ( status != RTEMS_SUCCESSFUL ) {
51 51 PRINTF("in SPIQ *** ERR suspending RECV Task\n")
52 52 }
53 53 status = rtems_task_suspend( Task_id[ TASKID_SEND ] );
54 54 if ( status != RTEMS_SUCCESSFUL ) {
55 55 PRINTF("in SPIQ *** ERR suspending SEND Task\n")
56 56 }
57 57
58 58 // [1] CHECK THE LINK
59 59 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (1)
60 60 if ( linkStatus != 5) {
61 61 PRINTF1("in SPIQ *** linkStatus %d, wait...\n", linkStatus)
62 62 status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms
63 63 }
64 64
65 65 // [2] RECHECK THE LINK AFTER SY_LFR_DPU_CONNECT_TIMEOUT
66 66 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (2)
67 67 if ( linkStatus != 5 ) // [2.a] not in run state, reset the link
68 68 {
69 69 spacewire_read_statistics();
70 70 status = spacewire_several_connect_attemps( );
71 71 }
72 72 else // [2.b] in run state, start the link
73 73 {
74 74 status = spacewire_stop_and_start_link( fdSPW ); // start the link
75 75 if ( status != RTEMS_SUCCESSFUL)
76 76 {
77 77 PRINTF1("in SPIQ *** ERR spacewire_stop_and_start_link %d\n", status)
78 78 }
79 79 }
80 80
81 81 // [3] COMPLETE RECOVERY ACTION AFTER SY_LFR_DPU_CONNECT_ATTEMPTS
82 82 if ( status == RTEMS_SUCCESSFUL ) // [3.a] the link is in run state and has been started successfully
83 83 {
84 84 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
85 85 if ( status != RTEMS_SUCCESSFUL ) {
86 86 PRINTF("in SPIQ *** ERR resuming SEND Task\n")
87 87 }
88 88 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
89 89 if ( status != RTEMS_SUCCESSFUL ) {
90 90 PRINTF("in SPIQ *** ERR resuming RECV Task\n")
91 91 }
92 92 }
93 93 else // [3.b] the link is not in run state, go in STANDBY mode
94 94 {
95 95 status = enter_mode_standby();
96 96 if ( status != RTEMS_SUCCESSFUL )
97 97 {
98 98 PRINTF1("in SPIQ *** ERR enter_standby_mode *** code %d\n", status)
99 99 }
100 100 {
101 101 updateLFRCurrentMode( LFR_MODE_STANDBY );
102 102 }
103 103 // wake the LINK task up to wait for the link recovery
104 104 status = rtems_event_send ( Task_id[TASKID_LINK], RTEMS_EVENT_0 );
105 105 status = rtems_task_suspend( RTEMS_SELF );
106 106 }
107 107 }
108 108 }
109 109
110 110 rtems_task recv_task( rtems_task_argument unused )
111 111 {
112 112 /** This RTEMS task is dedicated to the reception of incoming TeleCommands.
113 113 *
114 114 * @param unused is the starting argument of the RTEMS task
115 115 *
116 116 * The RECV task blocks on a call to the read system call, waiting for incoming SpaceWire data. When unblocked:
117 117 * 1. It reads the incoming data.
118 118 * 2. Launches the acceptance procedure.
119 119 * 3. If the Telecommand is valid, sends it to a dedicated RTEMS message queue.
120 120 *
121 121 */
122 122
123 123 int len;
124 124 ccsdsTelecommandPacket_t currentTC;
125 125 unsigned char computed_CRC[ 2 ];
126 126 unsigned char currentTC_LEN_RCV[ 2 ];
127 127 unsigned char destinationID;
128 128 unsigned int estimatedPacketLength;
129 129 unsigned int parserCode;
130 130 rtems_status_code status;
131 131 rtems_id queue_recv_id;
132 132 rtems_id queue_send_id;
133 133
134 134 initLookUpTableForCRC(); // the table is used to compute Cyclic Redundancy Codes
135 135
136 136 status = get_message_queue_id_recv( &queue_recv_id );
137 137 if (status != RTEMS_SUCCESSFUL)
138 138 {
139 139 PRINTF1("in RECV *** ERR get_message_queue_id_recv %d\n", status)
140 140 }
141 141
142 142 status = get_message_queue_id_send( &queue_send_id );
143 143 if (status != RTEMS_SUCCESSFUL)
144 144 {
145 145 PRINTF1("in RECV *** ERR get_message_queue_id_send %d\n", status)
146 146 }
147 147
148 148 BOOT_PRINTF("in RECV *** \n")
149 149
150 150 while(1)
151 151 {
152 152 len = read( fdSPW, (char*) &currentTC, CCSDS_TC_PKT_MAX_SIZE ); // the call to read is blocking
153 153 if (len == -1){ // error during the read call
154 154 PRINTF1("in RECV *** last read call returned -1, ERRNO %d\n", errno)
155 155 }
156 156 else {
157 157 if ( (len+1) < CCSDS_TC_PKT_MIN_SIZE ) {
158 158 PRINTF("in RECV *** packet lenght too short\n")
159 159 }
160 160 else {
161 161 estimatedPacketLength = (unsigned int) (len - CCSDS_TC_TM_PACKET_OFFSET - 3); // => -3 is for Prot ID, Reserved and User App bytes
162 162 currentTC_LEN_RCV[ 0 ] = (unsigned char) (estimatedPacketLength >> 8);
163 163 currentTC_LEN_RCV[ 1 ] = (unsigned char) (estimatedPacketLength );
164 164 // CHECK THE TC
165 165 parserCode = tc_parser( &currentTC, estimatedPacketLength, computed_CRC ) ;
166 166 if ( (parserCode == ILLEGAL_APID) || (parserCode == WRONG_LEN_PKT)
167 167 || (parserCode == INCOR_CHECKSUM) || (parserCode == ILL_TYPE)
168 168 || (parserCode == ILL_SUBTYPE) || (parserCode == WRONG_APP_DATA)
169 169 || (parserCode == WRONG_SRC_ID) )
170 170 { // send TM_LFR_TC_EXE_CORRUPTED
171 171 PRINTF1("TC corrupted received, with code: %d\n", parserCode)
172 172 if ( !( (currentTC.serviceType==TC_TYPE_TIME) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_TIME) )
173 173 &&
174 174 !( (currentTC.serviceType==TC_TYPE_GEN) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_INFO))
175 175 )
176 176 {
177 177 if ( parserCode == WRONG_SRC_ID )
178 178 {
179 179 destinationID = SID_TC_GROUND;
180 180 }
181 181 else
182 182 {
183 183 destinationID = currentTC.sourceID;
184 184 }
185 185 send_tm_lfr_tc_exe_corrupted( &currentTC, queue_send_id,
186 186 computed_CRC, currentTC_LEN_RCV,
187 187 destinationID );
188 188 }
189 189 }
190 190 else
191 191 { // send valid TC to the action launcher
192 192 status = rtems_message_queue_send( queue_recv_id, &currentTC,
193 193 estimatedPacketLength + CCSDS_TC_TM_PACKET_OFFSET + 3);
194 194 }
195 195 }
196 196 }
197 197
198 198 update_queue_max_count( queue_recv_id, &hk_lfr_q_rv_fifo_size_max );
199 199
200 200 }
201 201 }
202 202
203 203 rtems_task send_task( rtems_task_argument argument)
204 204 {
205 205 /** This RTEMS task is dedicated to the transmission of TeleMetry packets.
206 206 *
207 207 * @param unused is the starting argument of the RTEMS task
208 208 *
209 209 * The SEND task waits for a message to become available in the dedicated RTEMS queue. When a message arrives:
210 210 * - if the first byte is equal to CCSDS_DESTINATION_ID, the message is sent as is using the write system call.
211 211 * - if the first byte is not equal to CCSDS_DESTINATION_ID, the message is handled as a spw_ioctl_pkt_send. After
212 212 * analyzis, the packet is sent either using the write system call or using the ioctl call SPACEWIRE_IOCTRL_SEND, depending on the
213 213 * data it contains.
214 214 *
215 215 */
216 216
217 217 rtems_status_code status; // RTEMS status code
218 218 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
219 219 ring_node *incomingRingNodePtr;
220 220 int ring_node_address;
221 221 char *charPtr;
222 222 spw_ioctl_pkt_send *spw_ioctl_send;
223 223 size_t size; // size of the incoming TC packet
224 224 rtems_id queue_send_id;
225 225 unsigned int sid;
226 226 unsigned char sidAsUnsignedChar;
227 227 unsigned char type;
228 228
229 229 incomingRingNodePtr = NULL;
230 230 ring_node_address = 0;
231 231 charPtr = (char *) &ring_node_address;
232 232 sid = 0;
233 233 sidAsUnsignedChar = 0;
234 234
235 235 init_header_cwf( &headerCWF );
236 236 init_header_swf( &headerSWF );
237 237 init_header_asm( &headerASM );
238 238
239 239 status = get_message_queue_id_send( &queue_send_id );
240 240 if (status != RTEMS_SUCCESSFUL)
241 241 {
242 242 PRINTF1("in HOUS *** ERR get_message_queue_id_send %d\n", status)
243 243 }
244 244
245 245 BOOT_PRINTF("in SEND *** \n")
246 246
247 247 while(1)
248 248 {
249 249 status = rtems_message_queue_receive( queue_send_id, incomingData, &size,
250 250 RTEMS_WAIT, RTEMS_NO_TIMEOUT );
251 251
252 252 if (status!=RTEMS_SUCCESSFUL)
253 253 {
254 254 PRINTF1("in SEND *** (1) ERR = %d\n", status)
255 255 }
256 256 else
257 257 {
258 258 if ( size == sizeof(ring_node*) )
259 259 {
260 260 charPtr[0] = incomingData[0];
261 261 charPtr[1] = incomingData[1];
262 262 charPtr[2] = incomingData[2];
263 263 charPtr[3] = incomingData[3];
264 264 incomingRingNodePtr = (ring_node*) ring_node_address;
265 265 sid = incomingRingNodePtr->sid;
266 266 if ( (sid==SID_NORM_CWF_LONG_F3)
267 267 || (sid==SID_BURST_CWF_F2 )
268 268 || (sid==SID_SBM1_CWF_F1 )
269 269 || (sid==SID_SBM2_CWF_F2 ))
270 270 {
271 271 spw_send_waveform_CWF( incomingRingNodePtr, &headerCWF );
272 272 }
273 273 else if ( (sid==SID_NORM_SWF_F0) || (sid== SID_NORM_SWF_F1) || (sid==SID_NORM_SWF_F2) )
274 274 {
275 275 spw_send_waveform_SWF( incomingRingNodePtr, &headerSWF );
276 276 }
277 277 else if ( (sid==SID_NORM_CWF_F3) )
278 278 {
279 279 spw_send_waveform_CWF3_light( incomingRingNodePtr, &headerCWF );
280 280 }
281 281 else if (sid==SID_NORM_ASM_F0)
282 282 {
283 283 spw_send_asm_f0( incomingRingNodePtr, &headerASM );
284 284 }
285 285 else if (sid==SID_NORM_ASM_F1)
286 286 {
287 287 spw_send_asm_f1( incomingRingNodePtr, &headerASM );
288 288 }
289 289 else if (sid==SID_NORM_ASM_F2)
290 290 {
291 291 spw_send_asm_f2( incomingRingNodePtr, &headerASM );
292 292 }
293 293 else if ( sid==TM_CODE_K_DUMP )
294 294 {
295 295 spw_send_k_dump( incomingRingNodePtr );
296 296 }
297 297 else
298 298 {
299 299 PRINTF1("unexpected sid = %d\n", sid);
300 300 }
301 301 }
302 302 else if ( incomingData[0] == CCSDS_DESTINATION_ID ) // the incoming message is a ccsds packet
303 303 {
304 304 sidAsUnsignedChar = (unsigned char) incomingData[ PACKET_POS_PA_LFR_SID_PKT ];
305 305 sid = sidAsUnsignedChar;
306 306 type = (unsigned char) incomingData[ PACKET_POS_SERVICE_TYPE ];
307 307 if (type == TM_TYPE_LFR_SCIENCE) // this is a BP packet, all other types are handled differently
308 308 // SET THE SEQUENCE_CNT PARAMETER IN CASE OF BP0 OR BP1 PACKETS
309 309 {
310 310 increment_seq_counter_source_id( (unsigned char*) &incomingData[ PACKET_POS_SEQUENCE_CNT ], sid );
311 311 }
312 312
313 313 status = write( fdSPW, incomingData, size );
314 314 if (status == -1){
315 315 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
316 316 }
317 317 }
318 318 else // the incoming message is a spw_ioctl_pkt_send structure
319 319 {
320 320 spw_ioctl_send = (spw_ioctl_pkt_send*) incomingData;
321 321 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, spw_ioctl_send );
322 322 if (status == -1){
323 323 PRINTF2("in SEND *** (2.b) ERRNO = %d, RTEMS = %d\n", errno, status)
324 324 }
325 325 }
326 326 }
327 327
328 328 update_queue_max_count( queue_send_id, &hk_lfr_q_sd_fifo_size_max );
329 329
330 330 }
331 331 }
332 332
333 333 rtems_task link_task( rtems_task_argument argument )
334 334 {
335 335 rtems_event_set event_out;
336 336 rtems_status_code status;
337 337 int linkStatus;
338 338
339 339 BOOT_PRINTF("in LINK ***\n")
340 340
341 341 while(1)
342 342 {
343 343 // wait for an RTEMS_EVENT
344 344 rtems_event_receive( RTEMS_EVENT_0,
345 345 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
346 346 PRINTF("in LINK *** wait for the link\n")
347 347 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
348 348 while( linkStatus != 5) // wait for the link
349 349 {
350 350 status = rtems_task_wake_after( 10 ); // monitor the link each 100ms
351 351 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
352 352 watchdog_reload();
353 353 }
354 354
355 355 spacewire_read_statistics();
356 356 status = spacewire_stop_and_start_link( fdSPW );
357 357
358 358 if (status != RTEMS_SUCCESSFUL)
359 359 {
360 360 PRINTF1("in LINK *** ERR link not started %d\n", status)
361 361 }
362 362 else
363 363 {
364 364 PRINTF("in LINK *** OK link started\n")
365 365 }
366 366
367 367 // restart the SPIQ task
368 368 status = rtems_task_restart( Task_id[TASKID_SPIQ], 1 );
369 369 if ( status != RTEMS_SUCCESSFUL ) {
370 370 PRINTF("in SPIQ *** ERR restarting SPIQ Task\n")
371 371 }
372 372
373 373 // restart RECV and SEND
374 374 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
375 375 if ( status != RTEMS_SUCCESSFUL ) {
376 376 PRINTF("in SPIQ *** ERR restarting SEND Task\n")
377 377 }
378 378 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
379 379 if ( status != RTEMS_SUCCESSFUL ) {
380 380 PRINTF("in SPIQ *** ERR restarting RECV Task\n")
381 381 }
382 382 }
383 383 }
384 384
385 385 //****************
386 386 // OTHER FUNCTIONS
387 387 int spacewire_open_link( void ) // by default, the driver resets the core: [SPW_CTRL_WRITE(pDev, SPW_CTRL_RESET);]
388 388 {
389 389 /** This function opens the SpaceWire link.
390 390 *
391 391 * @return a valid file descriptor in case of success, -1 in case of a failure
392 392 *
393 393 */
394 394 rtems_status_code status;
395 395
396 396 fdSPW = open(GRSPW_DEVICE_NAME, O_RDWR); // open the device. the open call resets the hardware
397 397 if ( fdSPW < 0 ) {
398 398 PRINTF1("ERR *** in configure_spw_link *** error opening "GRSPW_DEVICE_NAME" with ERR %d\n", errno)
399 399 }
400 400 else
401 401 {
402 402 status = RTEMS_SUCCESSFUL;
403 403 }
404 404
405 405 return status;
406 406 }
407 407
408 408 int spacewire_start_link( int fd )
409 409 {
410 410 rtems_status_code status;
411 411
412 412 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
413 413 // -1 default hardcoded driver timeout
414 414
415 415 return status;
416 416 }
417 417
418 418 int spacewire_stop_and_start_link( int fd )
419 419 {
420 420 rtems_status_code status;
421 421
422 422 status = ioctl( fd, SPACEWIRE_IOCTRL_STOP); // start fails if link pDev->running != 0
423 423 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
424 424 // -1 default hardcoded driver timeout
425 425
426 426 return status;
427 427 }
428 428
429 429 int spacewire_configure_link( int fd )
430 430 {
431 431 /** This function configures the SpaceWire link.
432 432 *
433 433 * @return GR-RTEMS-DRIVER directive status codes:
434 434 * - 22 EINVAL - Null pointer or an out of range value was given as the argument.
435 435 * - 16 EBUSY - Only used for SEND. Returned when no descriptors are avialble in non-blocking mode.
436 436 * - 88 ENOSYS - Returned for SET_DESTKEY if RMAP command handler is not available or if a non-implemented call is used.
437 437 * - 116 ETIMEDOUT - REturned for SET_PACKET_SIZE and START if the link could not be brought up.
438 438 * - 12 ENOMEM - Returned for SET_PACKETSIZE if it was unable to allocate the new buffers.
439 439 * - 5 EIO - Error when writing to grswp hardware registers.
440 440 * - 2 ENOENT - No such file or directory
441 441 */
442 442
443 443 rtems_status_code status;
444 444
445 445 spacewire_set_NP(1, REGS_ADDR_GRSPW); // [N]o [P]ort force
446 446 spacewire_set_RE(1, REGS_ADDR_GRSPW); // [R]MAP [E]nable, the dedicated call seems to break the no port force configuration
447 447
448 448 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_RXBLOCK, 1); // sets the blocking mode for reception
449 449 if (status!=RTEMS_SUCCESSFUL) {
450 450 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_RXBLOCK\n")
451 451 }
452 452 //
453 453 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_EVENT_ID, Task_id[TASKID_SPIQ]); // sets the task ID to which an event is sent when a
454 454 if (status!=RTEMS_SUCCESSFUL) {
455 455 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_EVENT_ID\n") // link-error interrupt occurs
456 456 }
457 457 //
458 458 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_DISABLE_ERR, 0); // automatic link-disabling due to link-error interrupts
459 459 if (status!=RTEMS_SUCCESSFUL) {
460 460 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_DISABLE_ERR\n")
461 461 }
462 462 //
463 463 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ, 1); // sets the link-error interrupt bit
464 464 if (status!=RTEMS_SUCCESSFUL) {
465 465 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ\n")
466 466 }
467 467 //
468 468 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK, 1); // transmission blocks
469 469 if (status!=RTEMS_SUCCESSFUL) {
470 470 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK\n")
471 471 }
472 472 //
473 473 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL, 1); // transmission blocks when no transmission descriptor is available
474 474 if (status!=RTEMS_SUCCESSFUL) {
475 475 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL\n")
476 476 }
477 477 //
478 478 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TCODE_CTRL, 0x0909); // [Time Rx : Time Tx : Link error : Tick-out IRQ]
479 479 if (status!=RTEMS_SUCCESSFUL) {
480 480 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TCODE_CTRL,\n")
481 481 }
482 482
483 483 return status;
484 484 }
485 485
486 486 int spacewire_several_connect_attemps( void )
487 487 {
488 488 /** This function is executed by the SPIQ rtems_task wehn it has been awaken by an interruption raised by the SpaceWire driver.
489 489 *
490 490 * @return RTEMS directive status code:
491 491 * - RTEMS_UNSATISFIED is returned is the link is not in the running state after 10 s.
492 492 * - RTEMS_SUCCESSFUL is returned if the link is up before the timeout.
493 493 *
494 494 */
495 495
496 496 rtems_status_code status_spw;
497 497 rtems_status_code status;
498 498 int i;
499 499
500 500 for ( i=0; i<SY_LFR_DPU_CONNECT_ATTEMPT; i++ )
501 501 {
502 502 PRINTF1("in spacewire_reset_link *** link recovery, try %d\n", i);
503 503
504 504 // CLOSING THE DRIVER AT THIS POINT WILL MAKE THE SEND TASK BLOCK THE SYSTEM
505 505
506 506 status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms
507 507
508 508 status_spw = spacewire_stop_and_start_link( fdSPW );
509 509
510 510 if ( status_spw != RTEMS_SUCCESSFUL )
511 511 {
512 512 PRINTF1("in spacewire_reset_link *** ERR spacewire_start_link code %d\n", status_spw)
513 513 }
514 514
515 515 if ( status_spw == RTEMS_SUCCESSFUL)
516 516 {
517 517 break;
518 518 }
519 519 }
520 520
521 521 return status_spw;
522 522 }
523 523
524 524 void spacewire_set_NP( unsigned char val, unsigned int regAddr ) // [N]o [P]ort force
525 525 {
526 526 /** This function sets the [N]o [P]ort force bit of the GRSPW control register.
527 527 *
528 528 * @param val is the value, 0 or 1, used to set the value of the NP bit.
529 529 * @param regAddr is the address of the GRSPW control register.
530 530 *
531 531 * NP is the bit 20 of the GRSPW control register.
532 532 *
533 533 */
534 534
535 535 unsigned int *spwptr = (unsigned int*) regAddr;
536 536
537 537 if (val == 1) {
538 538 *spwptr = *spwptr | 0x00100000; // [NP] set the No port force bit
539 539 }
540 540 if (val== 0) {
541 541 *spwptr = *spwptr & 0xffdfffff;
542 542 }
543 543 }
544 544
545 545 void spacewire_set_RE( unsigned char val, unsigned int regAddr ) // [R]MAP [E]nable
546 546 {
547 547 /** This function sets the [R]MAP [E]nable bit of the GRSPW control register.
548 548 *
549 549 * @param val is the value, 0 or 1, used to set the value of the RE bit.
550 550 * @param regAddr is the address of the GRSPW control register.
551 551 *
552 552 * RE is the bit 16 of the GRSPW control register.
553 553 *
554 554 */
555 555
556 556 unsigned int *spwptr = (unsigned int*) regAddr;
557 557
558 558 if (val == 1)
559 559 {
560 560 *spwptr = *spwptr | 0x00010000; // [RE] set the RMAP Enable bit
561 561 }
562 562 if (val== 0)
563 563 {
564 564 *spwptr = *spwptr & 0xfffdffff;
565 565 }
566 566 }
567 567
568 568 void spacewire_read_statistics( void )
569 569 {
570 570 /** This function reads the SpaceWire statistics from the grspw RTEMS driver.
571 571 *
572 572 * @param void
573 573 *
574 574 * @return void
575 575 *
576 576 * Once they are read, the counters are stored in a global variable used during the building of the
577 577 * HK packets.
578 578 *
579 579 */
580 580
581 581 rtems_status_code status;
582 582 spw_stats current;
583 583
584 584 spacewire_get_last_error();
585 585
586 586 // read the current statistics
587 587 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &current );
588 588
589 589 // clear the counters
590 590 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_CLR_STATISTICS );
591 591
592 592 // typedef struct {
593 593 // unsigned int tx_link_err; // NOT IN HK
594 594 // unsigned int rx_rmap_header_crc_err; // NOT IN HK
595 595 // unsigned int rx_rmap_data_crc_err; // NOT IN HK
596 596 // unsigned int rx_eep_err;
597 597 // unsigned int rx_truncated;
598 598 // unsigned int parity_err;
599 599 // unsigned int escape_err;
600 600 // unsigned int credit_err;
601 601 // unsigned int write_sync_err;
602 602 // unsigned int disconnect_err;
603 603 // unsigned int early_ep;
604 604 // unsigned int invalid_address;
605 605 // unsigned int packets_sent;
606 606 // unsigned int packets_received;
607 607 // } spw_stats;
608 608
609 609 // rx_eep_err
610 610 grspw_stats.rx_eep_err = grspw_stats.rx_eep_err + current.rx_eep_err;
611 611 // rx_truncated
612 612 grspw_stats.rx_truncated = grspw_stats.rx_truncated + current.rx_truncated;
613 613 // parity_err
614 614 grspw_stats.parity_err = grspw_stats.parity_err + current.parity_err;
615 615 // escape_err
616 616 grspw_stats.escape_err = grspw_stats.escape_err + current.escape_err;
617 617 // credit_err
618 618 grspw_stats.credit_err = grspw_stats.credit_err + current.credit_err;
619 619 // write_sync_err
620 620 grspw_stats.write_sync_err = grspw_stats.write_sync_err + current.write_sync_err;
621 621 // disconnect_err
622 622 grspw_stats.disconnect_err = grspw_stats.disconnect_err + current.disconnect_err;
623 623 // early_ep
624 624 grspw_stats.early_ep = grspw_stats.early_ep + current.early_ep;
625 625 // invalid_address
626 626 grspw_stats.invalid_address = grspw_stats.invalid_address + current.invalid_address;
627 627 // packets_sent
628 628 grspw_stats.packets_sent = grspw_stats.packets_sent + current.packets_sent;
629 629 // packets_received
630 630 grspw_stats.packets_received= grspw_stats.packets_received + current.packets_received;
631 631
632 632 }
633 633
634 634 void spacewire_get_last_error( void )
635 635 {
636 636 static spw_stats previous;
637 637 spw_stats current;
638 638 rtems_status_code status;
639 639
640 640 unsigned int hk_lfr_last_er_rid;
641 641 unsigned char hk_lfr_last_er_code;
642 642 int coarseTime;
643 643 int fineTime;
644 644 unsigned char update_hk_lfr_last_er;
645 645
646 646 update_hk_lfr_last_er = 0;
647 647
648 648 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &current );
649 649
650 650 // get current time
651 651 coarseTime = time_management_regs->coarse_time;
652 652 fineTime = time_management_regs->fine_time;
653 653
654 654 // typedef struct {
655 655 // unsigned int tx_link_err; // NOT IN HK
656 656 // unsigned int rx_rmap_header_crc_err; // NOT IN HK
657 657 // unsigned int rx_rmap_data_crc_err; // NOT IN HK
658 658 // unsigned int rx_eep_err;
659 659 // unsigned int rx_truncated;
660 660 // unsigned int parity_err;
661 661 // unsigned int escape_err;
662 662 // unsigned int credit_err;
663 663 // unsigned int write_sync_err;
664 664 // unsigned int disconnect_err;
665 665 // unsigned int early_ep;
666 666 // unsigned int invalid_address;
667 667 // unsigned int packets_sent;
668 668 // unsigned int packets_received;
669 669 // } spw_stats;
670 670
671 671 // tx_link_err *** no code associated to this field
672 672 // rx_rmap_header_crc_err *** LE *** in HK
673 673 if (previous.rx_rmap_header_crc_err != current.rx_rmap_header_crc_err)
674 674 {
675 675 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
676 676 hk_lfr_last_er_code = CODE_HEADER_CRC;
677 677 update_hk_lfr_last_er = 1;
678 678 }
679 679 // rx_rmap_data_crc_err *** LE *** NOT IN HK
680 680 if (previous.rx_rmap_data_crc_err != current.rx_rmap_data_crc_err)
681 681 {
682 682 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
683 683 hk_lfr_last_er_code = CODE_DATA_CRC;
684 684 update_hk_lfr_last_er = 1;
685 685 }
686 686 // rx_eep_err
687 687 if (previous.rx_eep_err != current.rx_eep_err)
688 688 {
689 689 hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW;
690 690 hk_lfr_last_er_code = CODE_EEP;
691 691 update_hk_lfr_last_er = 1;
692 692 }
693 693 // rx_truncated
694 694 if (previous.rx_truncated != current.rx_truncated)
695 695 {
696 696 hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW;
697 697 hk_lfr_last_er_code = CODE_RX_TOO_BIG;
698 698 update_hk_lfr_last_er = 1;
699 699 }
700 700 // parity_err
701 701 if (previous.parity_err != current.parity_err)
702 702 {
703 703 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
704 704 hk_lfr_last_er_code = CODE_PARITY;
705 705 update_hk_lfr_last_er = 1;
706 706 }
707 707 // escape_err
708 708 if (previous.parity_err != current.parity_err)
709 709 {
710 710 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
711 711 hk_lfr_last_er_code = CODE_ESCAPE;
712 712 update_hk_lfr_last_er = 1;
713 713 }
714 714 // credit_err
715 715 if (previous.credit_err != current.credit_err)
716 716 {
717 717 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
718 718 hk_lfr_last_er_code = CODE_CREDIT;
719 719 update_hk_lfr_last_er = 1;
720 720 }
721 721 // write_sync_err
722 722 if (previous.write_sync_err != current.write_sync_err)
723 723 {
724 724 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
725 725 hk_lfr_last_er_code = CODE_WRITE_SYNC;
726 726 update_hk_lfr_last_er = 1;
727 727 }
728 728 // disconnect_err
729 729 if (previous.disconnect_err != current.disconnect_err)
730 730 {
731 731 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
732 732 hk_lfr_last_er_code = CODE_DISCONNECT;
733 733 update_hk_lfr_last_er = 1;
734 734 }
735 735 // early_ep
736 736 if (previous.early_ep != current.early_ep)
737 737 {
738 738 hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW;
739 739 hk_lfr_last_er_code = CODE_EARLY_EOP_EEP;
740 740 update_hk_lfr_last_er = 1;
741 741 }
742 742 // invalid_address
743 743 if (previous.invalid_address != current.invalid_address)
744 744 {
745 745 hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW;
746 746 hk_lfr_last_er_code = CODE_INVALID_ADDRESS;
747 747 update_hk_lfr_last_er = 1;
748 748 }
749 749
750 750 // if a field has changed, update the hk_last_er fields
751 751 if (update_hk_lfr_last_er == 1)
752 752 {
753 753 update_hk_lfr_last_er_fields( hk_lfr_last_er_rid, hk_lfr_last_er_code );
754 754 }
755 755
756 756 previous = current;
757 757 }
758 758
759 759 void update_hk_lfr_last_er_fields(unsigned int rid, unsigned char code)
760 760 {
761 761 unsigned char *coarseTimePtr;
762 762 unsigned char *fineTimePtr;
763 763
764 764 coarseTimePtr = (unsigned char*) &time_management_regs->coarse_time;
765 765 fineTimePtr = (unsigned char*) &time_management_regs->fine_time;
766 766
767 767 housekeeping_packet.hk_lfr_last_er_rid[0] = (unsigned char) ((rid & 0xff00) >> 8 );
768 768 housekeeping_packet.hk_lfr_last_er_rid[1] = (unsigned char) (rid & 0x00ff);
769 769 housekeeping_packet.hk_lfr_last_er_code = code;
770 770 housekeeping_packet.hk_lfr_last_er_time[0] = coarseTimePtr[0];
771 771 housekeeping_packet.hk_lfr_last_er_time[1] = coarseTimePtr[1];
772 772 housekeeping_packet.hk_lfr_last_er_time[2] = coarseTimePtr[2];
773 773 housekeeping_packet.hk_lfr_last_er_time[3] = coarseTimePtr[3];
774 774 housekeeping_packet.hk_lfr_last_er_time[4] = fineTimePtr[2];
775 775 housekeeping_packet.hk_lfr_last_er_time[5] = fineTimePtr[3];
776 776 }
777 777
778 778 void update_hk_with_grspw_stats( void )
779 779 {
780 780 //****************************
781 781 // DPU_SPACEWIRE_IF_STATISTICS
782 782 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[0] = (unsigned char) (grspw_stats.packets_received >> 8);
783 783 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[1] = (unsigned char) (grspw_stats.packets_received);
784 784 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[0] = (unsigned char) (grspw_stats.packets_sent >> 8);
785 785 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[1] = (unsigned char) (grspw_stats.packets_sent);
786 786
787 787 //******************************************
788 788 // ERROR COUNTERS / SPACEWIRE / LOW SEVERITY
789 789 housekeeping_packet.hk_lfr_dpu_spw_parity = (unsigned char) grspw_stats.parity_err;
790 790 housekeeping_packet.hk_lfr_dpu_spw_disconnect = (unsigned char) grspw_stats.disconnect_err;
791 791 housekeeping_packet.hk_lfr_dpu_spw_escape = (unsigned char) grspw_stats.escape_err;
792 792 housekeeping_packet.hk_lfr_dpu_spw_credit = (unsigned char) grspw_stats.credit_err;
793 793 housekeeping_packet.hk_lfr_dpu_spw_write_sync = (unsigned char) grspw_stats.write_sync_err;
794 794
795 795 //*********************************************
796 796 // ERROR COUNTERS / SPACEWIRE / MEDIUM SEVERITY
797 797 housekeeping_packet.hk_lfr_dpu_spw_early_eop = (unsigned char) grspw_stats.early_ep;
798 798 housekeeping_packet.hk_lfr_dpu_spw_invalid_addr = (unsigned char) grspw_stats.invalid_address;
799 799 housekeeping_packet.hk_lfr_dpu_spw_eep = (unsigned char) grspw_stats.rx_eep_err;
800 800 housekeeping_packet.hk_lfr_dpu_spw_rx_too_big = (unsigned char) grspw_stats.rx_truncated;
801 801 }
802 802
803 803 void spacewire_update_hk_lfr_link_state( unsigned char *hk_lfr_status_word_0 )
804 804 {
805 805 unsigned int *statusRegisterPtr;
806 806 unsigned char linkState;
807 807
808 808 statusRegisterPtr = (unsigned int *) (REGS_ADDR_GRSPW + APB_OFFSET_GRSPW_STATUS_REGISTER);
809 809 linkState = (unsigned char) ( ( (*statusRegisterPtr) >> 21) & 0x07); // [0000 0111]
810 810
811 811 *hk_lfr_status_word_0 = *hk_lfr_status_word_0 & 0xf8; // [1111 1000] set link state to 0
812 812
813 813 *hk_lfr_status_word_0 = *hk_lfr_status_word_0 | linkState; // update hk_lfr_dpu_spw_link_state
814 814 }
815 815
816 816 void increase_unsigned_char_counter( unsigned char *counter )
817 817 {
818 818 // update the number of valid timecodes that have been received
819 819 if (*counter == 255)
820 820 {
821 821 *counter = 0;
822 822 }
823 823 else
824 824 {
825 825 *counter = *counter + 1;
826 826 }
827 827 }
828 828
829 829 unsigned int check_timecode_and_previous_timecode_coherency(unsigned char currentTimecodeCtr)
830 830 {
831 831 /** This function checks the coherency between the incoming timecode and the last valid timecode.
832 832 *
833 833 * @param currentTimecodeCtr is the incoming timecode
834 834 *
835 835 * @return returned codes::
836 836 * - LFR_DEFAULT
837 837 * - LFR_SUCCESSFUL
838 838 *
839 839 */
840 840
841 841 static unsigned char firstTickout = 1;
842 842 unsigned char ret;
843 843
844 844 ret = LFR_DEFAULT;
845 845
846 846 if (firstTickout == 0)
847 847 {
848 848 if (currentTimecodeCtr == 0)
849 849 {
850 850 if (previousTimecodeCtr == 63)
851 851 {
852 852 ret = LFR_SUCCESSFUL;
853 853 }
854 854 else
855 855 {
856 856 ret = LFR_DEFAULT;
857 857 }
858 858 }
859 859 else
860 860 {
861 861 if (currentTimecodeCtr == (previousTimecodeCtr +1))
862 862 {
863 863 ret = LFR_SUCCESSFUL;
864 864 }
865 865 else
866 866 {
867 867 ret = LFR_DEFAULT;
868 868 }
869 869 }
870 870 }
871 871 else
872 872 {
873 873 firstTickout = 0;
874 874 ret = LFR_SUCCESSFUL;
875 875 }
876 876
877 877 return ret;
878 878 }
879 879
880 880 unsigned int check_timecode_and_internal_time_coherency(unsigned char timecode, unsigned char internalTime)
881 881 {
882 882 unsigned int ret;
883 883
884 884 ret = LFR_DEFAULT;
885 885
886 886 if (timecode == internalTime)
887 887 {
888 888 ret = LFR_SUCCESSFUL;
889 889 }
890 890 else
891 891 {
892 892 ret = LFR_DEFAULT;
893 893 }
894 894
895 895 return ret;
896 896 }
897 897
898 898 void timecode_irq_handler( void *pDev, void *regs, int minor, unsigned int tc )
899 899 {
900 900 // a tickout has been emitted, perform actions on the incoming timecode
901 901
902 902 unsigned char incomingTimecode;
903 903 unsigned char updateTime;
904 904 unsigned char internalTime;
905 905 rtems_status_code status;
906 906
907 907 incomingTimecode = (unsigned char) (grspwPtr[0] & TIMECODE_MASK);
908 908 updateTime = time_management_regs->coarse_time_load & TIMECODE_MASK;
909 909 internalTime = time_management_regs->coarse_time & TIMECODE_MASK;
910 910
911 911 housekeeping_packet.hk_lfr_dpu_spw_last_timc = incomingTimecode;
912 912
913 913 // update the number of tickout that have been generated
914 914 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt );
915 915
916 916 //**************************
917 917 // HK_LFR_TIMECODE_ERRONEOUS
918 918 // MISSING and INVALID are handled by the timecode_timer_routine service routine
919 919 if (check_timecode_and_previous_timecode_coherency( incomingTimecode ) == LFR_DEFAULT)
920 920 {
921 921 // this is unexpected but a tickout could have been raised despite of the timecode being erroneous
922 922 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_erroneous );
923 923 update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_ERRONEOUS );
924 924 }
925 925
926 926 //************************
927 927 // HK_LFR_TIME_TIMECODE_IT
928 928 // check the coherency between the SpaceWire timecode and the Internal Time
929 929 if (check_timecode_and_internal_time_coherency( incomingTimecode, internalTime ) == LFR_DEFAULT)
930 930 {
931 931 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_time_timecode_it );
932 932 update_hk_lfr_last_er_fields( RID_LE_LFR_TIME, CODE_TIMECODE_IT );
933 933 }
934 934
935 935 //********************
936 936 // HK_LFR_TIMECODE_CTR
937 937 // check the value of the timecode with respect to the last TC_LFR_UPDATE_TIME => SSS-CP-FS-370
938 938 if (oneTcLfrUpdateTimeReceived == 1)
939 939 {
940 940 if ( incomingTimecode != updateTime )
941 941 {
942 942 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_time_timecode_ctr );
943 943 update_hk_lfr_last_er_fields( RID_LE_LFR_TIME, CODE_TIMECODE_CTR );
944 944 }
945 945 }
946 946
947 947 // launch the timecode timer to detect missing or invalid timecodes
948 948 previousTimecodeCtr = incomingTimecode; // update the previousTimecodeCtr value
949 949 status = rtems_timer_fire_after( timecode_timer_id, TIMECODE_TIMER_TIMEOUT, timecode_timer_routine, NULL );
950 950 if (status != RTEMS_SUCCESSFUL)
951 951 {
952 952 rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_14 );
953 953 }
954 954 }
955 955
956 956 rtems_timer_service_routine timecode_timer_routine( rtems_id timer_id, void *user_data )
957 957 {
958 958 static unsigned char initStep = 1;
959 959
960 960 unsigned char currentTimecodeCtr;
961 961
962 962 currentTimecodeCtr = (unsigned char) (grspwPtr[0] & TIMECODE_MASK);
963 963
964 964 if (initStep == 1)
965 965 {
966 966 if (currentTimecodeCtr == previousTimecodeCtr)
967 967 {
968 968 //************************
969 969 // HK_LFR_TIMECODE_MISSING
970 970 // the timecode value has not changed, no valid timecode has been received, the timecode is MISSING
971 971 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_missing );
972 972 update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_MISSING );
973 973 }
974 974 else if (currentTimecodeCtr == (previousTimecodeCtr+1))
975 975 {
976 976 // the timecode value has changed and the value is valid, this is unexpected because
977 977 // the timer should not have fired, the timecode_irq_handler should have been raised
978 978 }
979 979 else
980 980 {
981 981 //************************
982 982 // HK_LFR_TIMECODE_INVALID
983 983 // the timecode value has changed and the value is not valid, no tickout has been generated
984 984 // this is why the timer has fired
985 985 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_invalid );
986 986 update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_INVALID );
987 987 }
988 988 }
989 989 else
990 990 {
991 991 initStep = 1;
992 992 //************************
993 993 // HK_LFR_TIMECODE_MISSING
994 994 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_missing );
995 995 update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_MISSING );
996 996 }
997 997
998 998 rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_13 );
999 999 }
1000 1000
1001 1001 void init_header_cwf( Header_TM_LFR_SCIENCE_CWF_t *header )
1002 1002 {
1003 1003 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
1004 1004 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1005 1005 header->reserved = DEFAULT_RESERVED;
1006 1006 header->userApplication = CCSDS_USER_APP;
1007 1007 header->packetSequenceControl[0]= TM_PACKET_SEQ_CTRL_STANDALONE;
1008 1008 header->packetSequenceControl[1]= TM_PACKET_SEQ_CNT_DEFAULT;
1009 1009 header->packetLength[0] = 0x00;
1010 1010 header->packetLength[1] = 0x00;
1011 1011 // DATA FIELD HEADER
1012 1012 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
1013 1013 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
1014 1014 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; // service subtype
1015 1015 header->destinationID = TM_DESTINATION_ID_GROUND;
1016 1016 header->time[0] = 0x00;
1017 1017 header->time[0] = 0x00;
1018 1018 header->time[0] = 0x00;
1019 1019 header->time[0] = 0x00;
1020 1020 header->time[0] = 0x00;
1021 1021 header->time[0] = 0x00;
1022 1022 // AUXILIARY DATA HEADER
1023 1023 header->sid = 0x00;
1024 header->hkBIA = DEFAULT_HKBIA;
1024 header->pa_bia_status_info = DEFAULT_HKBIA;
1025 1025 header->blkNr[0] = 0x00;
1026 1026 header->blkNr[1] = 0x00;
1027 1027 }
1028 1028
1029 1029 void init_header_swf( Header_TM_LFR_SCIENCE_SWF_t *header )
1030 1030 {
1031 1031 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
1032 1032 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1033 1033 header->reserved = DEFAULT_RESERVED;
1034 1034 header->userApplication = CCSDS_USER_APP;
1035 1035 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
1036 1036 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1037 1037 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1038 1038 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1039 1039 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8);
1040 1040 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
1041 1041 // DATA FIELD HEADER
1042 1042 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
1043 1043 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
1044 1044 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; // service subtype
1045 1045 header->destinationID = TM_DESTINATION_ID_GROUND;
1046 1046 header->time[0] = 0x00;
1047 1047 header->time[0] = 0x00;
1048 1048 header->time[0] = 0x00;
1049 1049 header->time[0] = 0x00;
1050 1050 header->time[0] = 0x00;
1051 1051 header->time[0] = 0x00;
1052 1052 // AUXILIARY DATA HEADER
1053 1053 header->sid = 0x00;
1054 header->hkBIA = DEFAULT_HKBIA;
1054 header->pa_bia_status_info = DEFAULT_HKBIA;
1055 1055 header->pktCnt = DEFAULT_PKTCNT; // PKT_CNT
1056 1056 header->pktNr = 0x00;
1057 1057 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8);
1058 1058 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
1059 1059 }
1060 1060
1061 1061 void init_header_asm( Header_TM_LFR_SCIENCE_ASM_t *header )
1062 1062 {
1063 1063 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
1064 1064 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1065 1065 header->reserved = DEFAULT_RESERVED;
1066 1066 header->userApplication = CCSDS_USER_APP;
1067 1067 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
1068 1068 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1069 1069 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1070 1070 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1071 1071 header->packetLength[0] = 0x00;
1072 1072 header->packetLength[1] = 0x00;
1073 1073 // DATA FIELD HEADER
1074 1074 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
1075 1075 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
1076 1076 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
1077 1077 header->destinationID = TM_DESTINATION_ID_GROUND;
1078 1078 header->time[0] = 0x00;
1079 1079 header->time[0] = 0x00;
1080 1080 header->time[0] = 0x00;
1081 1081 header->time[0] = 0x00;
1082 1082 header->time[0] = 0x00;
1083 1083 header->time[0] = 0x00;
1084 1084 // AUXILIARY DATA HEADER
1085 1085 header->sid = 0x00;
1086 header->biaStatusInfo = 0x00;
1086 header->pa_bia_status_info = 0x00;
1087 1087 header->pa_lfr_pkt_cnt_asm = 0x00;
1088 1088 header->pa_lfr_pkt_nr_asm = 0x00;
1089 1089 header->pa_lfr_asm_blk_nr[0] = 0x00;
1090 1090 header->pa_lfr_asm_blk_nr[1] = 0x00;
1091 1091 }
1092 1092
1093 1093 int spw_send_waveform_CWF( ring_node *ring_node_to_send,
1094 1094 Header_TM_LFR_SCIENCE_CWF_t *header )
1095 1095 {
1096 1096 /** This function sends CWF CCSDS packets (F2, F1 or F0).
1097 1097 *
1098 1098 * @param waveform points to the buffer containing the data that will be send.
1099 1099 * @param sid is the source identifier of the data that will be sent.
1100 1100 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
1101 1101 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
1102 1102 * contain information to setup the transmission of the data packets.
1103 1103 *
1104 1104 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
1105 1105 *
1106 1106 */
1107 1107
1108 1108 unsigned int i;
1109 1109 int ret;
1110 1110 unsigned int coarseTime;
1111 1111 unsigned int fineTime;
1112 1112 rtems_status_code status;
1113 1113 spw_ioctl_pkt_send spw_ioctl_send_CWF;
1114 1114 int *dataPtr;
1115 1115 unsigned char sid;
1116 1116
1117 1117 spw_ioctl_send_CWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_CWF;
1118 1118 spw_ioctl_send_CWF.options = 0;
1119 1119
1120 1120 ret = LFR_DEFAULT;
1121 1121 sid = (unsigned char) ring_node_to_send->sid;
1122 1122
1123 1123 coarseTime = ring_node_to_send->coarseTime;
1124 1124 fineTime = ring_node_to_send->fineTime;
1125 1125 dataPtr = (int*) ring_node_to_send->buffer_address;
1126 1126
1127 1127 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8);
1128 1128 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
1129 header->hkBIA = pa_bia_status_info;
1129 header->pa_bia_status_info = pa_bia_status_info;
1130 1130 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1131 1131 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8);
1132 1132 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
1133 1133
1134 1134 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF; i++) // send waveform
1135 1135 {
1136 1136 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF * NB_WORDS_SWF_BLK) ];
1137 1137 spw_ioctl_send_CWF.hdr = (char*) header;
1138 1138 // BUILD THE DATA
1139 1139 spw_ioctl_send_CWF.dlen = BLK_NR_CWF * NB_BYTES_SWF_BLK;
1140 1140
1141 1141 // SET PACKET SEQUENCE CONTROL
1142 1142 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1143 1143
1144 1144 // SET SID
1145 1145 header->sid = sid;
1146 1146
1147 1147 // SET PACKET TIME
1148 1148 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime);
1149 1149 //
1150 1150 header->time[0] = header->acquisitionTime[0];
1151 1151 header->time[1] = header->acquisitionTime[1];
1152 1152 header->time[2] = header->acquisitionTime[2];
1153 1153 header->time[3] = header->acquisitionTime[3];
1154 1154 header->time[4] = header->acquisitionTime[4];
1155 1155 header->time[5] = header->acquisitionTime[5];
1156 1156
1157 1157 // SET PACKET ID
1158 1158 if ( (sid == SID_SBM1_CWF_F1) || (sid == SID_SBM2_CWF_F2) )
1159 1159 {
1160 1160 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2 >> 8);
1161 1161 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2);
1162 1162 }
1163 1163 else
1164 1164 {
1165 1165 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
1166 1166 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1167 1167 }
1168 1168
1169 1169 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
1170 1170 if (status != RTEMS_SUCCESSFUL) {
1171 1171 ret = LFR_DEFAULT;
1172 1172 }
1173 1173 }
1174 1174
1175 1175 return ret;
1176 1176 }
1177 1177
1178 1178 int spw_send_waveform_SWF( ring_node *ring_node_to_send,
1179 1179 Header_TM_LFR_SCIENCE_SWF_t *header )
1180 1180 {
1181 1181 /** This function sends SWF CCSDS packets (F2, F1 or F0).
1182 1182 *
1183 1183 * @param waveform points to the buffer containing the data that will be send.
1184 1184 * @param sid is the source identifier of the data that will be sent.
1185 1185 * @param headerSWF points to a table of headers that have been prepared for the data transmission.
1186 1186 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
1187 1187 * contain information to setup the transmission of the data packets.
1188 1188 *
1189 1189 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
1190 1190 *
1191 1191 */
1192 1192
1193 1193 unsigned int i;
1194 1194 int ret;
1195 1195 unsigned int coarseTime;
1196 1196 unsigned int fineTime;
1197 1197 rtems_status_code status;
1198 1198 spw_ioctl_pkt_send spw_ioctl_send_SWF;
1199 1199 int *dataPtr;
1200 1200 unsigned char sid;
1201 1201
1202 1202 spw_ioctl_send_SWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_SWF;
1203 1203 spw_ioctl_send_SWF.options = 0;
1204 1204
1205 1205 ret = LFR_DEFAULT;
1206 1206
1207 1207 coarseTime = ring_node_to_send->coarseTime;
1208 1208 fineTime = ring_node_to_send->fineTime;
1209 1209 dataPtr = (int*) ring_node_to_send->buffer_address;
1210 1210 sid = ring_node_to_send->sid;
1211 1211
1212 header->hkBIA = pa_bia_status_info;
1212 header->pa_bia_status_info = pa_bia_status_info;
1213 1213 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1214 1214
1215 1215 for (i=0; i<7; i++) // send waveform
1216 1216 {
1217 1217 spw_ioctl_send_SWF.data = (char*) &dataPtr[ (i * BLK_NR_304 * NB_WORDS_SWF_BLK) ];
1218 1218 spw_ioctl_send_SWF.hdr = (char*) header;
1219 1219
1220 1220 // SET PACKET SEQUENCE CONTROL
1221 1221 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1222 1222
1223 1223 // SET PACKET LENGTH AND BLKNR
1224 1224 if (i == 6)
1225 1225 {
1226 1226 spw_ioctl_send_SWF.dlen = BLK_NR_224 * NB_BYTES_SWF_BLK;
1227 1227 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_224 >> 8);
1228 1228 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_224 );
1229 1229 header->blkNr[0] = (unsigned char) (BLK_NR_224 >> 8);
1230 1230 header->blkNr[1] = (unsigned char) (BLK_NR_224 );
1231 1231 }
1232 1232 else
1233 1233 {
1234 1234 spw_ioctl_send_SWF.dlen = BLK_NR_304 * NB_BYTES_SWF_BLK;
1235 1235 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_304 >> 8);
1236 1236 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_304 );
1237 1237 header->blkNr[0] = (unsigned char) (BLK_NR_304 >> 8);
1238 1238 header->blkNr[1] = (unsigned char) (BLK_NR_304 );
1239 1239 }
1240 1240
1241 1241 // SET PACKET TIME
1242 1242 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime );
1243 1243 //
1244 1244 header->time[0] = header->acquisitionTime[0];
1245 1245 header->time[1] = header->acquisitionTime[1];
1246 1246 header->time[2] = header->acquisitionTime[2];
1247 1247 header->time[3] = header->acquisitionTime[3];
1248 1248 header->time[4] = header->acquisitionTime[4];
1249 1249 header->time[5] = header->acquisitionTime[5];
1250 1250
1251 1251 // SET SID
1252 1252 header->sid = sid;
1253 1253
1254 1254 // SET PKTNR
1255 1255 header->pktNr = i+1; // PKT_NR
1256 1256
1257 1257 // SEND PACKET
1258 1258 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_SWF );
1259 1259 if (status != RTEMS_SUCCESSFUL) {
1260 1260 ret = LFR_DEFAULT;
1261 1261 }
1262 1262 }
1263 1263
1264 1264 return ret;
1265 1265 }
1266 1266
1267 1267 int spw_send_waveform_CWF3_light( ring_node *ring_node_to_send,
1268 1268 Header_TM_LFR_SCIENCE_CWF_t *header )
1269 1269 {
1270 1270 /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data.
1271 1271 *
1272 1272 * @param waveform points to the buffer containing the data that will be send.
1273 1273 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
1274 1274 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
1275 1275 * contain information to setup the transmission of the data packets.
1276 1276 *
1277 1277 * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer
1278 1278 * from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks.
1279 1279 *
1280 1280 */
1281 1281
1282 1282 unsigned int i;
1283 1283 int ret;
1284 1284 unsigned int coarseTime;
1285 1285 unsigned int fineTime;
1286 1286 rtems_status_code status;
1287 1287 spw_ioctl_pkt_send spw_ioctl_send_CWF;
1288 1288 char *dataPtr;
1289 1289 unsigned char sid;
1290 1290
1291 1291 spw_ioctl_send_CWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_CWF;
1292 1292 spw_ioctl_send_CWF.options = 0;
1293 1293
1294 1294 ret = LFR_DEFAULT;
1295 1295 sid = ring_node_to_send->sid;
1296 1296
1297 1297 coarseTime = ring_node_to_send->coarseTime;
1298 1298 fineTime = ring_node_to_send->fineTime;
1299 1299 dataPtr = (char*) ring_node_to_send->buffer_address;
1300 1300
1301 1301 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_672 >> 8);
1302 1302 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_672 );
1303 header->hkBIA = pa_bia_status_info;
1303 header->pa_bia_status_info = pa_bia_status_info;
1304 1304 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1305 1305 header->blkNr[0] = (unsigned char) (BLK_NR_CWF_SHORT_F3 >> 8);
1306 1306 header->blkNr[1] = (unsigned char) (BLK_NR_CWF_SHORT_F3 );
1307 1307
1308 1308 //*********************
1309 1309 // SEND CWF3_light DATA
1310 1310 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF_LIGHT; i++) // send waveform
1311 1311 {
1312 1312 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK) ];
1313 1313 spw_ioctl_send_CWF.hdr = (char*) header;
1314 1314 // BUILD THE DATA
1315 1315 spw_ioctl_send_CWF.dlen = BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK;
1316 1316
1317 1317 // SET PACKET SEQUENCE COUNTER
1318 1318 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1319 1319
1320 1320 // SET SID
1321 1321 header->sid = sid;
1322 1322
1323 1323 // SET PACKET TIME
1324 1324 compute_acquisition_time( coarseTime, fineTime, SID_NORM_CWF_F3, i, header->acquisitionTime );
1325 1325 //
1326 1326 header->time[0] = header->acquisitionTime[0];
1327 1327 header->time[1] = header->acquisitionTime[1];
1328 1328 header->time[2] = header->acquisitionTime[2];
1329 1329 header->time[3] = header->acquisitionTime[3];
1330 1330 header->time[4] = header->acquisitionTime[4];
1331 1331 header->time[5] = header->acquisitionTime[5];
1332 1332
1333 1333 // SET PACKET ID
1334 1334 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
1335 1335 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1336 1336
1337 1337 // SEND PACKET
1338 1338 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
1339 1339 if (status != RTEMS_SUCCESSFUL) {
1340 1340 ret = LFR_DEFAULT;
1341 1341 }
1342 1342 }
1343 1343
1344 1344 return ret;
1345 1345 }
1346 1346
1347 1347 void spw_send_asm_f0( ring_node *ring_node_to_send,
1348 1348 Header_TM_LFR_SCIENCE_ASM_t *header )
1349 1349 {
1350 1350 unsigned int i;
1351 1351 unsigned int length = 0;
1352 1352 rtems_status_code status;
1353 1353 unsigned int sid;
1354 1354 float *spectral_matrix;
1355 1355 int coarseTime;
1356 1356 int fineTime;
1357 1357 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1358 1358
1359 1359 sid = ring_node_to_send->sid;
1360 1360 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1361 1361 coarseTime = ring_node_to_send->coarseTime;
1362 1362 fineTime = ring_node_to_send->fineTime;
1363 1363
1364 header->biaStatusInfo = pa_bia_status_info;
1364 header->pa_bia_status_info = pa_bia_status_info;
1365 1365 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1366 1366
1367 1367 for (i=0; i<3; i++)
1368 1368 {
1369 1369 if ((i==0) || (i==1))
1370 1370 {
1371 1371 spw_ioctl_send_ASM.dlen = DLEN_ASM_F0_PKT_1;
1372 1372 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1373 1373 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0_1) ) * NB_VALUES_PER_SM )
1374 1374 ];
1375 1375 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0_1;
1376 1376 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1377 1377 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_1) >> 8 ); // BLK_NR MSB
1378 1378 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0_1); // BLK_NR LSB
1379 1379 }
1380 1380 else
1381 1381 {
1382 1382 spw_ioctl_send_ASM.dlen = DLEN_ASM_F0_PKT_2;
1383 1383 spw_ioctl_send_ASM.data = (char*) &spectral_matrix[
1384 1384 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0_1) ) * NB_VALUES_PER_SM )
1385 1385 ];
1386 1386 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0_2;
1387 1387 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1388 1388 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_2) >> 8 ); // BLK_NR MSB
1389 1389 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0_2); // BLK_NR LSB
1390 1390 }
1391 1391
1392 1392 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1393 1393 spw_ioctl_send_ASM.hdr = (char *) header;
1394 1394 spw_ioctl_send_ASM.options = 0;
1395 1395
1396 1396 // (2) BUILD THE HEADER
1397 1397 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1398 1398 header->packetLength[0] = (unsigned char) (length>>8);
1399 1399 header->packetLength[1] = (unsigned char) (length);
1400 1400 header->sid = (unsigned char) sid; // SID
1401 1401 header->pa_lfr_pkt_cnt_asm = 3;
1402 1402 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1403 1403
1404 1404 // (3) SET PACKET TIME
1405 1405 header->time[0] = (unsigned char) (coarseTime>>24);
1406 1406 header->time[1] = (unsigned char) (coarseTime>>16);
1407 1407 header->time[2] = (unsigned char) (coarseTime>>8);
1408 1408 header->time[3] = (unsigned char) (coarseTime);
1409 1409 header->time[4] = (unsigned char) (fineTime>>8);
1410 1410 header->time[5] = (unsigned char) (fineTime);
1411 1411 //
1412 1412 header->acquisitionTime[0] = header->time[0];
1413 1413 header->acquisitionTime[1] = header->time[1];
1414 1414 header->acquisitionTime[2] = header->time[2];
1415 1415 header->acquisitionTime[3] = header->time[3];
1416 1416 header->acquisitionTime[4] = header->time[4];
1417 1417 header->acquisitionTime[5] = header->time[5];
1418 1418
1419 1419 // (4) SEND PACKET
1420 1420 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1421 1421 if (status != RTEMS_SUCCESSFUL) {
1422 1422 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1423 1423 }
1424 1424 }
1425 1425 }
1426 1426
1427 1427 void spw_send_asm_f1( ring_node *ring_node_to_send,
1428 1428 Header_TM_LFR_SCIENCE_ASM_t *header )
1429 1429 {
1430 1430 unsigned int i;
1431 1431 unsigned int length = 0;
1432 1432 rtems_status_code status;
1433 1433 unsigned int sid;
1434 1434 float *spectral_matrix;
1435 1435 int coarseTime;
1436 1436 int fineTime;
1437 1437 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1438 1438
1439 1439 sid = ring_node_to_send->sid;
1440 1440 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1441 1441 coarseTime = ring_node_to_send->coarseTime;
1442 1442 fineTime = ring_node_to_send->fineTime;
1443 1443
1444 header->biaStatusInfo = pa_bia_status_info;
1444 header->pa_bia_status_info = pa_bia_status_info;
1445 1445 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1446 1446
1447 1447 for (i=0; i<3; i++)
1448 1448 {
1449 1449 if ((i==0) || (i==1))
1450 1450 {
1451 1451 spw_ioctl_send_ASM.dlen = DLEN_ASM_F1_PKT_1;
1452 1452 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1453 1453 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1_1) ) * NB_VALUES_PER_SM )
1454 1454 ];
1455 1455 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1_1;
1456 1456 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1457 1457 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_1) >> 8 ); // BLK_NR MSB
1458 1458 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1_1); // BLK_NR LSB
1459 1459 }
1460 1460 else
1461 1461 {
1462 1462 spw_ioctl_send_ASM.dlen = DLEN_ASM_F1_PKT_2;
1463 1463 spw_ioctl_send_ASM.data = (char*) &spectral_matrix[
1464 1464 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1_1) ) * NB_VALUES_PER_SM )
1465 1465 ];
1466 1466 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1_2;
1467 1467 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1468 1468 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_2) >> 8 ); // BLK_NR MSB
1469 1469 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1_2); // BLK_NR LSB
1470 1470 }
1471 1471
1472 1472 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1473 1473 spw_ioctl_send_ASM.hdr = (char *) header;
1474 1474 spw_ioctl_send_ASM.options = 0;
1475 1475
1476 1476 // (2) BUILD THE HEADER
1477 1477 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1478 1478 header->packetLength[0] = (unsigned char) (length>>8);
1479 1479 header->packetLength[1] = (unsigned char) (length);
1480 1480 header->sid = (unsigned char) sid; // SID
1481 1481 header->pa_lfr_pkt_cnt_asm = 3;
1482 1482 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1483 1483
1484 1484 // (3) SET PACKET TIME
1485 1485 header->time[0] = (unsigned char) (coarseTime>>24);
1486 1486 header->time[1] = (unsigned char) (coarseTime>>16);
1487 1487 header->time[2] = (unsigned char) (coarseTime>>8);
1488 1488 header->time[3] = (unsigned char) (coarseTime);
1489 1489 header->time[4] = (unsigned char) (fineTime>>8);
1490 1490 header->time[5] = (unsigned char) (fineTime);
1491 1491 //
1492 1492 header->acquisitionTime[0] = header->time[0];
1493 1493 header->acquisitionTime[1] = header->time[1];
1494 1494 header->acquisitionTime[2] = header->time[2];
1495 1495 header->acquisitionTime[3] = header->time[3];
1496 1496 header->acquisitionTime[4] = header->time[4];
1497 1497 header->acquisitionTime[5] = header->time[5];
1498 1498
1499 1499 // (4) SEND PACKET
1500 1500 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1501 1501 if (status != RTEMS_SUCCESSFUL) {
1502 1502 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1503 1503 }
1504 1504 }
1505 1505 }
1506 1506
1507 1507 void spw_send_asm_f2( ring_node *ring_node_to_send,
1508 1508 Header_TM_LFR_SCIENCE_ASM_t *header )
1509 1509 {
1510 1510 unsigned int i;
1511 1511 unsigned int length = 0;
1512 1512 rtems_status_code status;
1513 1513 unsigned int sid;
1514 1514 float *spectral_matrix;
1515 1515 int coarseTime;
1516 1516 int fineTime;
1517 1517 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1518 1518
1519 1519 sid = ring_node_to_send->sid;
1520 1520 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1521 1521 coarseTime = ring_node_to_send->coarseTime;
1522 1522 fineTime = ring_node_to_send->fineTime;
1523 1523
1524 header->biaStatusInfo = pa_bia_status_info;
1524 header->pa_bia_status_info = pa_bia_status_info;
1525 1525 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1526 1526
1527 1527 for (i=0; i<3; i++)
1528 1528 {
1529 1529
1530 1530 spw_ioctl_send_ASM.dlen = DLEN_ASM_F2_PKT;
1531 1531 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1532 1532 ( (ASM_F2_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F2) ) * NB_VALUES_PER_SM )
1533 1533 ];
1534 1534 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F2;
1535 1535 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3;
1536 1536 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F2) >> 8 ); // BLK_NR MSB
1537 1537 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F2); // BLK_NR LSB
1538 1538
1539 1539 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1540 1540 spw_ioctl_send_ASM.hdr = (char *) header;
1541 1541 spw_ioctl_send_ASM.options = 0;
1542 1542
1543 1543 // (2) BUILD THE HEADER
1544 1544 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1545 1545 header->packetLength[0] = (unsigned char) (length>>8);
1546 1546 header->packetLength[1] = (unsigned char) (length);
1547 1547 header->sid = (unsigned char) sid; // SID
1548 1548 header->pa_lfr_pkt_cnt_asm = 3;
1549 1549 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1550 1550
1551 1551 // (3) SET PACKET TIME
1552 1552 header->time[0] = (unsigned char) (coarseTime>>24);
1553 1553 header->time[1] = (unsigned char) (coarseTime>>16);
1554 1554 header->time[2] = (unsigned char) (coarseTime>>8);
1555 1555 header->time[3] = (unsigned char) (coarseTime);
1556 1556 header->time[4] = (unsigned char) (fineTime>>8);
1557 1557 header->time[5] = (unsigned char) (fineTime);
1558 1558 //
1559 1559 header->acquisitionTime[0] = header->time[0];
1560 1560 header->acquisitionTime[1] = header->time[1];
1561 1561 header->acquisitionTime[2] = header->time[2];
1562 1562 header->acquisitionTime[3] = header->time[3];
1563 1563 header->acquisitionTime[4] = header->time[4];
1564 1564 header->acquisitionTime[5] = header->time[5];
1565 1565
1566 1566 // (4) SEND PACKET
1567 1567 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1568 1568 if (status != RTEMS_SUCCESSFUL) {
1569 1569 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1570 1570 }
1571 1571 }
1572 1572 }
1573 1573
1574 1574 void spw_send_k_dump( ring_node *ring_node_to_send )
1575 1575 {
1576 1576 rtems_status_code status;
1577 1577 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump;
1578 1578 unsigned int packetLength;
1579 1579 unsigned int size;
1580 1580
1581 1581 PRINTF("spw_send_k_dump\n")
1582 1582
1583 1583 kcoefficients_dump = (Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *) ring_node_to_send->buffer_address;
1584 1584
1585 1585 packetLength = kcoefficients_dump->packetLength[0] * 256 + kcoefficients_dump->packetLength[1];
1586 1586
1587 1587 size = packetLength + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
1588 1588
1589 1589 PRINTF2("packetLength %d, size %d\n", packetLength, size )
1590 1590
1591 1591 status = write( fdSPW, (char *) ring_node_to_send->buffer_address, size );
1592 1592
1593 1593 if (status == -1){
1594 1594 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
1595 1595 }
1596 1596
1597 1597 ring_node_to_send->status = 0x00;
1598 1598 }
@@ -1,408 +1,408
1 1 /** Functions related to data processing.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 7 *
8 8 */
9 9
10 10 #include "avf0_prc0.h"
11 11 #include "fsw_processing.h"
12 12
13 13 nb_sm_before_bp_asm_f0 nb_sm_before_f0;
14 14
15 15 //***
16 16 // F0
17 17 ring_node_asm asm_ring_norm_f0 [ NB_RING_NODES_ASM_NORM_F0 ];
18 18 ring_node_asm asm_ring_burst_sbm_f0 [ NB_RING_NODES_ASM_BURST_SBM_F0 ];
19 19
20 20 ring_node ring_to_send_asm_f0 [ NB_RING_NODES_ASM_F0 ];
21 21 int buffer_asm_f0 [ NB_RING_NODES_ASM_F0 * TOTAL_SIZE_SM ];
22 22
23 23 float asm_f0_patched_norm [ TOTAL_SIZE_SM ];
24 24 float asm_f0_patched_burst_sbm [ TOTAL_SIZE_SM ];
25 25 float asm_f0_reorganized [ TOTAL_SIZE_SM ];
26 26
27 27 char asm_f0_char [ TIME_OFFSET_IN_BYTES + (TOTAL_SIZE_SM * 2) ];
28 28 float compressed_sm_norm_f0[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F0];
29 29 float compressed_sm_sbm_f0 [ TOTAL_SIZE_COMPRESSED_ASM_SBM_F0 ];
30 30
31 31 float k_coeff_intercalib_f0_norm[ NB_BINS_COMPRESSED_SM_F0 * NB_K_COEFF_PER_BIN ]; // 11 * 32 = 352
32 32 float k_coeff_intercalib_f0_sbm[ NB_BINS_COMPRESSED_SM_SBM_F0 * NB_K_COEFF_PER_BIN ]; // 22 * 32 = 704
33 33
34 34 //************
35 35 // RTEMS TASKS
36 36
37 37 rtems_task avf0_task( rtems_task_argument lfrRequestedMode )
38 38 {
39 39 int i;
40 40
41 41 rtems_event_set event_out;
42 42 rtems_status_code status;
43 43 rtems_id queue_id_prc0;
44 44 asm_msg msgForPRC;
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 msgForPRC.norm = current_ring_node_asm_norm_f0;
82 82 msgForPRC.burst_sbm = current_ring_node_asm_burst_sbm_f0;
83 83 msgForPRC.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 &msgForPRC );
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 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_BURST_BP1_F0;
118 118 }
119 119 else if ( (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
120 120 {
121 121 msgForPRC.event = msgForPRC.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 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_BURST_BP2_F0;
131 131 }
132 132 else if ( (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
133 133 {
134 134 msgForPRC.event = msgForPRC.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 msgForPRC.event = msgForPRC.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 msgForPRC.event = msgForPRC.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 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_ASM_F0;
167 167 }
168 168 }
169 169
170 170 //*************************
171 171 // send the message to PRC
172 172 if (msgForPRC.event != 0x00)
173 173 {
174 174 status = rtems_message_queue_send( queue_id_prc0, (char *) &msgForPRC, MSG_QUEUE_SIZE_PRC0);
175 175 }
176 176
177 177 if (status != RTEMS_SUCCESSFUL) {
178 178 PRINTF1("in AVF0 *** Error sending message to PRC, code %d\n", status)
179 179 }
180 180 }
181 181 }
182 182
183 183 rtems_task prc0_task( rtems_task_argument lfrRequestedMode )
184 184 {
185 185 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
186 186 size_t size; // size of the incoming TC packet
187 187 asm_msg *incomingMsg;
188 188 //
189 189 unsigned char sid;
190 190 rtems_status_code status;
191 191 rtems_id queue_id;
192 192 rtems_id queue_id_q_p0;
193 193 bp_packet_with_spare packet_norm_bp1;
194 194 bp_packet packet_norm_bp2;
195 195 bp_packet packet_sbm_bp1;
196 196 bp_packet packet_sbm_bp2;
197 197 ring_node *current_ring_node_to_send_asm_f0;
198 198
199 199 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
200 200 init_ring( ring_to_send_asm_f0, NB_RING_NODES_ASM_F0, (volatile int*) buffer_asm_f0, TOTAL_SIZE_SM );
201 201 current_ring_node_to_send_asm_f0 = ring_to_send_asm_f0;
202 202
203 203 //*************
204 204 // NORM headers
205 205 BP_init_header_with_spare( &packet_norm_bp1,
206 206 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F0,
207 207 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F0, NB_BINS_COMPRESSED_SM_F0 );
208 208 BP_init_header( &packet_norm_bp2,
209 209 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F0,
210 210 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F0, NB_BINS_COMPRESSED_SM_F0);
211 211
212 212 //****************************
213 213 // BURST SBM1 and SBM2 headers
214 214 if ( lfrRequestedMode == LFR_MODE_BURST )
215 215 {
216 216 BP_init_header( &packet_sbm_bp1,
217 217 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP1_F0,
218 218 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
219 219 BP_init_header( &packet_sbm_bp2,
220 220 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP2_F0,
221 221 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
222 222 }
223 223 else if ( lfrRequestedMode == LFR_MODE_SBM1 )
224 224 {
225 225 BP_init_header( &packet_sbm_bp1,
226 226 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM1_BP1_F0,
227 227 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
228 228 BP_init_header( &packet_sbm_bp2,
229 229 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM1_BP2_F0,
230 230 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
231 231 }
232 232 else if ( lfrRequestedMode == LFR_MODE_SBM2 )
233 233 {
234 234 BP_init_header( &packet_sbm_bp1,
235 235 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP1_F0,
236 236 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
237 237 BP_init_header( &packet_sbm_bp2,
238 238 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP2_F0,
239 239 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
240 240 }
241 241 else
242 242 {
243 243 PRINTF1("in PRC0 *** lfrRequestedMode is %d, several headers not initialized\n", (unsigned int) lfrRequestedMode)
244 244 }
245 245
246 246 status = get_message_queue_id_send( &queue_id );
247 247 if (status != RTEMS_SUCCESSFUL)
248 248 {
249 249 PRINTF1("in PRC0 *** ERR get_message_queue_id_send %d\n", status)
250 250 }
251 251 status = get_message_queue_id_prc0( &queue_id_q_p0);
252 252 if (status != RTEMS_SUCCESSFUL)
253 253 {
254 254 PRINTF1("in PRC0 *** ERR get_message_queue_id_prc0 %d\n", status)
255 255 }
256 256
257 257 BOOT_PRINTF1("in PRC0 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
258 258
259 259 while(1){
260 260 status = rtems_message_queue_receive( queue_id_q_p0, incomingData, &size, //************************************
261 261 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF0
262 262
263 263 incomingMsg = (asm_msg*) incomingData;
264 264
265 265 ASM_patch( incomingMsg->norm->matrix, asm_f0_patched_norm );
266 266 ASM_patch( incomingMsg->burst_sbm->matrix, asm_f0_patched_burst_sbm );
267 267
268 268 //****************
269 269 //****************
270 270 // BURST SBM1 SBM2
271 271 //****************
272 272 //****************
273 273 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP1_F0 ) || (incomingMsg->event & RTEMS_EVENT_SBM_BP1_F0 ) )
274 274 {
275 275 sid = getSID( incomingMsg->event );
276 276 // 1) compress the matrix for Basic Parameters calculation
277 277 ASM_compress_reorganize_and_divide_mask( asm_f0_patched_burst_sbm, compressed_sm_sbm_f0,
278 278 nb_sm_before_f0.burst_sbm_bp1,
279 279 NB_BINS_COMPRESSED_SM_SBM_F0, NB_BINS_TO_AVERAGE_ASM_SBM_F0,
280 280 ASM_F0_INDICE_START, CHANNELF0);
281 281 // 2) compute the BP1 set
282 282 BP1_set( compressed_sm_sbm_f0, k_coeff_intercalib_f0_sbm, NB_BINS_COMPRESSED_SM_SBM_F0, packet_sbm_bp1.data );
283 283 // 3) send the BP1 set
284 284 set_time( packet_sbm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
285 285 set_time( packet_sbm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
286 packet_sbm_bp1.biaStatusInfo = pa_bia_status_info;
286 packet_sbm_bp1.pa_bia_status_info = pa_bia_status_info;
287 287 packet_sbm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
288 288 BP_send_s1_s2( (char *) &packet_sbm_bp1, queue_id,
289 289 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0 + PACKET_LENGTH_DELTA,
290 290 sid);
291 291 // 4) compute the BP2 set if needed
292 292 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP2_F0) || (incomingMsg->event & RTEMS_EVENT_SBM_BP2_F0) )
293 293 {
294 294 // 1) compute the BP2 set
295 295 BP2_set( compressed_sm_sbm_f0, NB_BINS_COMPRESSED_SM_SBM_F0, packet_sbm_bp2.data );
296 296 // 2) send the BP2 set
297 297 set_time( packet_sbm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
298 298 set_time( packet_sbm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
299 packet_sbm_bp2.biaStatusInfo = pa_bia_status_info;
299 packet_sbm_bp2.pa_bia_status_info = pa_bia_status_info;
300 300 packet_sbm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
301 301 BP_send_s1_s2( (char *) &packet_sbm_bp2, queue_id,
302 302 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0 + PACKET_LENGTH_DELTA,
303 303 sid);
304 304 }
305 305 }
306 306
307 307 //*****
308 308 //*****
309 309 // NORM
310 310 //*****
311 311 //*****
312 312 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F0)
313 313 {
314 314 // 1) compress the matrix for Basic Parameters calculation
315 315 ASM_compress_reorganize_and_divide_mask( asm_f0_patched_norm, compressed_sm_norm_f0,
316 316 nb_sm_before_f0.norm_bp1,
317 317 NB_BINS_COMPRESSED_SM_F0, NB_BINS_TO_AVERAGE_ASM_F0,
318 318 ASM_F0_INDICE_START, CHANNELF0 );
319 319 // 2) compute the BP1 set
320 320 BP1_set( compressed_sm_norm_f0, k_coeff_intercalib_f0_norm, NB_BINS_COMPRESSED_SM_F0, packet_norm_bp1.data );
321 321 // 3) send the BP1 set
322 322 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
323 323 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
324 packet_norm_bp1.biaStatusInfo = pa_bia_status_info;
324 packet_norm_bp1.pa_bia_status_info = pa_bia_status_info;
325 325 packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
326 326 BP_send( (char *) &packet_norm_bp1, queue_id,
327 327 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F0 + PACKET_LENGTH_DELTA,
328 328 SID_NORM_BP1_F0 );
329 329 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F0)
330 330 {
331 331 // 1) compute the BP2 set using the same ASM as the one used for BP1
332 332 BP2_set( compressed_sm_norm_f0, NB_BINS_COMPRESSED_SM_F0, packet_norm_bp2.data );
333 333 // 2) send the BP2 set
334 334 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
335 335 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
336 packet_norm_bp2.biaStatusInfo = pa_bia_status_info;
336 packet_norm_bp2.pa_bia_status_info = pa_bia_status_info;
337 337 packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
338 338 BP_send( (char *) &packet_norm_bp2, queue_id,
339 339 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F0 + PACKET_LENGTH_DELTA,
340 340 SID_NORM_BP2_F0);
341 341 }
342 342 }
343 343
344 344 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F0)
345 345 {
346 346 // 1) reorganize the ASM and divide
347 347 ASM_reorganize_and_divide( asm_f0_patched_norm,
348 348 (float*) current_ring_node_to_send_asm_f0->buffer_address,
349 349 nb_sm_before_f0.norm_bp1 );
350 350 current_ring_node_to_send_asm_f0->coarseTime = incomingMsg->coarseTimeNORM;
351 351 current_ring_node_to_send_asm_f0->fineTime = incomingMsg->fineTimeNORM;
352 352 current_ring_node_to_send_asm_f0->sid = SID_NORM_ASM_F0;
353 353
354 354 // 3) send the spectral matrix packets
355 355 status = rtems_message_queue_send( queue_id, &current_ring_node_to_send_asm_f0, sizeof( ring_node* ) );
356 356 // change asm ring node
357 357 current_ring_node_to_send_asm_f0 = current_ring_node_to_send_asm_f0->next;
358 358 }
359 359
360 360 update_queue_max_count( queue_id_q_p0, &hk_lfr_q_p0_fifo_size_max );
361 361
362 362 }
363 363 }
364 364
365 365 //**********
366 366 // FUNCTIONS
367 367
368 368 void reset_nb_sm_f0( unsigned char lfrMode )
369 369 {
370 370 nb_sm_before_f0.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0 * 96;
371 371 nb_sm_before_f0.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1 * 96;
372 372 nb_sm_before_f0.norm_asm = (parameter_dump_packet.sy_lfr_n_asm_p[0] * 256 + parameter_dump_packet.sy_lfr_n_asm_p[1]) * 96;
373 373 nb_sm_before_f0.sbm1_bp1 = parameter_dump_packet.sy_lfr_s1_bp_p0 * 24; // 0.25 s per digit
374 374 nb_sm_before_f0.sbm1_bp2 = parameter_dump_packet.sy_lfr_s1_bp_p1 * 96;
375 375 nb_sm_before_f0.sbm2_bp1 = parameter_dump_packet.sy_lfr_s2_bp_p0 * 96;
376 376 nb_sm_before_f0.sbm2_bp2 = parameter_dump_packet.sy_lfr_s2_bp_p1 * 96;
377 377 nb_sm_before_f0.burst_bp1 = parameter_dump_packet.sy_lfr_b_bp_p0 * 96;
378 378 nb_sm_before_f0.burst_bp2 = parameter_dump_packet.sy_lfr_b_bp_p1 * 96;
379 379
380 380 if (lfrMode == LFR_MODE_SBM1)
381 381 {
382 382 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.sbm1_bp1;
383 383 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.sbm1_bp2;
384 384 }
385 385 else if (lfrMode == LFR_MODE_SBM2)
386 386 {
387 387 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.sbm2_bp1;
388 388 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.sbm2_bp2;
389 389 }
390 390 else if (lfrMode == LFR_MODE_BURST)
391 391 {
392 392 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.burst_bp1;
393 393 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.burst_bp2;
394 394 }
395 395 else
396 396 {
397 397 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.burst_bp1;
398 398 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.burst_bp2;
399 399 }
400 400 }
401 401
402 402 void init_k_coefficients_prc0( void )
403 403 {
404 404 init_k_coefficients( k_coeff_intercalib_f0_norm, NB_BINS_COMPRESSED_SM_F0 );
405 405
406 406 init_kcoeff_sbm_from_kcoeff_norm( k_coeff_intercalib_f0_norm, k_coeff_intercalib_f0_sbm, NB_BINS_COMPRESSED_SM_F0);
407 407 }
408 408
@@ -1,394 +1,394
1 1 /** Functions related to data processing.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 7 *
8 8 */
9 9
10 10 #include "avf1_prc1.h"
11 11
12 12 nb_sm_before_bp_asm_f1 nb_sm_before_f1;
13 13
14 14 extern ring_node sm_ring_f1[ ];
15 15
16 16 //***
17 17 // F1
18 18 ring_node_asm asm_ring_norm_f1 [ NB_RING_NODES_ASM_NORM_F1 ];
19 19 ring_node_asm asm_ring_burst_sbm_f1 [ NB_RING_NODES_ASM_BURST_SBM_F1 ];
20 20
21 21 ring_node ring_to_send_asm_f1 [ NB_RING_NODES_ASM_F1 ];
22 22 int buffer_asm_f1 [ NB_RING_NODES_ASM_F1 * TOTAL_SIZE_SM ];
23 23
24 24 float asm_f1_patched_norm [ TOTAL_SIZE_SM ];
25 25 float asm_f1_patched_burst_sbm [ TOTAL_SIZE_SM ];
26 26 float asm_f1_reorganized [ TOTAL_SIZE_SM ];
27 27
28 28 char asm_f1_char [ TOTAL_SIZE_SM * 2 ];
29 29 float compressed_sm_norm_f1[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F1];
30 30 float compressed_sm_sbm_f1 [ TOTAL_SIZE_COMPRESSED_ASM_SBM_F1 ];
31 31
32 32 float k_coeff_intercalib_f1_norm[ NB_BINS_COMPRESSED_SM_F1 * NB_K_COEFF_PER_BIN ]; // 13 * 32 = 416
33 33 float k_coeff_intercalib_f1_sbm[ NB_BINS_COMPRESSED_SM_SBM_F1 * NB_K_COEFF_PER_BIN ]; // 26 * 32 = 832
34 34
35 35 //************
36 36 // RTEMS TASKS
37 37
38 38 rtems_task avf1_task( rtems_task_argument lfrRequestedMode )
39 39 {
40 40 int i;
41 41
42 42 rtems_event_set event_out;
43 43 rtems_status_code status;
44 44 rtems_id queue_id_prc1;
45 45 asm_msg msgForPRC;
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 msgForPRC.norm = current_ring_node_asm_norm_f1;
83 83 msgForPRC.burst_sbm = current_ring_node_asm_burst_sbm_f1;
84 84 msgForPRC.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 &msgForPRC );
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 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_BURST_BP1_F1;
119 119 }
120 120 else if ( lfrCurrentMode == LFR_MODE_SBM2 )
121 121 {
122 122 msgForPRC.event = msgForPRC.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 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_BURST_BP2_F1;
132 132 }
133 133 else if ( lfrCurrentMode == LFR_MODE_SBM2 )
134 134 {
135 135 msgForPRC.event = msgForPRC.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 msgForPRC.event = msgForPRC.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 msgForPRC.event = msgForPRC.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 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_ASM_F1;
168 168 }
169 169 }
170 170
171 171 //*************************
172 172 // send the message to PRC
173 173 if (msgForPRC.event != 0x00)
174 174 {
175 175 status = rtems_message_queue_send( queue_id_prc1, (char *) &msgForPRC, MSG_QUEUE_SIZE_PRC1);
176 176 }
177 177
178 178 if (status != RTEMS_SUCCESSFUL) {
179 179 PRINTF1("in AVF1 *** Error sending message to PRC1, code %d\n", status)
180 180 }
181 181 }
182 182 }
183 183
184 184 rtems_task prc1_task( rtems_task_argument lfrRequestedMode )
185 185 {
186 186 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
187 187 size_t size; // size of the incoming TC packet
188 188 asm_msg *incomingMsg;
189 189 //
190 190 unsigned char sid;
191 191 rtems_status_code status;
192 192 rtems_id queue_id_send;
193 193 rtems_id queue_id_q_p1;
194 194 bp_packet_with_spare packet_norm_bp1;
195 195 bp_packet packet_norm_bp2;
196 196 bp_packet packet_sbm_bp1;
197 197 bp_packet packet_sbm_bp2;
198 198 ring_node *current_ring_node_to_send_asm_f1;
199 199
200 200 unsigned long long int localTime;
201 201
202 202 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
203 203 init_ring( ring_to_send_asm_f1, NB_RING_NODES_ASM_F1, (volatile int*) buffer_asm_f1, TOTAL_SIZE_SM );
204 204 current_ring_node_to_send_asm_f1 = ring_to_send_asm_f1;
205 205
206 206 //*************
207 207 // NORM headers
208 208 BP_init_header_with_spare( &packet_norm_bp1,
209 209 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F1,
210 210 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F1, NB_BINS_COMPRESSED_SM_F1 );
211 211 BP_init_header( &packet_norm_bp2,
212 212 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F1,
213 213 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F1, NB_BINS_COMPRESSED_SM_F1);
214 214
215 215 //***********************
216 216 // BURST and SBM2 headers
217 217 if ( lfrRequestedMode == LFR_MODE_BURST )
218 218 {
219 219 BP_init_header( &packet_sbm_bp1,
220 220 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP1_F1,
221 221 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
222 222 BP_init_header( &packet_sbm_bp2,
223 223 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP2_F1,
224 224 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
225 225 }
226 226 else if ( lfrRequestedMode == LFR_MODE_SBM2 )
227 227 {
228 228 BP_init_header( &packet_sbm_bp1,
229 229 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP1_F1,
230 230 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
231 231 BP_init_header( &packet_sbm_bp2,
232 232 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP2_F1,
233 233 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
234 234 }
235 235 else
236 236 {
237 237 PRINTF1("in PRC1 *** lfrRequestedMode is %d, several headers not initialized\n", (unsigned int) lfrRequestedMode)
238 238 }
239 239
240 240 status = get_message_queue_id_send( &queue_id_send );
241 241 if (status != RTEMS_SUCCESSFUL)
242 242 {
243 243 PRINTF1("in PRC1 *** ERR get_message_queue_id_send %d\n", status)
244 244 }
245 245 status = get_message_queue_id_prc1( &queue_id_q_p1);
246 246 if (status != RTEMS_SUCCESSFUL)
247 247 {
248 248 PRINTF1("in PRC1 *** ERR get_message_queue_id_prc1 %d\n", status)
249 249 }
250 250
251 251 BOOT_PRINTF1("in PRC1 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
252 252
253 253 while(1){
254 254 status = rtems_message_queue_receive( queue_id_q_p1, incomingData, &size, //************************************
255 255 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF0
256 256
257 257 incomingMsg = (asm_msg*) incomingData;
258 258
259 259 ASM_patch( incomingMsg->norm->matrix, asm_f1_patched_norm );
260 260 ASM_patch( incomingMsg->burst_sbm->matrix, asm_f1_patched_burst_sbm );
261 261
262 262 localTime = getTimeAsUnsignedLongLongInt( );
263 263 //***********
264 264 //***********
265 265 // BURST SBM2
266 266 //***********
267 267 //***********
268 268 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP1_F1) || (incomingMsg->event & RTEMS_EVENT_SBM_BP1_F1) )
269 269 {
270 270 sid = getSID( incomingMsg->event );
271 271 // 1) compress the matrix for Basic Parameters calculation
272 272 ASM_compress_reorganize_and_divide_mask( asm_f1_patched_burst_sbm, compressed_sm_sbm_f1,
273 273 nb_sm_before_f1.burst_sbm_bp1,
274 274 NB_BINS_COMPRESSED_SM_SBM_F1, NB_BINS_TO_AVERAGE_ASM_SBM_F1,
275 275 ASM_F1_INDICE_START, CHANNELF1);
276 276 // 2) compute the BP1 set
277 277 BP1_set( compressed_sm_sbm_f1, k_coeff_intercalib_f1_sbm, NB_BINS_COMPRESSED_SM_SBM_F1, packet_sbm_bp1.data );
278 278 // 3) send the BP1 set
279 279 set_time( packet_sbm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
280 280 set_time( packet_sbm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
281 packet_sbm_bp1.biaStatusInfo = pa_bia_status_info;
281 packet_sbm_bp1.pa_bia_status_info = pa_bia_status_info;
282 282 packet_sbm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
283 283 BP_send_s1_s2( (char *) &packet_sbm_bp1, queue_id_send,
284 284 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1 + PACKET_LENGTH_DELTA,
285 285 sid );
286 286 // 4) compute the BP2 set if needed
287 287 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP2_F1) || (incomingMsg->event & RTEMS_EVENT_SBM_BP2_F1) )
288 288 {
289 289 // 1) compute the BP2 set
290 290 BP2_set( compressed_sm_sbm_f1, NB_BINS_COMPRESSED_SM_SBM_F1, packet_sbm_bp2.data );
291 291 // 2) send the BP2 set
292 292 set_time( packet_sbm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
293 293 set_time( packet_sbm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
294 packet_sbm_bp2.biaStatusInfo = pa_bia_status_info;
294 packet_sbm_bp2.pa_bia_status_info = pa_bia_status_info;
295 295 packet_sbm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
296 296 BP_send_s1_s2( (char *) &packet_sbm_bp2, queue_id_send,
297 297 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1 + PACKET_LENGTH_DELTA,
298 298 sid );
299 299 }
300 300 }
301 301
302 302 //*****
303 303 //*****
304 304 // NORM
305 305 //*****
306 306 //*****
307 307 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F1)
308 308 {
309 309 // 1) compress the matrix for Basic Parameters calculation
310 310 ASM_compress_reorganize_and_divide_mask( asm_f1_patched_norm, compressed_sm_norm_f1,
311 311 nb_sm_before_f1.norm_bp1,
312 312 NB_BINS_COMPRESSED_SM_F1, NB_BINS_TO_AVERAGE_ASM_F1,
313 313 ASM_F1_INDICE_START, CHANNELF1 );
314 314 // 2) compute the BP1 set
315 315 BP1_set( compressed_sm_norm_f1, k_coeff_intercalib_f1_norm, NB_BINS_COMPRESSED_SM_F1, packet_norm_bp1.data );
316 316 // 3) send the BP1 set
317 317 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
318 318 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
319 packet_norm_bp1.biaStatusInfo = pa_bia_status_info;
319 packet_norm_bp1.pa_bia_status_info = pa_bia_status_info;
320 320 packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
321 321 BP_send( (char *) &packet_norm_bp1, queue_id_send,
322 322 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F1 + PACKET_LENGTH_DELTA,
323 323 SID_NORM_BP1_F1 );
324 324 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F1)
325 325 {
326 326 // 1) compute the BP2 set
327 327 BP2_set( compressed_sm_norm_f1, NB_BINS_COMPRESSED_SM_F1, packet_norm_bp2.data );
328 328 // 2) send the BP2 set
329 329 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
330 330 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
331 packet_norm_bp2.biaStatusInfo = pa_bia_status_info;
331 packet_norm_bp2.pa_bia_status_info = pa_bia_status_info;
332 332 packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
333 333 BP_send( (char *) &packet_norm_bp2, queue_id_send,
334 334 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F1 + PACKET_LENGTH_DELTA,
335 335 SID_NORM_BP2_F1 );
336 336 }
337 337 }
338 338
339 339 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F1)
340 340 {
341 341 // 1) reorganize the ASM and divide
342 342 ASM_reorganize_and_divide( asm_f1_patched_norm,
343 343 (float*) current_ring_node_to_send_asm_f1->buffer_address,
344 344 nb_sm_before_f1.norm_bp1 );
345 345 current_ring_node_to_send_asm_f1->coarseTime = incomingMsg->coarseTimeNORM;
346 346 current_ring_node_to_send_asm_f1->fineTime = incomingMsg->fineTimeNORM;
347 347 current_ring_node_to_send_asm_f1->sid = SID_NORM_ASM_F1;
348 348 // 3) send the spectral matrix packets
349 349 status = rtems_message_queue_send( queue_id_send, &current_ring_node_to_send_asm_f1, sizeof( ring_node* ) );
350 350 // change asm ring node
351 351 current_ring_node_to_send_asm_f1 = current_ring_node_to_send_asm_f1->next;
352 352 }
353 353
354 354 update_queue_max_count( queue_id_q_p1, &hk_lfr_q_p1_fifo_size_max );
355 355
356 356 }
357 357 }
358 358
359 359 //**********
360 360 // FUNCTIONS
361 361
362 362 void reset_nb_sm_f1( unsigned char lfrMode )
363 363 {
364 364 nb_sm_before_f1.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0 * 16;
365 365 nb_sm_before_f1.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1 * 16;
366 366 nb_sm_before_f1.norm_asm = (parameter_dump_packet.sy_lfr_n_asm_p[0] * 256 + parameter_dump_packet.sy_lfr_n_asm_p[1]) * 16;
367 367 nb_sm_before_f1.sbm2_bp1 = parameter_dump_packet.sy_lfr_s2_bp_p0 * 16;
368 368 nb_sm_before_f1.sbm2_bp2 = parameter_dump_packet.sy_lfr_s2_bp_p1 * 16;
369 369 nb_sm_before_f1.burst_bp1 = parameter_dump_packet.sy_lfr_b_bp_p0 * 16;
370 370 nb_sm_before_f1.burst_bp2 = parameter_dump_packet.sy_lfr_b_bp_p1 * 16;
371 371
372 372 if (lfrMode == LFR_MODE_SBM2)
373 373 {
374 374 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.sbm2_bp1;
375 375 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.sbm2_bp2;
376 376 }
377 377 else if (lfrMode == LFR_MODE_BURST)
378 378 {
379 379 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.burst_bp1;
380 380 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.burst_bp2;
381 381 }
382 382 else
383 383 {
384 384 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.burst_bp1;
385 385 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.burst_bp2;
386 386 }
387 387 }
388 388
389 389 void init_k_coefficients_prc1( void )
390 390 {
391 391 init_k_coefficients( k_coeff_intercalib_f1_norm, NB_BINS_COMPRESSED_SM_F1 );
392 392
393 393 init_kcoeff_sbm_from_kcoeff_norm( k_coeff_intercalib_f1_norm, k_coeff_intercalib_f1_sbm, NB_BINS_COMPRESSED_SM_F1);
394 394 }
@@ -1,281 +1,281
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 msgForPRC;
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 msgForPRC.norm = current_ring_node_asm_norm_f2;
71 71 msgForPRC.burst_sbm = NULL;
72 72 msgForPRC.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 78 // compute the average and store it in the averaged_sm_f2 buffer
79 79 SM_average_f2( current_ring_node_asm_norm_f2->matrix,
80 80 nodeForAveraging,
81 81 nb_norm_bp1,
82 82 &msgForPRC );
83 83
84 84 // update nb_average
85 85 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF2;
86 86 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF2;
87 87 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF2;
88 88
89 89 if (nb_norm_bp1 == nb_sm_before_f2.norm_bp1)
90 90 {
91 91 nb_norm_bp1 = 0;
92 92 // set another ring for the ASM storage
93 93 current_ring_node_asm_norm_f2 = current_ring_node_asm_norm_f2->next;
94 94 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
95 95 || (lfrCurrentMode == LFR_MODE_SBM2) )
96 96 {
97 97 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_BP1_F2;
98 98 }
99 99 }
100 100
101 101 if (nb_norm_bp2 == nb_sm_before_f2.norm_bp2)
102 102 {
103 103 nb_norm_bp2 = 0;
104 104 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
105 105 || (lfrCurrentMode == LFR_MODE_SBM2) )
106 106 {
107 107 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_BP2_F2;
108 108 }
109 109 }
110 110
111 111 if (nb_norm_asm == nb_sm_before_f2.norm_asm)
112 112 {
113 113 nb_norm_asm = 0;
114 114 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
115 115 || (lfrCurrentMode == LFR_MODE_SBM2) )
116 116 {
117 117 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_ASM_F2;
118 118 }
119 119 }
120 120
121 121 //*************************
122 122 // send the message to PRC2
123 123 if (msgForPRC.event != 0x00)
124 124 {
125 125 status = rtems_message_queue_send( queue_id_prc2, (char *) &msgForPRC, MSG_QUEUE_SIZE_PRC2);
126 126 }
127 127
128 128 if (status != RTEMS_SUCCESSFUL) {
129 129 PRINTF1("in AVF2 *** Error sending message to PRC2, code %d\n", status)
130 130 }
131 131 }
132 132 }
133 133
134 134 rtems_task prc2_task( rtems_task_argument argument )
135 135 {
136 136 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
137 137 size_t size; // size of the incoming TC packet
138 138 asm_msg *incomingMsg;
139 139 //
140 140 rtems_status_code status;
141 141 rtems_id queue_id_send;
142 142 rtems_id queue_id_q_p2;
143 143 bp_packet packet_norm_bp1;
144 144 bp_packet packet_norm_bp2;
145 145 ring_node *current_ring_node_to_send_asm_f2;
146 146
147 147 unsigned long long int localTime;
148 148
149 149 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
150 150 init_ring( ring_to_send_asm_f2, NB_RING_NODES_ASM_F2, (volatile int*) buffer_asm_f2, TOTAL_SIZE_SM );
151 151 current_ring_node_to_send_asm_f2 = ring_to_send_asm_f2;
152 152
153 153 //*************
154 154 // NORM headers
155 155 BP_init_header( &packet_norm_bp1,
156 156 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F2,
157 157 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F2, NB_BINS_COMPRESSED_SM_F2 );
158 158 BP_init_header( &packet_norm_bp2,
159 159 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F2,
160 160 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F2, NB_BINS_COMPRESSED_SM_F2 );
161 161
162 162 status = get_message_queue_id_send( &queue_id_send );
163 163 if (status != RTEMS_SUCCESSFUL)
164 164 {
165 165 PRINTF1("in PRC2 *** ERR get_message_queue_id_send %d\n", status)
166 166 }
167 167 status = get_message_queue_id_prc2( &queue_id_q_p2);
168 168 if (status != RTEMS_SUCCESSFUL)
169 169 {
170 170 PRINTF1("in PRC2 *** ERR get_message_queue_id_prc2 %d\n", status)
171 171 }
172 172
173 173 BOOT_PRINTF("in PRC2 ***\n")
174 174
175 175 while(1){
176 176 status = rtems_message_queue_receive( queue_id_q_p2, incomingData, &size, //************************************
177 177 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF2
178 178
179 179 incomingMsg = (asm_msg*) incomingData;
180 180
181 181 ASM_patch( incomingMsg->norm->matrix, asm_f2_patched_norm );
182 182
183 183 localTime = getTimeAsUnsignedLongLongInt( );
184 184
185 185 //*****
186 186 //*****
187 187 // NORM
188 188 //*****
189 189 //*****
190 190 // 1) compress the matrix for Basic Parameters calculation
191 191 ASM_compress_reorganize_and_divide_mask( asm_f2_patched_norm, compressed_sm_norm_f2,
192 192 nb_sm_before_f2.norm_bp1,
193 193 NB_BINS_COMPRESSED_SM_F2, NB_BINS_TO_AVERAGE_ASM_F2,
194 194 ASM_F2_INDICE_START, CHANNELF2 );
195 195 // BP1_F2
196 196 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F2)
197 197 {
198 198 // 1) compute the BP1 set
199 199 BP1_set( compressed_sm_norm_f2, k_coeff_intercalib_f2, NB_BINS_COMPRESSED_SM_F2, packet_norm_bp1.data );
200 200 // 2) send the BP1 set
201 201 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
202 202 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
203 packet_norm_bp1.biaStatusInfo = pa_bia_status_info;
203 packet_norm_bp1.pa_bia_status_info = pa_bia_status_info;
204 204 packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
205 205 BP_send( (char *) &packet_norm_bp1, queue_id_send,
206 206 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F2 + PACKET_LENGTH_DELTA,
207 207 SID_NORM_BP1_F2 );
208 208 }
209 209 // BP2_F2
210 210 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F2)
211 211 {
212 212 // 1) compute the BP2 set
213 213 BP2_set( compressed_sm_norm_f2, NB_BINS_COMPRESSED_SM_F2, packet_norm_bp2.data );
214 214 // 2) send the BP2 set
215 215 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
216 216 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
217 packet_norm_bp2.biaStatusInfo = pa_bia_status_info;
217 packet_norm_bp2.pa_bia_status_info = pa_bia_status_info;
218 218 packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
219 219 BP_send( (char *) &packet_norm_bp2, queue_id_send,
220 220 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F2 + PACKET_LENGTH_DELTA,
221 221 SID_NORM_BP2_F2 );
222 222 }
223 223
224 224 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F2)
225 225 {
226 226 // 1) reorganize the ASM and divide
227 227 ASM_reorganize_and_divide( asm_f2_patched_norm,
228 228 (float*) current_ring_node_to_send_asm_f2->buffer_address,
229 229 nb_sm_before_f2.norm_bp1 );
230 230 current_ring_node_to_send_asm_f2->coarseTime = incomingMsg->coarseTimeNORM;
231 231 current_ring_node_to_send_asm_f2->fineTime = incomingMsg->fineTimeNORM;
232 232 current_ring_node_to_send_asm_f2->sid = SID_NORM_ASM_F2;
233 233 // 3) send the spectral matrix packets
234 234 status = rtems_message_queue_send( queue_id_send, &current_ring_node_to_send_asm_f2, sizeof( ring_node* ) );
235 235 // change asm ring node
236 236 current_ring_node_to_send_asm_f2 = current_ring_node_to_send_asm_f2->next;
237 237 }
238 238
239 239 update_queue_max_count( queue_id_q_p2, &hk_lfr_q_p2_fifo_size_max );
240 240
241 241 }
242 242 }
243 243
244 244 //**********
245 245 // FUNCTIONS
246 246
247 247 void reset_nb_sm_f2( void )
248 248 {
249 249 nb_sm_before_f2.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0;
250 250 nb_sm_before_f2.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1;
251 251 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];
252 252 }
253 253
254 254 void SM_average_f2( float *averaged_spec_mat_f2,
255 255 ring_node *ring_node,
256 256 unsigned int nbAverageNormF2,
257 257 asm_msg *msgForMATR )
258 258 {
259 259 float sum;
260 260 unsigned int i;
261 261
262 262 for(i=0; i<TOTAL_SIZE_SM; i++)
263 263 {
264 264 sum = ( (int *) (ring_node->buffer_address) ) [ i ];
265 265 if ( (nbAverageNormF2 == 0) )
266 266 {
267 267 averaged_spec_mat_f2[ i ] = sum;
268 268 msgForMATR->coarseTimeNORM = ring_node->coarseTime;
269 269 msgForMATR->fineTimeNORM = ring_node->fineTime;
270 270 }
271 271 else
272 272 {
273 273 averaged_spec_mat_f2[ i ] = ( averaged_spec_mat_f2[ i ] + sum );
274 274 }
275 275 }
276 276 }
277 277
278 278 void init_k_coefficients_prc2( void )
279 279 {
280 280 init_k_coefficients( k_coeff_intercalib_f2, NB_BINS_COMPRESSED_SM_F2);
281 281 }
@@ -1,720 +1,720
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 typedef enum restartState_t
20 20 {
21 21 WAIT_FOR_F2,
22 22 WAIT_FOR_F1,
23 23 WAIT_FOR_F0
24 24 } restartState;
25 25
26 26 //************************
27 27 // spectral matrices rings
28 28 ring_node sm_ring_f0[ NB_RING_NODES_SM_F0 ];
29 29 ring_node sm_ring_f1[ NB_RING_NODES_SM_F1 ];
30 30 ring_node sm_ring_f2[ NB_RING_NODES_SM_F2 ];
31 31 ring_node *current_ring_node_sm_f0;
32 32 ring_node *current_ring_node_sm_f1;
33 33 ring_node *current_ring_node_sm_f2;
34 34 ring_node *ring_node_for_averaging_sm_f0;
35 35 ring_node *ring_node_for_averaging_sm_f1;
36 36 ring_node *ring_node_for_averaging_sm_f2;
37 37
38 38 //
39 39 ring_node * getRingNodeForAveraging( unsigned char frequencyChannel)
40 40 {
41 41 ring_node *node;
42 42
43 43 node = NULL;
44 44 switch ( frequencyChannel ) {
45 45 case 0:
46 46 node = ring_node_for_averaging_sm_f0;
47 47 break;
48 48 case 1:
49 49 node = ring_node_for_averaging_sm_f1;
50 50 break;
51 51 case 2:
52 52 node = ring_node_for_averaging_sm_f2;
53 53 break;
54 54 default:
55 55 break;
56 56 }
57 57
58 58 return node;
59 59 }
60 60
61 61 //***********************************************************
62 62 // Interrupt Service Routine for spectral matrices processing
63 63
64 64 void spectral_matrices_isr_f0( int statusReg )
65 65 {
66 66 unsigned char status;
67 67 rtems_status_code status_code;
68 68 ring_node *full_ring_node;
69 69
70 70 status = (unsigned char) (statusReg & 0x03); // [0011] get the status_ready_matrix_f0_x bits
71 71
72 72 switch(status)
73 73 {
74 74 case 0:
75 75 break;
76 76 case 3:
77 77 // UNEXPECTED VALUE
78 78 spectral_matrix_regs->status = 0x03; // [0011]
79 79 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
80 80 break;
81 81 case 1:
82 82 full_ring_node = current_ring_node_sm_f0->previous;
83 83 full_ring_node->coarseTime = spectral_matrix_regs->f0_0_coarse_time;
84 84 full_ring_node->fineTime = spectral_matrix_regs->f0_0_fine_time;
85 85 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
86 86 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->buffer_address;
87 87 // if there are enough ring nodes ready, wake up an AVFx task
88 88 nb_sm_f0 = nb_sm_f0 + 1;
89 89 if (nb_sm_f0 == NB_SM_BEFORE_AVF0)
90 90 {
91 91 ring_node_for_averaging_sm_f0 = full_ring_node;
92 92 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
93 93 {
94 94 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
95 95 }
96 96 nb_sm_f0 = 0;
97 97 }
98 98 spectral_matrix_regs->status = 0x01; // [0000 0001]
99 99 break;
100 100 case 2:
101 101 full_ring_node = current_ring_node_sm_f0->previous;
102 102 full_ring_node->coarseTime = spectral_matrix_regs->f0_1_coarse_time;
103 103 full_ring_node->fineTime = spectral_matrix_regs->f0_1_fine_time;
104 104 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
105 105 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
106 106 // if there are enough ring nodes ready, wake up an AVFx task
107 107 nb_sm_f0 = nb_sm_f0 + 1;
108 108 if (nb_sm_f0 == NB_SM_BEFORE_AVF0)
109 109 {
110 110 ring_node_for_averaging_sm_f0 = full_ring_node;
111 111 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
112 112 {
113 113 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
114 114 }
115 115 nb_sm_f0 = 0;
116 116 }
117 117 spectral_matrix_regs->status = 0x02; // [0000 0010]
118 118 break;
119 119 }
120 120 }
121 121
122 122 void spectral_matrices_isr_f1( int statusReg )
123 123 {
124 124 rtems_status_code status_code;
125 125 unsigned char status;
126 126 ring_node *full_ring_node;
127 127
128 128 status = (unsigned char) ((statusReg & 0x0c) >> 2); // [1100] get the status_ready_matrix_f1_x bits
129 129
130 130 switch(status)
131 131 {
132 132 case 0:
133 133 break;
134 134 case 3:
135 135 // UNEXPECTED VALUE
136 136 spectral_matrix_regs->status = 0xc0; // [1100]
137 137 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
138 138 break;
139 139 case 1:
140 140 full_ring_node = current_ring_node_sm_f1->previous;
141 141 full_ring_node->coarseTime = spectral_matrix_regs->f1_0_coarse_time;
142 142 full_ring_node->fineTime = spectral_matrix_regs->f1_0_fine_time;
143 143 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
144 144 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->buffer_address;
145 145 // if there are enough ring nodes ready, wake up an AVFx task
146 146 nb_sm_f1 = nb_sm_f1 + 1;
147 147 if (nb_sm_f1 == NB_SM_BEFORE_AVF1)
148 148 {
149 149 ring_node_for_averaging_sm_f1 = full_ring_node;
150 150 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
151 151 {
152 152 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
153 153 }
154 154 nb_sm_f1 = 0;
155 155 }
156 156 spectral_matrix_regs->status = 0x04; // [0000 0100]
157 157 break;
158 158 case 2:
159 159 full_ring_node = current_ring_node_sm_f1->previous;
160 160 full_ring_node->coarseTime = spectral_matrix_regs->f1_1_coarse_time;
161 161 full_ring_node->fineTime = spectral_matrix_regs->f1_1_fine_time;
162 162 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
163 163 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
164 164 // if there are enough ring nodes ready, wake up an AVFx task
165 165 nb_sm_f1 = nb_sm_f1 + 1;
166 166 if (nb_sm_f1 == NB_SM_BEFORE_AVF1)
167 167 {
168 168 ring_node_for_averaging_sm_f1 = full_ring_node;
169 169 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
170 170 {
171 171 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
172 172 }
173 173 nb_sm_f1 = 0;
174 174 }
175 175 spectral_matrix_regs->status = 0x08; // [1000 0000]
176 176 break;
177 177 }
178 178 }
179 179
180 180 void spectral_matrices_isr_f2( int statusReg )
181 181 {
182 182 unsigned char status;
183 183 rtems_status_code status_code;
184 184
185 185 status = (unsigned char) ((statusReg & 0x30) >> 4); // [0011 0000] get the status_ready_matrix_f2_x bits
186 186
187 187 switch(status)
188 188 {
189 189 case 0:
190 190 break;
191 191 case 3:
192 192 // UNEXPECTED VALUE
193 193 spectral_matrix_regs->status = 0x30; // [0011 0000]
194 194 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
195 195 break;
196 196 case 1:
197 197 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
198 198 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
199 199 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_0_coarse_time;
200 200 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_0_fine_time;
201 201 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->buffer_address;
202 202 spectral_matrix_regs->status = 0x10; // [0001 0000]
203 203 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
204 204 {
205 205 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
206 206 }
207 207 break;
208 208 case 2:
209 209 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
210 210 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
211 211 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_1_coarse_time;
212 212 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_1_fine_time;
213 213 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
214 214 spectral_matrix_regs->status = 0x20; // [0010 0000]
215 215 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
216 216 {
217 217 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
218 218 }
219 219 break;
220 220 }
221 221 }
222 222
223 223 void spectral_matrix_isr_error_handler( int statusReg )
224 224 {
225 225 // STATUS REGISTER
226 226 // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
227 227 // 10 9 8
228 228 // buffer_full ** [bad_component_err] ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
229 229 // 7 6 5 4 3 2 1 0
230 230 // [bad_component_err] not defined in the last version of the VHDL code
231 231
232 232 rtems_status_code status_code;
233 233
234 234 //***************************************************
235 235 // the ASM status register is copied in the HK packet
236 236 housekeeping_packet.hk_lfr_vhdl_aa_sm = (unsigned char) (statusReg & 0x780 >> 7); // [0111 1000 0000]
237 237
238 238 if (statusReg & 0x7c0) // [0111 1100 0000]
239 239 {
240 240 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_8 );
241 241 }
242 242
243 243 spectral_matrix_regs->status = spectral_matrix_regs->status & 0x7c0;
244 244
245 245 }
246 246
247 247 rtems_isr spectral_matrices_isr( rtems_vector_number vector )
248 248 {
249 249 // STATUS REGISTER
250 250 // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
251 251 // 10 9 8
252 252 // buffer_full ** bad_component_err ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
253 253 // 7 6 5 4 3 2 1 0
254 254
255 255 int statusReg;
256 256
257 257 static restartState state = WAIT_FOR_F2;
258 258
259 259 statusReg = spectral_matrix_regs->status;
260 260
261 261 if (thisIsAnASMRestart == 0)
262 262 { // this is not a restart sequence, process incoming matrices normally
263 263 spectral_matrices_isr_f0( statusReg );
264 264
265 265 spectral_matrices_isr_f1( statusReg );
266 266
267 267 spectral_matrices_isr_f2( statusReg );
268 268 }
269 269 else
270 270 { // a restart sequence has to be launched
271 271 switch (state) {
272 272 case WAIT_FOR_F2:
273 273 if ((statusReg & 0x30) != 0x00) // [0011 0000] check the status_ready_matrix_f2_x bits
274 274 {
275 275 state = WAIT_FOR_F1;
276 276 }
277 277 break;
278 278 case WAIT_FOR_F1:
279 279 if ((statusReg & 0x0c) != 0x00) // [0000 1100] check the status_ready_matrix_f1_x bits
280 280 {
281 281 state = WAIT_FOR_F0;
282 282 }
283 283 break;
284 284 case WAIT_FOR_F0:
285 285 if ((statusReg & 0x03) != 0x00) // [0000 0011] check the status_ready_matrix_f0_x bits
286 286 {
287 287 state = WAIT_FOR_F2;
288 288 thisIsAnASMRestart = 0;
289 289 }
290 290 break;
291 291 default:
292 292 break;
293 293 }
294 294 reset_sm_status();
295 295 }
296 296
297 297 spectral_matrix_isr_error_handler( statusReg );
298 298
299 299 }
300 300
301 301 //******************
302 302 // Spectral Matrices
303 303
304 304 void reset_nb_sm( void )
305 305 {
306 306 nb_sm_f0 = 0;
307 307 nb_sm_f0_aux_f1 = 0;
308 308 nb_sm_f0_aux_f2 = 0;
309 309
310 310 nb_sm_f1 = 0;
311 311 }
312 312
313 313 void SM_init_rings( void )
314 314 {
315 315 init_ring( sm_ring_f0, NB_RING_NODES_SM_F0, sm_f0, TOTAL_SIZE_SM );
316 316 init_ring( sm_ring_f1, NB_RING_NODES_SM_F1, sm_f1, TOTAL_SIZE_SM );
317 317 init_ring( sm_ring_f2, NB_RING_NODES_SM_F2, sm_f2, TOTAL_SIZE_SM );
318 318
319 319 DEBUG_PRINTF1("sm_ring_f0 @%x\n", (unsigned int) sm_ring_f0)
320 320 DEBUG_PRINTF1("sm_ring_f1 @%x\n", (unsigned int) sm_ring_f1)
321 321 DEBUG_PRINTF1("sm_ring_f2 @%x\n", (unsigned int) sm_ring_f2)
322 322 DEBUG_PRINTF1("sm_f0 @%x\n", (unsigned int) sm_f0)
323 323 DEBUG_PRINTF1("sm_f1 @%x\n", (unsigned int) sm_f1)
324 324 DEBUG_PRINTF1("sm_f2 @%x\n", (unsigned int) sm_f2)
325 325 }
326 326
327 327 void ASM_generic_init_ring( ring_node_asm *ring, unsigned char nbNodes )
328 328 {
329 329 unsigned char i;
330 330
331 331 ring[ nbNodes - 1 ].next
332 332 = (ring_node_asm*) &ring[ 0 ];
333 333
334 334 for(i=0; i<nbNodes-1; i++)
335 335 {
336 336 ring[ i ].next = (ring_node_asm*) &ring[ i + 1 ];
337 337 }
338 338 }
339 339
340 340 void SM_reset_current_ring_nodes( void )
341 341 {
342 342 current_ring_node_sm_f0 = sm_ring_f0[0].next;
343 343 current_ring_node_sm_f1 = sm_ring_f1[0].next;
344 344 current_ring_node_sm_f2 = sm_ring_f2[0].next;
345 345
346 346 ring_node_for_averaging_sm_f0 = NULL;
347 347 ring_node_for_averaging_sm_f1 = NULL;
348 348 ring_node_for_averaging_sm_f2 = NULL;
349 349 }
350 350
351 351 //*****************
352 352 // Basic Parameters
353 353
354 354 void BP_init_header( bp_packet *packet,
355 355 unsigned int apid, unsigned char sid,
356 356 unsigned int packetLength, unsigned char blkNr )
357 357 {
358 358 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
359 359 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
360 360 packet->reserved = 0x00;
361 361 packet->userApplication = CCSDS_USER_APP;
362 362 packet->packetID[0] = (unsigned char) (apid >> 8);
363 363 packet->packetID[1] = (unsigned char) (apid);
364 364 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
365 365 packet->packetSequenceControl[1] = 0x00;
366 366 packet->packetLength[0] = (unsigned char) (packetLength >> 8);
367 367 packet->packetLength[1] = (unsigned char) (packetLength);
368 368 // DATA FIELD HEADER
369 369 packet->spare1_pusVersion_spare2 = 0x10;
370 370 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
371 371 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
372 372 packet->destinationID = TM_DESTINATION_ID_GROUND;
373 373 packet->time[0] = 0x00;
374 374 packet->time[1] = 0x00;
375 375 packet->time[2] = 0x00;
376 376 packet->time[3] = 0x00;
377 377 packet->time[4] = 0x00;
378 378 packet->time[5] = 0x00;
379 379 // AUXILIARY DATA HEADER
380 380 packet->sid = sid;
381 packet->biaStatusInfo = 0x00;
381 packet->pa_bia_status_info = 0x00;
382 382 packet->sy_lfr_common_parameters_spare = 0x00;
383 383 packet->sy_lfr_common_parameters = 0x00;
384 384 packet->acquisitionTime[0] = 0x00;
385 385 packet->acquisitionTime[1] = 0x00;
386 386 packet->acquisitionTime[2] = 0x00;
387 387 packet->acquisitionTime[3] = 0x00;
388 388 packet->acquisitionTime[4] = 0x00;
389 389 packet->acquisitionTime[5] = 0x00;
390 390 packet->pa_lfr_bp_blk_nr[0] = 0x00; // BLK_NR MSB
391 391 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
392 392 }
393 393
394 394 void BP_init_header_with_spare( bp_packet_with_spare *packet,
395 395 unsigned int apid, unsigned char sid,
396 396 unsigned int packetLength , unsigned char blkNr)
397 397 {
398 398 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
399 399 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
400 400 packet->reserved = 0x00;
401 401 packet->userApplication = CCSDS_USER_APP;
402 402 packet->packetID[0] = (unsigned char) (apid >> 8);
403 403 packet->packetID[1] = (unsigned char) (apid);
404 404 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
405 405 packet->packetSequenceControl[1] = 0x00;
406 406 packet->packetLength[0] = (unsigned char) (packetLength >> 8);
407 407 packet->packetLength[1] = (unsigned char) (packetLength);
408 408 // DATA FIELD HEADER
409 409 packet->spare1_pusVersion_spare2 = 0x10;
410 410 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
411 411 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
412 412 packet->destinationID = TM_DESTINATION_ID_GROUND;
413 413 // AUXILIARY DATA HEADER
414 414 packet->sid = sid;
415 packet->biaStatusInfo = 0x00;
415 packet->pa_bia_status_info = 0x00;
416 416 packet->sy_lfr_common_parameters_spare = 0x00;
417 417 packet->sy_lfr_common_parameters = 0x00;
418 418 packet->time[0] = 0x00;
419 419 packet->time[0] = 0x00;
420 420 packet->time[0] = 0x00;
421 421 packet->time[0] = 0x00;
422 422 packet->time[0] = 0x00;
423 423 packet->time[0] = 0x00;
424 424 packet->source_data_spare = 0x00;
425 425 packet->pa_lfr_bp_blk_nr[0] = 0x00; // BLK_NR MSB
426 426 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
427 427 }
428 428
429 429 void BP_send(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
430 430 {
431 431 rtems_status_code status;
432 432
433 433 // SEND PACKET
434 434 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
435 435 if (status != RTEMS_SUCCESSFUL)
436 436 {
437 437 PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
438 438 }
439 439 }
440 440
441 441 void BP_send_s1_s2(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
442 442 {
443 443 /** This function is used to send the BP paquets when needed.
444 444 *
445 445 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
446 446 *
447 447 * @return void
448 448 *
449 449 * SBM1 and SBM2 paquets are sent depending on the type of the LFR mode transition.
450 450 * BURST paquets are sent everytime.
451 451 *
452 452 */
453 453
454 454 rtems_status_code status;
455 455
456 456 // SEND PACKET
457 457 // before lastValidTransitionDate, the data are drops even if they are ready
458 458 // this guarantees that no SBM packets will be received before the requested enter mode time
459 459 if ( time_management_regs->coarse_time >= lastValidEnterModeTime)
460 460 {
461 461 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
462 462 if (status != RTEMS_SUCCESSFUL)
463 463 {
464 464 PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
465 465 }
466 466 }
467 467 }
468 468
469 469 //******************
470 470 // general functions
471 471
472 472 void reset_sm_status( void )
473 473 {
474 474 // error
475 475 // 10 --------------- 9 ---------------- 8 ---------------- 7 ---------
476 476 // input_fif0_write_2 input_fifo_write_1 input_fifo_write_0 buffer_full
477 477 // ---------- 5 -- 4 -- 3 -- 2 -- 1 -- 0 --
478 478 // ready bits f2_1 f2_0 f1_1 f1_1 f0_1 f0_0
479 479
480 480 spectral_matrix_regs->status = 0x7ff; // [0111 1111 1111]
481 481 }
482 482
483 483 void reset_spectral_matrix_regs( void )
484 484 {
485 485 /** This function resets the spectral matrices module registers.
486 486 *
487 487 * The registers affected by this function are located at the following offset addresses:
488 488 *
489 489 * - 0x00 config
490 490 * - 0x04 status
491 491 * - 0x08 matrixF0_Address0
492 492 * - 0x10 matrixFO_Address1
493 493 * - 0x14 matrixF1_Address
494 494 * - 0x18 matrixF2_Address
495 495 *
496 496 */
497 497
498 498 set_sm_irq_onError( 0 );
499 499
500 500 set_sm_irq_onNewMatrix( 0 );
501 501
502 502 reset_sm_status();
503 503
504 504 // F1
505 505 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->previous->buffer_address;
506 506 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
507 507 // F2
508 508 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->previous->buffer_address;
509 509 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
510 510 // F3
511 511 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->previous->buffer_address;
512 512 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
513 513
514 514 spectral_matrix_regs->matrix_length = 0xc8; // 25 * 128 / 16 = 200 = 0xc8
515 515 }
516 516
517 517 void set_time( unsigned char *time, unsigned char * timeInBuffer )
518 518 {
519 519 time[0] = timeInBuffer[0];
520 520 time[1] = timeInBuffer[1];
521 521 time[2] = timeInBuffer[2];
522 522 time[3] = timeInBuffer[3];
523 523 time[4] = timeInBuffer[6];
524 524 time[5] = timeInBuffer[7];
525 525 }
526 526
527 527 unsigned long long int get_acquisition_time( unsigned char *timePtr )
528 528 {
529 529 unsigned long long int acquisitionTimeAslong;
530 530 acquisitionTimeAslong = 0x00;
531 531 acquisitionTimeAslong = ( (unsigned long long int) (timePtr[0] & 0x7f) << 40 ) // [0111 1111] mask the synchronization bit
532 532 + ( (unsigned long long int) timePtr[1] << 32 )
533 533 + ( (unsigned long long int) timePtr[2] << 24 )
534 534 + ( (unsigned long long int) timePtr[3] << 16 )
535 535 + ( (unsigned long long int) timePtr[6] << 8 )
536 536 + ( (unsigned long long int) timePtr[7] );
537 537 return acquisitionTimeAslong;
538 538 }
539 539
540 540 unsigned char getSID( rtems_event_set event )
541 541 {
542 542 unsigned char sid;
543 543
544 544 rtems_event_set eventSetBURST;
545 545 rtems_event_set eventSetSBM;
546 546
547 547 //******
548 548 // BURST
549 549 eventSetBURST = RTEMS_EVENT_BURST_BP1_F0
550 550 | RTEMS_EVENT_BURST_BP1_F1
551 551 | RTEMS_EVENT_BURST_BP2_F0
552 552 | RTEMS_EVENT_BURST_BP2_F1;
553 553
554 554 //****
555 555 // SBM
556 556 eventSetSBM = RTEMS_EVENT_SBM_BP1_F0
557 557 | RTEMS_EVENT_SBM_BP1_F1
558 558 | RTEMS_EVENT_SBM_BP2_F0
559 559 | RTEMS_EVENT_SBM_BP2_F1;
560 560
561 561 if (event & eventSetBURST)
562 562 {
563 563 sid = SID_BURST_BP1_F0;
564 564 }
565 565 else if (event & eventSetSBM)
566 566 {
567 567 sid = SID_SBM1_BP1_F0;
568 568 }
569 569 else
570 570 {
571 571 sid = 0;
572 572 }
573 573
574 574 return sid;
575 575 }
576 576
577 577 void extractReImVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
578 578 {
579 579 unsigned int i;
580 580 float re;
581 581 float im;
582 582
583 583 for (i=0; i<NB_BINS_PER_SM; i++){
584 584 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i * 2 ];
585 585 im = inputASM[ (asmComponent*NB_BINS_PER_SM) + i * 2 + 1];
586 586 outputASM[ (asmComponent *NB_BINS_PER_SM) + i] = re;
587 587 outputASM[ (asmComponent+1)*NB_BINS_PER_SM + i] = im;
588 588 }
589 589 }
590 590
591 591 void copyReVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
592 592 {
593 593 unsigned int i;
594 594 float re;
595 595
596 596 for (i=0; i<NB_BINS_PER_SM; i++){
597 597 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i];
598 598 outputASM[ (asmComponent*NB_BINS_PER_SM) + i] = re;
599 599 }
600 600 }
601 601
602 602 void ASM_patch( float *inputASM, float *outputASM )
603 603 {
604 604 extractReImVectors( inputASM, outputASM, 1); // b1b2
605 605 extractReImVectors( inputASM, outputASM, 3 ); // b1b3
606 606 extractReImVectors( inputASM, outputASM, 5 ); // b1e1
607 607 extractReImVectors( inputASM, outputASM, 7 ); // b1e2
608 608 extractReImVectors( inputASM, outputASM, 10 ); // b2b3
609 609 extractReImVectors( inputASM, outputASM, 12 ); // b2e1
610 610 extractReImVectors( inputASM, outputASM, 14 ); // b2e2
611 611 extractReImVectors( inputASM, outputASM, 17 ); // b3e1
612 612 extractReImVectors( inputASM, outputASM, 19 ); // b3e2
613 613 extractReImVectors( inputASM, outputASM, 22 ); // e1e2
614 614
615 615 copyReVectors(inputASM, outputASM, 0 ); // b1b1
616 616 copyReVectors(inputASM, outputASM, 9 ); // b2b2
617 617 copyReVectors(inputASM, outputASM, 16); // b3b3
618 618 copyReVectors(inputASM, outputASM, 21); // e1e1
619 619 copyReVectors(inputASM, outputASM, 24); // e2e2
620 620 }
621 621
622 622 void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
623 623 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage,
624 624 unsigned char ASMIndexStart,
625 625 unsigned char channel )
626 626 {
627 627 //*************
628 628 // input format
629 629 // component0[0 .. 127] component1[0 .. 127] .. component24[0 .. 127]
630 630 //**************
631 631 // output format
632 632 // matr0[0 .. 24] matr1[0 .. 24] .. matr127[0 .. 24]
633 633 //************
634 634 // compression
635 635 // matr0[0 .. 24] matr1[0 .. 24] .. matr11[0 .. 24] => f0 NORM
636 636 // matr0[0 .. 24] matr1[0 .. 24] .. matr22[0 .. 24] => f0 BURST, SBM
637 637
638 638 int frequencyBin;
639 639 int asmComponent;
640 640 int offsetASM;
641 641 int offsetCompressed;
642 642 int offsetFBin;
643 643 int fBinMask;
644 644 int k;
645 645
646 646 // BUILD DATA
647 647 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
648 648 {
649 649 for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
650 650 {
651 651 offsetCompressed = // NO TIME OFFSET
652 652 frequencyBin * NB_VALUES_PER_SM
653 653 + asmComponent;
654 654 offsetASM = // NO TIME OFFSET
655 655 asmComponent * NB_BINS_PER_SM
656 656 + ASMIndexStart
657 657 + frequencyBin * nbBinsToAverage;
658 658 offsetFBin = ASMIndexStart
659 659 + frequencyBin * nbBinsToAverage;
660 660 compressed_spec_mat[ offsetCompressed ] = 0;
661 661 for ( k = 0; k < nbBinsToAverage; k++ )
662 662 {
663 663 fBinMask = getFBinMask( offsetFBin + k, channel );
664 664 compressed_spec_mat[offsetCompressed ] =
665 665 ( compressed_spec_mat[ offsetCompressed ]
666 666 + averaged_spec_mat[ offsetASM + k ] * fBinMask );
667 667 }
668 668 compressed_spec_mat[ offsetCompressed ] =
669 669 compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
670 670 }
671 671 }
672 672
673 673 }
674 674
675 675 int getFBinMask( int index, unsigned char channel )
676 676 {
677 677 unsigned int indexInChar;
678 678 unsigned int indexInTheChar;
679 679 int fbin;
680 680 unsigned char *sy_lfr_fbins_fx_word1;
681 681
682 682 sy_lfr_fbins_fx_word1 = parameter_dump_packet.sy_lfr_fbins_f0_word1;
683 683
684 684 switch(channel)
685 685 {
686 686 case 0:
687 687 sy_lfr_fbins_fx_word1 = parameter_dump_packet.sy_lfr_fbins_f0_word1;
688 688 break;
689 689 case 1:
690 690 sy_lfr_fbins_fx_word1 = parameter_dump_packet.sy_lfr_fbins_f1_word1;
691 691 break;
692 692 case 2:
693 693 sy_lfr_fbins_fx_word1 = parameter_dump_packet.sy_lfr_fbins_f2_word1;
694 694 break;
695 695 default:
696 696 PRINTF("ERR *** in getFBinMask, wrong frequency channel")
697 697 }
698 698
699 699 indexInChar = index >> 3;
700 700 indexInTheChar = index - indexInChar * 8;
701 701
702 702 fbin = (int) ((sy_lfr_fbins_fx_word1[ NB_BYTES_PER_FREQ_MASK - 1 - indexInChar] >> indexInTheChar) & 0x1);
703 703
704 704 return fbin;
705 705 }
706 706
707 707 void init_kcoeff_sbm_from_kcoeff_norm(float *input_kcoeff, float *output_kcoeff, unsigned char nb_bins_norm)
708 708 {
709 709 unsigned char bin;
710 710 unsigned char kcoeff;
711 711
712 712 for (bin=0; bin<nb_bins_norm; bin++)
713 713 {
714 714 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
715 715 {
716 716 output_kcoeff[ (bin*NB_K_COEFF_PER_BIN + kcoeff)*2 ] = input_kcoeff[ bin*NB_K_COEFF_PER_BIN + kcoeff ];
717 717 output_kcoeff[ (bin*NB_K_COEFF_PER_BIN + kcoeff)*2 + 1 ] = input_kcoeff[ bin*NB_K_COEFF_PER_BIN + kcoeff ];
718 718 }
719 719 }
720 720 }
@@ -1,474 +1,474
1 1 /** Functions related to TeleCommand acceptance.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle TeleCommands parsing.\n
7 7 *
8 8 */
9 9
10 10 #include "tc_acceptance.h"
11 11 #include <stdio.h>
12 12
13 13 unsigned int lookUpTableForCRC[256];
14 14
15 15 //**********************
16 16 // GENERAL USE FUNCTIONS
17 17 unsigned int Crc_opt( unsigned char D, unsigned int Chk)
18 18 {
19 19 /** This function generate the CRC for one byte and returns the value of the new syndrome.
20 20 *
21 21 * @param D is the current byte of data.
22 22 * @param Chk is the current syndrom value.
23 23 *
24 24 * @return the value of the new syndrome on two bytes.
25 25 *
26 26 */
27 27
28 28 return(((Chk << 8) & 0xff00)^lookUpTableForCRC [(((Chk >> 8)^D) & 0x00ff)]);
29 29 }
30 30
31 31 void initLookUpTableForCRC( void )
32 32 {
33 33 /** This function is used to initiates the look-up table for fast CRC computation.
34 34 *
35 35 * The global table lookUpTableForCRC[256] is initiated.
36 36 *
37 37 */
38 38
39 39 unsigned int i;
40 40 unsigned int tmp;
41 41
42 42 for (i=0; i<256; i++)
43 43 {
44 44 tmp = 0;
45 45 if((i & 1) != 0) {
46 46 tmp = tmp ^ 0x1021;
47 47 }
48 48 if((i & 2) != 0) {
49 49 tmp = tmp ^ 0x2042;
50 50 }
51 51 if((i & 4) != 0) {
52 52 tmp = tmp ^ 0x4084;
53 53 }
54 54 if((i & 8) != 0) {
55 55 tmp = tmp ^ 0x8108;
56 56 }
57 57 if((i & 16) != 0) {
58 58 tmp = tmp ^ 0x1231;
59 59 }
60 60 if((i & 32) != 0) {
61 61 tmp = tmp ^ 0x2462;
62 62 }
63 63 if((i & 64) != 0) {
64 64 tmp = tmp ^ 0x48c4;
65 65 }
66 66 if((i & 128) != 0) {
67 67 tmp = tmp ^ 0x9188;
68 68 }
69 69 lookUpTableForCRC[i] = tmp;
70 70 }
71 71 }
72 72
73 73 void GetCRCAsTwoBytes(unsigned char* data, unsigned char* crcAsTwoBytes, unsigned int sizeOfData)
74 74 {
75 75 /** This function calculates a two bytes Cyclic Redundancy Code.
76 76 *
77 77 * @param data points to a buffer containing the data on which to compute the CRC.
78 78 * @param crcAsTwoBytes points points to a two bytes buffer in which the CRC is stored.
79 79 * @param sizeOfData is the number of bytes of *data* used to compute the CRC.
80 80 *
81 81 * The specification of the Cyclic Redundancy Code is described in the following document: ECSS-E-70-41-A.
82 82 *
83 83 */
84 84
85 85 unsigned int Chk;
86 86 int j;
87 87 Chk = 0xffff; // reset the syndrom to all ones
88 88 for (j=0; j<sizeOfData; j++) {
89 89 Chk = Crc_opt(data[j], Chk);
90 90 }
91 91 crcAsTwoBytes[0] = (unsigned char) (Chk >> 8);
92 92 crcAsTwoBytes[1] = (unsigned char) (Chk & 0x00ff);
93 93 }
94 94
95 95 //*********************
96 96 // ACCEPTANCE FUNCTIONS
97 97 int tc_parser(ccsdsTelecommandPacket_t * TCPacket, unsigned int estimatedPacketLength, unsigned char *computed_CRC)
98 98 {
99 99 /** This function parses TeleCommands.
100 100 *
101 101 * @param TC points to the TeleCommand that will be parsed.
102 102 * @param estimatedPacketLength is the PACKET_LENGTH field calculated from the effective length of the received packet.
103 103 *
104 104 * @return Status code of the parsing.
105 105 *
106 106 * The parsing checks:
107 107 * - process id
108 108 * - category
109 109 * - length: a global check is performed and a per subtype check also
110 110 * - type
111 111 * - subtype
112 112 * - crc
113 113 *
114 114 */
115 115
116 116 int status;
117 117 int status_crc;
118 118 unsigned char pid;
119 119 unsigned char category;
120 120 unsigned int packetLength;
121 121 unsigned char packetType;
122 122 unsigned char packetSubtype;
123 123 unsigned char sid;
124 124
125 125 status = CCSDS_TM_VALID;
126 126
127 127 // APID check *** APID on 2 bytes
128 128 pid = ((TCPacket->packetID[0] & 0x07)<<4) + ( (TCPacket->packetID[1]>>4) & 0x0f ); // PID = 11 *** 7 bits xxxxx210 7654xxxx
129 129 category = (TCPacket->packetID[1] & 0x0f); // PACKET_CATEGORY = 12 *** 4 bits xxxxxxxx xxxx3210
130 130 packetLength = (TCPacket->packetLength[0] * 256) + TCPacket->packetLength[1];
131 131 packetType = TCPacket->serviceType;
132 132 packetSubtype = TCPacket->serviceSubType;
133 133 sid = TCPacket->sourceID;
134 134
135 135 if ( pid != CCSDS_PROCESS_ID ) // CHECK THE PROCESS ID
136 136 {
137 137 status = ILLEGAL_APID;
138 138 }
139 139 if (status == CCSDS_TM_VALID) // CHECK THE CATEGORY
140 140 {
141 141 if ( category != CCSDS_PACKET_CATEGORY )
142 142 {
143 143 status = ILLEGAL_APID;
144 144 }
145 145 }
146 146 if (status == CCSDS_TM_VALID) // CHECK THE PACKET_LENGTH FIELD AND THE ESTIMATED PACKET_LENGTH COMPLIANCE
147 147 {
148 148 if (packetLength != estimatedPacketLength ) {
149 149 status = WRONG_LEN_PKT;
150 150 }
151 151 }
152 152 if (status == CCSDS_TM_VALID) // CHECK THAT THE PACKET DOES NOT EXCEED THE MAX SIZE
153 153 {
154 154 if ( packetLength >= CCSDS_TC_PKT_MAX_SIZE ) {
155 155 status = WRONG_LEN_PKT;
156 156 }
157 157 }
158 158 if (status == CCSDS_TM_VALID) // CHECK THE TYPE
159 159 {
160 160 status = tc_check_type( packetType );
161 161 }
162 162 if (status == CCSDS_TM_VALID) // CHECK THE SUBTYPE
163 163 {
164 164 status = tc_check_type_subtype( packetType, packetSubtype );
165 165 }
166 166 if (status == CCSDS_TM_VALID) // CHECK THE SID
167 167 {
168 168 status = tc_check_sid( sid );
169 169 }
170 170 if (status == CCSDS_TM_VALID) // CHECK THE SUBTYPE AND LENGTH COMPLIANCE
171 171 {
172 172 status = tc_check_length( packetSubtype, packetLength );
173 173 }
174 174 status_crc = tc_check_crc( TCPacket, estimatedPacketLength, computed_CRC );
175 175 if (status == CCSDS_TM_VALID ) // CHECK CRC
176 176 {
177 177 status = status_crc;
178 178 }
179 179
180 180 return status;
181 181 }
182 182
183 183 int tc_check_type( unsigned char packetType )
184 184 {
185 185 /** This function checks that the type of a TeleCommand is valid.
186 186 *
187 187 * @param packetType is the type to check.
188 188 *
189 189 * @return Status code CCSDS_TM_VALID or ILL_TYPE.
190 190 *
191 191 */
192 192
193 193 int status;
194 194
195 195 if ( (packetType == TC_TYPE_GEN) || (packetType == TC_TYPE_TIME))
196 196 {
197 197 status = CCSDS_TM_VALID;
198 198 }
199 199 else
200 200 {
201 201 status = ILL_TYPE;
202 202 }
203 203
204 204 return status;
205 205 }
206 206
207 207 int tc_check_type_subtype( unsigned char packetType, unsigned char packetSubType )
208 208 {
209 209 /** This function checks that the subtype of a TeleCommand is valid and coherent with the type.
210 210 *
211 211 * @param packetType is the type of the TC.
212 212 * @param packetSubType is the subtype to check.
213 213 *
214 214 * @return Status code CCSDS_TM_VALID or ILL_SUBTYPE.
215 215 *
216 216 */
217 217
218 218 int status;
219 219
220 220 switch(packetType)
221 221 {
222 222 case TC_TYPE_GEN:
223 223 if ( (packetSubType == TC_SUBTYPE_RESET)
224 224 || (packetSubType == TC_SUBTYPE_LOAD_COMM)
225 225 || (packetSubType == TC_SUBTYPE_LOAD_NORM) || (packetSubType == TC_SUBTYPE_LOAD_BURST)
226 226 || (packetSubType == TC_SUBTYPE_LOAD_SBM1) || (packetSubType == TC_SUBTYPE_LOAD_SBM2)
227 227 || (packetSubType == TC_SUBTYPE_DUMP)
228 228 || (packetSubType == TC_SUBTYPE_ENTER)
229 229 || (packetSubType == TC_SUBTYPE_UPDT_INFO)
230 230 || (packetSubType == TC_SUBTYPE_EN_CAL) || (packetSubType == TC_SUBTYPE_DIS_CAL)
231 231 || (packetSubType == TC_SUBTYPE_LOAD_K) || (packetSubType == TC_SUBTYPE_DUMP_K)
232 232 || (packetSubType == TC_SUBTYPE_LOAD_FBINS)
233 || (packetSubType == TC_SUBTYPE_LOAD_PAS_FILTER_PAR))
233 || (packetSubType == TC_SUBTYPE_LOAD_FILTER_PAR))
234 234 {
235 235 status = CCSDS_TM_VALID;
236 236 }
237 237 else
238 238 {
239 239 status = ILL_SUBTYPE;
240 240 }
241 241 break;
242 242
243 243 case TC_TYPE_TIME:
244 244 if (packetSubType == TC_SUBTYPE_UPDT_TIME)
245 245 {
246 246 status = CCSDS_TM_VALID;
247 247 }
248 248 else
249 249 {
250 250 status = ILL_SUBTYPE;
251 251 }
252 252 break;
253 253
254 254 default:
255 255 status = ILL_SUBTYPE;
256 256 break;
257 257 }
258 258
259 259 return status;
260 260 }
261 261
262 262 int tc_check_sid( unsigned char sid )
263 263 {
264 264 /** This function checks that the sid of a TeleCommand is valid.
265 265 *
266 266 * @param sid is the sid to check.
267 267 *
268 268 * @return Status code CCSDS_TM_VALID or CORRUPTED.
269 269 *
270 270 */
271 271
272 272 int status;
273 273
274 274 if ( (sid == SID_TC_MISSION_TIMELINE) || (sid == SID_TC_TC_SEQUENCES) || (sid == SID_TC_RECOVERY_ACTION_CMD)
275 275 || (sid == SID_TC_BACKUP_MISSION_TIMELINE)
276 276 || (sid == SID_TC_DIRECT_CMD) || (sid == SID_TC_SPARE_GRD_SRC1) || (sid == SID_TC_SPARE_GRD_SRC2)
277 277 || (sid == SID_TC_OBCP) || (sid == SID_TC_SYSTEM_CONTROL) || (sid == SID_TC_AOCS)
278 278 || (sid == SID_TC_RPW_INTERNAL))
279 279 {
280 280 status = CCSDS_TM_VALID;
281 281 }
282 282 else
283 283 {
284 284 status = WRONG_SRC_ID;
285 285 }
286 286
287 287 return status;
288 288 }
289 289
290 290 int tc_check_length( unsigned char packetSubType, unsigned int length )
291 291 {
292 292 /** This function checks that the subtype and the length are compliant.
293 293 *
294 294 * @param packetSubType is the subtype to check.
295 295 * @param length is the length to check.
296 296 *
297 297 * @return Status code CCSDS_TM_VALID or ILL_TYPE.
298 298 *
299 299 */
300 300
301 301 int status;
302 302
303 303 status = LFR_SUCCESSFUL;
304 304
305 305 switch(packetSubType)
306 306 {
307 307 case TC_SUBTYPE_RESET:
308 308 if (length!=(TC_LEN_RESET-CCSDS_TC_TM_PACKET_OFFSET)) {
309 309 status = WRONG_LEN_PKT;
310 310 }
311 311 else {
312 312 status = CCSDS_TM_VALID;
313 313 }
314 314 break;
315 315 case TC_SUBTYPE_LOAD_COMM:
316 316 if (length!=(TC_LEN_LOAD_COMM-CCSDS_TC_TM_PACKET_OFFSET)) {
317 317 status = WRONG_LEN_PKT;
318 318 }
319 319 else {
320 320 status = CCSDS_TM_VALID;
321 321 }
322 322 break;
323 323 case TC_SUBTYPE_LOAD_NORM:
324 324 if (length!=(TC_LEN_LOAD_NORM-CCSDS_TC_TM_PACKET_OFFSET)) {
325 325 status = WRONG_LEN_PKT;
326 326 }
327 327 else {
328 328 status = CCSDS_TM_VALID;
329 329 }
330 330 break;
331 331 case TC_SUBTYPE_LOAD_BURST:
332 332 if (length!=(TC_LEN_LOAD_BURST-CCSDS_TC_TM_PACKET_OFFSET)) {
333 333 status = WRONG_LEN_PKT;
334 334 }
335 335 else {
336 336 status = CCSDS_TM_VALID;
337 337 }
338 338 break;
339 339 case TC_SUBTYPE_LOAD_SBM1:
340 340 if (length!=(TC_LEN_LOAD_SBM1-CCSDS_TC_TM_PACKET_OFFSET)) {
341 341 status = WRONG_LEN_PKT;
342 342 }
343 343 else {
344 344 status = CCSDS_TM_VALID;
345 345 }
346 346 break;
347 347 case TC_SUBTYPE_LOAD_SBM2:
348 348 if (length!=(TC_LEN_LOAD_SBM2-CCSDS_TC_TM_PACKET_OFFSET)) {
349 349 status = WRONG_LEN_PKT;
350 350 }
351 351 else {
352 352 status = CCSDS_TM_VALID;
353 353 }
354 354 break;
355 355 case TC_SUBTYPE_DUMP:
356 356 if (length!=(TC_LEN_DUMP-CCSDS_TC_TM_PACKET_OFFSET)) {
357 357 status = WRONG_LEN_PKT;
358 358 }
359 359 else {
360 360 status = CCSDS_TM_VALID;
361 361 }
362 362 break;
363 363 case TC_SUBTYPE_ENTER:
364 364 if (length!=(TC_LEN_ENTER-CCSDS_TC_TM_PACKET_OFFSET)) {
365 365 status = WRONG_LEN_PKT;
366 366 }
367 367 else {
368 368 status = CCSDS_TM_VALID;
369 369 }
370 370 break;
371 371 case TC_SUBTYPE_UPDT_INFO:
372 372 if (length!=(TC_LEN_UPDT_INFO-CCSDS_TC_TM_PACKET_OFFSET)) {
373 373 status = WRONG_LEN_PKT;
374 374 }
375 375 else {
376 376 status = CCSDS_TM_VALID;
377 377 }
378 378 break;
379 379 case TC_SUBTYPE_EN_CAL:
380 380 if (length!=(TC_LEN_EN_CAL-CCSDS_TC_TM_PACKET_OFFSET)) {
381 381 status = WRONG_LEN_PKT;
382 382 }
383 383 else {
384 384 status = CCSDS_TM_VALID;
385 385 }
386 386 break;
387 387 case TC_SUBTYPE_DIS_CAL:
388 388 if (length!=(TC_LEN_DIS_CAL-CCSDS_TC_TM_PACKET_OFFSET)) {
389 389 status = WRONG_LEN_PKT;
390 390 }
391 391 else {
392 392 status = CCSDS_TM_VALID;
393 393 }
394 394 break;
395 395 case TC_SUBTYPE_LOAD_K:
396 396 if (length!=(TC_LEN_LOAD_K-CCSDS_TC_TM_PACKET_OFFSET)) {
397 397 status = WRONG_LEN_PKT;
398 398 }
399 399 else {
400 400 status = CCSDS_TM_VALID;
401 401 }
402 402 break;
403 403 case TC_SUBTYPE_DUMP_K:
404 404 if (length!=(TC_LEN_DUMP_K-CCSDS_TC_TM_PACKET_OFFSET)) {
405 405 status = WRONG_LEN_PKT;
406 406 }
407 407 else {
408 408 status = CCSDS_TM_VALID;
409 409 }
410 410 break;
411 411 case TC_SUBTYPE_LOAD_FBINS:
412 412 if (length!=(TC_LEN_LOAD_FBINS-CCSDS_TC_TM_PACKET_OFFSET)) {
413 413 status = WRONG_LEN_PKT;
414 414 }
415 415 else {
416 416 status = CCSDS_TM_VALID;
417 417 }
418 418 break;
419 case TC_SUBTYPE_LOAD_PAS_FILTER_PAR:
420 if (length!=(TC_LEN_LOAD_PAS_FILTER_PAR-CCSDS_TC_TM_PACKET_OFFSET)) {
419 case TC_SUBTYPE_LOAD_FILTER_PAR:
420 if (length!=(TC_LEN_LOAD_FILTER_PAR-CCSDS_TC_TM_PACKET_OFFSET)) {
421 421 status = WRONG_LEN_PKT;
422 422 }
423 423 else {
424 424 status = CCSDS_TM_VALID;
425 425 }
426 426 break;
427 427 case TC_SUBTYPE_UPDT_TIME:
428 428 if (length!=(TC_LEN_UPDT_TIME-CCSDS_TC_TM_PACKET_OFFSET)) {
429 429 status = WRONG_LEN_PKT;
430 430 }
431 431 else {
432 432 status = CCSDS_TM_VALID;
433 433 }
434 434 break;
435 435 default: // if the subtype is not a legal value, return ILL_SUBTYPE
436 436 status = ILL_SUBTYPE;
437 437 break ;
438 438 }
439 439
440 440 return status;
441 441 }
442 442
443 443 int tc_check_crc( ccsdsTelecommandPacket_t * TCPacket, unsigned int length, unsigned char *computed_CRC )
444 444 {
445 445 /** This function checks the CRC validity of the corresponding TeleCommand packet.
446 446 *
447 447 * @param TCPacket points to the TeleCommand packet to check.
448 448 * @param length is the length of the TC packet.
449 449 *
450 450 * @return Status code CCSDS_TM_VALID or INCOR_CHECKSUM.
451 451 *
452 452 */
453 453
454 454 int status;
455 455 unsigned char * CCSDSContent;
456 456
457 457 CCSDSContent = (unsigned char*) TCPacket->packetID;
458 458 GetCRCAsTwoBytes(CCSDSContent, computed_CRC, length + CCSDS_TC_TM_PACKET_OFFSET - 2); // 2 CRC bytes removed from the calculation of the CRC
459 459
460 460 if (computed_CRC[0] != CCSDSContent[length + CCSDS_TC_TM_PACKET_OFFSET -2]) {
461 461 status = INCOR_CHECKSUM;
462 462 }
463 463 else if (computed_CRC[1] != CCSDSContent[length + CCSDS_TC_TM_PACKET_OFFSET -1]) {
464 464 status = INCOR_CHECKSUM;
465 465 }
466 466 else {
467 467 status = CCSDS_TM_VALID;
468 468 }
469 469
470 470 return status;
471 471 }
472 472
473 473
474 474
@@ -1,1636 +1,1641
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 case TC_SUBTYPE_LOAD_PAS_FILTER_PAR:
127 result = action_load_pas_filter_par( &TC, queue_snd_id, time );
126 case TC_SUBTYPE_LOAD_FILTER_PAR:
127 result = action_load_filter_par( &TC, queue_snd_id, time );
128 128 close_action( &TC, result, queue_snd_id );
129 129 break;
130 130 case TC_SUBTYPE_UPDT_TIME:
131 131 result = action_update_time( &TC );
132 132 close_action( &TC, result, queue_snd_id );
133 133 break;
134 134 default:
135 135 break;
136 136 }
137 137 }
138 138 }
139 139 }
140 140
141 141 //***********
142 142 // TC ACTIONS
143 143
144 144 int action_reset(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
145 145 {
146 146 /** This function executes specific actions when a TC_LFR_RESET TeleCommand has been received.
147 147 *
148 148 * @param TC points to the TeleCommand packet that is being processed
149 149 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
150 150 *
151 151 */
152 152
153 153 PRINTF("this is the end!!!\n");
154 154 exit(0);
155 155
156 156 send_tm_lfr_tc_exe_not_implemented( TC, queue_id, time );
157 157
158 158 return LFR_DEFAULT;
159 159 }
160 160
161 161 int action_enter_mode(ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
162 162 {
163 163 /** This function executes specific actions when a TC_LFR_ENTER_MODE TeleCommand has been received.
164 164 *
165 165 * @param TC points to the TeleCommand packet that is being processed
166 166 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
167 167 *
168 168 */
169 169
170 170 rtems_status_code status;
171 171 unsigned char requestedMode;
172 172 unsigned int *transitionCoarseTime_ptr;
173 173 unsigned int transitionCoarseTime;
174 174 unsigned char * bytePosPtr;
175 175
176 176 bytePosPtr = (unsigned char *) &TC->packetID;
177 177
178 178 requestedMode = bytePosPtr[ BYTE_POS_CP_MODE_LFR_SET ];
179 179 transitionCoarseTime_ptr = (unsigned int *) ( &bytePosPtr[ BYTE_POS_CP_LFR_ENTER_MODE_TIME ] );
180 180 transitionCoarseTime = (*transitionCoarseTime_ptr) & 0x7fffffff;
181 181
182 182 status = check_mode_value( requestedMode );
183 183
184 184 if ( status != LFR_SUCCESSFUL ) // the mode value is inconsistent
185 185 {
186 186 send_tm_lfr_tc_exe_inconsistent( TC, queue_id, BYTE_POS_CP_MODE_LFR_SET, requestedMode );
187 187 }
188 188
189 189 else // the mode value is valid, check the transition
190 190 {
191 191 status = check_mode_transition(requestedMode);
192 192 if (status != LFR_SUCCESSFUL)
193 193 {
194 194 PRINTF("ERR *** in action_enter_mode *** check_mode_transition\n")
195 195 send_tm_lfr_tc_exe_not_executable( TC, queue_id );
196 196 }
197 197 }
198 198
199 199 if ( status == LFR_SUCCESSFUL ) // the transition is valid, check the date
200 200 {
201 201 status = check_transition_date( transitionCoarseTime );
202 202 if (status != LFR_SUCCESSFUL)
203 203 {
204 204 PRINTF("ERR *** in action_enter_mode *** check_transition_date\n");
205 205 send_tm_lfr_tc_exe_not_executable(TC, queue_id );
206 206 }
207 207 }
208 208
209 209 if ( status == LFR_SUCCESSFUL ) // the date is valid, enter the mode
210 210 {
211 211 PRINTF1("OK *** in action_enter_mode *** enter mode %d\n", requestedMode);
212 212
213 213 switch(requestedMode)
214 214 {
215 215 case LFR_MODE_STANDBY:
216 216 status = enter_mode_standby();
217 217 break;
218 218 case LFR_MODE_NORMAL:
219 219 status = enter_mode_normal( transitionCoarseTime );
220 220 break;
221 221 case LFR_MODE_BURST:
222 222 status = enter_mode_burst( transitionCoarseTime );
223 223 break;
224 224 case LFR_MODE_SBM1:
225 225 status = enter_mode_sbm1( transitionCoarseTime );
226 226 break;
227 227 case LFR_MODE_SBM2:
228 228 status = enter_mode_sbm2( transitionCoarseTime );
229 229 break;
230 230 default:
231 231 break;
232 232 }
233 233
234 234 if (status != RTEMS_SUCCESSFUL)
235 235 {
236 236 status = LFR_EXE_ERROR;
237 237 }
238 238 }
239 239
240 240 return status;
241 241 }
242 242
243 243 int action_update_info(ccsdsTelecommandPacket_t *TC, rtems_id queue_id)
244 244 {
245 245 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
246 246 *
247 247 * @param TC points to the TeleCommand packet that is being processed
248 248 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
249 249 *
250 250 * @return LFR directive status code:
251 251 * - LFR_DEFAULT
252 252 * - LFR_SUCCESSFUL
253 253 *
254 254 */
255 255
256 256 unsigned int val;
257 257 int result;
258 258 unsigned int status;
259 259 unsigned char mode;
260 260 unsigned char * bytePosPtr;
261 261
262 262 bytePosPtr = (unsigned char *) &TC->packetID;
263 263
264 264 // check LFR mode
265 265 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET5 ] & 0x1e) >> 1;
266 266 status = check_update_info_hk_lfr_mode( mode );
267 267 if (status == LFR_SUCCESSFUL) // check TDS mode
268 268 {
269 269 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET6 ] & 0xf0) >> 4;
270 270 status = check_update_info_hk_tds_mode( mode );
271 271 }
272 272 if (status == LFR_SUCCESSFUL) // check THR mode
273 273 {
274 274 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET6 ] & 0x0f);
275 275 status = check_update_info_hk_thr_mode( mode );
276 276 }
277 277 if (status == LFR_SUCCESSFUL) // if the parameter check is successful
278 278 {
279 279 val = housekeeping_packet.hk_lfr_update_info_tc_cnt[0] * 256
280 280 + housekeeping_packet.hk_lfr_update_info_tc_cnt[1];
281 281 val++;
282 282 housekeeping_packet.hk_lfr_update_info_tc_cnt[0] = (unsigned char) (val >> 8);
283 283 housekeeping_packet.hk_lfr_update_info_tc_cnt[1] = (unsigned char) (val);
284 284 }
285 285
286 286 // pa_bia_status_info
287 287 // => pa_bia_mode_mux_set 3 bits
288 288 // => pa_bia_mode_hv_enabled 1 bit
289 289 // => pa_bia_mode_bias1_enabled 1 bit
290 290 // => pa_bia_mode_bias2_enabled 1 bit
291 291 // => pa_bia_mode_bias3_enabled 1 bit
292 292 // => pa_bia_on_off (cp_dpu_bias_on_off)
293 293 pa_bia_status_info = bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET2 ] & 0xfe; // [1111 1110]
294 294 pa_bia_status_info = pa_bia_status_info
295 295 | (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET1 ] & 0x1);
296 296
297 // REACTION_WHEELS_FREQUENCY, copy the incoming parameters in the local variable (to be copied in HK packets)
298 cp_rpw_sc_rw_f_flags = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW_F_FLAGS ];
299 getReactionWheelsFrequencies( TC );
300 build_rw_fbins_masks();
301
297 302 result = status;
298 303
299 304 return result;
300 305 }
301 306
302 307 int action_enable_calibration(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
303 308 {
304 309 /** This function executes specific actions when a TC_LFR_ENABLE_CALIBRATION TeleCommand has been received.
305 310 *
306 311 * @param TC points to the TeleCommand packet that is being processed
307 312 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
308 313 *
309 314 */
310 315
311 316 int result;
312 317
313 318 result = LFR_DEFAULT;
314 319
315 320 setCalibration( true );
316 321
317 322 result = LFR_SUCCESSFUL;
318 323
319 324 return result;
320 325 }
321 326
322 327 int action_disable_calibration(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
323 328 {
324 329 /** This function executes specific actions when a TC_LFR_DISABLE_CALIBRATION TeleCommand has been received.
325 330 *
326 331 * @param TC points to the TeleCommand packet that is being processed
327 332 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
328 333 *
329 334 */
330 335
331 336 int result;
332 337
333 338 result = LFR_DEFAULT;
334 339
335 340 setCalibration( false );
336 341
337 342 result = LFR_SUCCESSFUL;
338 343
339 344 return result;
340 345 }
341 346
342 347 int action_update_time(ccsdsTelecommandPacket_t *TC)
343 348 {
344 349 /** This function executes specific actions when a TC_LFR_UPDATE_TIME TeleCommand has been received.
345 350 *
346 351 * @param TC points to the TeleCommand packet that is being processed
347 352 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
348 353 *
349 354 * @return LFR_SUCCESSFUL
350 355 *
351 356 */
352 357
353 358 unsigned int val;
354 359
355 360 time_management_regs->coarse_time_load = (TC->dataAndCRC[0] << 24)
356 361 + (TC->dataAndCRC[1] << 16)
357 362 + (TC->dataAndCRC[2] << 8)
358 363 + TC->dataAndCRC[3];
359 364
360 365 val = housekeeping_packet.hk_lfr_update_time_tc_cnt[0] * 256
361 366 + housekeeping_packet.hk_lfr_update_time_tc_cnt[1];
362 367 val++;
363 368 housekeeping_packet.hk_lfr_update_time_tc_cnt[0] = (unsigned char) (val >> 8);
364 369 housekeeping_packet.hk_lfr_update_time_tc_cnt[1] = (unsigned char) (val);
365 370
366 371 oneTcLfrUpdateTimeReceived = 1;
367 372
368 373 return LFR_SUCCESSFUL;
369 374 }
370 375
371 376 //*******************
372 377 // ENTERING THE MODES
373 378 int check_mode_value( unsigned char requestedMode )
374 379 {
375 380 int status;
376 381
377 382 if ( (requestedMode != LFR_MODE_STANDBY)
378 383 && (requestedMode != LFR_MODE_NORMAL) && (requestedMode != LFR_MODE_BURST)
379 384 && (requestedMode != LFR_MODE_SBM1) && (requestedMode != LFR_MODE_SBM2) )
380 385 {
381 386 status = LFR_DEFAULT;
382 387 }
383 388 else
384 389 {
385 390 status = LFR_SUCCESSFUL;
386 391 }
387 392
388 393 return status;
389 394 }
390 395
391 396 int check_mode_transition( unsigned char requestedMode )
392 397 {
393 398 /** This function checks the validity of the transition requested by the TC_LFR_ENTER_MODE.
394 399 *
395 400 * @param requestedMode is the mode requested by the TC_LFR_ENTER_MODE
396 401 *
397 402 * @return LFR directive status codes:
398 403 * - LFR_SUCCESSFUL - the transition is authorized
399 404 * - LFR_DEFAULT - the transition is not authorized
400 405 *
401 406 */
402 407
403 408 int status;
404 409
405 410 switch (requestedMode)
406 411 {
407 412 case LFR_MODE_STANDBY:
408 413 if ( lfrCurrentMode == LFR_MODE_STANDBY ) {
409 414 status = LFR_DEFAULT;
410 415 }
411 416 else
412 417 {
413 418 status = LFR_SUCCESSFUL;
414 419 }
415 420 break;
416 421 case LFR_MODE_NORMAL:
417 422 if ( lfrCurrentMode == LFR_MODE_NORMAL ) {
418 423 status = LFR_DEFAULT;
419 424 }
420 425 else {
421 426 status = LFR_SUCCESSFUL;
422 427 }
423 428 break;
424 429 case LFR_MODE_BURST:
425 430 if ( lfrCurrentMode == LFR_MODE_BURST ) {
426 431 status = LFR_DEFAULT;
427 432 }
428 433 else {
429 434 status = LFR_SUCCESSFUL;
430 435 }
431 436 break;
432 437 case LFR_MODE_SBM1:
433 438 if ( lfrCurrentMode == LFR_MODE_SBM1 ) {
434 439 status = LFR_DEFAULT;
435 440 }
436 441 else {
437 442 status = LFR_SUCCESSFUL;
438 443 }
439 444 break;
440 445 case LFR_MODE_SBM2:
441 446 if ( lfrCurrentMode == LFR_MODE_SBM2 ) {
442 447 status = LFR_DEFAULT;
443 448 }
444 449 else {
445 450 status = LFR_SUCCESSFUL;
446 451 }
447 452 break;
448 453 default:
449 454 status = LFR_DEFAULT;
450 455 break;
451 456 }
452 457
453 458 return status;
454 459 }
455 460
456 461 void update_last_valid_transition_date( unsigned int transitionCoarseTime )
457 462 {
458 463 if (transitionCoarseTime == 0)
459 464 {
460 465 lastValidEnterModeTime = time_management_regs->coarse_time + 1;
461 466 PRINTF1("lastValidEnterModeTime = 0x%x (transitionCoarseTime = 0 => coarse_time+1)\n", lastValidEnterModeTime);
462 467 }
463 468 else
464 469 {
465 470 lastValidEnterModeTime = transitionCoarseTime;
466 471 PRINTF1("lastValidEnterModeTime = 0x%x\n", transitionCoarseTime);
467 472 }
468 473 }
469 474
470 475 int check_transition_date( unsigned int transitionCoarseTime )
471 476 {
472 477 int status;
473 478 unsigned int localCoarseTime;
474 479 unsigned int deltaCoarseTime;
475 480
476 481 status = LFR_SUCCESSFUL;
477 482
478 483 if (transitionCoarseTime == 0) // transition time = 0 means an instant transition
479 484 {
480 485 status = LFR_SUCCESSFUL;
481 486 }
482 487 else
483 488 {
484 489 localCoarseTime = time_management_regs->coarse_time & 0x7fffffff;
485 490
486 491 PRINTF2("localTime = %x, transitionTime = %x\n", localCoarseTime, transitionCoarseTime);
487 492
488 493 if ( transitionCoarseTime <= localCoarseTime ) // SSS-CP-EQS-322
489 494 {
490 495 status = LFR_DEFAULT;
491 496 PRINTF("ERR *** in check_transition_date *** transitionCoarseTime <= localCoarseTime\n");
492 497 }
493 498
494 499 if (status == LFR_SUCCESSFUL)
495 500 {
496 501 deltaCoarseTime = transitionCoarseTime - localCoarseTime;
497 502 if ( deltaCoarseTime > 3 ) // SSS-CP-EQS-323
498 503 {
499 504 status = LFR_DEFAULT;
500 505 PRINTF1("ERR *** in check_transition_date *** deltaCoarseTime = %x\n", deltaCoarseTime)
501 506 }
502 507 }
503 508 }
504 509
505 510 return status;
506 511 }
507 512
508 513 int restart_asm_activities( unsigned char lfrRequestedMode )
509 514 {
510 515 rtems_status_code status;
511 516
512 517 status = stop_spectral_matrices();
513 518
514 519 thisIsAnASMRestart = 1;
515 520
516 521 status = restart_asm_tasks( lfrRequestedMode );
517 522
518 523 launch_spectral_matrix();
519 524
520 525 return status;
521 526 }
522 527
523 528 int stop_spectral_matrices( void )
524 529 {
525 530 /** This function stops and restarts the current mode average spectral matrices activities.
526 531 *
527 532 * @return RTEMS directive status codes:
528 533 * - RTEMS_SUCCESSFUL - task restarted successfully
529 534 * - RTEMS_INVALID_ID - task id invalid
530 535 * - RTEMS_ALREADY_SUSPENDED - task already suspended
531 536 *
532 537 */
533 538
534 539 rtems_status_code status;
535 540
536 541 status = RTEMS_SUCCESSFUL;
537 542
538 543 // (1) mask interruptions
539 544 LEON_Mask_interrupt( IRQ_SPECTRAL_MATRIX ); // mask spectral matrix interrupt
540 545
541 546 // (2) reset spectral matrices registers
542 547 set_sm_irq_onNewMatrix( 0 ); // stop the spectral matrices
543 548 reset_sm_status();
544 549
545 550 // (3) clear interruptions
546 551 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
547 552
548 553 // suspend several tasks
549 554 if (lfrCurrentMode != LFR_MODE_STANDBY) {
550 555 status = suspend_asm_tasks();
551 556 }
552 557
553 558 if (status != RTEMS_SUCCESSFUL)
554 559 {
555 560 PRINTF1("in stop_current_mode *** in suspend_science_tasks *** ERR code: %d\n", status)
556 561 }
557 562
558 563 return status;
559 564 }
560 565
561 566 int stop_current_mode( void )
562 567 {
563 568 /** This function stops the current mode by masking interrupt lines and suspending science tasks.
564 569 *
565 570 * @return RTEMS directive status codes:
566 571 * - RTEMS_SUCCESSFUL - task restarted successfully
567 572 * - RTEMS_INVALID_ID - task id invalid
568 573 * - RTEMS_ALREADY_SUSPENDED - task already suspended
569 574 *
570 575 */
571 576
572 577 rtems_status_code status;
573 578
574 579 status = RTEMS_SUCCESSFUL;
575 580
576 581 // (1) mask interruptions
577 582 LEON_Mask_interrupt( IRQ_WAVEFORM_PICKER ); // mask waveform picker interrupt
578 583 LEON_Mask_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
579 584
580 585 // (2) reset waveform picker registers
581 586 reset_wfp_burst_enable(); // reset burst and enable bits
582 587 reset_wfp_status(); // reset all the status bits
583 588
584 589 // (3) reset spectral matrices registers
585 590 set_sm_irq_onNewMatrix( 0 ); // stop the spectral matrices
586 591 reset_sm_status();
587 592
588 593 // reset lfr VHDL module
589 594 reset_lfr();
590 595
591 596 reset_extractSWF(); // reset the extractSWF flag to false
592 597
593 598 // (4) clear interruptions
594 599 LEON_Clear_interrupt( IRQ_WAVEFORM_PICKER ); // clear waveform picker interrupt
595 600 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
596 601
597 602 // suspend several tasks
598 603 if (lfrCurrentMode != LFR_MODE_STANDBY) {
599 604 status = suspend_science_tasks();
600 605 }
601 606
602 607 if (status != RTEMS_SUCCESSFUL)
603 608 {
604 609 PRINTF1("in stop_current_mode *** in suspend_science_tasks *** ERR code: %d\n", status)
605 610 }
606 611
607 612 return status;
608 613 }
609 614
610 615 int enter_mode_standby( void )
611 616 {
612 617 /** This function is used to put LFR in the STANDBY mode.
613 618 *
614 619 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
615 620 *
616 621 * @return RTEMS directive status codes:
617 622 * - RTEMS_SUCCESSFUL - task restarted successfully
618 623 * - RTEMS_INVALID_ID - task id invalid
619 624 * - RTEMS_INCORRECT_STATE - task never started
620 625 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
621 626 *
622 627 * The STANDBY mode does not depends on a specific transition date, the effect of the TC_LFR_ENTER_MODE
623 628 * is immediate.
624 629 *
625 630 */
626 631
627 632 int status;
628 633
629 634 status = stop_current_mode(); // STOP THE CURRENT MODE
630 635
631 636 #ifdef PRINT_TASK_STATISTICS
632 637 rtems_cpu_usage_report();
633 638 #endif
634 639
635 640 #ifdef PRINT_STACK_REPORT
636 641 PRINTF("stack report selected\n")
637 642 rtems_stack_checker_report_usage();
638 643 #endif
639 644
640 645 return status;
641 646 }
642 647
643 648 int enter_mode_normal( unsigned int transitionCoarseTime )
644 649 {
645 650 /** This function is used to start the NORMAL mode.
646 651 *
647 652 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
648 653 *
649 654 * @return RTEMS directive status codes:
650 655 * - RTEMS_SUCCESSFUL - task restarted successfully
651 656 * - RTEMS_INVALID_ID - task id invalid
652 657 * - RTEMS_INCORRECT_STATE - task never started
653 658 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
654 659 *
655 660 * The way the NORMAL mode is started depends on the LFR current mode. If LFR is in SBM1 or SBM2,
656 661 * the snapshots are not restarted, only ASM, BP and CWF data generation are affected.
657 662 *
658 663 */
659 664
660 665 int status;
661 666
662 667 #ifdef PRINT_TASK_STATISTICS
663 668 rtems_cpu_usage_reset();
664 669 #endif
665 670
666 671 status = RTEMS_UNSATISFIED;
667 672
668 673 switch( lfrCurrentMode )
669 674 {
670 675 case LFR_MODE_STANDBY:
671 676 status = restart_science_tasks( LFR_MODE_NORMAL ); // restart science tasks
672 677 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
673 678 {
674 679 launch_spectral_matrix( );
675 680 launch_waveform_picker( LFR_MODE_NORMAL, transitionCoarseTime );
676 681 }
677 682 break;
678 683 case LFR_MODE_BURST:
679 684 status = stop_current_mode(); // stop the current mode
680 685 status = restart_science_tasks( LFR_MODE_NORMAL ); // restart the science tasks
681 686 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
682 687 {
683 688 launch_spectral_matrix( );
684 689 launch_waveform_picker( LFR_MODE_NORMAL, transitionCoarseTime );
685 690 }
686 691 break;
687 692 case LFR_MODE_SBM1:
688 693 status = restart_asm_activities( LFR_MODE_NORMAL ); // this is necessary to restart ASM tasks to update the parameters
689 694 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
690 695 update_last_valid_transition_date( transitionCoarseTime );
691 696 break;
692 697 case LFR_MODE_SBM2:
693 698 status = restart_asm_activities( LFR_MODE_NORMAL ); // this is necessary to restart ASM tasks to update the parameters
694 699 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
695 700 update_last_valid_transition_date( transitionCoarseTime );
696 701 break;
697 702 default:
698 703 break;
699 704 }
700 705
701 706 if (status != RTEMS_SUCCESSFUL)
702 707 {
703 708 PRINTF1("ERR *** in enter_mode_normal *** status = %d\n", status)
704 709 status = RTEMS_UNSATISFIED;
705 710 }
706 711
707 712 return status;
708 713 }
709 714
710 715 int enter_mode_burst( unsigned int transitionCoarseTime )
711 716 {
712 717 /** This function is used to start the BURST mode.
713 718 *
714 719 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
715 720 *
716 721 * @return RTEMS directive status codes:
717 722 * - RTEMS_SUCCESSFUL - task restarted successfully
718 723 * - RTEMS_INVALID_ID - task id invalid
719 724 * - RTEMS_INCORRECT_STATE - task never started
720 725 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
721 726 *
722 727 * The way the BURST mode is started does not depend on the LFR current mode.
723 728 *
724 729 */
725 730
726 731
727 732 int status;
728 733
729 734 #ifdef PRINT_TASK_STATISTICS
730 735 rtems_cpu_usage_reset();
731 736 #endif
732 737
733 738 status = stop_current_mode(); // stop the current mode
734 739 status = restart_science_tasks( LFR_MODE_BURST ); // restart the science tasks
735 740 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
736 741 {
737 742 launch_spectral_matrix( );
738 743 launch_waveform_picker( LFR_MODE_BURST, transitionCoarseTime );
739 744 }
740 745
741 746 if (status != RTEMS_SUCCESSFUL)
742 747 {
743 748 PRINTF1("ERR *** in enter_mode_burst *** status = %d\n", status)
744 749 status = RTEMS_UNSATISFIED;
745 750 }
746 751
747 752 return status;
748 753 }
749 754
750 755 int enter_mode_sbm1( unsigned int transitionCoarseTime )
751 756 {
752 757 /** This function is used to start the SBM1 mode.
753 758 *
754 759 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
755 760 *
756 761 * @return RTEMS directive status codes:
757 762 * - RTEMS_SUCCESSFUL - task restarted successfully
758 763 * - RTEMS_INVALID_ID - task id invalid
759 764 * - RTEMS_INCORRECT_STATE - task never started
760 765 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
761 766 *
762 767 * The way the SBM1 mode is started depends on the LFR current mode. If LFR is in NORMAL or SBM2,
763 768 * the snapshots are not restarted, only ASM, BP and CWF data generation are affected. In other
764 769 * cases, the acquisition is completely restarted.
765 770 *
766 771 */
767 772
768 773 int status;
769 774
770 775 #ifdef PRINT_TASK_STATISTICS
771 776 rtems_cpu_usage_reset();
772 777 #endif
773 778
774 779 status = RTEMS_UNSATISFIED;
775 780
776 781 switch( lfrCurrentMode )
777 782 {
778 783 case LFR_MODE_STANDBY:
779 784 status = restart_science_tasks( LFR_MODE_SBM1 ); // restart science tasks
780 785 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
781 786 {
782 787 launch_spectral_matrix( );
783 788 launch_waveform_picker( LFR_MODE_SBM1, transitionCoarseTime );
784 789 }
785 790 break;
786 791 case LFR_MODE_NORMAL: // lfrCurrentMode will be updated after the execution of close_action
787 792 status = restart_asm_activities( LFR_MODE_SBM1 );
788 793 status = LFR_SUCCESSFUL;
789 794 update_last_valid_transition_date( transitionCoarseTime );
790 795 break;
791 796 case LFR_MODE_BURST:
792 797 status = stop_current_mode(); // stop the current mode
793 798 status = restart_science_tasks( LFR_MODE_SBM1 ); // restart the science tasks
794 799 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
795 800 {
796 801 launch_spectral_matrix( );
797 802 launch_waveform_picker( LFR_MODE_SBM1, transitionCoarseTime );
798 803 }
799 804 break;
800 805 case LFR_MODE_SBM2:
801 806 status = restart_asm_activities( LFR_MODE_SBM1 );
802 807 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
803 808 update_last_valid_transition_date( transitionCoarseTime );
804 809 break;
805 810 default:
806 811 break;
807 812 }
808 813
809 814 if (status != RTEMS_SUCCESSFUL)
810 815 {
811 816 PRINTF1("ERR *** in enter_mode_sbm1 *** status = %d\n", status);
812 817 status = RTEMS_UNSATISFIED;
813 818 }
814 819
815 820 return status;
816 821 }
817 822
818 823 int enter_mode_sbm2( unsigned int transitionCoarseTime )
819 824 {
820 825 /** This function is used to start the SBM2 mode.
821 826 *
822 827 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
823 828 *
824 829 * @return RTEMS directive status codes:
825 830 * - RTEMS_SUCCESSFUL - task restarted successfully
826 831 * - RTEMS_INVALID_ID - task id invalid
827 832 * - RTEMS_INCORRECT_STATE - task never started
828 833 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
829 834 *
830 835 * The way the SBM2 mode is started depends on the LFR current mode. If LFR is in NORMAL or SBM1,
831 836 * the snapshots are not restarted, only ASM, BP and CWF data generation are affected. In other
832 837 * cases, the acquisition is completely restarted.
833 838 *
834 839 */
835 840
836 841 int status;
837 842
838 843 #ifdef PRINT_TASK_STATISTICS
839 844 rtems_cpu_usage_reset();
840 845 #endif
841 846
842 847 status = RTEMS_UNSATISFIED;
843 848
844 849 switch( lfrCurrentMode )
845 850 {
846 851 case LFR_MODE_STANDBY:
847 852 status = restart_science_tasks( LFR_MODE_SBM2 ); // restart science tasks
848 853 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
849 854 {
850 855 launch_spectral_matrix( );
851 856 launch_waveform_picker( LFR_MODE_SBM2, transitionCoarseTime );
852 857 }
853 858 break;
854 859 case LFR_MODE_NORMAL:
855 860 status = restart_asm_activities( LFR_MODE_SBM2 );
856 861 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
857 862 update_last_valid_transition_date( transitionCoarseTime );
858 863 break;
859 864 case LFR_MODE_BURST:
860 865 status = stop_current_mode(); // stop the current mode
861 866 status = restart_science_tasks( LFR_MODE_SBM2 ); // restart the science tasks
862 867 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
863 868 {
864 869 launch_spectral_matrix( );
865 870 launch_waveform_picker( LFR_MODE_SBM2, transitionCoarseTime );
866 871 }
867 872 break;
868 873 case LFR_MODE_SBM1:
869 874 status = restart_asm_activities( LFR_MODE_SBM2 );
870 875 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
871 876 update_last_valid_transition_date( transitionCoarseTime );
872 877 break;
873 878 default:
874 879 break;
875 880 }
876 881
877 882 if (status != RTEMS_SUCCESSFUL)
878 883 {
879 884 PRINTF1("ERR *** in enter_mode_sbm2 *** status = %d\n", status)
880 885 status = RTEMS_UNSATISFIED;
881 886 }
882 887
883 888 return status;
884 889 }
885 890
886 891 int restart_science_tasks( unsigned char lfrRequestedMode )
887 892 {
888 893 /** This function is used to restart all science tasks.
889 894 *
890 895 * @return RTEMS directive status codes:
891 896 * - RTEMS_SUCCESSFUL - task restarted successfully
892 897 * - RTEMS_INVALID_ID - task id invalid
893 898 * - RTEMS_INCORRECT_STATE - task never started
894 899 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
895 900 *
896 901 * Science tasks are AVF0, PRC0, WFRM, CWF3, CW2, CWF1
897 902 *
898 903 */
899 904
900 905 rtems_status_code status[10];
901 906 rtems_status_code ret;
902 907
903 908 ret = RTEMS_SUCCESSFUL;
904 909
905 910 status[0] = rtems_task_restart( Task_id[TASKID_AVF0], lfrRequestedMode );
906 911 if (status[0] != RTEMS_SUCCESSFUL)
907 912 {
908 913 PRINTF1("in restart_science_task *** AVF0 ERR %d\n", status[0])
909 914 }
910 915
911 916 status[1] = rtems_task_restart( Task_id[TASKID_PRC0], lfrRequestedMode );
912 917 if (status[1] != RTEMS_SUCCESSFUL)
913 918 {
914 919 PRINTF1("in restart_science_task *** PRC0 ERR %d\n", status[1])
915 920 }
916 921
917 922 status[2] = rtems_task_restart( Task_id[TASKID_WFRM],1 );
918 923 if (status[2] != RTEMS_SUCCESSFUL)
919 924 {
920 925 PRINTF1("in restart_science_task *** WFRM ERR %d\n", status[2])
921 926 }
922 927
923 928 status[3] = rtems_task_restart( Task_id[TASKID_CWF3],1 );
924 929 if (status[3] != RTEMS_SUCCESSFUL)
925 930 {
926 931 PRINTF1("in restart_science_task *** CWF3 ERR %d\n", status[3])
927 932 }
928 933
929 934 status[4] = rtems_task_restart( Task_id[TASKID_CWF2],1 );
930 935 if (status[4] != RTEMS_SUCCESSFUL)
931 936 {
932 937 PRINTF1("in restart_science_task *** CWF2 ERR %d\n", status[4])
933 938 }
934 939
935 940 status[5] = rtems_task_restart( Task_id[TASKID_CWF1],1 );
936 941 if (status[5] != RTEMS_SUCCESSFUL)
937 942 {
938 943 PRINTF1("in restart_science_task *** CWF1 ERR %d\n", status[5])
939 944 }
940 945
941 946 status[6] = rtems_task_restart( Task_id[TASKID_AVF1], lfrRequestedMode );
942 947 if (status[6] != RTEMS_SUCCESSFUL)
943 948 {
944 949 PRINTF1("in restart_science_task *** AVF1 ERR %d\n", status[6])
945 950 }
946 951
947 952 status[7] = rtems_task_restart( Task_id[TASKID_PRC1],lfrRequestedMode );
948 953 if (status[7] != RTEMS_SUCCESSFUL)
949 954 {
950 955 PRINTF1("in restart_science_task *** PRC1 ERR %d\n", status[7])
951 956 }
952 957
953 958 status[8] = rtems_task_restart( Task_id[TASKID_AVF2], 1 );
954 959 if (status[8] != RTEMS_SUCCESSFUL)
955 960 {
956 961 PRINTF1("in restart_science_task *** AVF2 ERR %d\n", status[8])
957 962 }
958 963
959 964 status[9] = rtems_task_restart( Task_id[TASKID_PRC2], 1 );
960 965 if (status[9] != RTEMS_SUCCESSFUL)
961 966 {
962 967 PRINTF1("in restart_science_task *** PRC2 ERR %d\n", status[9])
963 968 }
964 969
965 970 if ( (status[0] != RTEMS_SUCCESSFUL) || (status[1] != RTEMS_SUCCESSFUL) ||
966 971 (status[2] != RTEMS_SUCCESSFUL) || (status[3] != RTEMS_SUCCESSFUL) ||
967 972 (status[4] != RTEMS_SUCCESSFUL) || (status[5] != RTEMS_SUCCESSFUL) ||
968 973 (status[6] != RTEMS_SUCCESSFUL) || (status[7] != RTEMS_SUCCESSFUL) ||
969 974 (status[8] != RTEMS_SUCCESSFUL) || (status[9] != RTEMS_SUCCESSFUL) )
970 975 {
971 976 ret = RTEMS_UNSATISFIED;
972 977 }
973 978
974 979 return ret;
975 980 }
976 981
977 982 int restart_asm_tasks( unsigned char lfrRequestedMode )
978 983 {
979 984 /** This function is used to restart average spectral matrices tasks.
980 985 *
981 986 * @return RTEMS directive status codes:
982 987 * - RTEMS_SUCCESSFUL - task restarted successfully
983 988 * - RTEMS_INVALID_ID - task id invalid
984 989 * - RTEMS_INCORRECT_STATE - task never started
985 990 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
986 991 *
987 992 * ASM tasks are AVF0, PRC0, AVF1, PRC1, AVF2 and PRC2
988 993 *
989 994 */
990 995
991 996 rtems_status_code status[6];
992 997 rtems_status_code ret;
993 998
994 999 ret = RTEMS_SUCCESSFUL;
995 1000
996 1001 status[0] = rtems_task_restart( Task_id[TASKID_AVF0], lfrRequestedMode );
997 1002 if (status[0] != RTEMS_SUCCESSFUL)
998 1003 {
999 1004 PRINTF1("in restart_science_task *** AVF0 ERR %d\n", status[0])
1000 1005 }
1001 1006
1002 1007 status[1] = rtems_task_restart( Task_id[TASKID_PRC0], lfrRequestedMode );
1003 1008 if (status[1] != RTEMS_SUCCESSFUL)
1004 1009 {
1005 1010 PRINTF1("in restart_science_task *** PRC0 ERR %d\n", status[1])
1006 1011 }
1007 1012
1008 1013 status[2] = rtems_task_restart( Task_id[TASKID_AVF1], lfrRequestedMode );
1009 1014 if (status[2] != RTEMS_SUCCESSFUL)
1010 1015 {
1011 1016 PRINTF1("in restart_science_task *** AVF1 ERR %d\n", status[2])
1012 1017 }
1013 1018
1014 1019 status[3] = rtems_task_restart( Task_id[TASKID_PRC1],lfrRequestedMode );
1015 1020 if (status[3] != RTEMS_SUCCESSFUL)
1016 1021 {
1017 1022 PRINTF1("in restart_science_task *** PRC1 ERR %d\n", status[3])
1018 1023 }
1019 1024
1020 1025 status[4] = rtems_task_restart( Task_id[TASKID_AVF2], 1 );
1021 1026 if (status[4] != RTEMS_SUCCESSFUL)
1022 1027 {
1023 1028 PRINTF1("in restart_science_task *** AVF2 ERR %d\n", status[4])
1024 1029 }
1025 1030
1026 1031 status[5] = rtems_task_restart( Task_id[TASKID_PRC2], 1 );
1027 1032 if (status[5] != RTEMS_SUCCESSFUL)
1028 1033 {
1029 1034 PRINTF1("in restart_science_task *** PRC2 ERR %d\n", status[5])
1030 1035 }
1031 1036
1032 1037 if ( (status[0] != RTEMS_SUCCESSFUL) || (status[1] != RTEMS_SUCCESSFUL) ||
1033 1038 (status[2] != RTEMS_SUCCESSFUL) || (status[3] != RTEMS_SUCCESSFUL) ||
1034 1039 (status[4] != RTEMS_SUCCESSFUL) || (status[5] != RTEMS_SUCCESSFUL) )
1035 1040 {
1036 1041 ret = RTEMS_UNSATISFIED;
1037 1042 }
1038 1043
1039 1044 return ret;
1040 1045 }
1041 1046
1042 1047 int suspend_science_tasks( void )
1043 1048 {
1044 1049 /** This function suspends the science tasks.
1045 1050 *
1046 1051 * @return RTEMS directive status codes:
1047 1052 * - RTEMS_SUCCESSFUL - task restarted successfully
1048 1053 * - RTEMS_INVALID_ID - task id invalid
1049 1054 * - RTEMS_ALREADY_SUSPENDED - task already suspended
1050 1055 *
1051 1056 */
1052 1057
1053 1058 rtems_status_code status;
1054 1059
1055 1060 PRINTF("in suspend_science_tasks\n")
1056 1061
1057 1062 status = rtems_task_suspend( Task_id[TASKID_AVF0] ); // suspend AVF0
1058 1063 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1059 1064 {
1060 1065 PRINTF1("in suspend_science_task *** AVF0 ERR %d\n", status)
1061 1066 }
1062 1067 else
1063 1068 {
1064 1069 status = RTEMS_SUCCESSFUL;
1065 1070 }
1066 1071 if (status == RTEMS_SUCCESSFUL) // suspend PRC0
1067 1072 {
1068 1073 status = rtems_task_suspend( Task_id[TASKID_PRC0] );
1069 1074 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1070 1075 {
1071 1076 PRINTF1("in suspend_science_task *** PRC0 ERR %d\n", status)
1072 1077 }
1073 1078 else
1074 1079 {
1075 1080 status = RTEMS_SUCCESSFUL;
1076 1081 }
1077 1082 }
1078 1083 if (status == RTEMS_SUCCESSFUL) // suspend AVF1
1079 1084 {
1080 1085 status = rtems_task_suspend( Task_id[TASKID_AVF1] );
1081 1086 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1082 1087 {
1083 1088 PRINTF1("in suspend_science_task *** AVF1 ERR %d\n", status)
1084 1089 }
1085 1090 else
1086 1091 {
1087 1092 status = RTEMS_SUCCESSFUL;
1088 1093 }
1089 1094 }
1090 1095 if (status == RTEMS_SUCCESSFUL) // suspend PRC1
1091 1096 {
1092 1097 status = rtems_task_suspend( Task_id[TASKID_PRC1] );
1093 1098 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1094 1099 {
1095 1100 PRINTF1("in suspend_science_task *** PRC1 ERR %d\n", status)
1096 1101 }
1097 1102 else
1098 1103 {
1099 1104 status = RTEMS_SUCCESSFUL;
1100 1105 }
1101 1106 }
1102 1107 if (status == RTEMS_SUCCESSFUL) // suspend AVF2
1103 1108 {
1104 1109 status = rtems_task_suspend( Task_id[TASKID_AVF2] );
1105 1110 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1106 1111 {
1107 1112 PRINTF1("in suspend_science_task *** AVF2 ERR %d\n", status)
1108 1113 }
1109 1114 else
1110 1115 {
1111 1116 status = RTEMS_SUCCESSFUL;
1112 1117 }
1113 1118 }
1114 1119 if (status == RTEMS_SUCCESSFUL) // suspend PRC2
1115 1120 {
1116 1121 status = rtems_task_suspend( Task_id[TASKID_PRC2] );
1117 1122 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1118 1123 {
1119 1124 PRINTF1("in suspend_science_task *** PRC2 ERR %d\n", status)
1120 1125 }
1121 1126 else
1122 1127 {
1123 1128 status = RTEMS_SUCCESSFUL;
1124 1129 }
1125 1130 }
1126 1131 if (status == RTEMS_SUCCESSFUL) // suspend WFRM
1127 1132 {
1128 1133 status = rtems_task_suspend( Task_id[TASKID_WFRM] );
1129 1134 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1130 1135 {
1131 1136 PRINTF1("in suspend_science_task *** WFRM ERR %d\n", status)
1132 1137 }
1133 1138 else
1134 1139 {
1135 1140 status = RTEMS_SUCCESSFUL;
1136 1141 }
1137 1142 }
1138 1143 if (status == RTEMS_SUCCESSFUL) // suspend CWF3
1139 1144 {
1140 1145 status = rtems_task_suspend( Task_id[TASKID_CWF3] );
1141 1146 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1142 1147 {
1143 1148 PRINTF1("in suspend_science_task *** CWF3 ERR %d\n", status)
1144 1149 }
1145 1150 else
1146 1151 {
1147 1152 status = RTEMS_SUCCESSFUL;
1148 1153 }
1149 1154 }
1150 1155 if (status == RTEMS_SUCCESSFUL) // suspend CWF2
1151 1156 {
1152 1157 status = rtems_task_suspend( Task_id[TASKID_CWF2] );
1153 1158 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1154 1159 {
1155 1160 PRINTF1("in suspend_science_task *** CWF2 ERR %d\n", status)
1156 1161 }
1157 1162 else
1158 1163 {
1159 1164 status = RTEMS_SUCCESSFUL;
1160 1165 }
1161 1166 }
1162 1167 if (status == RTEMS_SUCCESSFUL) // suspend CWF1
1163 1168 {
1164 1169 status = rtems_task_suspend( Task_id[TASKID_CWF1] );
1165 1170 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1166 1171 {
1167 1172 PRINTF1("in suspend_science_task *** CWF1 ERR %d\n", status)
1168 1173 }
1169 1174 else
1170 1175 {
1171 1176 status = RTEMS_SUCCESSFUL;
1172 1177 }
1173 1178 }
1174 1179
1175 1180 return status;
1176 1181 }
1177 1182
1178 1183 int suspend_asm_tasks( void )
1179 1184 {
1180 1185 /** This function suspends the science tasks.
1181 1186 *
1182 1187 * @return RTEMS directive status codes:
1183 1188 * - RTEMS_SUCCESSFUL - task restarted successfully
1184 1189 * - RTEMS_INVALID_ID - task id invalid
1185 1190 * - RTEMS_ALREADY_SUSPENDED - task already suspended
1186 1191 *
1187 1192 */
1188 1193
1189 1194 rtems_status_code status;
1190 1195
1191 1196 PRINTF("in suspend_science_tasks\n")
1192 1197
1193 1198 status = rtems_task_suspend( Task_id[TASKID_AVF0] ); // suspend AVF0
1194 1199 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1195 1200 {
1196 1201 PRINTF1("in suspend_science_task *** AVF0 ERR %d\n", status)
1197 1202 }
1198 1203 else
1199 1204 {
1200 1205 status = RTEMS_SUCCESSFUL;
1201 1206 }
1202 1207
1203 1208 if (status == RTEMS_SUCCESSFUL) // suspend PRC0
1204 1209 {
1205 1210 status = rtems_task_suspend( Task_id[TASKID_PRC0] );
1206 1211 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1207 1212 {
1208 1213 PRINTF1("in suspend_science_task *** PRC0 ERR %d\n", status)
1209 1214 }
1210 1215 else
1211 1216 {
1212 1217 status = RTEMS_SUCCESSFUL;
1213 1218 }
1214 1219 }
1215 1220
1216 1221 if (status == RTEMS_SUCCESSFUL) // suspend AVF1
1217 1222 {
1218 1223 status = rtems_task_suspend( Task_id[TASKID_AVF1] );
1219 1224 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1220 1225 {
1221 1226 PRINTF1("in suspend_science_task *** AVF1 ERR %d\n", status)
1222 1227 }
1223 1228 else
1224 1229 {
1225 1230 status = RTEMS_SUCCESSFUL;
1226 1231 }
1227 1232 }
1228 1233
1229 1234 if (status == RTEMS_SUCCESSFUL) // suspend PRC1
1230 1235 {
1231 1236 status = rtems_task_suspend( Task_id[TASKID_PRC1] );
1232 1237 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1233 1238 {
1234 1239 PRINTF1("in suspend_science_task *** PRC1 ERR %d\n", status)
1235 1240 }
1236 1241 else
1237 1242 {
1238 1243 status = RTEMS_SUCCESSFUL;
1239 1244 }
1240 1245 }
1241 1246
1242 1247 if (status == RTEMS_SUCCESSFUL) // suspend AVF2
1243 1248 {
1244 1249 status = rtems_task_suspend( Task_id[TASKID_AVF2] );
1245 1250 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1246 1251 {
1247 1252 PRINTF1("in suspend_science_task *** AVF2 ERR %d\n", status)
1248 1253 }
1249 1254 else
1250 1255 {
1251 1256 status = RTEMS_SUCCESSFUL;
1252 1257 }
1253 1258 }
1254 1259
1255 1260 if (status == RTEMS_SUCCESSFUL) // suspend PRC2
1256 1261 {
1257 1262 status = rtems_task_suspend( Task_id[TASKID_PRC2] );
1258 1263 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1259 1264 {
1260 1265 PRINTF1("in suspend_science_task *** PRC2 ERR %d\n", status)
1261 1266 }
1262 1267 else
1263 1268 {
1264 1269 status = RTEMS_SUCCESSFUL;
1265 1270 }
1266 1271 }
1267 1272
1268 1273 return status;
1269 1274 }
1270 1275
1271 1276 void launch_waveform_picker( unsigned char mode, unsigned int transitionCoarseTime )
1272 1277 {
1273 1278
1274 1279 WFP_reset_current_ring_nodes();
1275 1280
1276 1281 reset_waveform_picker_regs();
1277 1282
1278 1283 set_wfp_burst_enable_register( mode );
1279 1284
1280 1285 LEON_Clear_interrupt( IRQ_WAVEFORM_PICKER );
1281 1286 LEON_Unmask_interrupt( IRQ_WAVEFORM_PICKER );
1282 1287
1283 1288 if (transitionCoarseTime == 0)
1284 1289 {
1285 1290 // instant transition means transition on the next valid date
1286 1291 // this is mandatory to have a good snapshot period and a good correction of the snapshot period
1287 1292 waveform_picker_regs->start_date = time_management_regs->coarse_time + 1;
1288 1293 }
1289 1294 else
1290 1295 {
1291 1296 waveform_picker_regs->start_date = transitionCoarseTime;
1292 1297 }
1293 1298
1294 1299 update_last_valid_transition_date(waveform_picker_regs->start_date);
1295 1300
1296 1301 }
1297 1302
1298 1303 void launch_spectral_matrix( void )
1299 1304 {
1300 1305 SM_reset_current_ring_nodes();
1301 1306
1302 1307 reset_spectral_matrix_regs();
1303 1308
1304 1309 reset_nb_sm();
1305 1310
1306 1311 set_sm_irq_onNewMatrix( 1 );
1307 1312
1308 1313 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX );
1309 1314 LEON_Unmask_interrupt( IRQ_SPECTRAL_MATRIX );
1310 1315
1311 1316 }
1312 1317
1313 1318 void set_sm_irq_onNewMatrix( unsigned char value )
1314 1319 {
1315 1320 if (value == 1)
1316 1321 {
1317 1322 spectral_matrix_regs->config = spectral_matrix_regs->config | 0x01;
1318 1323 }
1319 1324 else
1320 1325 {
1321 1326 spectral_matrix_regs->config = spectral_matrix_regs->config & 0xfffffffe; // 1110
1322 1327 }
1323 1328 }
1324 1329
1325 1330 void set_sm_irq_onError( unsigned char value )
1326 1331 {
1327 1332 if (value == 1)
1328 1333 {
1329 1334 spectral_matrix_regs->config = spectral_matrix_regs->config | 0x02;
1330 1335 }
1331 1336 else
1332 1337 {
1333 1338 spectral_matrix_regs->config = spectral_matrix_regs->config & 0xfffffffd; // 1101
1334 1339 }
1335 1340 }
1336 1341
1337 1342 //*****************************
1338 1343 // CONFIGURE CALIBRATION SIGNAL
1339 1344 void setCalibrationPrescaler( unsigned int prescaler )
1340 1345 {
1341 1346 // prescaling of the master clock (25 MHz)
1342 1347 // master clock is divided by 2^prescaler
1343 1348 time_management_regs->calPrescaler = prescaler;
1344 1349 }
1345 1350
1346 1351 void setCalibrationDivisor( unsigned int divisionFactor )
1347 1352 {
1348 1353 // division of the prescaled clock by the division factor
1349 1354 time_management_regs->calDivisor = divisionFactor;
1350 1355 }
1351 1356
1352 1357 void setCalibrationData( void ){
1353 1358 unsigned int k;
1354 1359 unsigned short data;
1355 1360 float val;
1356 1361 float f0;
1357 1362 float f1;
1358 1363 float fs;
1359 1364 float Ts;
1360 1365 float scaleFactor;
1361 1366
1362 1367 f0 = 625;
1363 1368 f1 = 10000;
1364 1369 fs = 160256.410;
1365 1370 Ts = 1. / fs;
1366 1371 scaleFactor = 0.250 / 0.000654; // 191, 500 mVpp, 2 sinus waves => 500 mVpp each, amplitude = 250 mV
1367 1372
1368 1373 time_management_regs->calDataPtr = 0x00;
1369 1374
1370 1375 // build the signal for the SCM calibration
1371 1376 for (k=0; k<256; k++)
1372 1377 {
1373 1378 val = sin( 2 * pi * f0 * k * Ts )
1374 1379 + sin( 2 * pi * f1 * k * Ts );
1375 1380 data = (unsigned short) ((val * scaleFactor) + 2048);
1376 1381 time_management_regs->calData = data & 0xfff;
1377 1382 }
1378 1383 }
1379 1384
1380 1385 void setCalibrationDataInterleaved( void ){
1381 1386 unsigned int k;
1382 1387 float val;
1383 1388 float f0;
1384 1389 float f1;
1385 1390 float fs;
1386 1391 float Ts;
1387 1392 unsigned short data[384];
1388 1393 unsigned char *dataPtr;
1389 1394
1390 1395 f0 = 625;
1391 1396 f1 = 10000;
1392 1397 fs = 240384.615;
1393 1398 Ts = 1. / fs;
1394 1399
1395 1400 time_management_regs->calDataPtr = 0x00;
1396 1401
1397 1402 // build the signal for the SCM calibration
1398 1403 for (k=0; k<384; k++)
1399 1404 {
1400 1405 val = sin( 2 * pi * f0 * k * Ts )
1401 1406 + sin( 2 * pi * f1 * k * Ts );
1402 1407 data[k] = (unsigned short) (val * 512 + 2048);
1403 1408 }
1404 1409
1405 1410 // write the signal in interleaved mode
1406 1411 for (k=0; k<128; k++)
1407 1412 {
1408 1413 dataPtr = (unsigned char*) &data[k*3 + 2];
1409 1414 time_management_regs->calData = (data[k*3] & 0xfff)
1410 1415 + ( (dataPtr[0] & 0x3f) << 12);
1411 1416 time_management_regs->calData = (data[k*3 + 1] & 0xfff)
1412 1417 + ( (dataPtr[1] & 0x3f) << 12);
1413 1418 }
1414 1419 }
1415 1420
1416 1421 void setCalibrationReload( bool state)
1417 1422 {
1418 1423 if (state == true)
1419 1424 {
1420 1425 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | 0x00000010; // [0001 0000]
1421 1426 }
1422 1427 else
1423 1428 {
1424 1429 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & 0xffffffef; // [1110 1111]
1425 1430 }
1426 1431 }
1427 1432
1428 1433 void setCalibrationEnable( bool state )
1429 1434 {
1430 1435 // this bit drives the multiplexer
1431 1436 if (state == true)
1432 1437 {
1433 1438 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | 0x00000040; // [0100 0000]
1434 1439 }
1435 1440 else
1436 1441 {
1437 1442 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & 0xffffffbf; // [1011 1111]
1438 1443 }
1439 1444 }
1440 1445
1441 1446 void setCalibrationInterleaved( bool state )
1442 1447 {
1443 1448 // this bit drives the multiplexer
1444 1449 if (state == true)
1445 1450 {
1446 1451 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | 0x00000020; // [0010 0000]
1447 1452 }
1448 1453 else
1449 1454 {
1450 1455 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & 0xffffffdf; // [1101 1111]
1451 1456 }
1452 1457 }
1453 1458
1454 1459 void setCalibration( bool state )
1455 1460 {
1456 1461 if (state == true)
1457 1462 {
1458 1463 setCalibrationEnable( true );
1459 1464 setCalibrationReload( false );
1460 1465 set_hk_lfr_calib_enable( true );
1461 1466 }
1462 1467 else
1463 1468 {
1464 1469 setCalibrationEnable( false );
1465 1470 setCalibrationReload( true );
1466 1471 set_hk_lfr_calib_enable( false );
1467 1472 }
1468 1473 }
1469 1474
1470 1475 void configureCalibration( bool interleaved )
1471 1476 {
1472 1477 setCalibration( false );
1473 1478 if ( interleaved == true )
1474 1479 {
1475 1480 setCalibrationInterleaved( true );
1476 1481 setCalibrationPrescaler( 0 ); // 25 MHz => 25 000 000
1477 1482 setCalibrationDivisor( 26 ); // => 240 384
1478 1483 setCalibrationDataInterleaved();
1479 1484 }
1480 1485 else
1481 1486 {
1482 1487 setCalibrationPrescaler( 0 ); // 25 MHz => 25 000 000
1483 1488 setCalibrationDivisor( 38 ); // => 160 256 (39 - 1)
1484 1489 setCalibrationData();
1485 1490 }
1486 1491 }
1487 1492
1488 1493 //****************
1489 1494 // CLOSING ACTIONS
1490 1495 void update_last_TC_exe( ccsdsTelecommandPacket_t *TC, unsigned char * time )
1491 1496 {
1492 1497 /** This function is used to update the HK packets statistics after a successful TC execution.
1493 1498 *
1494 1499 * @param TC points to the TC being processed
1495 1500 * @param time is the time used to date the TC execution
1496 1501 *
1497 1502 */
1498 1503
1499 1504 unsigned int val;
1500 1505
1501 1506 housekeeping_packet.hk_lfr_last_exe_tc_id[0] = TC->packetID[0];
1502 1507 housekeeping_packet.hk_lfr_last_exe_tc_id[1] = TC->packetID[1];
1503 1508 housekeeping_packet.hk_lfr_last_exe_tc_type[0] = 0x00;
1504 1509 housekeeping_packet.hk_lfr_last_exe_tc_type[1] = TC->serviceType;
1505 1510 housekeeping_packet.hk_lfr_last_exe_tc_subtype[0] = 0x00;
1506 1511 housekeeping_packet.hk_lfr_last_exe_tc_subtype[1] = TC->serviceSubType;
1507 1512 housekeeping_packet.hk_lfr_last_exe_tc_time[0] = time[0];
1508 1513 housekeeping_packet.hk_lfr_last_exe_tc_time[1] = time[1];
1509 1514 housekeeping_packet.hk_lfr_last_exe_tc_time[2] = time[2];
1510 1515 housekeeping_packet.hk_lfr_last_exe_tc_time[3] = time[3];
1511 1516 housekeeping_packet.hk_lfr_last_exe_tc_time[4] = time[4];
1512 1517 housekeeping_packet.hk_lfr_last_exe_tc_time[5] = time[5];
1513 1518
1514 1519 val = housekeeping_packet.hk_lfr_exe_tc_cnt[0] * 256 + housekeeping_packet.hk_lfr_exe_tc_cnt[1];
1515 1520 val++;
1516 1521 housekeeping_packet.hk_lfr_exe_tc_cnt[0] = (unsigned char) (val >> 8);
1517 1522 housekeeping_packet.hk_lfr_exe_tc_cnt[1] = (unsigned char) (val);
1518 1523 }
1519 1524
1520 1525 void update_last_TC_rej(ccsdsTelecommandPacket_t *TC, unsigned char * time )
1521 1526 {
1522 1527 /** This function is used to update the HK packets statistics after a TC rejection.
1523 1528 *
1524 1529 * @param TC points to the TC being processed
1525 1530 * @param time is the time used to date the TC rejection
1526 1531 *
1527 1532 */
1528 1533
1529 1534 unsigned int val;
1530 1535
1531 1536 housekeeping_packet.hk_lfr_last_rej_tc_id[0] = TC->packetID[0];
1532 1537 housekeeping_packet.hk_lfr_last_rej_tc_id[1] = TC->packetID[1];
1533 1538 housekeeping_packet.hk_lfr_last_rej_tc_type[0] = 0x00;
1534 1539 housekeeping_packet.hk_lfr_last_rej_tc_type[1] = TC->serviceType;
1535 1540 housekeeping_packet.hk_lfr_last_rej_tc_subtype[0] = 0x00;
1536 1541 housekeeping_packet.hk_lfr_last_rej_tc_subtype[1] = TC->serviceSubType;
1537 1542 housekeeping_packet.hk_lfr_last_rej_tc_time[0] = time[0];
1538 1543 housekeeping_packet.hk_lfr_last_rej_tc_time[1] = time[1];
1539 1544 housekeeping_packet.hk_lfr_last_rej_tc_time[2] = time[2];
1540 1545 housekeeping_packet.hk_lfr_last_rej_tc_time[3] = time[3];
1541 1546 housekeeping_packet.hk_lfr_last_rej_tc_time[4] = time[4];
1542 1547 housekeeping_packet.hk_lfr_last_rej_tc_time[5] = time[5];
1543 1548
1544 1549 val = housekeeping_packet.hk_lfr_rej_tc_cnt[0] * 256 + housekeeping_packet.hk_lfr_rej_tc_cnt[1];
1545 1550 val++;
1546 1551 housekeeping_packet.hk_lfr_rej_tc_cnt[0] = (unsigned char) (val >> 8);
1547 1552 housekeeping_packet.hk_lfr_rej_tc_cnt[1] = (unsigned char) (val);
1548 1553 }
1549 1554
1550 1555 void close_action(ccsdsTelecommandPacket_t *TC, int result, rtems_id queue_id )
1551 1556 {
1552 1557 /** This function is the last step of the TC execution workflow.
1553 1558 *
1554 1559 * @param TC points to the TC being processed
1555 1560 * @param result is the result of the TC execution (LFR_SUCCESSFUL / LFR_DEFAULT)
1556 1561 * @param queue_id is the id of the RTEMS message queue used to send TM packets
1557 1562 * @param time is the time used to date the TC execution
1558 1563 *
1559 1564 */
1560 1565
1561 1566 unsigned char requestedMode;
1562 1567
1563 1568 if (result == LFR_SUCCESSFUL)
1564 1569 {
1565 1570 if ( !( (TC->serviceType==TC_TYPE_TIME) & (TC->serviceSubType==TC_SUBTYPE_UPDT_TIME) )
1566 1571 &
1567 1572 !( (TC->serviceType==TC_TYPE_GEN) & (TC->serviceSubType==TC_SUBTYPE_UPDT_INFO))
1568 1573 )
1569 1574 {
1570 1575 send_tm_lfr_tc_exe_success( TC, queue_id );
1571 1576 }
1572 1577 if ( (TC->serviceType == TC_TYPE_GEN) & (TC->serviceSubType == TC_SUBTYPE_ENTER) )
1573 1578 {
1574 1579 //**********************************
1575 1580 // UPDATE THE LFRMODE LOCAL VARIABLE
1576 1581 requestedMode = TC->dataAndCRC[1];
1577 1582 updateLFRCurrentMode( requestedMode );
1578 1583 }
1579 1584 }
1580 1585 else if (result == LFR_EXE_ERROR)
1581 1586 {
1582 1587 send_tm_lfr_tc_exe_error( TC, queue_id );
1583 1588 }
1584 1589 }
1585 1590
1586 1591 //***************************
1587 1592 // Interrupt Service Routines
1588 1593 rtems_isr commutation_isr1( rtems_vector_number vector )
1589 1594 {
1590 1595 if (rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
1591 1596 PRINTF("In commutation_isr1 *** Error sending event to DUMB\n")
1592 1597 }
1593 1598 }
1594 1599
1595 1600 rtems_isr commutation_isr2( rtems_vector_number vector )
1596 1601 {
1597 1602 if (rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
1598 1603 PRINTF("In commutation_isr2 *** Error sending event to DUMB\n")
1599 1604 }
1600 1605 }
1601 1606
1602 1607 //****************
1603 1608 // OTHER FUNCTIONS
1604 1609 void updateLFRCurrentMode( unsigned char requestedMode )
1605 1610 {
1606 1611 /** This function updates the value of the global variable lfrCurrentMode.
1607 1612 *
1608 1613 * lfrCurrentMode is a parameter used by several functions to know in which mode LFR is running.
1609 1614 *
1610 1615 */
1611 1616
1612 1617 // update the local value of lfrCurrentMode with the value contained in the housekeeping_packet structure
1613 1618 housekeeping_packet.lfr_status_word[0] = (unsigned char) ((requestedMode << 4) + 0x0d);
1614 1619 lfrCurrentMode = requestedMode;
1615 1620 }
1616 1621
1617 1622 void set_lfr_soft_reset( unsigned char value )
1618 1623 {
1619 1624 if (value == 1)
1620 1625 {
1621 1626 time_management_regs->ctrl = time_management_regs->ctrl | 0x00000004; // [0100]
1622 1627 }
1623 1628 else
1624 1629 {
1625 1630 time_management_regs->ctrl = time_management_regs->ctrl & 0xfffffffb; // [1011]
1626 1631 }
1627 1632 }
1628 1633
1629 1634 void reset_lfr( void )
1630 1635 {
1631 1636 set_lfr_soft_reset( 1 );
1632 1637
1633 1638 set_lfr_soft_reset( 0 );
1634 1639
1635 1640 set_hk_lfr_sc_potential_flag( true );
1636 1641 }
@@ -1,1280 +1,1430
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_normal_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 int action_load_pas_filter_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
313 int action_load_filter_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
314 314 {
315 315 /** This function updates the LFR registers with the incoming sbm2 parameters.
316 316 *
317 317 * @param TC points to the TeleCommand packet that is being processed
318 318 * @param queue_id is the id of the queue which handles TM related to this execution step
319 319 *
320 320 */
321 321
322 322 int flag;
323 323
324 324 flag = LFR_DEFAULT;
325 325
326 326 flag = check_sy_lfr_pas_filter_parameters( TC, queue_id );
327 327
328 328 if (flag == LFR_SUCCESSFUL)
329 329 {
330 parameter_dump_packet.spare_sy_lfr_pas_filter_enabled = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED ];
331 parameter_dump_packet.sy_lfr_pas_filter_modulus = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS ];
332 parameter_dump_packet.sy_lfr_pas_filter_nstd = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_NSTD ];
333 parameter_dump_packet.sy_lfr_pas_filter_offset = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET ];
330 parameter_dump_packet.spare_sy_lfr_pas_filter_enabled = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED ];
331 parameter_dump_packet.sy_lfr_pas_filter_modulus = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS ];
332 parameter_dump_packet.sy_lfr_pas_filter_tbad[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + 0 ];
333 parameter_dump_packet.sy_lfr_pas_filter_tbad[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + 1 ];
334 parameter_dump_packet.sy_lfr_pas_filter_tbad[2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + 2 ];
335 parameter_dump_packet.sy_lfr_pas_filter_tbad[3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + 3 ];
336 parameter_dump_packet.sy_lfr_pas_filter_offset = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET ];
337 parameter_dump_packet.sy_lfr_pas_filter_shift[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + 0 ];
338 parameter_dump_packet.sy_lfr_pas_filter_shift[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + 1 ];
339 parameter_dump_packet.sy_lfr_pas_filter_shift[2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + 2 ];
340 parameter_dump_packet.sy_lfr_pas_filter_shift[3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + 3 ];
341 parameter_dump_packet.sy_lfr_sc_rw_delta_f[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + 0 ];
342 parameter_dump_packet.sy_lfr_sc_rw_delta_f[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + 1 ];
343 parameter_dump_packet.sy_lfr_sc_rw_delta_f[2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + 2 ];
344 parameter_dump_packet.sy_lfr_sc_rw_delta_f[3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + 3 ];
334 345 }
335 346
336 347 return flag;
337 348 }
338 349
339 350 int action_dump_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
340 351 {
341 352 /** This function updates the LFR registers with the incoming sbm2 parameters.
342 353 *
343 354 * @param TC points to the TeleCommand packet that is being processed
344 355 * @param queue_id is the id of the queue which handles TM related to this execution step
345 356 *
346 357 */
347 358
348 359 unsigned int address;
349 360 rtems_status_code status;
350 361 unsigned int freq;
351 362 unsigned int bin;
352 363 unsigned int coeff;
353 364 unsigned char *kCoeffPtr;
354 365 unsigned char *kCoeffDumpPtr;
355 366
356 367 // for each sy_lfr_kcoeff_frequency there is 32 kcoeff
357 368 // F0 => 11 bins
358 369 // F1 => 13 bins
359 370 // F2 => 12 bins
360 371 // 36 bins to dump in two packets (30 bins max per packet)
361 372
362 373 //*********
363 374 // PACKET 1
364 375 // 11 F0 bins, 13 F1 bins and 6 F2 bins
365 376 kcoefficients_dump_1.destinationID = TC->sourceID;
366 377 increment_seq_counter_destination_id_dump( kcoefficients_dump_1.packetSequenceControl, TC->sourceID );
367 378 for( freq=0;
368 379 freq<NB_BINS_COMPRESSED_SM_F0;
369 380 freq++ )
370 381 {
371 382 kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1] = freq;
372 383 bin = freq;
373 384 // printKCoefficients( freq, bin, k_coeff_intercalib_f0_norm);
374 385 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
375 386 {
376 387 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
377 388 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f0_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
378 389 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
379 390 }
380 391 }
381 392 for( freq=NB_BINS_COMPRESSED_SM_F0;
382 393 freq<(NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1);
383 394 freq++ )
384 395 {
385 396 kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1 ] = freq;
386 397 bin = freq - NB_BINS_COMPRESSED_SM_F0;
387 398 // printKCoefficients( freq, bin, k_coeff_intercalib_f1_norm);
388 399 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
389 400 {
390 401 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
391 402 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f1_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
392 403 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
393 404 }
394 405 }
395 406 for( freq=(NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1);
396 407 freq<(NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1+6);
397 408 freq++ )
398 409 {
399 410 kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1 ] = freq;
400 411 bin = freq - (NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1);
401 412 // printKCoefficients( freq, bin, k_coeff_intercalib_f2);
402 413 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
403 414 {
404 415 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
405 416 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
406 417 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
407 418 }
408 419 }
409 420 kcoefficients_dump_1.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
410 421 kcoefficients_dump_1.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
411 422 kcoefficients_dump_1.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
412 423 kcoefficients_dump_1.time[3] = (unsigned char) (time_management_regs->coarse_time);
413 424 kcoefficients_dump_1.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
414 425 kcoefficients_dump_1.time[5] = (unsigned char) (time_management_regs->fine_time);
415 426 // SEND DATA
416 427 kcoefficient_node_1.status = 1;
417 428 address = (unsigned int) &kcoefficient_node_1;
418 429 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
419 430 if (status != RTEMS_SUCCESSFUL) {
420 431 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 1 , code %d", status)
421 432 }
422 433
423 434 //********
424 435 // PACKET 2
425 436 // 6 F2 bins
426 437 kcoefficients_dump_2.destinationID = TC->sourceID;
427 438 increment_seq_counter_destination_id_dump( kcoefficients_dump_2.packetSequenceControl, TC->sourceID );
428 439 for( freq=0; freq<6; freq++ )
429 440 {
430 441 kcoefficients_dump_2.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1 ] = NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 + 6 + freq;
431 442 bin = freq + 6;
432 443 // printKCoefficients( freq, bin, k_coeff_intercalib_f2);
433 444 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
434 445 {
435 446 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_2.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
436 447 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
437 448 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
438 449 }
439 450 }
440 451 kcoefficients_dump_2.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
441 452 kcoefficients_dump_2.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
442 453 kcoefficients_dump_2.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
443 454 kcoefficients_dump_2.time[3] = (unsigned char) (time_management_regs->coarse_time);
444 455 kcoefficients_dump_2.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
445 456 kcoefficients_dump_2.time[5] = (unsigned char) (time_management_regs->fine_time);
446 457 // SEND DATA
447 458 kcoefficient_node_2.status = 1;
448 459 address = (unsigned int) &kcoefficient_node_2;
449 460 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
450 461 if (status != RTEMS_SUCCESSFUL) {
451 462 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 2, code %d", status)
452 463 }
453 464
454 465 return status;
455 466 }
456 467
457 468 int action_dump_par( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
458 469 {
459 470 /** This function dumps the LFR parameters by sending the appropriate TM packet to the dedicated RTEMS message queue.
460 471 *
461 472 * @param queue_id is the id of the queue which handles TM related to this execution step.
462 473 *
463 474 * @return RTEMS directive status codes:
464 475 * - RTEMS_SUCCESSFUL - message sent successfully
465 476 * - RTEMS_INVALID_ID - invalid queue id
466 477 * - RTEMS_INVALID_SIZE - invalid message size
467 478 * - RTEMS_INVALID_ADDRESS - buffer is NULL
468 479 * - RTEMS_UNSATISFIED - out of message buffers
469 480 * - RTEMS_TOO_MANY - queue s limit has been reached
470 481 *
471 482 */
472 483
473 484 int status;
474 485
475 486 increment_seq_counter_destination_id_dump( parameter_dump_packet.packetSequenceControl, TC->sourceID );
476 487 parameter_dump_packet.destinationID = TC->sourceID;
477 488
478 489 // UPDATE TIME
479 490 parameter_dump_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
480 491 parameter_dump_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
481 492 parameter_dump_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
482 493 parameter_dump_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
483 494 parameter_dump_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
484 495 parameter_dump_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
485 496 // SEND DATA
486 497 status = rtems_message_queue_send( queue_id, &parameter_dump_packet,
487 498 PACKET_LENGTH_PARAMETER_DUMP + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
488 499 if (status != RTEMS_SUCCESSFUL) {
489 500 PRINTF1("in action_dump *** ERR sending packet, code %d", status)
490 501 }
491 502
492 503 return status;
493 504 }
494 505
495 506 //***********************
496 507 // NORMAL MODE PARAMETERS
497 508
498 509 int check_normal_par_consistency( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
499 510 {
500 511 unsigned char msb;
501 512 unsigned char lsb;
502 513 int flag;
503 514 float aux;
504 515 rtems_status_code status;
505 516
506 517 unsigned int sy_lfr_n_swf_l;
507 518 unsigned int sy_lfr_n_swf_p;
508 519 unsigned int sy_lfr_n_asm_p;
509 520 unsigned char sy_lfr_n_bp_p0;
510 521 unsigned char sy_lfr_n_bp_p1;
511 522 unsigned char sy_lfr_n_cwf_long_f3;
512 523
513 524 flag = LFR_SUCCESSFUL;
514 525
515 526 //***************
516 527 // get parameters
517 528 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
518 529 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
519 530 sy_lfr_n_swf_l = msb * 256 + lsb;
520 531
521 532 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
522 533 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
523 534 sy_lfr_n_swf_p = msb * 256 + lsb;
524 535
525 536 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
526 537 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
527 538 sy_lfr_n_asm_p = msb * 256 + lsb;
528 539
529 540 sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
530 541
531 542 sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
532 543
533 544 sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
534 545
535 546 //******************
536 547 // check consistency
537 548 // sy_lfr_n_swf_l
538 549 if (sy_lfr_n_swf_l != 2048)
539 550 {
540 551 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_L+10, sy_lfr_n_swf_l );
541 552 flag = WRONG_APP_DATA;
542 553 }
543 554 // sy_lfr_n_swf_p
544 555 if (flag == LFR_SUCCESSFUL)
545 556 {
546 557 if ( sy_lfr_n_swf_p < 22 )
547 558 {
548 559 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_P+10, sy_lfr_n_swf_p );
549 560 flag = WRONG_APP_DATA;
550 561 }
551 562 }
552 563 // sy_lfr_n_bp_p0
553 564 if (flag == LFR_SUCCESSFUL)
554 565 {
555 566 if (sy_lfr_n_bp_p0 < DFLT_SY_LFR_N_BP_P0)
556 567 {
557 568 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P0+10, sy_lfr_n_bp_p0 );
558 569 flag = WRONG_APP_DATA;
559 570 }
560 571 }
561 572 // sy_lfr_n_asm_p
562 573 if (flag == LFR_SUCCESSFUL)
563 574 {
564 575 if (sy_lfr_n_asm_p == 0)
565 576 {
566 577 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P+10, sy_lfr_n_asm_p );
567 578 flag = WRONG_APP_DATA;
568 579 }
569 580 }
570 581 // sy_lfr_n_asm_p shall be a whole multiple of sy_lfr_n_bp_p0
571 582 if (flag == LFR_SUCCESSFUL)
572 583 {
573 584 aux = ( (float ) sy_lfr_n_asm_p / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_asm_p / sy_lfr_n_bp_p0);
574 585 if (aux > FLOAT_EQUAL_ZERO)
575 586 {
576 587 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P+10, sy_lfr_n_asm_p );
577 588 flag = WRONG_APP_DATA;
578 589 }
579 590 }
580 591 // sy_lfr_n_bp_p1
581 592 if (flag == LFR_SUCCESSFUL)
582 593 {
583 594 if (sy_lfr_n_bp_p1 < DFLT_SY_LFR_N_BP_P1)
584 595 {
585 596 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1+10, sy_lfr_n_bp_p1 );
586 597 flag = WRONG_APP_DATA;
587 598 }
588 599 }
589 600 // sy_lfr_n_bp_p1 shall be a whole multiple of sy_lfr_n_bp_p0
590 601 if (flag == LFR_SUCCESSFUL)
591 602 {
592 603 aux = ( (float ) sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0);
593 604 if (aux > FLOAT_EQUAL_ZERO)
594 605 {
595 606 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1+10, sy_lfr_n_bp_p1 );
596 607 flag = LFR_DEFAULT;
597 608 }
598 609 }
599 610 // sy_lfr_n_cwf_long_f3
600 611
601 612 return flag;
602 613 }
603 614
604 615 int set_sy_lfr_n_swf_l( ccsdsTelecommandPacket_t *TC )
605 616 {
606 617 /** This function sets the number of points of a snapshot (sy_lfr_n_swf_l).
607 618 *
608 619 * @param TC points to the TeleCommand packet that is being processed
609 620 * @param queue_id is the id of the queue which handles TM related to this execution step
610 621 *
611 622 */
612 623
613 624 int result;
614 625
615 626 result = LFR_SUCCESSFUL;
616 627
617 628 parameter_dump_packet.sy_lfr_n_swf_l[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
618 629 parameter_dump_packet.sy_lfr_n_swf_l[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
619 630
620 631 return result;
621 632 }
622 633
623 634 int set_sy_lfr_n_swf_p(ccsdsTelecommandPacket_t *TC )
624 635 {
625 636 /** This function sets the time between two snapshots, in s (sy_lfr_n_swf_p).
626 637 *
627 638 * @param TC points to the TeleCommand packet that is being processed
628 639 * @param queue_id is the id of the queue which handles TM related to this execution step
629 640 *
630 641 */
631 642
632 643 int result;
633 644
634 645 result = LFR_SUCCESSFUL;
635 646
636 647 parameter_dump_packet.sy_lfr_n_swf_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
637 648 parameter_dump_packet.sy_lfr_n_swf_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
638 649
639 650 return result;
640 651 }
641 652
642 653 int set_sy_lfr_n_asm_p( ccsdsTelecommandPacket_t *TC )
643 654 {
644 655 /** This function sets the time between two full spectral matrices transmission, in s (SY_LFR_N_ASM_P).
645 656 *
646 657 * @param TC points to the TeleCommand packet that is being processed
647 658 * @param queue_id is the id of the queue which handles TM related to this execution step
648 659 *
649 660 */
650 661
651 662 int result;
652 663
653 664 result = LFR_SUCCESSFUL;
654 665
655 666 parameter_dump_packet.sy_lfr_n_asm_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
656 667 parameter_dump_packet.sy_lfr_n_asm_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
657 668
658 669 return result;
659 670 }
660 671
661 672 int set_sy_lfr_n_bp_p0( ccsdsTelecommandPacket_t *TC )
662 673 {
663 674 /** This function sets the time between two basic parameter sets, in s (DFLT_SY_LFR_N_BP_P0).
664 675 *
665 676 * @param TC points to the TeleCommand packet that is being processed
666 677 * @param queue_id is the id of the queue which handles TM related to this execution step
667 678 *
668 679 */
669 680
670 681 int status;
671 682
672 683 status = LFR_SUCCESSFUL;
673 684
674 685 parameter_dump_packet.sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
675 686
676 687 return status;
677 688 }
678 689
679 690 int set_sy_lfr_n_bp_p1(ccsdsTelecommandPacket_t *TC )
680 691 {
681 692 /** This function sets the time between two basic parameter sets (autocorrelation + crosscorrelation), in s (sy_lfr_n_bp_p1).
682 693 *
683 694 * @param TC points to the TeleCommand packet that is being processed
684 695 * @param queue_id is the id of the queue which handles TM related to this execution step
685 696 *
686 697 */
687 698
688 699 int status;
689 700
690 701 status = LFR_SUCCESSFUL;
691 702
692 703 parameter_dump_packet.sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
693 704
694 705 return status;
695 706 }
696 707
697 708 int set_sy_lfr_n_cwf_long_f3(ccsdsTelecommandPacket_t *TC )
698 709 {
699 710 /** This function allows to switch from CWF_F3 packets to CWF_LONG_F3 packets.
700 711 *
701 712 * @param TC points to the TeleCommand packet that is being processed
702 713 * @param queue_id is the id of the queue which handles TM related to this execution step
703 714 *
704 715 */
705 716
706 717 int status;
707 718
708 719 status = LFR_SUCCESSFUL;
709 720
710 721 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
711 722
712 723 return status;
713 724 }
714 725
715 726 //**********************
716 727 // BURST MODE PARAMETERS
717 728 int set_sy_lfr_b_bp_p0(ccsdsTelecommandPacket_t *TC)
718 729 {
719 730 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P0).
720 731 *
721 732 * @param TC points to the TeleCommand packet that is being processed
722 733 * @param queue_id is the id of the queue which handles TM related to this execution step
723 734 *
724 735 */
725 736
726 737 int status;
727 738
728 739 status = LFR_SUCCESSFUL;
729 740
730 741 parameter_dump_packet.sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
731 742
732 743 return status;
733 744 }
734 745
735 746 int set_sy_lfr_b_bp_p1( ccsdsTelecommandPacket_t *TC )
736 747 {
737 748 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P1).
738 749 *
739 750 * @param TC points to the TeleCommand packet that is being processed
740 751 * @param queue_id is the id of the queue which handles TM related to this execution step
741 752 *
742 753 */
743 754
744 755 int status;
745 756
746 757 status = LFR_SUCCESSFUL;
747 758
748 759 parameter_dump_packet.sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
749 760
750 761 return status;
751 762 }
752 763
753 764 //*********************
754 765 // SBM1 MODE PARAMETERS
755 766 int set_sy_lfr_s1_bp_p0( ccsdsTelecommandPacket_t *TC )
756 767 {
757 768 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P0).
758 769 *
759 770 * @param TC points to the TeleCommand packet that is being processed
760 771 * @param queue_id is the id of the queue which handles TM related to this execution step
761 772 *
762 773 */
763 774
764 775 int status;
765 776
766 777 status = LFR_SUCCESSFUL;
767 778
768 779 parameter_dump_packet.sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
769 780
770 781 return status;
771 782 }
772 783
773 784 int set_sy_lfr_s1_bp_p1( ccsdsTelecommandPacket_t *TC )
774 785 {
775 786 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P1).
776 787 *
777 788 * @param TC points to the TeleCommand packet that is being processed
778 789 * @param queue_id is the id of the queue which handles TM related to this execution step
779 790 *
780 791 */
781 792
782 793 int status;
783 794
784 795 status = LFR_SUCCESSFUL;
785 796
786 797 parameter_dump_packet.sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
787 798
788 799 return status;
789 800 }
790 801
791 802 //*********************
792 803 // SBM2 MODE PARAMETERS
793 int set_sy_lfr_s2_bp_p0(ccsdsTelecommandPacket_t *TC)
804 int set_sy_lfr_s2_bp_p0( ccsdsTelecommandPacket_t *TC )
794 805 {
795 806 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P0).
796 807 *
797 808 * @param TC points to the TeleCommand packet that is being processed
798 809 * @param queue_id is the id of the queue which handles TM related to this execution step
799 810 *
800 811 */
801 812
802 813 int status;
803 814
804 815 status = LFR_SUCCESSFUL;
805 816
806 817 parameter_dump_packet.sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
807 818
808 819 return status;
809 820 }
810 821
811 822 int set_sy_lfr_s2_bp_p1( ccsdsTelecommandPacket_t *TC )
812 823 {
813 824 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P1).
814 825 *
815 826 * @param TC points to the TeleCommand packet that is being processed
816 827 * @param queue_id is the id of the queue which handles TM related to this execution step
817 828 *
818 829 */
819 830
820 831 int status;
821 832
822 833 status = LFR_SUCCESSFUL;
823 834
824 835 parameter_dump_packet.sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
825 836
826 837 return status;
827 838 }
828 839
829 840 //*******************
830 841 // TC_LFR_UPDATE_INFO
831 842 unsigned int check_update_info_hk_lfr_mode( unsigned char mode )
832 843 {
833 844 unsigned int status;
834 845
835 846 if ( (mode == LFR_MODE_STANDBY) || (mode == LFR_MODE_NORMAL)
836 847 || (mode == LFR_MODE_BURST)
837 848 || (mode == LFR_MODE_SBM1) || (mode == LFR_MODE_SBM2))
838 849 {
839 850 status = LFR_SUCCESSFUL;
840 851 }
841 852 else
842 853 {
843 854 status = LFR_DEFAULT;
844 855 }
845 856
846 857 return status;
847 858 }
848 859
849 860 unsigned int check_update_info_hk_tds_mode( unsigned char mode )
850 861 {
851 862 unsigned int status;
852 863
853 864 if ( (mode == TDS_MODE_STANDBY) || (mode == TDS_MODE_NORMAL)
854 865 || (mode == TDS_MODE_BURST)
855 866 || (mode == TDS_MODE_SBM1) || (mode == TDS_MODE_SBM2)
856 867 || (mode == TDS_MODE_LFM))
857 868 {
858 869 status = LFR_SUCCESSFUL;
859 870 }
860 871 else
861 872 {
862 873 status = LFR_DEFAULT;
863 874 }
864 875
865 876 return status;
866 877 }
867 878
868 879 unsigned int check_update_info_hk_thr_mode( unsigned char mode )
869 880 {
870 881 unsigned int status;
871 882
872 883 if ( (mode == THR_MODE_STANDBY) || (mode == THR_MODE_NORMAL)
873 884 || (mode == THR_MODE_BURST))
874 885 {
875 886 status = LFR_SUCCESSFUL;
876 887 }
877 888 else
878 889 {
879 890 status = LFR_DEFAULT;
880 891 }
881 892
882 893 return status;
883 894 }
884 895
896 void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC )
897 {
898 /** This function get the reaction wheels frequencies in the incoming TC_LFR_UPDATE_INFO and copy the values locally.
899 *
900 * @param TC points to the TeleCommand packet that is being processed
901 *
902 */
903
904 unsigned char * bytePosPtr; // pointer to the beginning of the incoming TC packet
905 unsigned char * floatPtr; // pointer to the Most Significant Byte of the considered float
906
907 bytePosPtr = (unsigned char *) &TC->packetID;
908
909 // cp_rpw_sc_rw1_f1
910 floatPtr = (unsigned char *) &cp_rpw_sc_rw1_f1;
911 floatPtr[0] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1 ];
912 floatPtr[1] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1 + 1 ];
913 floatPtr[2] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1 + 2 ];
914 floatPtr[3] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1 + 3 ];
915 // cp_rpw_sc_rw1_f2
916 floatPtr = (unsigned char *) &cp_rpw_sc_rw1_f2;
917 floatPtr[0] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2 ];
918 floatPtr[1] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2 + 1 ];
919 floatPtr[2] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2 + 2 ];
920 floatPtr[3] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2 + 3 ];
921 // cp_rpw_sc_rw2_f1
922 floatPtr = (unsigned char *) &cp_rpw_sc_rw2_f1;
923 floatPtr[0] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1 ];
924 floatPtr[1] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1 + 1 ];
925 floatPtr[2] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1 + 2 ];
926 floatPtr[3] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1 + 3 ];
927 // cp_rpw_sc_rw2_f2
928 floatPtr = (unsigned char *) &cp_rpw_sc_rw2_f2;
929 floatPtr[0] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2 ];
930 floatPtr[1] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2 + 1 ];
931 floatPtr[2] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2 + 2 ];
932 floatPtr[3] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2 + 3 ];
933 // cp_rpw_sc_rw3_f1
934 floatPtr = (unsigned char *) &cp_rpw_sc_rw3_f1;
935 floatPtr[0] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1 ];
936 floatPtr[1] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1 + 1 ];
937 floatPtr[2] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1 + 2 ];
938 floatPtr[3] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1 + 3 ];
939 // cp_rpw_sc_rw3_f2
940 floatPtr = (unsigned char *) &cp_rpw_sc_rw3_f2;
941 floatPtr[0] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2 ];
942 floatPtr[1] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2 + 1 ];
943 floatPtr[2] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2 + 2 ];
944 floatPtr[3] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2 + 3 ];
945 // cp_rpw_sc_rw4_f1
946 floatPtr = (unsigned char *) &cp_rpw_sc_rw4_f1;
947 floatPtr[0] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1 ];
948 floatPtr[1] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1 + 1 ];
949 floatPtr[2] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1 + 2 ];
950 floatPtr[3] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1 + 3 ];
951 // cp_rpw_sc_rw4_f2
952 floatPtr = (unsigned char *) &cp_rpw_sc_rw4_f2;
953 floatPtr[0] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 ];
954 floatPtr[1] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 + 1 ];
955 floatPtr[2] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 + 2 ];
956 floatPtr[3] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 + 3 ];
957 }
958
959 void setFBinMask( unsigned char *fbins_mask, float freq, unsigned char deltaFreq, unsigned char flag )
960 {
961 unsigned int fBelow;
962
963 // compute the index of the frequency immediately below the reaction wheel frequency
964 fBelow = (unsigned int) ( floor( ((double) cp_rpw_sc_rw1_f1) / ((double) deltaFreq)) );
965
966 if (fBelow < 127) // if fbelow is greater than 127 or equal to 127, this means that the reaction wheel frequency is outside the frequency range
967 {
968 if (flag == 1)
969 {
970 // rw_fbins_mask[k] = (1 << fBelow) | (1 << fAbove);
971 }
972 }
973 }
974
975 void build_rw_fbins_mask( unsigned int channel )
976 {
977 unsigned char rw_fbins_mask[16];
978 unsigned char *maskPtr;
979 double deltaF;
980 unsigned k;
981
982 k = 0;
983
984 switch (channel)
985 {
986 case 0:
987 maskPtr = rw_fbins_mask_f0;
988 deltaF = 96.;
989 break;
990 case 1:
991 maskPtr = rw_fbins_mask_f1;
992 deltaF = 16.;
993 break;
994 case 2:
995 maskPtr = rw_fbins_mask_f2;
996 deltaF = 1.;
997 break;
998 default:
999 break;
1000 }
1001
1002 for (k = 0; k < 16; k++)
1003 {
1004 rw_fbins_mask[k] = 0x00;
1005 }
1006
1007 // RW1 F1
1008 // setFBinMask( rw_fbins_mask, fBelow );
1009
1010 // RW1 F2
1011
1012 // RW2 F1
1013
1014 // RW2 F2
1015
1016 // RW3 F1
1017
1018 // RW3 F2
1019
1020 // RW4 F1
1021
1022 // RW4 F2
1023
1024
1025 // update the value of the fbins related to reaction wheels frequency filtering
1026 for (k = 0; k < 16; k++)
1027 {
1028 maskPtr[k] = rw_fbins_mask[k];
1029 }
1030 }
1031
1032 void build_rw_fbins_masks()
1033 {
1034 build_rw_fbins_mask( 0 );
1035 build_rw_fbins_mask( 1 );
1036 build_rw_fbins_mask( 2 );
1037 }
1038
885 1039 //***********
886 1040 // FBINS MASK
887 1041
888 1042 int set_sy_lfr_fbins( ccsdsTelecommandPacket_t *TC )
889 1043 {
890 1044 int status;
891 1045 unsigned int k;
892 1046 unsigned char *fbins_mask_dump;
893 1047 unsigned char *fbins_mask_TC;
894 1048
895 1049 status = LFR_SUCCESSFUL;
896 1050
897 1051 fbins_mask_dump = parameter_dump_packet.sy_lfr_fbins_f0_word1;
898 1052 fbins_mask_TC = TC->dataAndCRC;
899 1053
900 1054 for (k=0; k < NB_FBINS_MASKS * NB_BYTES_PER_FBINS_MASK; k++)
901 1055 {
902 1056 fbins_mask_dump[k] = fbins_mask_TC[k];
903 1057 }
904 1058 for (k=0; k < NB_FBINS_MASKS; k++)
905 1059 {
906 1060 unsigned char *auxPtr;
907 1061 auxPtr = &parameter_dump_packet.sy_lfr_fbins_f0_word1[k*NB_BYTES_PER_FBINS_MASK];
908 1062 }
909 1063
910 1064
911 1065 return status;
912 1066 }
913 1067
914 1068 //***************************
915 1069 // TC_LFR_LOAD_PAS_FILTER_PAR
916 1070
917 1071 int check_sy_lfr_pas_filter_parameters( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
918 1072 {
919 1073 int flag;
920 1074 rtems_status_code status;
921 1075
922 1076 unsigned char sy_lfr_pas_filter_enabled;
923 1077 unsigned char sy_lfr_pas_filter_modulus;
924 unsigned char sy_lfr_pas_filter_nstd;
1078 float sy_lfr_pas_filter_tbad;
925 1079 unsigned char sy_lfr_pas_filter_offset;
1080 float sy_lfr_pas_filtershift;
1081 float sy_lfr_sc_rw_delta_f;
926 1082
927 1083 flag = LFR_SUCCESSFUL;
928 1084
929 1085 //***************
930 1086 // get parameters
931 1087 sy_lfr_pas_filter_enabled = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED ] & 0x01; // [0000 0001]
932 1088 sy_lfr_pas_filter_modulus = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS ];
933 sy_lfr_pas_filter_nstd = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_NSTD ];
1089
934 1090 sy_lfr_pas_filter_offset = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET ];
935 1091
936 1092 //******************
937 1093 // check consistency
938 1094 // sy_lfr_pas_filter_enabled
939 1095 // sy_lfr_pas_filter_modulus
940 1096 if ( (sy_lfr_pas_filter_modulus < 4) || (sy_lfr_pas_filter_modulus > 8) )
941 1097 {
942 1098 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS+10, sy_lfr_pas_filter_modulus );
943 1099 flag = WRONG_APP_DATA;
944 1100 }
945 // sy_lfr_pas_filter_nstd
946 if (flag == LFR_SUCCESSFUL)
947 {
948 if ( sy_lfr_pas_filter_nstd > 8 )
949 {
950 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_NSTD+10, sy_lfr_pas_filter_nstd );
951 flag = WRONG_APP_DATA;
952 }
953 }
1101 // sy_lfr_pas_filter_tbad
954 1102 // sy_lfr_pas_filter_offset
955 1103 if (flag == LFR_SUCCESSFUL)
956 1104 {
957 1105 if (sy_lfr_pas_filter_offset > 7)
958 1106 {
959 1107 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET+10, sy_lfr_pas_filter_offset );
960 1108 flag = WRONG_APP_DATA;
961 1109 }
962 1110 }
1111 // sy_lfr_pas_filtershift
1112 // sy_lfr_sc_rw_delta_f
963 1113
964 1114 return flag;
965 1115 }
966 1116
967 1117 //**************
968 1118 // KCOEFFICIENTS
969 1119 int set_sy_lfr_kcoeff( ccsdsTelecommandPacket_t *TC,rtems_id queue_id )
970 1120 {
971 1121 unsigned int kcoeff;
972 1122 unsigned short sy_lfr_kcoeff_frequency;
973 1123 unsigned short bin;
974 1124 unsigned short *freqPtr;
975 1125 float *kcoeffPtr_norm;
976 1126 float *kcoeffPtr_sbm;
977 1127 int status;
978 1128 unsigned char *kcoeffLoadPtr;
979 1129 unsigned char *kcoeffNormPtr;
980 1130 unsigned char *kcoeffSbmPtr_a;
981 1131 unsigned char *kcoeffSbmPtr_b;
982 1132
983 1133 status = LFR_SUCCESSFUL;
984 1134
985 1135 kcoeffPtr_norm = NULL;
986 1136 kcoeffPtr_sbm = NULL;
987 1137 bin = 0;
988 1138
989 1139 freqPtr = (unsigned short *) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY];
990 1140 sy_lfr_kcoeff_frequency = *freqPtr;
991 1141
992 1142 if ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM )
993 1143 {
994 1144 PRINTF1("ERR *** in set_sy_lfr_kcoeff_frequency *** sy_lfr_kcoeff_frequency = %d\n", sy_lfr_kcoeff_frequency)
995 1145 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + 10 + 1,
996 1146 TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + 1] ); // +1 to get the LSB instead of the MSB
997 1147 status = LFR_DEFAULT;
998 1148 }
999 1149 else
1000 1150 {
1001 1151 if ( ( sy_lfr_kcoeff_frequency >= 0 )
1002 1152 && ( sy_lfr_kcoeff_frequency < NB_BINS_COMPRESSED_SM_F0 ) )
1003 1153 {
1004 1154 kcoeffPtr_norm = k_coeff_intercalib_f0_norm;
1005 1155 kcoeffPtr_sbm = k_coeff_intercalib_f0_sbm;
1006 1156 bin = sy_lfr_kcoeff_frequency;
1007 1157 }
1008 1158 else if ( ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM_F0 )
1009 1159 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) ) )
1010 1160 {
1011 1161 kcoeffPtr_norm = k_coeff_intercalib_f1_norm;
1012 1162 kcoeffPtr_sbm = k_coeff_intercalib_f1_sbm;
1013 1163 bin = sy_lfr_kcoeff_frequency - NB_BINS_COMPRESSED_SM_F0;
1014 1164 }
1015 1165 else if ( ( sy_lfr_kcoeff_frequency >= (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) )
1016 1166 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 + NB_BINS_COMPRESSED_SM_F2) ) )
1017 1167 {
1018 1168 kcoeffPtr_norm = k_coeff_intercalib_f2;
1019 1169 kcoeffPtr_sbm = NULL;
1020 1170 bin = sy_lfr_kcoeff_frequency - (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1);
1021 1171 }
1022 1172 }
1023 1173
1024 1174 if (kcoeffPtr_norm != NULL ) // update K coefficient for NORMAL data products
1025 1175 {
1026 1176 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
1027 1177 {
1028 1178 // destination
1029 1179 kcoeffNormPtr = (unsigned char*) &kcoeffPtr_norm[ (bin * NB_K_COEFF_PER_BIN) + kcoeff ];
1030 1180 // source
1031 1181 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + NB_BYTES_PER_FLOAT * kcoeff];
1032 1182 // copy source to destination
1033 1183 copyFloatByChar( kcoeffNormPtr, kcoeffLoadPtr );
1034 1184 }
1035 1185 }
1036 1186
1037 1187 if (kcoeffPtr_sbm != NULL ) // update K coefficient for SBM data products
1038 1188 {
1039 1189 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
1040 1190 {
1041 1191 // destination
1042 1192 kcoeffSbmPtr_a= (unsigned char*) &kcoeffPtr_sbm[ ( (bin * NB_K_COEFF_PER_BIN) + kcoeff) * 2 ];
1043 1193 kcoeffSbmPtr_b= (unsigned char*) &kcoeffPtr_sbm[ ( (bin * NB_K_COEFF_PER_BIN) + kcoeff) * 2 + 1 ];
1044 1194 // source
1045 1195 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + NB_BYTES_PER_FLOAT * kcoeff];
1046 1196 // copy source to destination
1047 1197 copyFloatByChar( kcoeffSbmPtr_a, kcoeffLoadPtr );
1048 1198 copyFloatByChar( kcoeffSbmPtr_b, kcoeffLoadPtr );
1049 1199 }
1050 1200 }
1051 1201
1052 1202 // print_k_coeff();
1053 1203
1054 1204 return status;
1055 1205 }
1056 1206
1057 1207 void copyFloatByChar( unsigned char *destination, unsigned char *source )
1058 1208 {
1059 1209 destination[0] = source[0];
1060 1210 destination[1] = source[1];
1061 1211 destination[2] = source[2];
1062 1212 destination[3] = source[3];
1063 1213 }
1064 1214
1065 1215 //**********
1066 1216 // init dump
1067 1217
1068 1218 void init_parameter_dump( void )
1069 1219 {
1070 1220 /** This function initialize the parameter_dump_packet global variable with default values.
1071 1221 *
1072 1222 */
1073 1223
1074 1224 unsigned int k;
1075 1225
1076 1226 parameter_dump_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
1077 1227 parameter_dump_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
1078 1228 parameter_dump_packet.reserved = CCSDS_RESERVED;
1079 1229 parameter_dump_packet.userApplication = CCSDS_USER_APP;
1080 1230 parameter_dump_packet.packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> 8);
1081 1231 parameter_dump_packet.packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;
1082 1232 parameter_dump_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1083 1233 parameter_dump_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1084 1234 parameter_dump_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_PARAMETER_DUMP >> 8);
1085 1235 parameter_dump_packet.packetLength[1] = (unsigned char) PACKET_LENGTH_PARAMETER_DUMP;
1086 1236 // DATA FIELD HEADER
1087 1237 parameter_dump_packet.spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1088 1238 parameter_dump_packet.serviceType = TM_TYPE_PARAMETER_DUMP;
1089 1239 parameter_dump_packet.serviceSubType = TM_SUBTYPE_PARAMETER_DUMP;
1090 1240 parameter_dump_packet.destinationID = TM_DESTINATION_ID_GROUND;
1091 1241 parameter_dump_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
1092 1242 parameter_dump_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
1093 1243 parameter_dump_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
1094 1244 parameter_dump_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
1095 1245 parameter_dump_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
1096 1246 parameter_dump_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
1097 1247 parameter_dump_packet.sid = SID_PARAMETER_DUMP;
1098 1248
1099 1249 //******************
1100 1250 // COMMON PARAMETERS
1101 1251 parameter_dump_packet.sy_lfr_common_parameters_spare = DEFAULT_SY_LFR_COMMON0;
1102 1252 parameter_dump_packet.sy_lfr_common_parameters = DEFAULT_SY_LFR_COMMON1;
1103 1253
1104 1254 //******************
1105 1255 // NORMAL PARAMETERS
1106 1256 parameter_dump_packet.sy_lfr_n_swf_l[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_L >> 8);
1107 1257 parameter_dump_packet.sy_lfr_n_swf_l[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_L );
1108 1258 parameter_dump_packet.sy_lfr_n_swf_p[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_P >> 8);
1109 1259 parameter_dump_packet.sy_lfr_n_swf_p[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_P );
1110 1260 parameter_dump_packet.sy_lfr_n_asm_p[0] = (unsigned char) (DFLT_SY_LFR_N_ASM_P >> 8);
1111 1261 parameter_dump_packet.sy_lfr_n_asm_p[1] = (unsigned char) (DFLT_SY_LFR_N_ASM_P );
1112 1262 parameter_dump_packet.sy_lfr_n_bp_p0 = (unsigned char) DFLT_SY_LFR_N_BP_P0;
1113 1263 parameter_dump_packet.sy_lfr_n_bp_p1 = (unsigned char) DFLT_SY_LFR_N_BP_P1;
1114 1264 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = (unsigned char) DFLT_SY_LFR_N_CWF_LONG_F3;
1115 1265
1116 1266 //*****************
1117 1267 // BURST PARAMETERS
1118 1268 parameter_dump_packet.sy_lfr_b_bp_p0 = (unsigned char) DEFAULT_SY_LFR_B_BP_P0;
1119 1269 parameter_dump_packet.sy_lfr_b_bp_p1 = (unsigned char) DEFAULT_SY_LFR_B_BP_P1;
1120 1270
1121 1271 //****************
1122 1272 // SBM1 PARAMETERS
1123 1273 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
1124 1274 parameter_dump_packet.sy_lfr_s1_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S1_BP_P1;
1125 1275
1126 1276 //****************
1127 1277 // SBM2 PARAMETERS
1128 1278 parameter_dump_packet.sy_lfr_s2_bp_p0 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P0;
1129 1279 parameter_dump_packet.sy_lfr_s2_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P1;
1130 1280
1131 1281 //************
1132 1282 // FBINS MASKS
1133 1283 for (k=0; k < NB_FBINS_MASKS * NB_BYTES_PER_FBINS_MASK; k++)
1134 1284 {
1135 1285 parameter_dump_packet.sy_lfr_fbins_f0_word1[k] = 0xff;
1136 1286 }
1137 1287 }
1138 1288
1139 1289 void init_kcoefficients_dump( void )
1140 1290 {
1141 1291 init_kcoefficients_dump_packet( &kcoefficients_dump_1, 1, 30 );
1142 1292 init_kcoefficients_dump_packet( &kcoefficients_dump_2, 2, 6 );
1143 1293
1144 1294 kcoefficient_node_1.previous = NULL;
1145 1295 kcoefficient_node_1.next = NULL;
1146 1296 kcoefficient_node_1.sid = TM_CODE_K_DUMP;
1147 1297 kcoefficient_node_1.coarseTime = 0x00;
1148 1298 kcoefficient_node_1.fineTime = 0x00;
1149 1299 kcoefficient_node_1.buffer_address = (int) &kcoefficients_dump_1;
1150 1300 kcoefficient_node_1.status = 0x00;
1151 1301
1152 1302 kcoefficient_node_2.previous = NULL;
1153 1303 kcoefficient_node_2.next = NULL;
1154 1304 kcoefficient_node_2.sid = TM_CODE_K_DUMP;
1155 1305 kcoefficient_node_2.coarseTime = 0x00;
1156 1306 kcoefficient_node_2.fineTime = 0x00;
1157 1307 kcoefficient_node_2.buffer_address = (int) &kcoefficients_dump_2;
1158 1308 kcoefficient_node_2.status = 0x00;
1159 1309 }
1160 1310
1161 1311 void init_kcoefficients_dump_packet( Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump, unsigned char pkt_nr, unsigned char blk_nr )
1162 1312 {
1163 1313 unsigned int k;
1164 1314 unsigned int packetLength;
1165 1315
1166 1316 packetLength = blk_nr * 130 + 20 - CCSDS_TC_TM_PACKET_OFFSET; // 4 bytes for the CCSDS header
1167 1317
1168 1318 kcoefficients_dump->targetLogicalAddress = CCSDS_DESTINATION_ID;
1169 1319 kcoefficients_dump->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1170 1320 kcoefficients_dump->reserved = CCSDS_RESERVED;
1171 1321 kcoefficients_dump->userApplication = CCSDS_USER_APP;
1172 1322 kcoefficients_dump->packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> 8);;
1173 1323 kcoefficients_dump->packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;;
1174 1324 kcoefficients_dump->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1175 1325 kcoefficients_dump->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1176 1326 kcoefficients_dump->packetLength[0] = (unsigned char) (packetLength >> 8);
1177 1327 kcoefficients_dump->packetLength[1] = (unsigned char) packetLength;
1178 1328 // DATA FIELD HEADER
1179 1329 kcoefficients_dump->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1180 1330 kcoefficients_dump->serviceType = TM_TYPE_K_DUMP;
1181 1331 kcoefficients_dump->serviceSubType = TM_SUBTYPE_K_DUMP;
1182 1332 kcoefficients_dump->destinationID= TM_DESTINATION_ID_GROUND;
1183 1333 kcoefficients_dump->time[0] = 0x00;
1184 1334 kcoefficients_dump->time[1] = 0x00;
1185 1335 kcoefficients_dump->time[2] = 0x00;
1186 1336 kcoefficients_dump->time[3] = 0x00;
1187 1337 kcoefficients_dump->time[4] = 0x00;
1188 1338 kcoefficients_dump->time[5] = 0x00;
1189 1339 kcoefficients_dump->sid = SID_K_DUMP;
1190 1340
1191 1341 kcoefficients_dump->pkt_cnt = 2;
1192 1342 kcoefficients_dump->pkt_nr = pkt_nr;
1193 1343 kcoefficients_dump->blk_nr = blk_nr;
1194 1344
1195 1345 //******************
1196 1346 // SOURCE DATA repeated N times with N in [0 .. PA_LFR_KCOEFF_BLK_NR]
1197 1347 // one blk is 2 + 4 * 32 = 130 bytes, 30 blks max in one packet (30 * 130 = 3900)
1198 1348 for (k=0; k<3900; k++)
1199 1349 {
1200 1350 kcoefficients_dump->kcoeff_blks[k] = 0x00;
1201 1351 }
1202 1352 }
1203 1353
1204 1354 void increment_seq_counter_destination_id_dump( unsigned char *packet_sequence_control, unsigned char destination_id )
1205 1355 {
1206 1356 /** This function increment the packet sequence control parameter of a TC, depending on its destination ID.
1207 1357 *
1208 1358 * @param packet_sequence_control points to the packet sequence control which will be incremented
1209 1359 * @param destination_id is the destination ID of the TM, there is one counter by destination ID
1210 1360 *
1211 1361 * If the destination ID is not known, a dedicated counter is incremented.
1212 1362 *
1213 1363 */
1214 1364
1215 1365 unsigned short sequence_cnt;
1216 1366 unsigned short segmentation_grouping_flag;
1217 1367 unsigned short new_packet_sequence_control;
1218 1368 unsigned char i;
1219 1369
1220 1370 switch (destination_id)
1221 1371 {
1222 1372 case SID_TC_GROUND:
1223 1373 i = GROUND;
1224 1374 break;
1225 1375 case SID_TC_MISSION_TIMELINE:
1226 1376 i = MISSION_TIMELINE;
1227 1377 break;
1228 1378 case SID_TC_TC_SEQUENCES:
1229 1379 i = TC_SEQUENCES;
1230 1380 break;
1231 1381 case SID_TC_RECOVERY_ACTION_CMD:
1232 1382 i = RECOVERY_ACTION_CMD;
1233 1383 break;
1234 1384 case SID_TC_BACKUP_MISSION_TIMELINE:
1235 1385 i = BACKUP_MISSION_TIMELINE;
1236 1386 break;
1237 1387 case SID_TC_DIRECT_CMD:
1238 1388 i = DIRECT_CMD;
1239 1389 break;
1240 1390 case SID_TC_SPARE_GRD_SRC1:
1241 1391 i = SPARE_GRD_SRC1;
1242 1392 break;
1243 1393 case SID_TC_SPARE_GRD_SRC2:
1244 1394 i = SPARE_GRD_SRC2;
1245 1395 break;
1246 1396 case SID_TC_OBCP:
1247 1397 i = OBCP;
1248 1398 break;
1249 1399 case SID_TC_SYSTEM_CONTROL:
1250 1400 i = SYSTEM_CONTROL;
1251 1401 break;
1252 1402 case SID_TC_AOCS:
1253 1403 i = AOCS;
1254 1404 break;
1255 1405 case SID_TC_RPW_INTERNAL:
1256 1406 i = RPW_INTERNAL;
1257 1407 break;
1258 1408 default:
1259 1409 i = GROUND;
1260 1410 break;
1261 1411 }
1262 1412
1263 1413 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
1264 1414 sequence_cnt = sequenceCounters_TM_DUMP[ i ] & 0x3fff;
1265 1415
1266 1416 new_packet_sequence_control = segmentation_grouping_flag | sequence_cnt ;
1267 1417
1268 1418 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> 8);
1269 1419 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
1270 1420
1271 1421 // increment the sequence counter
1272 1422 if ( sequenceCounters_TM_DUMP[ i ] < SEQ_CNT_MAX )
1273 1423 {
1274 1424 sequenceCounters_TM_DUMP[ i ] = sequenceCounters_TM_DUMP[ i ] + 1;
1275 1425 }
1276 1426 else
1277 1427 {
1278 1428 sequenceCounters_TM_DUMP[ i ] = 0;
1279 1429 }
1280 1430 }
General Comments 0
You need to be logged in to leave comments. Login now