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

Fork of HG_REPOSITORIES/LPP/INSTRUMENTATION/SOLO_LFR/DEV_PLE

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

R3 updates. TC handlers added for the new telecommands:...
paul -
r192:a0b224a4fc7c VHDL_0_1_28
parent child
Show More
Show file before | Show file after | Hide comments
@@ -1,2 +1,2
1 1 a586fe639ac179e95bdc150ebdbab0312f31dc30 LFR_basic-parameters
2 611fe904e4b4e05736a8a618c561980d10bceead header/lfr_common_headers
2 d700fe1774be46689e78de1efae2ed50655b0f1c header/lfr_common_headers
Show file before | Show file after | Hide comments
@@ -1,112 +1,112
1 1 TEMPLATE = app
2 2 # CONFIG += console v8 sim
3 3 # CONFIG options = verbose *** boot_messages *** debug_messages *** cpu_usage_report *** stack_report *** vhdl_dev *** debug_tch
4 4 # lpp_dpu_destid
5 5 CONFIG += console verbose lpp_dpu_destid
6 6 CONFIG -= qt
7 7
8 8 include(./sparc.pri)
9 9
10 10 # flight software version
11 11 SWVERSION=-1-0
12 DEFINES += SW_VERSION_N1=2 # major
12 DEFINES += SW_VERSION_N1=3 # major
13 13 DEFINES += SW_VERSION_N2=0 # minor
14 DEFINES += SW_VERSION_N3=2 # patch
15 DEFINES += SW_VERSION_N4=3 # internal
14 DEFINES += SW_VERSION_N3=0 # patch
15 DEFINES += SW_VERSION_N4=0 # internal
16 16
17 17 # <GCOV>
18 18 #QMAKE_CFLAGS_RELEASE += -fprofile-arcs -ftest-coverage
19 19 #LIBS += -lgcov /opt/GCOV/01A/lib/overload.o -lc
20 20 # </GCOV>
21 21
22 22 # <CHANGE BEFORE FLIGHT>
23 23 contains( CONFIG, lpp_dpu_destid ) {
24 24 DEFINES += LPP_DPU_DESTID
25 25 }
26 26 # </CHANGE BEFORE FLIGHT>
27 27
28 28 contains( CONFIG, debug_tch ) {
29 29 DEFINES += DEBUG_TCH
30 30 }
31 31 DEFINES += MSB_FIRST_TCH
32 32
33 33 contains( CONFIG, vhdl_dev ) {
34 34 DEFINES += VHDL_DEV
35 35 }
36 36
37 37 contains( CONFIG, verbose ) {
38 38 DEFINES += PRINT_MESSAGES_ON_CONSOLE
39 39 }
40 40
41 41 contains( CONFIG, debug_messages ) {
42 42 DEFINES += DEBUG_MESSAGES
43 43 }
44 44
45 45 contains( CONFIG, cpu_usage_report ) {
46 46 DEFINES += PRINT_TASK_STATISTICS
47 47 }
48 48
49 49 contains( CONFIG, stack_report ) {
50 50 DEFINES += PRINT_STACK_REPORT
51 51 }
52 52
53 53 contains( CONFIG, boot_messages ) {
54 54 DEFINES += BOOT_MESSAGES
55 55 }
56 56
57 57 #doxygen.target = doxygen
58 58 #doxygen.commands = doxygen ../doc/Doxyfile
59 59 #QMAKE_EXTRA_TARGETS += doxygen
60 60
61 61 TARGET = fsw
62 62
63 63 INCLUDEPATH += \
64 64 $${PWD}/../src \
65 65 $${PWD}/../header \
66 66 $${PWD}/../header/lfr_common_headers \
67 67 $${PWD}/../header/processing \
68 68 $${PWD}/../LFR_basic-parameters
69 69
70 70 SOURCES += \
71 71 ../src/wf_handler.c \
72 72 ../src/tc_handler.c \
73 73 ../src/fsw_misc.c \
74 74 ../src/fsw_init.c \
75 75 ../src/fsw_globals.c \
76 76 ../src/fsw_spacewire.c \
77 77 ../src/tc_load_dump_parameters.c \
78 78 ../src/tm_lfr_tc_exe.c \
79 79 ../src/tc_acceptance.c \
80 80 ../src/processing/fsw_processing.c \
81 81 ../src/processing/avf0_prc0.c \
82 82 ../src/processing/avf1_prc1.c \
83 83 ../src/processing/avf2_prc2.c \
84 84 ../src/lfr_cpu_usage_report.c \
85 85 ../LFR_basic-parameters/basic_parameters.c
86 86
87 87 HEADERS += \
88 88 ../header/wf_handler.h \
89 89 ../header/tc_handler.h \
90 90 ../header/grlib_regs.h \
91 91 ../header/fsw_misc.h \
92 92 ../header/fsw_init.h \
93 93 ../header/fsw_spacewire.h \
94 94 ../header/tc_load_dump_parameters.h \
95 95 ../header/tm_lfr_tc_exe.h \
96 96 ../header/tc_acceptance.h \
97 97 ../header/processing/fsw_processing.h \
98 98 ../header/processing/avf0_prc0.h \
99 99 ../header/processing/avf1_prc1.h \
100 100 ../header/processing/avf2_prc2.h \
101 101 ../header/fsw_params_wf_handler.h \
102 102 ../header/lfr_cpu_usage_report.h \
103 103 ../header/lfr_common_headers/ccsds_types.h \
104 104 ../header/lfr_common_headers/fsw_params.h \
105 105 ../header/lfr_common_headers/fsw_params_nb_bytes.h \
106 106 ../header/lfr_common_headers/fsw_params_processing.h \
107 107 ../header/lfr_common_headers/TC_types.h \
108 108 ../header/lfr_common_headers/tm_byte_positions.h \
109 109 ../LFR_basic-parameters/basic_parameters.h \
110 110 ../LFR_basic-parameters/basic_parameters_params.h \
111 111 ../header/GscMemoryLPP.hpp
112 112
Show file before | Show file after | Hide comments
@@ -1,309 +1,313
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> // printf()
9 9 #include <math.h>
10 10 #include <grlib_regs.h>
11 11
12 12 #include "fsw_params.h"
13 13 #include "fsw_spacewire.h"
14 14
15 15 typedef struct ring_node_asm
16 16 {
17 17 struct ring_node_asm *next;
18 18 float matrix[ TOTAL_SIZE_SM ];
19 19 unsigned int status;
20 20 } ring_node_asm;
21 21
22 22 typedef struct
23 23 {
24 24 unsigned char targetLogicalAddress;
25 25 unsigned char protocolIdentifier;
26 26 unsigned char reserved;
27 27 unsigned char userApplication;
28 28 unsigned char packetID[2];
29 29 unsigned char packetSequenceControl[2];
30 30 unsigned char packetLength[2];
31 31 // DATA FIELD HEADER
32 32 unsigned char spare1_pusVersion_spare2;
33 33 unsigned char serviceType;
34 34 unsigned char serviceSubType;
35 35 unsigned char destinationID;
36 36 unsigned char time[6];
37 37 // AUXILIARY HEADER
38 38 unsigned char sid;
39 39 unsigned char biaStatusInfo;
40 unsigned char sy_lfr_common_parameters_spare;
41 unsigned char sy_lfr_common_parameters;
40 42 unsigned char acquisitionTime[6];
41 43 unsigned char pa_lfr_bp_blk_nr[2];
42 44 // SOURCE DATA
43 45 unsigned char data[ 780 ]; // MAX size is 26 bins * 30 Bytes [TM_LFR_SCIENCE_BURST_BP2_F1]
44 46 } bp_packet;
45 47
46 48 typedef struct
47 49 {
48 50 unsigned char targetLogicalAddress;
49 51 unsigned char protocolIdentifier;
50 52 unsigned char reserved;
51 53 unsigned char userApplication;
52 54 unsigned char packetID[2];
53 55 unsigned char packetSequenceControl[2];
54 56 unsigned char packetLength[2];
55 57 // DATA FIELD HEADER
56 58 unsigned char spare1_pusVersion_spare2;
57 59 unsigned char serviceType;
58 60 unsigned char serviceSubType;
59 61 unsigned char destinationID;
60 62 unsigned char time[6];
61 63 // AUXILIARY HEADER
62 64 unsigned char sid;
63 65 unsigned char biaStatusInfo;
66 unsigned char sy_lfr_common_parameters_spare;
67 unsigned char sy_lfr_common_parameters;
64 68 unsigned char acquisitionTime[6];
65 69 unsigned char source_data_spare;
66 70 unsigned char pa_lfr_bp_blk_nr[2];
67 71 // SOURCE DATA
68 72 unsigned char data[ 117 ]; // 13 bins * 9 Bytes only for TM_LFR_SCIENCE_NORMAL_BP1_F0 and F1
69 73 } bp_packet_with_spare;
70 74
71 75 typedef struct
72 76 {
73 77 ring_node_asm *norm;
74 78 ring_node_asm *burst_sbm;
75 79 rtems_event_set event;
76 80 unsigned int coarseTimeNORM;
77 81 unsigned int fineTimeNORM;
78 82 unsigned int coarseTimeSBM;
79 83 unsigned int fineTimeSBM;
80 84 } asm_msg;
81 85
82 86 extern volatile int sm_f0[ ];
83 87 extern volatile int sm_f1[ ];
84 88 extern volatile int sm_f2[ ];
85 89
86 90 // parameters
87 91 extern struct param_local_str param_local;
88 92
89 93 // registers
90 94 extern time_management_regs_t *time_management_regs;
91 95 extern volatile spectral_matrix_regs_t *spectral_matrix_regs;
92 96
93 97 extern rtems_name misc_name[5];
94 98 extern rtems_id Task_id[20]; /* array of task ids */
95 99
96 100 //
97 101 ring_node * getRingNodeForAveraging( unsigned char frequencyChannel);
98 102 // ISR
99 103 rtems_isr spectral_matrices_isr( rtems_vector_number vector );
100 104 rtems_isr spectral_matrices_isr_simu( rtems_vector_number vector );
101 105
102 106 //******************
103 107 // Spectral Matrices
104 108 void reset_nb_sm( void );
105 109 // SM
106 110 void SM_init_rings( void );
107 111 void SM_reset_current_ring_nodes( void );
108 112 // ASM
109 113 void ASM_generic_init_ring(ring_node_asm *ring, unsigned char nbNodes );
110 114
111 115 //*****************
112 116 // Basic Parameters
113 117
114 118 void BP_reset_current_ring_nodes( void );
115 119 void BP_init_header(bp_packet *packet,
116 120 unsigned int apid, unsigned char sid,
117 121 unsigned int packetLength , unsigned char blkNr);
118 122 void BP_init_header_with_spare(bp_packet_with_spare *packet,
119 123 unsigned int apid, unsigned char sid,
120 124 unsigned int packetLength, unsigned char blkNr );
121 125 void BP_send( char *data,
122 126 rtems_id queue_id ,
123 127 unsigned int nbBytesToSend , unsigned int sid );
124 128
125 129 //******************
126 130 // general functions
127 131 void reset_sm_status( void );
128 132 void reset_spectral_matrix_regs( void );
129 133 void set_time(unsigned char *time, unsigned char *timeInBuffer );
130 134 unsigned long long int get_acquisition_time( unsigned char *timePtr );
131 135 unsigned char getSID( rtems_event_set event );
132 136
133 137 extern rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id );
134 138 extern rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id );
135 139
136 140 //***************************************
137 141 // DEFINITIONS OF STATIC INLINE FUNCTIONS
138 142 static inline void SM_average(float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
139 143 ring_node *ring_node_tab[],
140 144 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
141 145 asm_msg *msgForMATR );
142 146 static inline void SM_average_debug(float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
143 147 ring_node *ring_node_tab[],
144 148 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
145 149 asm_msg *msgForMATR );
146 150
147 151 void ASM_patch( float *inputASM, float *outputASM );
148 152 void extractReImVectors(float *inputASM, float *outputASM, unsigned int asmComponent );
149 153
150 154 static inline void ASM_reorganize_and_divide(float *averaged_spec_mat, float *averaged_spec_mat_reorganized,
151 155 float divider );
152 156 static inline void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat,
153 157 float divider,
154 158 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart);
155 159 static inline void ASM_convert(volatile float *input_matrix, char *output_matrix);
156 160
157 161 void SM_average( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
158 162 ring_node *ring_node_tab[],
159 163 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
160 164 asm_msg *msgForMATR )
161 165 {
162 166 float sum;
163 167 unsigned int i;
164 168
165 169 for(i=0; i<TOTAL_SIZE_SM; i++)
166 170 {
167 171 sum = ( (int *) (ring_node_tab[0]->buffer_address) ) [ i ]
168 172 + ( (int *) (ring_node_tab[1]->buffer_address) ) [ i ]
169 173 + ( (int *) (ring_node_tab[2]->buffer_address) ) [ i ]
170 174 + ( (int *) (ring_node_tab[3]->buffer_address) ) [ i ]
171 175 + ( (int *) (ring_node_tab[4]->buffer_address) ) [ i ]
172 176 + ( (int *) (ring_node_tab[5]->buffer_address) ) [ i ]
173 177 + ( (int *) (ring_node_tab[6]->buffer_address) ) [ i ]
174 178 + ( (int *) (ring_node_tab[7]->buffer_address) ) [ i ];
175 179
176 180 if ( (nbAverageNORM == 0) && (nbAverageSBM == 0) )
177 181 {
178 182 averaged_spec_mat_NORM[ i ] = sum;
179 183 averaged_spec_mat_SBM[ i ] = sum;
180 184 msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime;
181 185 msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime;
182 186 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
183 187 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
184 188 }
185 189 else if ( (nbAverageNORM != 0) && (nbAverageSBM != 0) )
186 190 {
187 191 averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum );
188 192 averaged_spec_mat_SBM[ i ] = ( averaged_spec_mat_SBM[ i ] + sum );
189 193 }
190 194 else if ( (nbAverageNORM != 0) && (nbAverageSBM == 0) )
191 195 {
192 196 averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum );
193 197 averaged_spec_mat_SBM[ i ] = sum;
194 198 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
195 199 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
196 200 }
197 201 else
198 202 {
199 203 PRINTF2("ERR *** in SM_average *** unexpected parameters %d %d\n", nbAverageNORM, nbAverageSBM)
200 204 }
201 205 }
202 206 }
203 207
204 208 void SM_average_debug( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
205 209 ring_node *ring_node_tab[],
206 210 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
207 211 asm_msg *msgForMATR )
208 212 {
209 213 float sum;
210 214 unsigned int i;
211 215
212 216 for(i=0; i<TOTAL_SIZE_SM; i++)
213 217 {
214 218 sum = ( (int *) (ring_node_tab[0]->buffer_address) ) [ i ];
215 219 averaged_spec_mat_NORM[ i ] = sum;
216 220 averaged_spec_mat_SBM[ i ] = sum;
217 221 msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime;
218 222 msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime;
219 223 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
220 224 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
221 225 }
222 226 }
223 227
224 228 void ASM_reorganize_and_divide( float *averaged_spec_mat, float *averaged_spec_mat_reorganized, float divider )
225 229 {
226 230 int frequencyBin;
227 231 int asmComponent;
228 232 unsigned int offsetASM;
229 233 unsigned int offsetASMReorganized;
230 234
231 235 // BUILD DATA
232 236 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
233 237 {
234 238 for( frequencyBin = 0; frequencyBin < NB_BINS_PER_SM; frequencyBin++ )
235 239 {
236 240 offsetASMReorganized =
237 241 frequencyBin * NB_VALUES_PER_SM
238 242 + asmComponent;
239 243 offsetASM =
240 244 asmComponent * NB_BINS_PER_SM
241 245 + frequencyBin;
242 246 averaged_spec_mat_reorganized[offsetASMReorganized ] =
243 247 averaged_spec_mat[ offsetASM ] / divider;
244 248 }
245 249 }
246 250 }
247 251
248 252 void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
249 253 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage, unsigned char ASMIndexStart )
250 254 {
251 255 int frequencyBin;
252 256 int asmComponent;
253 257 int offsetASM;
254 258 int offsetCompressed;
255 259 int k;
256 260
257 261 // BUILD DATA
258 262 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
259 263 {
260 264 for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
261 265 {
262 266 offsetCompressed = // NO TIME OFFSET
263 267 frequencyBin * NB_VALUES_PER_SM
264 268 + asmComponent;
265 269 offsetASM = // NO TIME OFFSET
266 270 asmComponent * NB_BINS_PER_SM
267 271 + ASMIndexStart
268 272 + frequencyBin * nbBinsToAverage;
269 273 compressed_spec_mat[ offsetCompressed ] = 0;
270 274 for ( k = 0; k < nbBinsToAverage; k++ )
271 275 {
272 276 compressed_spec_mat[offsetCompressed ] =
273 277 ( compressed_spec_mat[ offsetCompressed ]
274 278 + averaged_spec_mat[ offsetASM + k ] );
275 279 }
276 280 compressed_spec_mat[ offsetCompressed ] =
277 281 compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
278 282 }
279 283 }
280 284 }
281 285
282 286 void ASM_convert( volatile float *input_matrix, char *output_matrix)
283 287 {
284 288 unsigned int frequencyBin;
285 289 unsigned int asmComponent;
286 290 char * pt_char_input;
287 291 char * pt_char_output;
288 292 unsigned int offsetInput;
289 293 unsigned int offsetOutput;
290 294
291 295 pt_char_input = (char*) &input_matrix;
292 296 pt_char_output = (char*) &output_matrix;
293 297
294 298 // convert all other data
295 299 for( frequencyBin=0; frequencyBin<NB_BINS_PER_SM; frequencyBin++)
296 300 {
297 301 for ( asmComponent=0; asmComponent<NB_VALUES_PER_SM; asmComponent++)
298 302 {
299 303 offsetInput = (frequencyBin*NB_VALUES_PER_SM) + asmComponent ;
300 304 offsetOutput = 2 * ( (frequencyBin*NB_VALUES_PER_SM) + asmComponent ) ;
301 305 pt_char_input = (char*) &input_matrix [ offsetInput ];
302 306 pt_char_output = (char*) &output_matrix[ offsetOutput ];
303 307 pt_char_output[0] = pt_char_input[0]; // bits 31 downto 24 of the float
304 308 pt_char_output[1] = pt_char_input[1]; // bits 23 downto 16 of the float
305 309 }
306 310 }
307 311 }
308 312
309 313 #endif // FSW_PROCESSING_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -1,51 +1,54
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
12 12 #define FLOAT_EQUAL_ZERO 0.001
13 13
14 14 extern unsigned short sequenceCounterParameterDump;
15 15
16 16 int action_load_common_par( ccsdsTelecommandPacket_t *TC );
17 17 int action_load_normal_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
18 18 int action_load_burst_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
19 19 int action_load_sbm1_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
20 20 int action_load_sbm2_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
21 int action_load_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
22 int action_load_fbins_mask(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
23 int action_dump_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
21 24 int action_dump_par(rtems_id queue_id );
22 25
23 26 // NORMAL
24 27 int check_common_par_consistency( ccsdsTelecommandPacket_t *TC, rtems_id queue_id );
25 28 int set_sy_lfr_n_swf_l( ccsdsTelecommandPacket_t *TC );
26 29 int set_sy_lfr_n_swf_p( ccsdsTelecommandPacket_t *TC );
27 30 int set_sy_lfr_n_asm_p( ccsdsTelecommandPacket_t *TC );
28 31 int set_sy_lfr_n_bp_p0( ccsdsTelecommandPacket_t *TC );
29 32 int set_sy_lfr_n_bp_p1( ccsdsTelecommandPacket_t *TC );
30 33 int set_sy_lfr_n_cwf_long_f3( ccsdsTelecommandPacket_t *TC );
31 34
32 35 // BURST
33 36 int set_sy_lfr_b_bp_p0( ccsdsTelecommandPacket_t *TC );
34 37 int set_sy_lfr_b_bp_p1( ccsdsTelecommandPacket_t *TC );
35 38
36 39 // SBM1
37 40 int set_sy_lfr_s1_bp_p0( ccsdsTelecommandPacket_t *TC );
38 41 int set_sy_lfr_s1_bp_p1( ccsdsTelecommandPacket_t *TC );
39 42
40 43 // SBM2
41 44 int set_sy_lfr_s2_bp_p0( ccsdsTelecommandPacket_t *TC );
42 45 int set_sy_lfr_s2_bp_p1( ccsdsTelecommandPacket_t *TC );
43 46
44 47 // TC_LFR_UPDATE_INFO
45 48 unsigned int check_update_info_hk_lfr_mode( unsigned char mode );
46 49 unsigned int check_update_info_hk_tds_mode( unsigned char mode );
47 50 unsigned int check_update_info_hk_thr_mode( unsigned char mode );
48 51
49 52 void init_parameter_dump( void );
50 53
51 54 #endif // TC_LOAD_DUMP_PARAMETERS_H
Show file before | Show file after | Hide comments
@@ -1,13 +1,14
1 1 # LOAD FSW USING LINK 1
2 2 SpwPlugin0.StarDundeeSelectLinkNumber( 1 )
3 3
4 dsu3plugin0.openFile("/opt/DEV_PLE/FSW-qt/bin/fsw")
4 #dsu3plugin0.openFile("/opt/DEV_PLE/FSW-qt/bin/fsw")
5 dsu3plugin0.openFile("/opt/LFR/LFR-FSW/2.0.2.3/fsw")
5 6 dsu3plugin0.loadFile()
6 7
7 8 dsu3plugin0.run()
8 9
9 10 # START SENDING TIMECODES AT 1 Hz
10 11 SpwPlugin0.StarDundeeStartTimecodes( 1 )
11 12
12 13 # it is possible to change the time code frequency
13 14 #RMAPPlugin0.changeTimecodeFrequency(2)
Show file before | Show file after | Hide comments
@@ -1,74 +1,73
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 rtems_id misc_id[5];
28 27 rtems_name Task_name[20]; /* array of task names */
29 28 rtems_id Task_id[20]; /* array of task ids */
30 29 unsigned int maxCount;
31 30 int fdSPW = 0;
32 31 int fdUART = 0;
33 32 unsigned char lfrCurrentMode;
34 33
35 34 // WAVEFORMS GLOBAL VARIABLES // 2048 * 3 * 4 + 2 * 4 = 24576 + 8 bytes = 24584
36 35 // 97 * 256 = 24832 => delta = 248 bytes = 62 words
37 36 // WAVEFORMS GLOBAL VARIABLES // 2688 * 3 * 4 + 2 * 4 = 32256 + 8 bytes = 32264
38 37 // 127 * 256 = 32512 => delta = 248 bytes = 62 words
39 38 // F0 F1 F2 F3
40 39 volatile int wf_buffer_f0[ NB_RING_NODES_F0 * WFRM_BUFFER ] __attribute__((aligned(0x100)));
41 40 volatile int wf_buffer_f1[ NB_RING_NODES_F1 * WFRM_BUFFER ] __attribute__((aligned(0x100)));
42 41 volatile int wf_buffer_f2[ NB_RING_NODES_F2 * WFRM_BUFFER ] __attribute__((aligned(0x100)));
43 42 volatile int wf_buffer_f3[ NB_RING_NODES_F3 * WFRM_BUFFER ] __attribute__((aligned(0x100)));
44 43
45 44 //***********************************
46 45 // SPECTRAL MATRICES GLOBAL VARIABLES
47 46
48 47 // alignment constraints for the spectral matrices buffers => the first data after the time (8 bytes) shall be aligned on 0x00
49 48 volatile int sm_f0[ NB_RING_NODES_SM_F0 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100)));
50 49 volatile int sm_f1[ NB_RING_NODES_SM_F1 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100)));
51 50 volatile int sm_f2[ NB_RING_NODES_SM_F2 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100)));
52 51
53 52 // APB CONFIGURATION REGISTERS
54 53 time_management_regs_t *time_management_regs = (time_management_regs_t*) REGS_ADDR_TIME_MANAGEMENT;
55 54 gptimer_regs_t *gptimer_regs = (gptimer_regs_t *) REGS_ADDR_GPTIMER;
56 55 waveform_picker_regs_0_1_18_t *waveform_picker_regs = (waveform_picker_regs_0_1_18_t*) REGS_ADDR_WAVEFORM_PICKER;
57 56 spectral_matrix_regs_t *spectral_matrix_regs = (spectral_matrix_regs_t*) REGS_ADDR_SPECTRAL_MATRIX;
58 57
59 58 // MODE PARAMETERS
60 59 Packet_TM_LFR_PARAMETER_DUMP_t parameter_dump_packet;
61 60 struct param_local_str param_local;
62 61
63 62 // HK PACKETS
64 63 Packet_TM_LFR_HK_t housekeeping_packet;
65 64 // sequence counters are incremented by APID (PID + CAT) and destination ID
66 65 unsigned short sequenceCounters_SCIENCE_NORMAL_BURST;
67 66 unsigned short sequenceCounters_SCIENCE_SBM1_SBM2;
68 67 unsigned short sequenceCounters_TC_EXE[SEQ_CNT_NB_DEST_ID];
69 68 unsigned short sequenceCounterHK;
70 69 unsigned short sequenceCounterParameterDump;
71 70 spw_stats spacewire_stats;
72 71 spw_stats spacewire_stats_backup;
73 72
74 73
Show file before | Show file after | Hide comments
@@ -1,1117 +1,1126
1 1 /** Functions related to the SpaceWire interface.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle SpaceWire transmissions:
7 7 * - configuration of the SpaceWire link
8 8 * - SpaceWire related interruption requests processing
9 9 * - transmission of TeleMetry packets by a dedicated RTEMS task
10 10 * - reception of TeleCommands by a dedicated RTEMS task
11 11 *
12 12 */
13 13
14 14 #include "fsw_spacewire.h"
15 15
16 16 rtems_name semq_name;
17 17 rtems_id semq_id;
18 18
19 19 //*****************
20 20 // waveform headers
21 21 Header_TM_LFR_SCIENCE_CWF_t headerCWF;
22 22 Header_TM_LFR_SCIENCE_SWF_t headerSWF;
23 23 Header_TM_LFR_SCIENCE_ASM_t headerASM;
24 24
25 25 //***********
26 26 // RTEMS TASK
27 27 rtems_task spiq_task(rtems_task_argument unused)
28 28 {
29 29 /** This RTEMS task is awaken by an rtems_event sent by the interruption subroutine of the SpaceWire driver.
30 30 *
31 31 * @param unused is the starting argument of the RTEMS task
32 32 *
33 33 */
34 34
35 35 rtems_event_set event_out;
36 36 rtems_status_code status;
37 37 int linkStatus;
38 38
39 39 BOOT_PRINTF("in SPIQ *** \n")
40 40
41 41 while(true){
42 42 rtems_event_receive(SPW_LINKERR_EVENT, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an SPW_LINKERR_EVENT
43 43 PRINTF("in SPIQ *** got SPW_LINKERR_EVENT\n")
44 44
45 45 // [0] SUSPEND RECV AND SEND TASKS
46 46 status = rtems_task_suspend( Task_id[ TASKID_RECV ] );
47 47 if ( status != RTEMS_SUCCESSFUL ) {
48 48 PRINTF("in SPIQ *** ERR suspending RECV Task\n")
49 49 }
50 50 status = rtems_task_suspend( Task_id[ TASKID_SEND ] );
51 51 if ( status != RTEMS_SUCCESSFUL ) {
52 52 PRINTF("in SPIQ *** ERR suspending SEND Task\n")
53 53 }
54 54
55 55 // [1] CHECK THE LINK
56 56 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (1)
57 57 if ( linkStatus != 5) {
58 58 PRINTF1("in SPIQ *** linkStatus %d, wait...\n", linkStatus)
59 59 status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms
60 60 }
61 61
62 62 // [2] RECHECK THE LINK AFTER SY_LFR_DPU_CONNECT_TIMEOUT
63 63 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (2)
64 64 if ( linkStatus != 5 ) // [2.a] not in run state, reset the link
65 65 {
66 66 spacewire_compute_stats_offsets();
67 67 status = spacewire_reset_link( );
68 68 }
69 69 else // [2.b] in run state, start the link
70 70 {
71 71 status = spacewire_stop_and_start_link( fdSPW ); // start the link
72 72 if ( status != RTEMS_SUCCESSFUL)
73 73 {
74 74 PRINTF1("in SPIQ *** ERR spacewire_start_link %d\n", status)
75 75 }
76 76 }
77 77
78 78 // [3] COMPLETE RECOVERY ACTION AFTER SY_LFR_DPU_CONNECT_ATTEMPTS
79 79 if ( status == RTEMS_SUCCESSFUL ) // [3.a] the link is in run state and has been started successfully
80 80 {
81 81 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
82 82 if ( status != RTEMS_SUCCESSFUL ) {
83 83 PRINTF("in SPIQ *** ERR resuming SEND Task\n")
84 84 }
85 85 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
86 86 if ( status != RTEMS_SUCCESSFUL ) {
87 87 PRINTF("in SPIQ *** ERR resuming RECV Task\n")
88 88 }
89 89 }
90 90 else // [3.b] the link is not in run state, go in STANDBY mode
91 91 {
92 92 status = stop_current_mode();
93 93 if ( status != RTEMS_SUCCESSFUL ) {
94 94 PRINTF1("in SPIQ *** ERR stop_current_mode *** code %d\n", status)
95 95 }
96 96 status = enter_mode( LFR_MODE_STANDBY, 0 );
97 97 if ( status != RTEMS_SUCCESSFUL ) {
98 98 PRINTF1("in SPIQ *** ERR enter_standby_mode *** code %d\n", status)
99 99 }
100 100 // wake the WTDG task up to wait for the link recovery
101 101 status = rtems_event_send ( Task_id[TASKID_WTDG], RTEMS_EVENT_0 );
102 102 status = rtems_task_suspend( RTEMS_SELF );
103 103 }
104 104 }
105 105 }
106 106
107 107 rtems_task recv_task( rtems_task_argument unused )
108 108 {
109 109 /** This RTEMS task is dedicated to the reception of incoming TeleCommands.
110 110 *
111 111 * @param unused is the starting argument of the RTEMS task
112 112 *
113 113 * The RECV task blocks on a call to the read system call, waiting for incoming SpaceWire data. When unblocked:
114 114 * 1. It reads the incoming data.
115 115 * 2. Launches the acceptance procedure.
116 116 * 3. If the Telecommand is valid, sends it to a dedicated RTEMS message queue.
117 117 *
118 118 */
119 119
120 120 int len;
121 121 ccsdsTelecommandPacket_t currentTC;
122 122 unsigned char computed_CRC[ 2 ];
123 123 unsigned char currentTC_LEN_RCV[ 2 ];
124 124 unsigned char destinationID;
125 125 unsigned int estimatedPacketLength;
126 126 unsigned int parserCode;
127 127 rtems_status_code status;
128 128 rtems_id queue_recv_id;
129 129 rtems_id queue_send_id;
130 130
131 131 initLookUpTableForCRC(); // the table is used to compute Cyclic Redundancy Codes
132 132
133 133 status = get_message_queue_id_recv( &queue_recv_id );
134 134 if (status != RTEMS_SUCCESSFUL)
135 135 {
136 136 PRINTF1("in RECV *** ERR get_message_queue_id_recv %d\n", status)
137 137 }
138 138
139 139 status = get_message_queue_id_send( &queue_send_id );
140 140 if (status != RTEMS_SUCCESSFUL)
141 141 {
142 142 PRINTF1("in RECV *** ERR get_message_queue_id_send %d\n", status)
143 143 }
144 144
145 145 BOOT_PRINTF("in RECV *** \n")
146 146
147 147 while(1)
148 148 {
149 149 len = read( fdSPW, (char*) &currentTC, CCSDS_TC_PKT_MAX_SIZE ); // the call to read is blocking
150 150 if (len == -1){ // error during the read call
151 151 PRINTF1("in RECV *** last read call returned -1, ERRNO %d\n", errno)
152 152 }
153 153 else {
154 154 if ( (len+1) < CCSDS_TC_PKT_MIN_SIZE ) {
155 155 PRINTF("in RECV *** packet lenght too short\n")
156 156 }
157 157 else {
158 158 estimatedPacketLength = (unsigned int) (len - CCSDS_TC_TM_PACKET_OFFSET - 3); // => -3 is for Prot ID, Reserved and User App bytes
159 159 currentTC_LEN_RCV[ 0 ] = (unsigned char) (estimatedPacketLength >> 8);
160 160 currentTC_LEN_RCV[ 1 ] = (unsigned char) (estimatedPacketLength );
161 161 // CHECK THE TC
162 162 parserCode = tc_parser( &currentTC, estimatedPacketLength, computed_CRC ) ;
163 163 if ( (parserCode == ILLEGAL_APID) || (parserCode == WRONG_LEN_PKT)
164 164 || (parserCode == INCOR_CHECKSUM) || (parserCode == ILL_TYPE)
165 165 || (parserCode == ILL_SUBTYPE) || (parserCode == WRONG_APP_DATA)
166 166 || (parserCode == WRONG_SRC_ID) )
167 167 { // send TM_LFR_TC_EXE_CORRUPTED
168 168 PRINTF1("TC corrupted received, with code: %d\n", parserCode)
169 169 if ( !( (currentTC.serviceType==TC_TYPE_TIME) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_TIME) )
170 170 &&
171 171 !( (currentTC.serviceType==TC_TYPE_GEN) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_INFO))
172 172 )
173 173 {
174 174 if ( parserCode == WRONG_SRC_ID )
175 175 {
176 176 destinationID = SID_TC_GROUND;
177 177 }
178 178 else
179 179 {
180 180 destinationID = currentTC.sourceID;
181 181 }
182 182 send_tm_lfr_tc_exe_corrupted( &currentTC, queue_send_id,
183 183 computed_CRC, currentTC_LEN_RCV,
184 184 destinationID );
185 185 }
186 186 }
187 187 else
188 188 { // send valid TC to the action launcher
189 189 status = rtems_message_queue_send( queue_recv_id, &currentTC,
190 190 estimatedPacketLength + CCSDS_TC_TM_PACKET_OFFSET + 3);
191 191 }
192 192 }
193 193 }
194 194 }
195 195 }
196 196
197 197 rtems_task send_task( rtems_task_argument argument)
198 198 {
199 199 /** This RTEMS task is dedicated to the transmission of TeleMetry packets.
200 200 *
201 201 * @param unused is the starting argument of the RTEMS task
202 202 *
203 203 * The SEND task waits for a message to become available in the dedicated RTEMS queue. When a message arrives:
204 204 * - if the first byte is equal to CCSDS_DESTINATION_ID, the message is sent as is using the write system call.
205 205 * - if the first byte is not equal to CCSDS_DESTINATION_ID, the message is handled as a spw_ioctl_pkt_send. After
206 206 * analyzis, the packet is sent either using the write system call or using the ioctl call SPACEWIRE_IOCTRL_SEND, depending on the
207 207 * data it contains.
208 208 *
209 209 */
210 210
211 211 rtems_status_code status; // RTEMS status code
212 212 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
213 213 ring_node *incomingRingNodePtr;
214 214 int ring_node_address;
215 215 char *charPtr;
216 216 spw_ioctl_pkt_send *spw_ioctl_send;
217 217 size_t size; // size of the incoming TC packet
218 218 u_int32_t count;
219 219 rtems_id queue_id;
220 220 unsigned char sid;
221 221
222 222 incomingRingNodePtr = NULL;
223 223 ring_node_address = 0;
224 224 charPtr = (char *) &ring_node_address;
225 225 sid = 0;
226 226
227 227 init_header_cwf( &headerCWF );
228 228 init_header_swf( &headerSWF );
229 229 init_header_asm( &headerASM );
230 230
231 231 status = get_message_queue_id_send( &queue_id );
232 232 if (status != RTEMS_SUCCESSFUL)
233 233 {
234 234 PRINTF1("in HOUS *** ERR get_message_queue_id_send %d\n", status)
235 235 }
236 236
237 237 BOOT_PRINTF("in SEND *** \n")
238 238
239 239 while(1)
240 240 {
241 241 status = rtems_message_queue_receive( queue_id, incomingData, &size,
242 242 RTEMS_WAIT, RTEMS_NO_TIMEOUT );
243 243
244 244 if (status!=RTEMS_SUCCESSFUL)
245 245 {
246 246 PRINTF1("in SEND *** (1) ERR = %d\n", status)
247 247 }
248 248 else
249 249 {
250 250 if ( size == sizeof(ring_node*) )
251 251 {
252 252 charPtr[0] = incomingData[0];
253 253 charPtr[1] = incomingData[1];
254 254 charPtr[2] = incomingData[2];
255 255 charPtr[3] = incomingData[3];
256 256 incomingRingNodePtr = (ring_node*) ring_node_address;
257 257 sid = incomingRingNodePtr->sid;
258 258 if ( (sid==SID_NORM_CWF_LONG_F3)
259 259 || (sid==SID_BURST_CWF_F2 )
260 260 || (sid==SID_SBM1_CWF_F1 )
261 261 || (sid==SID_SBM2_CWF_F2 ))
262 262 {
263 263 spw_send_waveform_CWF( incomingRingNodePtr, &headerCWF );
264 264 }
265 265 else if ( (sid==SID_NORM_SWF_F0) || (sid== SID_NORM_SWF_F1) || (sid==SID_NORM_SWF_F2) )
266 266 {
267 267 spw_send_waveform_SWF( incomingRingNodePtr, &headerSWF );
268 268 }
269 269 else if ( (sid==SID_NORM_CWF_F3) )
270 270 {
271 271 spw_send_waveform_CWF3_light( incomingRingNodePtr, &headerCWF );
272 272 }
273 273 else if ( (sid==SID_NORM_ASM_F0) || (SID_NORM_ASM_F1) || (SID_NORM_ASM_F2) )
274 274 {
275 275 spw_send_asm( incomingRingNodePtr, &headerASM );
276 276 }
277 277 else
278 278 {
279 279 printf("unexpected sid = %d\n", sid);
280 280 }
281 281 }
282 282 else if ( incomingData[0] == CCSDS_DESTINATION_ID ) // the incoming message is a ccsds packet
283 283 {
284 284 status = write( fdSPW, incomingData, size );
285 285 if (status == -1){
286 286 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
287 287 }
288 288 }
289 289 else // the incoming message is a spw_ioctl_pkt_send structure
290 290 {
291 291 spw_ioctl_send = (spw_ioctl_pkt_send*) incomingData;
292 292 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, spw_ioctl_send );
293 293 if (status == -1){
294 294 printf("size = %d, %x, %x, %x, %x, %x\n",
295 295 size,
296 296 incomingData[0],
297 297 incomingData[1],
298 298 incomingData[2],
299 299 incomingData[3],
300 300 incomingData[4]);
301 301 PRINTF2("in SEND *** (2.b) ERRNO = %d, RTEMS = %d\n", errno, status)
302 302 }
303 303 }
304 304 }
305 305
306 306 status = rtems_message_queue_get_number_pending( queue_id, &count );
307 307 if (status != RTEMS_SUCCESSFUL)
308 308 {
309 309 PRINTF1("in SEND *** (3) ERR = %d\n", status)
310 310 }
311 311 else
312 312 {
313 313 if (count > maxCount)
314 314 {
315 315 maxCount = count;
316 316 }
317 317 }
318 318 }
319 319 }
320 320
321 321 rtems_task wtdg_task( rtems_task_argument argument )
322 322 {
323 323 rtems_event_set event_out;
324 324 rtems_status_code status;
325 325 int linkStatus;
326 326
327 327 BOOT_PRINTF("in WTDG ***\n")
328 328
329 329 while(1)
330 330 {
331 331 // wait for an RTEMS_EVENT
332 332 rtems_event_receive( RTEMS_EVENT_0,
333 333 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
334 334 PRINTF("in WTDG *** wait for the link\n")
335 335 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
336 336 while( linkStatus != 5) // wait for the link
337 337 {
338 338 status = rtems_task_wake_after( 10 ); // monitor the link each 100ms
339 339 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
340 340 }
341 341
342 342 status = spacewire_stop_and_start_link( fdSPW );
343 343
344 344 if (status != RTEMS_SUCCESSFUL)
345 345 {
346 346 PRINTF1("in WTDG *** ERR link not started %d\n", status)
347 347 }
348 348 else
349 349 {
350 350 PRINTF("in WTDG *** OK link started\n")
351 351 }
352 352
353 353 // restart the SPIQ task
354 354 status = rtems_task_restart( Task_id[TASKID_SPIQ], 1 );
355 355 if ( status != RTEMS_SUCCESSFUL ) {
356 356 PRINTF("in SPIQ *** ERR restarting SPIQ Task\n")
357 357 }
358 358
359 359 // restart RECV and SEND
360 360 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
361 361 if ( status != RTEMS_SUCCESSFUL ) {
362 362 PRINTF("in SPIQ *** ERR restarting SEND Task\n")
363 363 }
364 364 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
365 365 if ( status != RTEMS_SUCCESSFUL ) {
366 366 PRINTF("in SPIQ *** ERR restarting RECV Task\n")
367 367 }
368 368 }
369 369 }
370 370
371 371 //****************
372 372 // OTHER FUNCTIONS
373 373 int spacewire_open_link( void ) // by default, the driver resets the core: [SPW_CTRL_WRITE(pDev, SPW_CTRL_RESET);]
374 374 {
375 375 /** This function opens the SpaceWire link.
376 376 *
377 377 * @return a valid file descriptor in case of success, -1 in case of a failure
378 378 *
379 379 */
380 380 rtems_status_code status;
381 381
382 382 fdSPW = open(GRSPW_DEVICE_NAME, O_RDWR); // open the device. the open call resets the hardware
383 383 if ( fdSPW < 0 ) {
384 384 PRINTF1("ERR *** in configure_spw_link *** error opening "GRSPW_DEVICE_NAME" with ERR %d\n", errno)
385 385 }
386 386 else
387 387 {
388 388 status = RTEMS_SUCCESSFUL;
389 389 }
390 390
391 391 return status;
392 392 }
393 393
394 394 int spacewire_start_link( int fd )
395 395 {
396 396 rtems_status_code status;
397 397
398 398 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
399 399 // -1 default hardcoded driver timeout
400 400
401 401 return status;
402 402 }
403 403
404 404 int spacewire_stop_and_start_link( int fd )
405 405 {
406 406 rtems_status_code status;
407 407
408 408 status = ioctl( fd, SPACEWIRE_IOCTRL_STOP); // start fails if link pDev->running != 0
409 409 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
410 410 // -1 default hardcoded driver timeout
411 411
412 412 return status;
413 413 }
414 414
415 415 int spacewire_configure_link( int fd )
416 416 {
417 417 /** This function configures the SpaceWire link.
418 418 *
419 419 * @return GR-RTEMS-DRIVER directive status codes:
420 420 * - 22 EINVAL - Null pointer or an out of range value was given as the argument.
421 421 * - 16 EBUSY - Only used for SEND. Returned when no descriptors are avialble in non-blocking mode.
422 422 * - 88 ENOSYS - Returned for SET_DESTKEY if RMAP command handler is not available or if a non-implemented call is used.
423 423 * - 116 ETIMEDOUT - REturned for SET_PACKET_SIZE and START if the link could not be brought up.
424 424 * - 12 ENOMEM - Returned for SET_PACKETSIZE if it was unable to allocate the new buffers.
425 425 * - 5 EIO - Error when writing to grswp hardware registers.
426 426 * - 2 ENOENT - No such file or directory
427 427 */
428 428
429 429 rtems_status_code status;
430 430
431 431 spacewire_set_NP(1, REGS_ADDR_GRSPW); // [N]o [P]ort force
432 432 spacewire_set_RE(1, REGS_ADDR_GRSPW); // [R]MAP [E]nable, the dedicated call seems to break the no port force configuration
433 433
434 434 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_RXBLOCK, 1); // sets the blocking mode for reception
435 435 if (status!=RTEMS_SUCCESSFUL) {
436 436 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_RXBLOCK\n")
437 437 }
438 438 //
439 439 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_EVENT_ID, Task_id[TASKID_SPIQ]); // sets the task ID to which an event is sent when a
440 440 if (status!=RTEMS_SUCCESSFUL) {
441 441 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_EVENT_ID\n") // link-error interrupt occurs
442 442 }
443 443 //
444 444 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_DISABLE_ERR, 0); // automatic link-disabling due to link-error interrupts
445 445 if (status!=RTEMS_SUCCESSFUL) {
446 446 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_DISABLE_ERR\n")
447 447 }
448 448 //
449 449 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ, 1); // sets the link-error interrupt bit
450 450 if (status!=RTEMS_SUCCESSFUL) {
451 451 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ\n")
452 452 }
453 453 //
454 454 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK, 1); // transmission blocks
455 455 if (status!=RTEMS_SUCCESSFUL) {
456 456 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK\n")
457 457 }
458 458 //
459 459 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL, 1); // transmission blocks when no transmission descriptor is available
460 460 if (status!=RTEMS_SUCCESSFUL) {
461 461 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL\n")
462 462 }
463 463 //
464 464 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TCODE_CTRL, 0x0909); // [Time Rx : Time Tx : Link error : Tick-out IRQ]
465 465 if (status!=RTEMS_SUCCESSFUL) {
466 466 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TCODE_CTRL,\n")
467 467 }
468 468
469 469 return status;
470 470 }
471 471
472 472 int spacewire_reset_link( void )
473 473 {
474 474 /** This function is executed by the SPIQ rtems_task wehn it has been awaken by an interruption raised by the SpaceWire driver.
475 475 *
476 476 * @return RTEMS directive status code:
477 477 * - RTEMS_UNSATISFIED is returned is the link is not in the running state after 10 s.
478 478 * - RTEMS_SUCCESSFUL is returned if the link is up before the timeout.
479 479 *
480 480 */
481 481
482 482 rtems_status_code status_spw;
483 483 rtems_status_code status;
484 484 int i;
485 485
486 486 for ( i=0; i<SY_LFR_DPU_CONNECT_ATTEMPT; i++ )
487 487 {
488 488 PRINTF1("in spacewire_reset_link *** link recovery, try %d\n", i);
489 489
490 490 // CLOSING THE DRIVER AT THIS POINT WILL MAKE THE SEND TASK BLOCK THE SYSTEM
491 491
492 492 status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms
493 493
494 494 status_spw = spacewire_stop_and_start_link( fdSPW );
495 495 if ( status_spw != RTEMS_SUCCESSFUL )
496 496 {
497 497 PRINTF1("in spacewire_reset_link *** ERR spacewire_start_link code %d\n", status_spw)
498 498 }
499 499
500 500 if ( status_spw == RTEMS_SUCCESSFUL)
501 501 {
502 502 break;
503 503 }
504 504 }
505 505
506 506 return status_spw;
507 507 }
508 508
509 509 void spacewire_set_NP( unsigned char val, unsigned int regAddr ) // [N]o [P]ort force
510 510 {
511 511 /** This function sets the [N]o [P]ort force bit of the GRSPW control register.
512 512 *
513 513 * @param val is the value, 0 or 1, used to set the value of the NP bit.
514 514 * @param regAddr is the address of the GRSPW control register.
515 515 *
516 516 * NP is the bit 20 of the GRSPW control register.
517 517 *
518 518 */
519 519
520 520 unsigned int *spwptr = (unsigned int*) regAddr;
521 521
522 522 if (val == 1) {
523 523 *spwptr = *spwptr | 0x00100000; // [NP] set the No port force bit
524 524 }
525 525 if (val== 0) {
526 526 *spwptr = *spwptr & 0xffdfffff;
527 527 }
528 528 }
529 529
530 530 void spacewire_set_RE( unsigned char val, unsigned int regAddr ) // [R]MAP [E]nable
531 531 {
532 532 /** This function sets the [R]MAP [E]nable bit of the GRSPW control register.
533 533 *
534 534 * @param val is the value, 0 or 1, used to set the value of the RE bit.
535 535 * @param regAddr is the address of the GRSPW control register.
536 536 *
537 537 * RE is the bit 16 of the GRSPW control register.
538 538 *
539 539 */
540 540
541 541 unsigned int *spwptr = (unsigned int*) regAddr;
542 542
543 543 if (val == 1)
544 544 {
545 545 *spwptr = *spwptr | 0x00010000; // [RE] set the RMAP Enable bit
546 546 }
547 547 if (val== 0)
548 548 {
549 549 *spwptr = *spwptr & 0xfffdffff;
550 550 }
551 551 }
552 552
553 553 void spacewire_compute_stats_offsets( void )
554 554 {
555 555 /** This function computes the SpaceWire statistics offsets in case of a SpaceWire related interruption raising.
556 556 *
557 557 * The offsets keep a record of the statistics in case of a reset of the statistics. They are added to the current statistics
558 558 * to keep the counters consistent even after a reset of the SpaceWire driver (the counter are set to zero by the driver when it
559 559 * during the open systel call).
560 560 *
561 561 */
562 562
563 563 spw_stats spacewire_stats_grspw;
564 564 rtems_status_code status;
565 565
566 566 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &spacewire_stats_grspw );
567 567
568 568 spacewire_stats_backup.packets_received = spacewire_stats_grspw.packets_received
569 569 + spacewire_stats.packets_received;
570 570 spacewire_stats_backup.packets_sent = spacewire_stats_grspw.packets_sent
571 571 + spacewire_stats.packets_sent;
572 572 spacewire_stats_backup.parity_err = spacewire_stats_grspw.parity_err
573 573 + spacewire_stats.parity_err;
574 574 spacewire_stats_backup.disconnect_err = spacewire_stats_grspw.disconnect_err
575 575 + spacewire_stats.disconnect_err;
576 576 spacewire_stats_backup.escape_err = spacewire_stats_grspw.escape_err
577 577 + spacewire_stats.escape_err;
578 578 spacewire_stats_backup.credit_err = spacewire_stats_grspw.credit_err
579 579 + spacewire_stats.credit_err;
580 580 spacewire_stats_backup.write_sync_err = spacewire_stats_grspw.write_sync_err
581 581 + spacewire_stats.write_sync_err;
582 582 spacewire_stats_backup.rx_rmap_header_crc_err = spacewire_stats_grspw.rx_rmap_header_crc_err
583 583 + spacewire_stats.rx_rmap_header_crc_err;
584 584 spacewire_stats_backup.rx_rmap_data_crc_err = spacewire_stats_grspw.rx_rmap_data_crc_err
585 585 + spacewire_stats.rx_rmap_data_crc_err;
586 586 spacewire_stats_backup.early_ep = spacewire_stats_grspw.early_ep
587 587 + spacewire_stats.early_ep;
588 588 spacewire_stats_backup.invalid_address = spacewire_stats_grspw.invalid_address
589 589 + spacewire_stats.invalid_address;
590 590 spacewire_stats_backup.rx_eep_err = spacewire_stats_grspw.rx_eep_err
591 591 + spacewire_stats.rx_eep_err;
592 592 spacewire_stats_backup.rx_truncated = spacewire_stats_grspw.rx_truncated
593 593 + spacewire_stats.rx_truncated;
594 594 }
595 595
596 596 void spacewire_update_statistics( void )
597 597 {
598 598 rtems_status_code status;
599 599 spw_stats spacewire_stats_grspw;
600 600
601 601 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &spacewire_stats_grspw );
602 602
603 603 spacewire_stats.packets_received = spacewire_stats_backup.packets_received
604 604 + spacewire_stats_grspw.packets_received;
605 605 spacewire_stats.packets_sent = spacewire_stats_backup.packets_sent
606 606 + spacewire_stats_grspw.packets_sent;
607 607 spacewire_stats.parity_err = spacewire_stats_backup.parity_err
608 608 + spacewire_stats_grspw.parity_err;
609 609 spacewire_stats.disconnect_err = spacewire_stats_backup.disconnect_err
610 610 + spacewire_stats_grspw.disconnect_err;
611 611 spacewire_stats.escape_err = spacewire_stats_backup.escape_err
612 612 + spacewire_stats_grspw.escape_err;
613 613 spacewire_stats.credit_err = spacewire_stats_backup.credit_err
614 614 + spacewire_stats_grspw.credit_err;
615 615 spacewire_stats.write_sync_err = spacewire_stats_backup.write_sync_err
616 616 + spacewire_stats_grspw.write_sync_err;
617 617 spacewire_stats.rx_rmap_header_crc_err = spacewire_stats_backup.rx_rmap_header_crc_err
618 618 + spacewire_stats_grspw.rx_rmap_header_crc_err;
619 619 spacewire_stats.rx_rmap_data_crc_err = spacewire_stats_backup.rx_rmap_data_crc_err
620 620 + spacewire_stats_grspw.rx_rmap_data_crc_err;
621 621 spacewire_stats.early_ep = spacewire_stats_backup.early_ep
622 622 + spacewire_stats_grspw.early_ep;
623 623 spacewire_stats.invalid_address = spacewire_stats_backup.invalid_address
624 624 + spacewire_stats_grspw.invalid_address;
625 625 spacewire_stats.rx_eep_err = spacewire_stats_backup.rx_eep_err
626 626 + spacewire_stats_grspw.rx_eep_err;
627 627 spacewire_stats.rx_truncated = spacewire_stats_backup.rx_truncated
628 628 + spacewire_stats_grspw.rx_truncated;
629 629 //spacewire_stats.tx_link_err;
630 630
631 631 //****************************
632 632 // DPU_SPACEWIRE_IF_STATISTICS
633 633 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[0] = (unsigned char) (spacewire_stats.packets_received >> 8);
634 634 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[1] = (unsigned char) (spacewire_stats.packets_received);
635 635 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[0] = (unsigned char) (spacewire_stats.packets_sent >> 8);
636 636 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[1] = (unsigned char) (spacewire_stats.packets_sent);
637 637 //housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt;
638 638 //housekeeping_packet.hk_lfr_dpu_spw_last_timc;
639 639
640 640 //******************************************
641 641 // ERROR COUNTERS / SPACEWIRE / LOW SEVERITY
642 642 housekeeping_packet.hk_lfr_dpu_spw_parity = (unsigned char) spacewire_stats.parity_err;
643 643 housekeeping_packet.hk_lfr_dpu_spw_disconnect = (unsigned char) spacewire_stats.disconnect_err;
644 644 housekeeping_packet.hk_lfr_dpu_spw_escape = (unsigned char) spacewire_stats.escape_err;
645 645 housekeeping_packet.hk_lfr_dpu_spw_credit = (unsigned char) spacewire_stats.credit_err;
646 646 housekeeping_packet.hk_lfr_dpu_spw_write_sync = (unsigned char) spacewire_stats.write_sync_err;
647 647
648 648 //*********************************************
649 649 // ERROR COUNTERS / SPACEWIRE / MEDIUM SEVERITY
650 650 housekeeping_packet.hk_lfr_dpu_spw_early_eop = (unsigned char) spacewire_stats.early_ep;
651 651 housekeeping_packet.hk_lfr_dpu_spw_invalid_addr = (unsigned char) spacewire_stats.invalid_address;
652 652 housekeeping_packet.hk_lfr_dpu_spw_eep = (unsigned char) spacewire_stats.rx_eep_err;
653 653 housekeeping_packet.hk_lfr_dpu_spw_rx_too_big = (unsigned char) spacewire_stats.rx_truncated;
654 654 }
655 655
656 656 void timecode_irq_handler( void *pDev, void *regs, int minor, unsigned int tc )
657 657 {
658 658 // a valid timecode has been received, write it in the HK report
659 659 unsigned int * grspwPtr;
660 660
661 661 grspwPtr = (unsigned int *) (REGS_ADDR_GRSPW + APB_OFFSET_GRSPW_TIME_REGISTER);
662 662
663 663 housekeeping_packet.hk_lfr_dpu_spw_last_timc = (unsigned char) (grspwPtr[0] & 0xff); // [11 1111]
664 664
665 665 // update the number of valid timecodes that have been received
666 666 if (housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt == 255)
667 667 {
668 668 housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt = 0;
669 669 }
670 670 else
671 671 {
672 672 housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt = housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt + 1;
673 673 }
674 674 }
675 675
676 676 rtems_timer_service_routine user_routine( rtems_id timer_id, void *user_data )
677 677 {
678 678 int linkStatus;
679 679 rtems_status_code status;
680 680
681 681 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
682 682
683 683 if ( linkStatus == 5) {
684 684 PRINTF("in spacewire_reset_link *** link is running\n")
685 685 status = RTEMS_SUCCESSFUL;
686 686 }
687 687 }
688 688
689 689 void init_header_cwf( Header_TM_LFR_SCIENCE_CWF_t *header )
690 690 {
691 691 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
692 692 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
693 693 header->reserved = DEFAULT_RESERVED;
694 694 header->userApplication = CCSDS_USER_APP;
695 695 header->packetSequenceControl[0]= TM_PACKET_SEQ_CTRL_STANDALONE;
696 696 header->packetSequenceControl[1]= TM_PACKET_SEQ_CNT_DEFAULT;
697 697 header->packetLength[0] = 0x00;
698 698 header->packetLength[1] = 0x00;
699 699 // DATA FIELD HEADER
700 700 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
701 701 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
702 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE; // service subtype
702 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; // service subtype
703 703 header->destinationID = TM_DESTINATION_ID_GROUND;
704 704 header->time[0] = 0x00;
705 705 header->time[0] = 0x00;
706 706 header->time[0] = 0x00;
707 707 header->time[0] = 0x00;
708 708 header->time[0] = 0x00;
709 709 header->time[0] = 0x00;
710 710 // AUXILIARY DATA HEADER
711 711 header->sid = 0x00;
712 712 header->hkBIA = DEFAULT_HKBIA;
713 713 header->blkNr[0] = 0x00;
714 714 header->blkNr[1] = 0x00;
715 715 }
716 716
717 717 void init_header_swf( Header_TM_LFR_SCIENCE_SWF_t *header )
718 718 {
719 719 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
720 720 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
721 721 header->reserved = DEFAULT_RESERVED;
722 722 header->userApplication = CCSDS_USER_APP;
723 723 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
724 724 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
725 725 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
726 726 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
727 727 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8);
728 728 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
729 729 // DATA FIELD HEADER
730 730 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
731 731 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
732 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE; // service subtype
732 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
733 733 header->destinationID = TM_DESTINATION_ID_GROUND;
734 734 header->time[0] = 0x00;
735 735 header->time[0] = 0x00;
736 736 header->time[0] = 0x00;
737 737 header->time[0] = 0x00;
738 738 header->time[0] = 0x00;
739 739 header->time[0] = 0x00;
740 740 // AUXILIARY DATA HEADER
741 741 header->sid = 0x00;
742 742 header->hkBIA = DEFAULT_HKBIA;
743 743 header->pktCnt = DEFAULT_PKTCNT; // PKT_CNT
744 744 header->pktNr = 0x00;
745 745 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8);
746 746 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
747 747 }
748 748
749 749 void init_header_asm( Header_TM_LFR_SCIENCE_ASM_t *header )
750 750 {
751 751 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
752 752 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
753 753 header->reserved = DEFAULT_RESERVED;
754 754 header->userApplication = CCSDS_USER_APP;
755 755 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
756 756 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
757 757 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
758 758 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
759 759 header->packetLength[0] = 0x00;
760 760 header->packetLength[1] = 0x00;
761 761 // DATA FIELD HEADER
762 762 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
763 763 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
764 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE; // service subtype
764 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
765 765 header->destinationID = TM_DESTINATION_ID_GROUND;
766 766 header->time[0] = 0x00;
767 767 header->time[0] = 0x00;
768 768 header->time[0] = 0x00;
769 769 header->time[0] = 0x00;
770 770 header->time[0] = 0x00;
771 771 header->time[0] = 0x00;
772 772 // AUXILIARY DATA HEADER
773 773 header->sid = 0x00;
774 774 header->biaStatusInfo = 0x00;
775 775 header->pa_lfr_pkt_cnt_asm = 0x00;
776 776 header->pa_lfr_pkt_nr_asm = 0x00;
777 777 header->pa_lfr_asm_blk_nr[0] = 0x00;
778 778 header->pa_lfr_asm_blk_nr[1] = 0x00;
779 779 }
780 780
781 781 int spw_send_waveform_CWF( ring_node *ring_node_to_send,
782 782 Header_TM_LFR_SCIENCE_CWF_t *header )
783 783 {
784 784 /** This function sends CWF CCSDS packets (F2, F1 or F0).
785 785 *
786 786 * @param waveform points to the buffer containing the data that will be send.
787 787 * @param sid is the source identifier of the data that will be sent.
788 788 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
789 789 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
790 790 * contain information to setup the transmission of the data packets.
791 791 *
792 792 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
793 793 *
794 794 */
795 795
796 796 unsigned int i;
797 797 int ret;
798 798 unsigned int coarseTime;
799 799 unsigned int fineTime;
800 800 rtems_status_code status;
801 801 spw_ioctl_pkt_send spw_ioctl_send_CWF;
802 802 int *dataPtr;
803 803 unsigned char sid;
804 804
805 805 spw_ioctl_send_CWF.hlen = TM_HEADER_LEN + 4 + 10; // + 4 is for the protocole extra header, + 10 is for the auxiliary header
806 806 spw_ioctl_send_CWF.options = 0;
807 807
808 808 ret = LFR_DEFAULT;
809 809 sid = (unsigned char) ring_node_to_send->sid;
810 810
811 811 coarseTime = ring_node_to_send->coarseTime;
812 812 fineTime = ring_node_to_send->fineTime;
813 813 dataPtr = (int*) ring_node_to_send->buffer_address;
814 814
815 815 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8);
816 816 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
817 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
817 818 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8);
818 819 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
819 820
820 821 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF; i++) // send waveform
821 822 {
822 823 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF * NB_WORDS_SWF_BLK) ];
823 824 spw_ioctl_send_CWF.hdr = (char*) header;
824 825 // BUILD THE DATA
825 826 spw_ioctl_send_CWF.dlen = BLK_NR_CWF * NB_BYTES_SWF_BLK;
826 827
827 828 // SET PACKET SEQUENCE CONTROL
828 829 increment_seq_counter_source_id( header->packetSequenceControl, sid );
829 830
830 831 // SET SID
831 832 header->sid = sid;
832 833
833 834 // SET PACKET TIME
834 835 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime);
835 836 //
836 837 header->time[0] = header->acquisitionTime[0];
837 838 header->time[1] = header->acquisitionTime[1];
838 839 header->time[2] = header->acquisitionTime[2];
839 840 header->time[3] = header->acquisitionTime[3];
840 841 header->time[4] = header->acquisitionTime[4];
841 842 header->time[5] = header->acquisitionTime[5];
842 843
843 844 // SET PACKET ID
844 845 if ( (sid == SID_SBM1_CWF_F1) || (sid == SID_SBM2_CWF_F2) )
845 846 {
846 847 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2 >> 8);
847 848 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2);
848 849 }
849 850 else
850 851 {
851 852 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
852 853 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
853 854 }
854 855
855 856 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
856 857 if (status != RTEMS_SUCCESSFUL) {
857 858 printf("%d-%d, ERR %d\n", sid, i, (int) status);
858 859 ret = LFR_DEFAULT;
859 860 }
860 861 }
861 862
862 863 return ret;
863 864 }
864 865
865 866 int spw_send_waveform_SWF( ring_node *ring_node_to_send,
866 867 Header_TM_LFR_SCIENCE_SWF_t *header )
867 868 {
868 869 /** This function sends SWF CCSDS packets (F2, F1 or F0).
869 870 *
870 871 * @param waveform points to the buffer containing the data that will be send.
871 872 * @param sid is the source identifier of the data that will be sent.
872 873 * @param headerSWF points to a table of headers that have been prepared for the data transmission.
873 874 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
874 875 * contain information to setup the transmission of the data packets.
875 876 *
876 877 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
877 878 *
878 879 */
879 880
880 881 unsigned int i;
881 882 int ret;
882 883 unsigned int coarseTime;
883 884 unsigned int fineTime;
884 885 rtems_status_code status;
885 886 spw_ioctl_pkt_send spw_ioctl_send_SWF;
886 887 int *dataPtr;
887 888 unsigned char sid;
888 889
889 890 spw_ioctl_send_SWF.hlen = TM_HEADER_LEN + 4 + 12; // + 4 is for the protocole extra header, + 12 is for the auxiliary header
890 891 spw_ioctl_send_SWF.options = 0;
891 892
892 893 ret = LFR_DEFAULT;
893 894
894 895 coarseTime = ring_node_to_send->coarseTime;
895 896 fineTime = ring_node_to_send->fineTime;
896 897 dataPtr = (int*) ring_node_to_send->buffer_address;
897 898 sid = ring_node_to_send->sid;
898 899
900 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
901
899 902 for (i=0; i<7; i++) // send waveform
900 903 {
901 904 spw_ioctl_send_SWF.data = (char*) &dataPtr[ (i * BLK_NR_304 * NB_WORDS_SWF_BLK) ];
902 905 spw_ioctl_send_SWF.hdr = (char*) header;
903 906
904 907 // SET PACKET SEQUENCE CONTROL
905 908 increment_seq_counter_source_id( header->packetSequenceControl, sid );
906 909
907 910 // SET PACKET LENGTH AND BLKNR
908 911 if (i == 6)
909 912 {
910 913 spw_ioctl_send_SWF.dlen = BLK_NR_224 * NB_BYTES_SWF_BLK;
911 914 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_224 >> 8);
912 915 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_224 );
913 916 header->blkNr[0] = (unsigned char) (BLK_NR_224 >> 8);
914 917 header->blkNr[1] = (unsigned char) (BLK_NR_224 );
915 918 }
916 919 else
917 920 {
918 921 spw_ioctl_send_SWF.dlen = BLK_NR_304 * NB_BYTES_SWF_BLK;
919 922 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_304 >> 8);
920 923 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_304 );
921 924 header->blkNr[0] = (unsigned char) (BLK_NR_304 >> 8);
922 925 header->blkNr[1] = (unsigned char) (BLK_NR_304 );
923 926 }
924 927
925 928 // SET PACKET TIME
926 929 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime );
927 930 //
928 931 header->time[0] = header->acquisitionTime[0];
929 932 header->time[1] = header->acquisitionTime[1];
930 933 header->time[2] = header->acquisitionTime[2];
931 934 header->time[3] = header->acquisitionTime[3];
932 935 header->time[4] = header->acquisitionTime[4];
933 936 header->time[5] = header->acquisitionTime[5];
934 937
935 938 // SET SID
936 939 header->sid = sid;
937 940
938 941 // SET PKTNR
939 942 header->pktNr = i+1; // PKT_NR
940 943
941 944 // SEND PACKET
942 945 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_SWF );
943 946 if (status != RTEMS_SUCCESSFUL) {
944 947 printf("%d-%d, ERR %d\n", sid, i, (int) status);
945 948 ret = LFR_DEFAULT;
946 949 }
947 950 }
948 951
949 952 return ret;
950 953 }
951 954
952 955 int spw_send_waveform_CWF3_light( ring_node *ring_node_to_send,
953 956 Header_TM_LFR_SCIENCE_CWF_t *header )
954 957 {
955 958 /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data.
956 959 *
957 960 * @param waveform points to the buffer containing the data that will be send.
958 961 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
959 962 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
960 963 * contain information to setup the transmission of the data packets.
961 964 *
962 965 * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer
963 966 * from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks.
964 967 *
965 968 */
966 969
967 970 unsigned int i;
968 971 int ret;
969 972 unsigned int coarseTime;
970 973 unsigned int fineTime;
971 974 rtems_status_code status;
972 975 spw_ioctl_pkt_send spw_ioctl_send_CWF;
973 976 char *dataPtr;
974 977 unsigned char sid;
975 978
976 979 spw_ioctl_send_CWF.hlen = TM_HEADER_LEN + 4 + 10; // + 4 is for the protocole extra header, + 10 is for the auxiliary header
977 980 spw_ioctl_send_CWF.options = 0;
978 981
979 982 ret = LFR_DEFAULT;
980 983 sid = ring_node_to_send->sid;
981 984
982 985 coarseTime = ring_node_to_send->coarseTime;
983 986 fineTime = ring_node_to_send->fineTime;
984 987 dataPtr = (char*) ring_node_to_send->buffer_address;
985 988
986 989 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_672 >> 8);
987 990 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_672 );
991 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
988 992 header->blkNr[0] = (unsigned char) (BLK_NR_CWF_SHORT_F3 >> 8);
989 993 header->blkNr[1] = (unsigned char) (BLK_NR_CWF_SHORT_F3 );
990 994
991 995 //*********************
992 996 // SEND CWF3_light DATA
993 997 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF_LIGHT; i++) // send waveform
994 998 {
995 999 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK) ];
996 1000 spw_ioctl_send_CWF.hdr = (char*) header;
997 1001 // BUILD THE DATA
998 1002 spw_ioctl_send_CWF.dlen = BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK;
999 1003
1000 1004 // SET PACKET SEQUENCE COUNTER
1001 1005 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1002 1006
1003 1007 // SET SID
1004 1008 header->sid = sid;
1005 1009
1006 1010 // SET PACKET TIME
1007 1011 compute_acquisition_time( coarseTime, fineTime, SID_NORM_CWF_F3, i, header->acquisitionTime );
1008 1012 //
1009 1013 header->time[0] = header->acquisitionTime[0];
1010 1014 header->time[1] = header->acquisitionTime[1];
1011 1015 header->time[2] = header->acquisitionTime[2];
1012 1016 header->time[3] = header->acquisitionTime[3];
1013 1017 header->time[4] = header->acquisitionTime[4];
1014 1018 header->time[5] = header->acquisitionTime[5];
1015 1019
1016 1020 // SET PACKET ID
1017 1021 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
1018 1022 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1019 1023
1020 1024 // SEND PACKET
1021 1025 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
1022 1026 if (status != RTEMS_SUCCESSFUL) {
1023 1027 printf("%d-%d, ERR %d\n", sid, i, (int) status);
1024 1028 ret = LFR_DEFAULT;
1025 1029 }
1026 1030 }
1027 1031
1028 1032 return ret;
1029 1033 }
1030 1034
1031 1035 void spw_send_asm( ring_node *ring_node_to_send,
1032 1036 Header_TM_LFR_SCIENCE_ASM_t *header )
1033 1037 {
1034 1038 unsigned int i;
1035 1039 unsigned int length = 0;
1036 1040 rtems_status_code status;
1037 1041 unsigned int sid;
1038 1042 char *spectral_matrix;
1039 1043 int coarseTime;
1040 1044 int fineTime;
1041 1045 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1042 1046
1043 1047 sid = ring_node_to_send->sid;
1044 1048 spectral_matrix = (char*) ring_node_to_send->buffer_address;
1045 1049 coarseTime = ring_node_to_send->coarseTime;
1046 1050 fineTime = ring_node_to_send->fineTime;
1047 1051
1052 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1053
1048 1054 for (i=0; i<2; i++)
1049 1055 {
1050 1056 // (1) BUILD THE DATA
1051 1057 switch(sid)
1052 1058 {
1053 1059 case SID_NORM_ASM_F0:
1054 1060 spw_ioctl_send_ASM.dlen = TOTAL_SIZE_ASM_F0_IN_BYTES / 2; // 2 packets will be sent
1055 1061 spw_ioctl_send_ASM.data = &spectral_matrix[
1056 1062 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0) ) * NB_VALUES_PER_SM ) * 2
1057 1063 ];
1058 1064 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0;
1065 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1059 1066 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0) >> 8 ); // BLK_NR MSB
1060 1067 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0); // BLK_NR LSB
1061 1068 break;
1062 1069 case SID_NORM_ASM_F1:
1063 1070 spw_ioctl_send_ASM.dlen = TOTAL_SIZE_ASM_F1_IN_BYTES / 2; // 2 packets will be sent
1064 1071 spw_ioctl_send_ASM.data = &spectral_matrix[
1065 1072 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1) ) * NB_VALUES_PER_SM ) * 2
1066 1073 ];
1067 1074 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1;
1075 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1068 1076 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1) >> 8 ); // BLK_NR MSB
1069 1077 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1); // BLK_NR LSB
1070 1078 break;
1071 1079 case SID_NORM_ASM_F2:
1072 1080 spw_ioctl_send_ASM.dlen = TOTAL_SIZE_ASM_F2_IN_BYTES / 2; // 2 packets will be sent
1073 1081 spw_ioctl_send_ASM.data = &spectral_matrix[
1074 1082 ( (ASM_F2_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F2) ) * NB_VALUES_PER_SM ) * 2
1075 1083 ];
1076 1084 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F2;
1085 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3;
1077 1086 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F2) >> 8 ); // BLK_NR MSB
1078 1087 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F2); // BLK_NR LSB
1079 1088 break;
1080 1089 default:
1081 1090 PRINTF1("ERR *** in spw_send_asm *** unexpected sid %d\n", sid)
1082 1091 break;
1083 1092 }
1084 1093 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM + CCSDS_PROTOCOLE_EXTRA_BYTES;
1085 1094 spw_ioctl_send_ASM.hdr = (char *) header;
1086 1095 spw_ioctl_send_ASM.options = 0;
1087 1096
1088 1097 // (2) BUILD THE HEADER
1089 1098 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1090 1099 header->packetLength[0] = (unsigned char) (length>>8);
1091 1100 header->packetLength[1] = (unsigned char) (length);
1092 1101 header->sid = (unsigned char) sid; // SID
1093 1102 header->pa_lfr_pkt_cnt_asm = 2;
1094 1103 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1095 1104
1096 1105 // (3) SET PACKET TIME
1097 1106 header->time[0] = (unsigned char) (coarseTime>>24);
1098 1107 header->time[1] = (unsigned char) (coarseTime>>16);
1099 1108 header->time[2] = (unsigned char) (coarseTime>>8);
1100 1109 header->time[3] = (unsigned char) (coarseTime);
1101 1110 header->time[4] = (unsigned char) (fineTime>>8);
1102 1111 header->time[5] = (unsigned char) (fineTime);
1103 1112 //
1104 1113 header->acquisitionTime[0] = header->time[0];
1105 1114 header->acquisitionTime[1] = header->time[1];
1106 1115 header->acquisitionTime[2] = header->time[2];
1107 1116 header->acquisitionTime[3] = header->time[3];
1108 1117 header->acquisitionTime[4] = header->time[4];
1109 1118 header->acquisitionTime[5] = header->time[5];
1110 1119
1111 1120 // (4) SEND PACKET
1112 1121 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1113 1122 if (status != RTEMS_SUCCESSFUL) {
1114 1123 printf("in ASM_send *** ERR %d\n", (int) status);
1115 1124 }
1116 1125 }
1117 1126 }
Show file before | Show file after | Hide comments
@@ -1,397 +1,401
1 1 /** Functions related to data processing.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 7 *
8 8 */
9 9
10 10 #include "avf0_prc0.h"
11 11 #include "fsw_processing.h"
12 12
13 13 nb_sm_before_bp_asm_f0 nb_sm_before_f0;
14 14
15 15 //***
16 16 // F0
17 17 ring_node_asm asm_ring_norm_f0 [ NB_RING_NODES_ASM_NORM_F0 ];
18 18 ring_node_asm asm_ring_burst_sbm_f0 [ NB_RING_NODES_ASM_BURST_SBM_F0 ];
19 19
20 20 ring_node ring_to_send_asm_f0 [ NB_RING_NODES_ASM_F0 ];
21 21 int buffer_asm_f0 [ NB_RING_NODES_ASM_F0 * TOTAL_SIZE_SM ];
22 22
23 23 float asm_f0_patched_norm [ TOTAL_SIZE_SM ];
24 24 float asm_f0_patched_burst_sbm [ TOTAL_SIZE_SM ];
25 25 float asm_f0_reorganized [ TOTAL_SIZE_SM ];
26 26
27 27 char asm_f0_char [ TIME_OFFSET_IN_BYTES + (TOTAL_SIZE_SM * 2) ];
28 28 float compressed_sm_norm_f0[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F0];
29 29 float compressed_sm_sbm_f0 [ TOTAL_SIZE_COMPRESSED_ASM_SBM_F0 ];
30 30
31 31 float k_coeff_intercalib_f0_norm[ NB_BINS_COMPRESSED_SM_F0 * NB_K_COEFF_PER_BIN ]; // 11 * 32 = 352
32 32 float k_coeff_intercalib_f0_sbm[ NB_BINS_COMPRESSED_SM_SBM_F0 * NB_K_COEFF_PER_BIN ]; // 22 * 32 = 704
33 33
34 34 //************
35 35 // RTEMS TASKS
36 36
37 37 rtems_task avf0_task( rtems_task_argument lfrRequestedMode )
38 38 {
39 39 int i;
40 40
41 41 rtems_event_set event_out;
42 42 rtems_status_code status;
43 43 rtems_id queue_id_prc0;
44 44 asm_msg msgForMATR;
45 45 ring_node *nodeForAveraging;
46 46 ring_node *ring_node_tab[8];
47 47 ring_node_asm *current_ring_node_asm_burst_sbm_f0;
48 48 ring_node_asm *current_ring_node_asm_norm_f0;
49 49
50 50 unsigned int nb_norm_bp1;
51 51 unsigned int nb_norm_bp2;
52 52 unsigned int nb_norm_asm;
53 53 unsigned int nb_sbm_bp1;
54 54 unsigned int nb_sbm_bp2;
55 55
56 56 nb_norm_bp1 = 0;
57 57 nb_norm_bp2 = 0;
58 58 nb_norm_asm = 0;
59 59 nb_sbm_bp1 = 0;
60 60 nb_sbm_bp2 = 0;
61 61
62 62 reset_nb_sm_f0( lfrRequestedMode ); // reset the sm counters that drive the BP and ASM computations / transmissions
63 63 ASM_generic_init_ring( asm_ring_norm_f0, NB_RING_NODES_ASM_NORM_F0 );
64 64 ASM_generic_init_ring( asm_ring_burst_sbm_f0, NB_RING_NODES_ASM_BURST_SBM_F0 );
65 65 current_ring_node_asm_norm_f0 = asm_ring_norm_f0;
66 66 current_ring_node_asm_burst_sbm_f0 = asm_ring_burst_sbm_f0;
67 67
68 68 BOOT_PRINTF1("in AVFO *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
69 69
70 70 status = get_message_queue_id_prc0( &queue_id_prc0 );
71 71 if (status != RTEMS_SUCCESSFUL)
72 72 {
73 73 PRINTF1("in MATR *** ERR get_message_queue_id_prc0 %d\n", status)
74 74 }
75 75
76 76 while(1){
77 77 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
78 78
79 79 //****************************************
80 80 // initialize the mesage for the MATR task
81 81 msgForMATR.norm = current_ring_node_asm_norm_f0;
82 82 msgForMATR.burst_sbm = current_ring_node_asm_burst_sbm_f0;
83 83 msgForMATR.event = 0x00; // this composite event will be sent to the PRC0 task
84 84 //
85 85 //****************************************
86 86
87 87 nodeForAveraging = getRingNodeForAveraging( 0 );
88 88
89 89 ring_node_tab[NB_SM_BEFORE_AVF0-1] = nodeForAveraging;
90 90 for ( i = 2; i < (NB_SM_BEFORE_AVF0+1); i++ )
91 91 {
92 92 nodeForAveraging = nodeForAveraging->previous;
93 93 ring_node_tab[NB_SM_BEFORE_AVF0-i] = nodeForAveraging;
94 94 }
95 95
96 96 // compute the average and store it in the averaged_sm_f1 buffer
97 97 SM_average( current_ring_node_asm_norm_f0->matrix,
98 98 current_ring_node_asm_burst_sbm_f0->matrix,
99 99 ring_node_tab,
100 100 nb_norm_bp1, nb_sbm_bp1,
101 101 &msgForMATR );
102 102
103 103 // update nb_average
104 104 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF0;
105 105 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF0;
106 106 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF0;
107 107 nb_sbm_bp1 = nb_sbm_bp1 + NB_SM_BEFORE_AVF0;
108 108 nb_sbm_bp2 = nb_sbm_bp2 + NB_SM_BEFORE_AVF0;
109 109
110 110 if (nb_sbm_bp1 == nb_sm_before_f0.burst_sbm_bp1)
111 111 {
112 112 nb_sbm_bp1 = 0;
113 113 // set another ring for the ASM storage
114 114 current_ring_node_asm_burst_sbm_f0 = current_ring_node_asm_burst_sbm_f0->next;
115 115 if ( lfrCurrentMode == LFR_MODE_BURST )
116 116 {
117 117 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_BP1_F0;
118 118 }
119 119 else if ( (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
120 120 {
121 121 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_SBM_BP1_F0;
122 122 }
123 123 }
124 124
125 125 if (nb_sbm_bp2 == nb_sm_before_f0.burst_sbm_bp2)
126 126 {
127 127 nb_sbm_bp2 = 0;
128 128 if ( lfrCurrentMode == LFR_MODE_BURST )
129 129 {
130 130 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_BP2_F0;
131 131 }
132 132 else if ( (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
133 133 {
134 134 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_SBM_BP2_F0;
135 135 }
136 136 }
137 137
138 138 if (nb_norm_bp1 == nb_sm_before_f0.norm_bp1)
139 139 {
140 140 nb_norm_bp1 = 0;
141 141 // set another ring for the ASM storage
142 142 current_ring_node_asm_norm_f0 = current_ring_node_asm_norm_f0->next;
143 143 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
144 144 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
145 145 {
146 146 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP1_F0;
147 147 }
148 148 }
149 149
150 150 if (nb_norm_bp2 == nb_sm_before_f0.norm_bp2)
151 151 {
152 152 nb_norm_bp2 = 0;
153 153 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
154 154 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
155 155 {
156 156 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP2_F0;
157 157 }
158 158 }
159 159
160 160 if (nb_norm_asm == nb_sm_before_f0.norm_asm)
161 161 {
162 162 nb_norm_asm = 0;
163 163 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
164 164 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
165 165 {
166 166 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_ASM_F0;
167 167 }
168 168 }
169 169
170 170 //*************************
171 171 // send the message to MATR
172 172 if (msgForMATR.event != 0x00)
173 173 {
174 174 status = rtems_message_queue_send( queue_id_prc0, (char *) &msgForMATR, MSG_QUEUE_SIZE_PRC0);
175 175 }
176 176
177 177 if (status != RTEMS_SUCCESSFUL) {
178 178 printf("in AVF0 *** Error sending message to MATR, code %d\n", status);
179 179 }
180 180 }
181 181 }
182 182
183 183 rtems_task prc0_task( rtems_task_argument lfrRequestedMode )
184 184 {
185 185 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
186 186 size_t size; // size of the incoming TC packet
187 187 asm_msg *incomingMsg;
188 188 //
189 189 unsigned char sid;
190 190 rtems_status_code status;
191 191 rtems_id queue_id;
192 192 rtems_id queue_id_q_p0;
193 193 bp_packet_with_spare packet_norm_bp1;
194 194 bp_packet packet_norm_bp2;
195 195 bp_packet packet_sbm_bp1;
196 196 bp_packet packet_sbm_bp2;
197 197 ring_node *current_ring_node_to_send_asm_f0;
198 198
199 199 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
200 200 init_ring( ring_to_send_asm_f0, NB_RING_NODES_ASM_F0, (volatile int*) buffer_asm_f0, TOTAL_SIZE_SM );
201 201 current_ring_node_to_send_asm_f0 = ring_to_send_asm_f0;
202 202
203 203 //*************
204 204 // NORM headers
205 205 BP_init_header_with_spare( &packet_norm_bp1,
206 206 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F0,
207 207 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F0, NB_BINS_COMPRESSED_SM_F0 );
208 208 BP_init_header( &packet_norm_bp2,
209 209 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F0,
210 210 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F0, NB_BINS_COMPRESSED_SM_F0);
211 211
212 212 //****************************
213 213 // BURST SBM1 and SBM2 headers
214 214 if ( lfrRequestedMode == LFR_MODE_BURST )
215 215 {
216 216 BP_init_header( &packet_sbm_bp1,
217 217 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP1_F0,
218 218 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
219 219 BP_init_header( &packet_sbm_bp2,
220 220 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP2_F0,
221 221 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
222 222 }
223 223 else if ( lfrRequestedMode == LFR_MODE_SBM1 )
224 224 {
225 225 BP_init_header( &packet_sbm_bp1,
226 226 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM1_BP1_F0,
227 227 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
228 228 BP_init_header( &packet_sbm_bp2,
229 229 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM1_BP2_F0,
230 230 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
231 231 }
232 232 else if ( lfrRequestedMode == LFR_MODE_SBM2 )
233 233 {
234 234 BP_init_header( &packet_sbm_bp1,
235 235 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP1_F0,
236 236 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
237 237 BP_init_header( &packet_sbm_bp2,
238 238 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP2_F0,
239 239 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
240 240 }
241 241 else
242 242 {
243 243 PRINTF1("in PRC0 *** lfrRequestedMode is %d, several headers not initialized\n", (unsigned int) lfrRequestedMode)
244 244 }
245 245
246 246 status = get_message_queue_id_send( &queue_id );
247 247 if (status != RTEMS_SUCCESSFUL)
248 248 {
249 249 PRINTF1("in PRC0 *** ERR get_message_queue_id_send %d\n", status)
250 250 }
251 251 status = get_message_queue_id_prc0( &queue_id_q_p0);
252 252 if (status != RTEMS_SUCCESSFUL)
253 253 {
254 254 PRINTF1("in PRC0 *** ERR get_message_queue_id_prc0 %d\n", status)
255 255 }
256 256
257 257 BOOT_PRINTF1("in PRC0 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
258 258
259 259 while(1){
260 260 status = rtems_message_queue_receive( queue_id_q_p0, incomingData, &size, //************************************
261 261 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF0
262 262
263 263 incomingMsg = (asm_msg*) incomingData;
264 264
265 265 ASM_patch( incomingMsg->norm->matrix, asm_f0_patched_norm );
266 266 ASM_patch( incomingMsg->burst_sbm->matrix, asm_f0_patched_burst_sbm );
267 267
268 268 //****************
269 269 //****************
270 270 // BURST SBM1 SBM2
271 271 //****************
272 272 //****************
273 273 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP1_F0 ) || (incomingMsg->event & RTEMS_EVENT_SBM_BP1_F0 ) )
274 274 {
275 275 sid = getSID( incomingMsg->event );
276 276 // 1) compress the matrix for Basic Parameters calculation
277 277 ASM_compress_reorganize_and_divide( asm_f0_patched_burst_sbm, compressed_sm_sbm_f0,
278 278 nb_sm_before_f0.burst_sbm_bp1,
279 279 NB_BINS_COMPRESSED_SM_SBM_F0, NB_BINS_TO_AVERAGE_ASM_SBM_F0,
280 280 ASM_F0_INDICE_START);
281 281 // 2) compute the BP1 set
282 282 BP1_set( compressed_sm_sbm_f0, k_coeff_intercalib_f0_sbm, NB_BINS_COMPRESSED_SM_SBM_F0, packet_sbm_bp1.data );
283 283 // 3) send the BP1 set
284 284 set_time( packet_sbm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
285 285 set_time( packet_sbm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
286 packet_sbm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
286 287 BP_send( (char *) &packet_sbm_bp1, queue_id,
287 288 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0 + PACKET_LENGTH_DELTA,
288 289 sid);
289 290 // 4) compute the BP2 set if needed
290 291 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP2_F0) || (incomingMsg->event & RTEMS_EVENT_SBM_BP2_F0) )
291 292 {
292 293 // 1) compute the BP2 set
293 294 BP2_set( compressed_sm_sbm_f0, NB_BINS_COMPRESSED_SM_SBM_F0, packet_sbm_bp2.data );
294 295 // 2) send the BP2 set
295 296 set_time( packet_sbm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
296 297 set_time( packet_sbm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
298 packet_sbm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
297 299 BP_send( (char *) &packet_sbm_bp2, queue_id,
298 300 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0 + PACKET_LENGTH_DELTA,
299 301 sid);
300 302 }
301 303 }
302 304
303 305 //*****
304 306 //*****
305 307 // NORM
306 308 //*****
307 309 //*****
308 310 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F0)
309 311 {
310 312 // 1) compress the matrix for Basic Parameters calculation
311 313 ASM_compress_reorganize_and_divide( asm_f0_patched_norm, compressed_sm_norm_f0,
312 314 nb_sm_before_f0.norm_bp1,
313 315 NB_BINS_COMPRESSED_SM_F0, NB_BINS_TO_AVERAGE_ASM_F0,
314 316 ASM_F0_INDICE_START );
315 317 // 2) compute the BP1 set
316 318 BP1_set( compressed_sm_norm_f0, k_coeff_intercalib_f0_norm, NB_BINS_COMPRESSED_SM_F0, packet_norm_bp1.data );
317 319 // 3) send the BP1 set
318 320 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
319 321 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
322 packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
320 323 BP_send( (char *) &packet_norm_bp1, queue_id,
321 324 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F0 + PACKET_LENGTH_DELTA,
322 325 SID_NORM_BP1_F0 );
323 326 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F0)
324 327 {
325 328 // 1) compute the BP2 set using the same ASM as the one used for BP1
326 329 BP2_set( compressed_sm_norm_f0, NB_BINS_COMPRESSED_SM_F0, packet_norm_bp2.data );
327 330 // 2) send the BP2 set
328 331 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
329 332 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
333 packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
330 334 BP_send( (char *) &packet_norm_bp2, queue_id,
331 335 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F0 + PACKET_LENGTH_DELTA,
332 336 SID_NORM_BP2_F0);
333 337 }
334 338 }
335 339
336 340 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F0)
337 341 {
338 342 // 1) reorganize the ASM and divide
339 343 ASM_reorganize_and_divide( asm_f0_patched_norm,
340 344 asm_f0_reorganized,
341 345 nb_sm_before_f0.norm_bp1 );
342 346 // 2) convert the float array in a char array
343 347 ASM_convert( asm_f0_reorganized, (char*) current_ring_node_to_send_asm_f0->buffer_address );
344 348 current_ring_node_to_send_asm_f0->coarseTime = incomingMsg->coarseTimeNORM;
345 349 current_ring_node_to_send_asm_f0->fineTime = incomingMsg->fineTimeNORM;
346 350 current_ring_node_to_send_asm_f0->sid = SID_NORM_ASM_F0;
347 351
348 352 // 3) send the spectral matrix packets
349 353 status = rtems_message_queue_send( queue_id, &current_ring_node_to_send_asm_f0, sizeof( ring_node* ) );
350 354 // change asm ring node
351 355 current_ring_node_to_send_asm_f0 = current_ring_node_to_send_asm_f0->next;
352 356 }
353 357 }
354 358 }
355 359
356 360 //**********
357 361 // FUNCTIONS
358 362
359 363 void reset_nb_sm_f0( unsigned char lfrMode )
360 364 {
361 365 nb_sm_before_f0.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0 * 96;
362 366 nb_sm_before_f0.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1 * 96;
363 367 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;
364 368 nb_sm_before_f0.sbm1_bp1 = parameter_dump_packet.sy_lfr_s1_bp_p0 * 24; // 0.25 s per digit
365 369 nb_sm_before_f0.sbm1_bp2 = parameter_dump_packet.sy_lfr_s1_bp_p1 * 96;
366 370 nb_sm_before_f0.sbm2_bp1 = parameter_dump_packet.sy_lfr_s2_bp_p0 * 96;
367 371 nb_sm_before_f0.sbm2_bp2 = parameter_dump_packet.sy_lfr_s2_bp_p1 * 96;
368 372 nb_sm_before_f0.burst_bp1 = parameter_dump_packet.sy_lfr_b_bp_p0 * 96;
369 373 nb_sm_before_f0.burst_bp2 = parameter_dump_packet.sy_lfr_b_bp_p1 * 96;
370 374
371 375 if (lfrMode == LFR_MODE_SBM1)
372 376 {
373 377 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.sbm1_bp1;
374 378 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.sbm1_bp2;
375 379 }
376 380 else if (lfrMode == LFR_MODE_SBM2)
377 381 {
378 382 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.sbm2_bp1;
379 383 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.sbm2_bp2;
380 384 }
381 385 else if (lfrMode == LFR_MODE_BURST)
382 386 {
383 387 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.burst_bp1;
384 388 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.burst_bp2;
385 389 }
386 390 else
387 391 {
388 392 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.burst_bp1;
389 393 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.burst_bp2;
390 394 }
391 395 }
392 396
393 397 void init_k_coefficients_f0( void )
394 398 {
395 399 init_k_coefficients( k_coeff_intercalib_f0_norm, NB_BINS_COMPRESSED_SM_F0 );
396 400 init_k_coefficients( k_coeff_intercalib_f0_sbm, NB_BINS_COMPRESSED_SM_SBM_F0);
397 401 }
Show file before | Show file after | Hide comments
@@ -1,385 +1,389
1 1 /** Functions related to data processing.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 7 *
8 8 */
9 9
10 10 #include "avf1_prc1.h"
11 11
12 12 nb_sm_before_bp_asm_f1 nb_sm_before_f1;
13 13
14 14 extern ring_node sm_ring_f1[ ];
15 15
16 16 //***
17 17 // F1
18 18 ring_node_asm asm_ring_norm_f1 [ NB_RING_NODES_ASM_NORM_F1 ];
19 19 ring_node_asm asm_ring_burst_sbm_f1 [ NB_RING_NODES_ASM_BURST_SBM_F1 ];
20 20
21 21 ring_node ring_to_send_asm_f1 [ NB_RING_NODES_ASM_F1 ];
22 22 int buffer_asm_f1 [ NB_RING_NODES_ASM_F1 * TOTAL_SIZE_SM ];
23 23
24 24 float asm_f1_patched_norm [ TOTAL_SIZE_SM ];
25 25 float asm_f1_patched_burst_sbm [ TOTAL_SIZE_SM ];
26 26 float asm_f1_reorganized [ TOTAL_SIZE_SM ];
27 27
28 28 char asm_f1_char [ TOTAL_SIZE_SM * 2 ];
29 29 float compressed_sm_norm_f1[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F1];
30 30 float compressed_sm_sbm_f1 [ TOTAL_SIZE_COMPRESSED_ASM_SBM_F1 ];
31 31
32 32 float k_coeff_intercalib_f1_norm[ NB_BINS_COMPRESSED_SM_F1 * NB_K_COEFF_PER_BIN ]; // 13 * 32 = 416
33 33 float k_coeff_intercalib_f1_sbm[ NB_BINS_COMPRESSED_SM_SBM_F1 * NB_K_COEFF_PER_BIN ]; // 26 * 32 = 832
34 34
35 35 //************
36 36 // RTEMS TASKS
37 37
38 38 rtems_task avf1_task( rtems_task_argument lfrRequestedMode )
39 39 {
40 40 int i;
41 41
42 42 rtems_event_set event_out;
43 43 rtems_status_code status;
44 44 rtems_id queue_id_prc1;
45 45 asm_msg msgForMATR;
46 46 ring_node *nodeForAveraging;
47 47 ring_node *ring_node_tab[NB_SM_BEFORE_AVF0];
48 48 ring_node_asm *current_ring_node_asm_burst_sbm_f1;
49 49 ring_node_asm *current_ring_node_asm_norm_f1;
50 50
51 51 unsigned int nb_norm_bp1;
52 52 unsigned int nb_norm_bp2;
53 53 unsigned int nb_norm_asm;
54 54 unsigned int nb_sbm_bp1;
55 55 unsigned int nb_sbm_bp2;
56 56
57 57 nb_norm_bp1 = 0;
58 58 nb_norm_bp2 = 0;
59 59 nb_norm_asm = 0;
60 60 nb_sbm_bp1 = 0;
61 61 nb_sbm_bp2 = 0;
62 62
63 63 reset_nb_sm_f1( lfrRequestedMode ); // reset the sm counters that drive the BP and ASM computations / transmissions
64 64 ASM_generic_init_ring( asm_ring_norm_f1, NB_RING_NODES_ASM_NORM_F1 );
65 65 ASM_generic_init_ring( asm_ring_burst_sbm_f1, NB_RING_NODES_ASM_BURST_SBM_F1 );
66 66 current_ring_node_asm_norm_f1 = asm_ring_norm_f1;
67 67 current_ring_node_asm_burst_sbm_f1 = asm_ring_burst_sbm_f1;
68 68
69 69 BOOT_PRINTF1("in AVF1 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
70 70
71 71 status = get_message_queue_id_prc1( &queue_id_prc1 );
72 72 if (status != RTEMS_SUCCESSFUL)
73 73 {
74 74 PRINTF1("in AVF1 *** ERR get_message_queue_id_prc1 %d\n", status)
75 75 }
76 76
77 77 while(1){
78 78 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
79 79
80 80 //****************************************
81 81 // initialize the mesage for the MATR task
82 82 msgForMATR.norm = current_ring_node_asm_norm_f1;
83 83 msgForMATR.burst_sbm = current_ring_node_asm_burst_sbm_f1;
84 84 msgForMATR.event = 0x00; // this composite event will be sent to the PRC1 task
85 85 //
86 86 //****************************************
87 87
88 88 nodeForAveraging = getRingNodeForAveraging( 1 );
89 89
90 90 ring_node_tab[NB_SM_BEFORE_AVF1-1] = nodeForAveraging;
91 91 for ( i = 2; i < (NB_SM_BEFORE_AVF1+1); i++ )
92 92 {
93 93 nodeForAveraging = nodeForAveraging->previous;
94 94 ring_node_tab[NB_SM_BEFORE_AVF1-i] = nodeForAveraging;
95 95 }
96 96
97 97 // compute the average and store it in the averaged_sm_f1 buffer
98 98 SM_average( current_ring_node_asm_norm_f1->matrix,
99 99 current_ring_node_asm_burst_sbm_f1->matrix,
100 100 ring_node_tab,
101 101 nb_norm_bp1, nb_sbm_bp1,
102 102 &msgForMATR );
103 103
104 104 // update nb_average
105 105 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF1;
106 106 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF1;
107 107 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF1;
108 108 nb_sbm_bp1 = nb_sbm_bp1 + NB_SM_BEFORE_AVF1;
109 109 nb_sbm_bp2 = nb_sbm_bp2 + NB_SM_BEFORE_AVF1;
110 110
111 111 if (nb_sbm_bp1 == nb_sm_before_f1.burst_sbm_bp1)
112 112 {
113 113 nb_sbm_bp1 = 0;
114 114 // set another ring for the ASM storage
115 115 current_ring_node_asm_burst_sbm_f1 = current_ring_node_asm_burst_sbm_f1->next;
116 116 if ( lfrCurrentMode == LFR_MODE_BURST )
117 117 {
118 118 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_BP1_F1;
119 119 }
120 120 else if ( lfrCurrentMode == LFR_MODE_SBM2 )
121 121 {
122 122 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_SBM_BP1_F1;
123 123 }
124 124 }
125 125
126 126 if (nb_sbm_bp2 == nb_sm_before_f1.burst_sbm_bp2)
127 127 {
128 128 nb_sbm_bp2 = 0;
129 129 if ( lfrCurrentMode == LFR_MODE_BURST )
130 130 {
131 131 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_BURST_BP2_F1;
132 132 }
133 133 else if ( lfrCurrentMode == LFR_MODE_SBM2 )
134 134 {
135 135 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_SBM_BP2_F1;
136 136 }
137 137 }
138 138
139 139 if (nb_norm_bp1 == nb_sm_before_f1.norm_bp1)
140 140 {
141 141 nb_norm_bp1 = 0;
142 142 // set another ring for the ASM storage
143 143 current_ring_node_asm_norm_f1 = current_ring_node_asm_norm_f1->next;
144 144 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
145 145 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
146 146 {
147 147 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP1_F1;
148 148 }
149 149 }
150 150
151 151 if (nb_norm_bp2 == nb_sm_before_f1.norm_bp2)
152 152 {
153 153 nb_norm_bp2 = 0;
154 154 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
155 155 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
156 156 {
157 157 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP2_F1;
158 158 }
159 159 }
160 160
161 161 if (nb_norm_asm == nb_sm_before_f1.norm_asm)
162 162 {
163 163 nb_norm_asm = 0;
164 164 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
165 165 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
166 166 {
167 167 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_ASM_F1;
168 168 }
169 169 }
170 170
171 171 //*************************
172 172 // send the message to MATR
173 173 if (msgForMATR.event != 0x00)
174 174 {
175 175 status = rtems_message_queue_send( queue_id_prc1, (char *) &msgForMATR, MSG_QUEUE_SIZE_PRC1);
176 176 }
177 177
178 178 if (status != RTEMS_SUCCESSFUL) {
179 179 printf("in AVF1 *** Error sending message to PRC1, code %d\n", status);
180 180 }
181 181 }
182 182 }
183 183
184 184 rtems_task prc1_task( rtems_task_argument lfrRequestedMode )
185 185 {
186 186 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
187 187 size_t size; // size of the incoming TC packet
188 188 asm_msg *incomingMsg;
189 189 //
190 190 unsigned char sid;
191 191 rtems_status_code status;
192 192 rtems_id queue_id_send;
193 193 rtems_id queue_id_q_p1;
194 194 bp_packet_with_spare packet_norm_bp1;
195 195 bp_packet packet_norm_bp2;
196 196 bp_packet packet_sbm_bp1;
197 197 bp_packet packet_sbm_bp2;
198 198 ring_node *current_ring_node_to_send_asm_f1;
199 199
200 200 unsigned long long int localTime;
201 201
202 202 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
203 203 init_ring( ring_to_send_asm_f1, NB_RING_NODES_ASM_F1, (volatile int*) buffer_asm_f1, TOTAL_SIZE_SM );
204 204 current_ring_node_to_send_asm_f1 = ring_to_send_asm_f1;
205 205
206 206 //*************
207 207 // NORM headers
208 208 BP_init_header_with_spare( &packet_norm_bp1,
209 209 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F1,
210 210 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F1, NB_BINS_COMPRESSED_SM_F1 );
211 211 BP_init_header( &packet_norm_bp2,
212 212 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F1,
213 213 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F1, NB_BINS_COMPRESSED_SM_F1);
214 214
215 215 //***********************
216 216 // BURST and SBM2 headers
217 217 if ( lfrRequestedMode == LFR_MODE_BURST )
218 218 {
219 219 BP_init_header( &packet_sbm_bp1,
220 220 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP1_F1,
221 221 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
222 222 BP_init_header( &packet_sbm_bp2,
223 223 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP2_F1,
224 224 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
225 225 }
226 226 else if ( lfrRequestedMode == LFR_MODE_SBM2 )
227 227 {
228 228 BP_init_header( &packet_sbm_bp1,
229 229 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP1_F1,
230 230 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
231 231 BP_init_header( &packet_sbm_bp2,
232 232 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP2_F1,
233 233 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
234 234 }
235 235 else
236 236 {
237 237 PRINTF1("in PRC1 *** lfrRequestedMode is %d, several headers not initialized\n", (unsigned int) lfrRequestedMode)
238 238 }
239 239
240 240 status = get_message_queue_id_send( &queue_id_send );
241 241 if (status != RTEMS_SUCCESSFUL)
242 242 {
243 243 PRINTF1("in PRC1 *** ERR get_message_queue_id_send %d\n", status)
244 244 }
245 245 status = get_message_queue_id_prc1( &queue_id_q_p1);
246 246 if (status != RTEMS_SUCCESSFUL)
247 247 {
248 248 PRINTF1("in PRC1 *** ERR get_message_queue_id_prc1 %d\n", status)
249 249 }
250 250
251 251 BOOT_PRINTF1("in PRC1 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
252 252
253 253 while(1){
254 254 status = rtems_message_queue_receive( queue_id_q_p1, incomingData, &size, //************************************
255 255 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF0
256 256
257 257 incomingMsg = (asm_msg*) incomingData;
258 258
259 259 ASM_patch( incomingMsg->norm->matrix, asm_f1_patched_norm );
260 260 ASM_patch( incomingMsg->burst_sbm->matrix, asm_f1_patched_burst_sbm );
261 261
262 262 localTime = getTimeAsUnsignedLongLongInt( );
263 263 //***********
264 264 //***********
265 265 // BURST SBM2
266 266 //***********
267 267 //***********
268 268 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP1_F1) || (incomingMsg->event & RTEMS_EVENT_SBM_BP1_F1) )
269 269 {
270 270 sid = getSID( incomingMsg->event );
271 271 // 1) compress the matrix for Basic Parameters calculation
272 272 ASM_compress_reorganize_and_divide( asm_f1_patched_burst_sbm, compressed_sm_sbm_f1,
273 273 nb_sm_before_f1.burst_sbm_bp1,
274 274 NB_BINS_COMPRESSED_SM_SBM_F1, NB_BINS_TO_AVERAGE_ASM_SBM_F1,
275 275 ASM_F1_INDICE_START);
276 276 // 2) compute the BP1 set
277 277 BP1_set( compressed_sm_sbm_f1, k_coeff_intercalib_f1_sbm, NB_BINS_COMPRESSED_SM_SBM_F1, packet_sbm_bp1.data );
278 278 // 3) send the BP1 set
279 279 set_time( packet_sbm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
280 280 set_time( packet_sbm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
281 packet_sbm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
281 282 BP_send( (char *) &packet_sbm_bp1, queue_id_send,
282 283 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1 + PACKET_LENGTH_DELTA,
283 284 sid );
284 285 // 4) compute the BP2 set if needed
285 286 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP2_F1) || (incomingMsg->event & RTEMS_EVENT_SBM_BP2_F1) )
286 287 {
287 288 // 1) compute the BP2 set
288 289 BP2_set( compressed_sm_sbm_f1, NB_BINS_COMPRESSED_SM_SBM_F1, packet_norm_bp2.data );
289 290 // 2) send the BP2 set
290 291 set_time( packet_sbm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
291 292 set_time( packet_sbm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
293 packet_sbm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
292 294 BP_send( (char *) &packet_sbm_bp2, queue_id_send,
293 295 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1 + PACKET_LENGTH_DELTA,
294 296 sid );
295 297 }
296 298 }
297 299
298 300 //*****
299 301 //*****
300 302 // NORM
301 303 //*****
302 304 //*****
303 305 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F1)
304 306 {
305 307 // 1) compress the matrix for Basic Parameters calculation
306 308 ASM_compress_reorganize_and_divide( asm_f1_patched_norm, compressed_sm_norm_f1,
307 309 nb_sm_before_f1.norm_bp1,
308 310 NB_BINS_COMPRESSED_SM_F1, NB_BINS_TO_AVERAGE_ASM_F1,
309 311 ASM_F1_INDICE_START );
310 312 // 2) compute the BP1 set
311 313 BP1_set( compressed_sm_norm_f1, k_coeff_intercalib_f1_norm, NB_BINS_COMPRESSED_SM_F1, packet_norm_bp1.data );
312 314 // 3) send the BP1 set
313 315 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
314 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
316 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
317 packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
315 318 BP_send( (char *) &packet_norm_bp1, queue_id_send,
316 319 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F1 + PACKET_LENGTH_DELTA,
317 320 SID_NORM_BP1_F1 );
318 321 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F1)
319 322 {
320 323 // 1) compute the BP2 set
321 324 BP2_set( compressed_sm_norm_f1, NB_BINS_COMPRESSED_SM_F1, packet_norm_bp2.data );
322 325 // 2) send the BP2 set
323 326 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
324 327 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
328 packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
325 329 BP_send( (char *) &packet_norm_bp2, queue_id_send,
326 330 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F1 + PACKET_LENGTH_DELTA,
327 331 SID_NORM_BP2_F1 );
328 332 }
329 333 }
330 334
331 335 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F1)
332 336 {
333 337 // 1) reorganize the ASM and divide
334 338 ASM_reorganize_and_divide( asm_f1_patched_norm,
335 339 asm_f1_reorganized,
336 340 nb_sm_before_f1.norm_bp1 );
337 341 // 2) convert the float array in a char array
338 342 ASM_convert( asm_f1_reorganized, (char*) current_ring_node_to_send_asm_f1->buffer_address );
339 343 current_ring_node_to_send_asm_f1->coarseTime = incomingMsg->coarseTimeNORM;
340 344 current_ring_node_to_send_asm_f1->fineTime = incomingMsg->fineTimeNORM;
341 345 current_ring_node_to_send_asm_f1->sid = SID_NORM_ASM_F1;
342 346 // 3) send the spectral matrix packets
343 347 status = rtems_message_queue_send( queue_id_send, &current_ring_node_to_send_asm_f1, sizeof( ring_node* ) );
344 348 // change asm ring node
345 349 current_ring_node_to_send_asm_f1 = current_ring_node_to_send_asm_f1->next;
346 350 }
347 351
348 352 }
349 353 }
350 354
351 355 //**********
352 356 // FUNCTIONS
353 357
354 358 void reset_nb_sm_f1( unsigned char lfrMode )
355 359 {
356 360 nb_sm_before_f1.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0 * 16;
357 361 nb_sm_before_f1.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1 * 16;
358 362 nb_sm_before_f1.norm_asm = (parameter_dump_packet.sy_lfr_n_asm_p[0] * 256 + parameter_dump_packet.sy_lfr_n_asm_p[1]) * 16;
359 363 nb_sm_before_f1.sbm2_bp1 = parameter_dump_packet.sy_lfr_s2_bp_p0 * 16;
360 364 nb_sm_before_f1.sbm2_bp2 = parameter_dump_packet.sy_lfr_s2_bp_p1 * 16;
361 365 nb_sm_before_f1.burst_bp1 = parameter_dump_packet.sy_lfr_b_bp_p0 * 16;
362 366 nb_sm_before_f1.burst_bp2 = parameter_dump_packet.sy_lfr_b_bp_p1 * 16;
363 367
364 368 if (lfrMode == LFR_MODE_SBM2)
365 369 {
366 370 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.sbm2_bp1;
367 371 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.sbm2_bp2;
368 372 }
369 373 else if (lfrMode == LFR_MODE_BURST)
370 374 {
371 375 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.burst_bp1;
372 376 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.burst_bp2;
373 377 }
374 378 else
375 379 {
376 380 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.burst_bp1;
377 381 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.burst_bp2;
378 382 }
379 383 }
380 384
381 385 void init_k_coefficients_f1( void )
382 386 {
383 387 init_k_coefficients( k_coeff_intercalib_f1_norm, NB_BINS_COMPRESSED_SM_F1 );
384 388 init_k_coefficients( k_coeff_intercalib_f1_sbm, NB_BINS_COMPRESSED_SM_SBM_F1);
385 389 }
Show file before | Show file after | Hide comments
@@ -1,289 +1,291
1 1 /** Functions related to data processing.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 7 *
8 8 */
9 9
10 10 #include "avf2_prc2.h"
11 11
12 12 nb_sm_before_bp_asm_f2 nb_sm_before_f2;
13 13
14 14 extern ring_node sm_ring_f2[ ];
15 15
16 16 //***
17 17 // F2
18 18 ring_node_asm asm_ring_norm_f2 [ NB_RING_NODES_ASM_NORM_F2 ];
19 19
20 20 ring_node ring_to_send_asm_f2 [ NB_RING_NODES_ASM_F2 ];
21 21 int buffer_asm_f2 [ NB_RING_NODES_ASM_F2 * TOTAL_SIZE_SM ];
22 22
23 23 float asm_f2_patched_norm [ TOTAL_SIZE_SM ];
24 24 float asm_f2_reorganized [ TOTAL_SIZE_SM ];
25 25
26 26 char asm_f2_char [ TOTAL_SIZE_SM * 2 ];
27 27 float compressed_sm_norm_f2[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F2];
28 28
29 29 float k_coeff_intercalib_f2[ NB_BINS_COMPRESSED_SM_F2 * NB_K_COEFF_PER_BIN ]; // 12 * 32 = 384
30 30
31 31 //************
32 32 // RTEMS TASKS
33 33
34 34 //***
35 35 // F2
36 36 rtems_task avf2_task( rtems_task_argument argument )
37 37 {
38 38 rtems_event_set event_out;
39 39 rtems_status_code status;
40 40 rtems_id queue_id_prc2;
41 41 asm_msg msgForMATR;
42 42 ring_node *nodeForAveraging;
43 43 ring_node_asm *current_ring_node_asm_norm_f2;
44 44
45 45 unsigned int nb_norm_bp1;
46 46 unsigned int nb_norm_bp2;
47 47 unsigned int nb_norm_asm;
48 48
49 49 nb_norm_bp1 = 0;
50 50 nb_norm_bp2 = 0;
51 51 nb_norm_asm = 0;
52 52
53 53 reset_nb_sm_f2( ); // reset the sm counters that drive the BP and ASM computations / transmissions
54 54 ASM_generic_init_ring( asm_ring_norm_f2, NB_RING_NODES_ASM_NORM_F2 );
55 55 current_ring_node_asm_norm_f2 = asm_ring_norm_f2;
56 56
57 57 BOOT_PRINTF("in AVF2 ***\n")
58 58
59 59 status = get_message_queue_id_prc2( &queue_id_prc2 );
60 60 if (status != RTEMS_SUCCESSFUL)
61 61 {
62 62 PRINTF1("in AVF2 *** ERR get_message_queue_id_prc2 %d\n", status)
63 63 }
64 64
65 65 while(1){
66 66 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
67 67
68 68 //****************************************
69 69 // initialize the mesage for the MATR task
70 70 msgForMATR.norm = current_ring_node_asm_norm_f2;
71 71 msgForMATR.burst_sbm = NULL;
72 72 msgForMATR.event = 0x00; // this composite event will be sent to the PRC2 task
73 73 //
74 74 //****************************************
75 75
76 76 nodeForAveraging = getRingNodeForAveraging( 2 );
77 77
78 78 // printf(" **0** %x . %x", sm_ring_f2[0].coarseTime, sm_ring_f2[0].fineTime);
79 79 // printf(" **1** %x . %x", sm_ring_f2[1].coarseTime, sm_ring_f2[1].fineTime);
80 80 // printf(" **2** %x . %x", sm_ring_f2[2].coarseTime, sm_ring_f2[2].fineTime);
81 81 // printf(" **3** %x . %x", sm_ring_f2[3].coarseTime, sm_ring_f2[3].fineTime);
82 82 // printf(" **4** %x . %x", sm_ring_f2[4].coarseTime, sm_ring_f2[4].fineTime);
83 83 // printf(" **5** %x . %x", sm_ring_f2[5].coarseTime, sm_ring_f2[5].fineTime);
84 84 // printf(" **6** %x . %x", sm_ring_f2[6].coarseTime, sm_ring_f2[6].fineTime);
85 85 // printf(" **7** %x . %x", sm_ring_f2[7].coarseTime, sm_ring_f2[7].fineTime);
86 86 // printf(" **8** %x . %x", sm_ring_f2[8].coarseTime, sm_ring_f2[8].fineTime);
87 87 // printf(" **9** %x . %x", sm_ring_f2[9].coarseTime, sm_ring_f2[9].fineTime);
88 88 // printf(" **10** %x . %x\n", sm_ring_f2[10].coarseTime, sm_ring_f2[10].fineTime);
89 89
90 90 // compute the average and store it in the averaged_sm_f2 buffer
91 91 SM_average_f2( current_ring_node_asm_norm_f2->matrix,
92 92 nodeForAveraging,
93 93 nb_norm_bp1,
94 94 &msgForMATR );
95 95
96 96 // update nb_average
97 97 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF2;
98 98 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF2;
99 99 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF2;
100 100
101 101 if (nb_norm_bp1 == nb_sm_before_f2.norm_bp1)
102 102 {
103 103 nb_norm_bp1 = 0;
104 104 // set another ring for the ASM storage
105 105 current_ring_node_asm_norm_f2 = current_ring_node_asm_norm_f2->next;
106 106 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
107 107 || (lfrCurrentMode == LFR_MODE_SBM2) )
108 108 {
109 109 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP1_F2;
110 110 }
111 111 }
112 112
113 113 if (nb_norm_bp2 == nb_sm_before_f2.norm_bp2)
114 114 {
115 115 nb_norm_bp2 = 0;
116 116 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
117 117 || (lfrCurrentMode == LFR_MODE_SBM2) )
118 118 {
119 119 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_BP2_F2;
120 120 }
121 121 }
122 122
123 123 if (nb_norm_asm == nb_sm_before_f2.norm_asm)
124 124 {
125 125 nb_norm_asm = 0;
126 126 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
127 127 || (lfrCurrentMode == LFR_MODE_SBM2) )
128 128 {
129 129 msgForMATR.event = msgForMATR.event | RTEMS_EVENT_NORM_ASM_F2;
130 130 }
131 131 }
132 132
133 133 //*************************
134 134 // send the message to MATR
135 135 if (msgForMATR.event != 0x00)
136 136 {
137 137 status = rtems_message_queue_send( queue_id_prc2, (char *) &msgForMATR, MSG_QUEUE_SIZE_PRC2);
138 138 }
139 139
140 140 if (status != RTEMS_SUCCESSFUL) {
141 141 printf("in AVF2 *** Error sending message to MATR, code %d\n", status);
142 142 }
143 143 }
144 144 }
145 145
146 146 rtems_task prc2_task( rtems_task_argument argument )
147 147 {
148 148 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
149 149 size_t size; // size of the incoming TC packet
150 150 asm_msg *incomingMsg;
151 151 //
152 152 rtems_status_code status;
153 153 rtems_id queue_id_send;
154 154 rtems_id queue_id_q_p2;
155 155 bp_packet packet_norm_bp1;
156 156 bp_packet packet_norm_bp2;
157 157 ring_node *current_ring_node_to_send_asm_f2;
158 158
159 159 unsigned long long int localTime;
160 160
161 161 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
162 162 init_ring( ring_to_send_asm_f2, NB_RING_NODES_ASM_F2, (volatile int*) buffer_asm_f2, TOTAL_SIZE_SM );
163 163 current_ring_node_to_send_asm_f2 = ring_to_send_asm_f2;
164 164
165 165 //*************
166 166 // NORM headers
167 167 BP_init_header( &packet_norm_bp1,
168 168 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F2,
169 169 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F2, NB_BINS_COMPRESSED_SM_F2 );
170 170 BP_init_header( &packet_norm_bp2,
171 171 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F2,
172 172 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F2, NB_BINS_COMPRESSED_SM_F2 );
173 173
174 174 status = get_message_queue_id_send( &queue_id_send );
175 175 if (status != RTEMS_SUCCESSFUL)
176 176 {
177 177 PRINTF1("in PRC2 *** ERR get_message_queue_id_send %d\n", status)
178 178 }
179 179 status = get_message_queue_id_prc2( &queue_id_q_p2);
180 180 if (status != RTEMS_SUCCESSFUL)
181 181 {
182 182 PRINTF1("in PRC2 *** ERR get_message_queue_id_prc2 %d\n", status)
183 183 }
184 184
185 185 BOOT_PRINTF("in PRC2 ***\n")
186 186
187 187 while(1){
188 188 status = rtems_message_queue_receive( queue_id_q_p2, incomingData, &size, //************************************
189 189 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF2
190 190
191 191 incomingMsg = (asm_msg*) incomingData;
192 192
193 193 ASM_patch( incomingMsg->norm->matrix, asm_f2_patched_norm );
194 194
195 195 localTime = getTimeAsUnsignedLongLongInt( );
196 196
197 197 //*****
198 198 //*****
199 199 // NORM
200 200 //*****
201 201 //*****
202 202 // 1) compress the matrix for Basic Parameters calculation
203 203 ASM_compress_reorganize_and_divide( asm_f2_patched_norm, compressed_sm_norm_f2,
204 204 nb_sm_before_f2.norm_bp1,
205 205 NB_BINS_COMPRESSED_SM_F2, NB_BINS_TO_AVERAGE_ASM_F2,
206 206 ASM_F2_INDICE_START );
207 207 // BP1_F2
208 208 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F2)
209 209 {
210 210 // 1) compute the BP1 set
211 211 BP1_set( compressed_sm_norm_f2, k_coeff_intercalib_f2, NB_BINS_COMPRESSED_SM_F2, packet_norm_bp1.data );
212 212 // 2) send the BP1 set
213 213 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
214 214 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
215 packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
215 216 BP_send( (char *) &packet_norm_bp1, queue_id_send,
216 217 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F2 + PACKET_LENGTH_DELTA,
217 218 SID_NORM_BP1_F2 );
218 219 }
219 220 // BP2_F2
220 221 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F2)
221 222 {
222 223 // 1) compute the BP2 set
223 224 BP2_set( compressed_sm_norm_f2, NB_BINS_COMPRESSED_SM_F2, packet_norm_bp2.data );
224 225 // 2) send the BP2 set
225 226 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
226 227 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
228 packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
227 229 BP_send( (char *) &packet_norm_bp2, queue_id_send,
228 230 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F2 + PACKET_LENGTH_DELTA,
229 231 SID_NORM_BP2_F2 );
230 232 }
231 233
232 234 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F2)
233 235 {
234 236 // 1) reorganize the ASM and divide
235 237 ASM_reorganize_and_divide( asm_f2_patched_norm,
236 238 asm_f2_reorganized,
237 239 nb_sm_before_f2.norm_bp1 );
238 240 // 2) convert the float array in a char array
239 241 ASM_convert( asm_f2_reorganized, (char*) current_ring_node_to_send_asm_f2->buffer_address );
240 242 current_ring_node_to_send_asm_f2->coarseTime = incomingMsg->coarseTimeNORM;
241 243 current_ring_node_to_send_asm_f2->fineTime = incomingMsg->fineTimeNORM;
242 244 current_ring_node_to_send_asm_f2->sid = SID_NORM_ASM_F2;
243 245 // 3) send the spectral matrix packets
244 246 status = rtems_message_queue_send( queue_id_send, &current_ring_node_to_send_asm_f2, sizeof( ring_node* ) );
245 247 // change asm ring node
246 248 current_ring_node_to_send_asm_f2 = current_ring_node_to_send_asm_f2->next;
247 249 }
248 250
249 251 }
250 252 }
251 253
252 254 //**********
253 255 // FUNCTIONS
254 256
255 257 void reset_nb_sm_f2( void )
256 258 {
257 259 nb_sm_before_f2.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0;
258 260 nb_sm_before_f2.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1;
259 261 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];
260 262 }
261 263
262 264 void SM_average_f2( float *averaged_spec_mat_f2,
263 265 ring_node *ring_node,
264 266 unsigned int nbAverageNormF2,
265 267 asm_msg *msgForMATR )
266 268 {
267 269 float sum;
268 270 unsigned int i;
269 271
270 272 for(i=0; i<TOTAL_SIZE_SM; i++)
271 273 {
272 274 sum = ( (int *) (ring_node->buffer_address) ) [ i ];
273 275 if ( (nbAverageNormF2 == 0) )
274 276 {
275 277 averaged_spec_mat_f2[ i ] = sum;
276 278 msgForMATR->coarseTimeNORM = ring_node->coarseTime;
277 279 msgForMATR->fineTimeNORM = ring_node->fineTime;
278 280 }
279 281 else
280 282 {
281 283 averaged_spec_mat_f2[ i ] = ( averaged_spec_mat_f2[ i ] + sum );
282 284 }
283 285 }
284 286 }
285 287
286 288 void init_k_coefficients_f2( void )
287 289 {
288 290 init_k_coefficients( k_coeff_intercalib_f2, NB_BINS_COMPRESSED_SM_F2);
289 291 }
Show file before | Show file after | Hide comments
@@ -1,586 +1,586
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
13 13 unsigned int nb_sm_f0;
14 14 unsigned int nb_sm_f0_aux_f1;
15 15 unsigned int nb_sm_f1;
16 16 unsigned int nb_sm_f0_aux_f2;
17 17
18 18 //************************
19 19 // spectral matrices rings
20 20 ring_node sm_ring_f0[ NB_RING_NODES_SM_F0 ];
21 21 ring_node sm_ring_f1[ NB_RING_NODES_SM_F1 ];
22 22 ring_node sm_ring_f2[ NB_RING_NODES_SM_F2 ];
23 23 ring_node *current_ring_node_sm_f0;
24 24 ring_node *current_ring_node_sm_f1;
25 25 ring_node *current_ring_node_sm_f2;
26 26 ring_node *ring_node_for_averaging_sm_f0;
27 27 ring_node *ring_node_for_averaging_sm_f1;
28 28 ring_node *ring_node_for_averaging_sm_f2;
29 29
30 30 //
31 31 ring_node * getRingNodeForAveraging( unsigned char frequencyChannel)
32 32 {
33 33 ring_node *node;
34 34
35 35 node = NULL;
36 36 switch ( frequencyChannel ) {
37 37 case 0:
38 38 node = ring_node_for_averaging_sm_f0;
39 39 break;
40 40 case 1:
41 41 node = ring_node_for_averaging_sm_f1;
42 42 break;
43 43 case 2:
44 44 node = ring_node_for_averaging_sm_f2;
45 45 break;
46 46 default:
47 47 break;
48 48 }
49 49
50 50 return node;
51 51 }
52 52
53 53 //***********************************************************
54 54 // Interrupt Service Routine for spectral matrices processing
55 55
56 56 void spectral_matrices_isr_f0( unsigned char statusReg )
57 57 {
58 58 unsigned char status;
59 59 rtems_status_code status_code;
60 60 ring_node *full_ring_node;
61 61
62 62 status = statusReg & 0x03; // [0011] get the status_ready_matrix_f0_x bits
63 63
64 64 switch(status)
65 65 {
66 66 case 0:
67 67 break;
68 68 case 3:
69 69 // UNEXPECTED VALUE
70 70 spectral_matrix_regs->status = 0x03; // [0011]
71 71 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
72 72 break;
73 73 case 1:
74 74 full_ring_node = current_ring_node_sm_f0->previous;
75 75 full_ring_node->coarseTime = spectral_matrix_regs->f0_0_coarse_time;
76 76 full_ring_node->fineTime = spectral_matrix_regs->f0_0_fine_time;
77 77 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
78 78 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->buffer_address;
79 79 // if there are enough ring nodes ready, wake up an AVFx task
80 80 nb_sm_f0 = nb_sm_f0 + 1;
81 81 if (nb_sm_f0 == NB_SM_BEFORE_AVF0)
82 82 {
83 83 ring_node_for_averaging_sm_f0 = full_ring_node;
84 84 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
85 85 {
86 86 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
87 87 }
88 88 nb_sm_f0 = 0;
89 89 }
90 90 spectral_matrix_regs->status = 0x01; // [0000 0001]
91 91 break;
92 92 case 2:
93 93 full_ring_node = current_ring_node_sm_f0->previous;
94 94 full_ring_node->coarseTime = spectral_matrix_regs->f0_1_coarse_time;
95 95 full_ring_node->fineTime = spectral_matrix_regs->f0_1_fine_time;
96 96 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
97 97 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
98 98 // if there are enough ring nodes ready, wake up an AVFx task
99 99 nb_sm_f0 = nb_sm_f0 + 1;
100 100 if (nb_sm_f0 == NB_SM_BEFORE_AVF0)
101 101 {
102 102 ring_node_for_averaging_sm_f0 = full_ring_node;
103 103 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
104 104 {
105 105 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
106 106 }
107 107 nb_sm_f0 = 0;
108 108 }
109 109 spectral_matrix_regs->status = 0x02; // [0000 0010]
110 110 break;
111 111 }
112 112 }
113 113
114 114 void spectral_matrices_isr_f1( unsigned char statusReg )
115 115 {
116 116 rtems_status_code status_code;
117 117 unsigned char status;
118 118 ring_node *full_ring_node;
119 119
120 120 status = (statusReg & 0x0c) >> 2; // [1100] get the status_ready_matrix_f0_x bits
121 121
122 122 switch(status)
123 123 {
124 124 case 0:
125 125 break;
126 126 case 3:
127 127 // UNEXPECTED VALUE
128 128 spectral_matrix_regs->status = 0xc0; // [1100]
129 129 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
130 130 break;
131 131 case 1:
132 132 full_ring_node = current_ring_node_sm_f1->previous;
133 133 full_ring_node->coarseTime = spectral_matrix_regs->f1_0_coarse_time;
134 134 full_ring_node->fineTime = spectral_matrix_regs->f1_0_fine_time;
135 135 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
136 136 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->buffer_address;
137 137 // if there are enough ring nodes ready, wake up an AVFx task
138 138 nb_sm_f1 = nb_sm_f1 + 1;
139 139 if (nb_sm_f1 == NB_SM_BEFORE_AVF1)
140 140 {
141 141 ring_node_for_averaging_sm_f1 = full_ring_node;
142 142 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
143 143 {
144 144 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
145 145 }
146 146 nb_sm_f1 = 0;
147 147 }
148 148 spectral_matrix_regs->status = 0x04; // [0000 0100]
149 149 break;
150 150 case 2:
151 151 full_ring_node = current_ring_node_sm_f1->previous;
152 152 full_ring_node->coarseTime = spectral_matrix_regs->f1_1_coarse_time;
153 153 full_ring_node->fineTime = spectral_matrix_regs->f1_1_fine_time;
154 154 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
155 155 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
156 156 // if there are enough ring nodes ready, wake up an AVFx task
157 157 nb_sm_f1 = nb_sm_f1 + 1;
158 158 if (nb_sm_f1 == NB_SM_BEFORE_AVF1)
159 159 {
160 160 ring_node_for_averaging_sm_f1 = full_ring_node;
161 161 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
162 162 {
163 163 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
164 164 }
165 165 nb_sm_f1 = 0;
166 166 }
167 167 spectral_matrix_regs->status = 0x08; // [1000 0000]
168 168 break;
169 169 }
170 170 }
171 171
172 172 void spectral_matrices_isr_f2( unsigned char statusReg )
173 173 {
174 174 unsigned char status;
175 175 rtems_status_code status_code;
176 176
177 177 status = (statusReg & 0x30) >> 4; // [0011 0000] get the status_ready_matrix_f0_x bits
178 178
179 179 switch(status)
180 180 {
181 181 case 0:
182 182 break;
183 183 case 3:
184 184 // UNEXPECTED VALUE
185 185 spectral_matrix_regs->status = 0x30; // [0011 0000]
186 186 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
187 187 break;
188 188 case 1:
189 189 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
190 190 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
191 191 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_0_coarse_time;
192 192 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_0_fine_time;
193 193 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->buffer_address;
194 194 spectral_matrix_regs->status = 0x10; // [0001 0000]
195 195 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
196 196 {
197 197 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
198 198 }
199 199 break;
200 200 case 2:
201 201 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
202 202 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
203 203 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_1_coarse_time;
204 204 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_1_fine_time;
205 205 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
206 206 spectral_matrix_regs->status = 0x20; // [0010 0000]
207 207 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
208 208 {
209 209 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
210 210 }
211 211 break;
212 212 }
213 213 }
214 214
215 215 void spectral_matrix_isr_error_handler( unsigned char statusReg )
216 216 {
217 217 rtems_status_code status_code;
218 218
219 219 if (statusReg & 0x7c0) // [0111 1100 0000]
220 220 {
221 221 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_8 );
222 222 }
223 223
224 224 spectral_matrix_regs->status = spectral_matrix_regs->status & 0x7c0;
225 225 }
226 226
227 227 rtems_isr spectral_matrices_isr( rtems_vector_number vector )
228 228 {
229 229 // STATUS REGISTER
230 230 // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
231 231 // 10 9 8
232 232 // buffer_full ** bad_component_err ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
233 233 // 7 6 5 4 3 2 1 0
234 234
235 235 unsigned char statusReg;
236 236
237 237 statusReg = spectral_matrix_regs->status;
238 238
239 239 spectral_matrices_isr_f0( statusReg );
240 240
241 241 spectral_matrices_isr_f1( statusReg );
242 242
243 243 spectral_matrices_isr_f2( statusReg );
244 244
245 245 spectral_matrix_isr_error_handler( statusReg );
246 246 }
247 247
248 248 rtems_isr spectral_matrices_isr_simu( rtems_vector_number vector )
249 249 {
250 250 rtems_status_code status_code;
251 251
252 252 //***
253 253 // F0
254 254 nb_sm_f0 = nb_sm_f0 + 1;
255 255 if (nb_sm_f0 == NB_SM_BEFORE_AVF0 )
256 256 {
257 257 ring_node_for_averaging_sm_f0 = current_ring_node_sm_f0;
258 258 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
259 259 {
260 260 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
261 261 }
262 262 nb_sm_f0 = 0;
263 263 }
264 264
265 265 //***
266 266 // F1
267 267 nb_sm_f0_aux_f1 = nb_sm_f0_aux_f1 + 1;
268 268 if (nb_sm_f0_aux_f1 == 6)
269 269 {
270 270 nb_sm_f0_aux_f1 = 0;
271 271 nb_sm_f1 = nb_sm_f1 + 1;
272 272 }
273 273 if (nb_sm_f1 == NB_SM_BEFORE_AVF1 )
274 274 {
275 275 ring_node_for_averaging_sm_f1 = current_ring_node_sm_f1;
276 276 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
277 277 {
278 278 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
279 279 }
280 280 nb_sm_f1 = 0;
281 281 }
282 282
283 283 //***
284 284 // F2
285 285 nb_sm_f0_aux_f2 = nb_sm_f0_aux_f2 + 1;
286 286 if (nb_sm_f0_aux_f2 == 96)
287 287 {
288 288 nb_sm_f0_aux_f2 = 0;
289 289 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2;
290 290 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
291 291 {
292 292 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
293 293 }
294 294 }
295 295 }
296 296
297 297 //******************
298 298 // Spectral Matrices
299 299
300 300 void reset_nb_sm( void )
301 301 {
302 302 nb_sm_f0 = 0;
303 303 nb_sm_f0_aux_f1 = 0;
304 304 nb_sm_f0_aux_f2 = 0;
305 305
306 306 nb_sm_f1 = 0;
307 307 }
308 308
309 309 void SM_init_rings( void )
310 310 {
311 311 init_ring( sm_ring_f0, NB_RING_NODES_SM_F0, sm_f0, TOTAL_SIZE_SM );
312 312 init_ring( sm_ring_f1, NB_RING_NODES_SM_F1, sm_f1, TOTAL_SIZE_SM );
313 313 init_ring( sm_ring_f2, NB_RING_NODES_SM_F2, sm_f2, TOTAL_SIZE_SM );
314 314
315 315 DEBUG_PRINTF1("sm_ring_f0 @%x\n", (unsigned int) sm_ring_f0)
316 316 DEBUG_PRINTF1("sm_ring_f1 @%x\n", (unsigned int) sm_ring_f1)
317 317 DEBUG_PRINTF1("sm_ring_f2 @%x\n", (unsigned int) sm_ring_f2)
318 318 DEBUG_PRINTF1("sm_f0 @%x\n", (unsigned int) sm_f0)
319 319 DEBUG_PRINTF1("sm_f1 @%x\n", (unsigned int) sm_f1)
320 320 DEBUG_PRINTF1("sm_f2 @%x\n", (unsigned int) sm_f2)
321 321 }
322 322
323 323 void ASM_generic_init_ring( ring_node_asm *ring, unsigned char nbNodes )
324 324 {
325 325 unsigned char i;
326 326
327 327 ring[ nbNodes - 1 ].next
328 328 = (ring_node_asm*) &ring[ 0 ];
329 329
330 330 for(i=0; i<nbNodes-1; i++)
331 331 {
332 332 ring[ i ].next = (ring_node_asm*) &ring[ i + 1 ];
333 333 }
334 334 }
335 335
336 336 void SM_reset_current_ring_nodes( void )
337 337 {
338 338 current_ring_node_sm_f0 = sm_ring_f0[0].next;
339 339 current_ring_node_sm_f1 = sm_ring_f1[0].next;
340 340 current_ring_node_sm_f2 = sm_ring_f2[0].next;
341 341
342 342 ring_node_for_averaging_sm_f0 = NULL;
343 343 ring_node_for_averaging_sm_f1 = NULL;
344 344 ring_node_for_averaging_sm_f2 = NULL;
345 345 }
346 346
347 347 //*****************
348 348 // Basic Parameters
349 349
350 350 void BP_init_header( bp_packet *packet,
351 351 unsigned int apid, unsigned char sid,
352 352 unsigned int packetLength, unsigned char blkNr )
353 353 {
354 354 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
355 355 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
356 356 packet->reserved = 0x00;
357 357 packet->userApplication = CCSDS_USER_APP;
358 358 packet->packetID[0] = (unsigned char) (apid >> 8);
359 359 packet->packetID[1] = (unsigned char) (apid);
360 360 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
361 361 packet->packetSequenceControl[1] = 0x00;
362 362 packet->packetLength[0] = (unsigned char) (packetLength >> 8);
363 363 packet->packetLength[1] = (unsigned char) (packetLength);
364 364 // DATA FIELD HEADER
365 365 packet->spare1_pusVersion_spare2 = 0x10;
366 366 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
367 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE; // service subtype
367 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
368 368 packet->destinationID = TM_DESTINATION_ID_GROUND;
369 369 packet->time[0] = 0x00;
370 370 packet->time[1] = 0x00;
371 371 packet->time[2] = 0x00;
372 372 packet->time[3] = 0x00;
373 373 packet->time[4] = 0x00;
374 374 packet->time[5] = 0x00;
375 375 // AUXILIARY DATA HEADER
376 376 packet->sid = sid;
377 377 packet->biaStatusInfo = 0x00;
378 378 packet->acquisitionTime[0] = 0x00;
379 379 packet->acquisitionTime[1] = 0x00;
380 380 packet->acquisitionTime[2] = 0x00;
381 381 packet->acquisitionTime[3] = 0x00;
382 382 packet->acquisitionTime[4] = 0x00;
383 383 packet->acquisitionTime[5] = 0x00;
384 384 packet->pa_lfr_bp_blk_nr[0] = 0x00; // BLK_NR MSB
385 385 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
386 386 }
387 387
388 388 void BP_init_header_with_spare( bp_packet_with_spare *packet,
389 389 unsigned int apid, unsigned char sid,
390 390 unsigned int packetLength , unsigned char blkNr)
391 391 {
392 392 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
393 393 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
394 394 packet->reserved = 0x00;
395 395 packet->userApplication = CCSDS_USER_APP;
396 396 packet->packetID[0] = (unsigned char) (apid >> 8);
397 397 packet->packetID[1] = (unsigned char) (apid);
398 398 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
399 399 packet->packetSequenceControl[1] = 0x00;
400 400 packet->packetLength[0] = (unsigned char) (packetLength >> 8);
401 401 packet->packetLength[1] = (unsigned char) (packetLength);
402 402 // DATA FIELD HEADER
403 403 packet->spare1_pusVersion_spare2 = 0x10;
404 404 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
405 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE; // service subtype
405 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
406 406 packet->destinationID = TM_DESTINATION_ID_GROUND;
407 407 // AUXILIARY DATA HEADER
408 408 packet->sid = sid;
409 409 packet->biaStatusInfo = 0x00;
410 410 packet->time[0] = 0x00;
411 411 packet->time[0] = 0x00;
412 412 packet->time[0] = 0x00;
413 413 packet->time[0] = 0x00;
414 414 packet->time[0] = 0x00;
415 415 packet->time[0] = 0x00;
416 416 packet->source_data_spare = 0x00;
417 417 packet->pa_lfr_bp_blk_nr[0] = 0x00; // BLK_NR MSB
418 418 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
419 419 }
420 420
421 421 void BP_send(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
422 422 {
423 423 rtems_status_code status;
424 424
425 425 // SET THE SEQUENCE_CNT PARAMETER
426 426 increment_seq_counter_source_id( (unsigned char*) &data[ PACKET_POS_SEQUENCE_CNT ], sid );
427 427 // SEND PACKET
428 428 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
429 429 if (status != RTEMS_SUCCESSFUL)
430 430 {
431 431 printf("ERR *** in BP_send *** ERR %d\n", (int) status);
432 432 }
433 433 }
434 434
435 435 //******************
436 436 // general functions
437 437
438 438 void reset_sm_status( void )
439 439 {
440 440 // error
441 441 // 10 --------------- 9 ---------------- 8 ---------------- 7 ---------
442 442 // input_fif0_write_2 input_fifo_write_1 input_fifo_write_0 buffer_full
443 443 // ---------- 5 -- 4 -- 3 -- 2 -- 1 -- 0 --
444 444 // ready bits f2_1 f2_0 f1_1 f1_1 f0_1 f0_0
445 445
446 446 spectral_matrix_regs->status = 0x7ff; // [0111 1111 1111]
447 447 }
448 448
449 449 void reset_spectral_matrix_regs( void )
450 450 {
451 451 /** This function resets the spectral matrices module registers.
452 452 *
453 453 * The registers affected by this function are located at the following offset addresses:
454 454 *
455 455 * - 0x00 config
456 456 * - 0x04 status
457 457 * - 0x08 matrixF0_Address0
458 458 * - 0x10 matrixFO_Address1
459 459 * - 0x14 matrixF1_Address
460 460 * - 0x18 matrixF2_Address
461 461 *
462 462 */
463 463
464 464 set_sm_irq_onError( 0 );
465 465
466 466 set_sm_irq_onNewMatrix( 0 );
467 467
468 468 reset_sm_status();
469 469
470 470 // F1
471 471 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->previous->buffer_address;
472 472 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
473 473 // F2
474 474 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->previous->buffer_address;
475 475 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
476 476 // F3
477 477 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->previous->buffer_address;
478 478 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
479 479
480 480 spectral_matrix_regs->matrix_length = 0xc8; // 25 * 128 / 16 = 200 = 0xc8
481 481 }
482 482
483 483 void set_time( unsigned char *time, unsigned char * timeInBuffer )
484 484 {
485 485 time[0] = timeInBuffer[0];
486 486 time[1] = timeInBuffer[1];
487 487 time[2] = timeInBuffer[2];
488 488 time[3] = timeInBuffer[3];
489 489 time[4] = timeInBuffer[6];
490 490 time[5] = timeInBuffer[7];
491 491 }
492 492
493 493 unsigned long long int get_acquisition_time( unsigned char *timePtr )
494 494 {
495 495 unsigned long long int acquisitionTimeAslong;
496 496 acquisitionTimeAslong = 0x00;
497 497 acquisitionTimeAslong = ( (unsigned long long int) (timePtr[0] & 0x7f) << 40 ) // [0111 1111] mask the synchronization bit
498 498 + ( (unsigned long long int) timePtr[1] << 32 )
499 499 + ( (unsigned long long int) timePtr[2] << 24 )
500 500 + ( (unsigned long long int) timePtr[3] << 16 )
501 501 + ( (unsigned long long int) timePtr[6] << 8 )
502 502 + ( (unsigned long long int) timePtr[7] );
503 503 return acquisitionTimeAslong;
504 504 }
505 505
506 506 unsigned char getSID( rtems_event_set event )
507 507 {
508 508 unsigned char sid;
509 509
510 510 rtems_event_set eventSetBURST;
511 511 rtems_event_set eventSetSBM;
512 512
513 513 //******
514 514 // BURST
515 515 eventSetBURST = RTEMS_EVENT_BURST_BP1_F0
516 516 | RTEMS_EVENT_BURST_BP1_F1
517 517 | RTEMS_EVENT_BURST_BP2_F0
518 518 | RTEMS_EVENT_BURST_BP2_F1;
519 519
520 520 //****
521 521 // SBM
522 522 eventSetSBM = RTEMS_EVENT_SBM_BP1_F0
523 523 | RTEMS_EVENT_SBM_BP1_F1
524 524 | RTEMS_EVENT_SBM_BP2_F0
525 525 | RTEMS_EVENT_SBM_BP2_F1;
526 526
527 527 if (event & eventSetBURST)
528 528 {
529 529 sid = SID_BURST_BP1_F0;
530 530 }
531 531 else if (event & eventSetSBM)
532 532 {
533 533 sid = SID_SBM1_BP1_F0;
534 534 }
535 535 else
536 536 {
537 537 sid = 0;
538 538 }
539 539
540 540 return sid;
541 541 }
542 542
543 543 void extractReImVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
544 544 {
545 545 unsigned int i;
546 546 float re;
547 547 float im;
548 548
549 549 for (i=0; i<NB_BINS_PER_SM; i++){
550 550 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i * 2 ];
551 551 im = inputASM[ (asmComponent*NB_BINS_PER_SM) + i * 2 + 1];
552 552 outputASM[ (asmComponent *NB_BINS_PER_SM) + i] = re;
553 553 outputASM[ (asmComponent+1)*NB_BINS_PER_SM + i] = im;
554 554 }
555 555 }
556 556
557 557 void copyReVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
558 558 {
559 559 unsigned int i;
560 560 float re;
561 561
562 562 for (i=0; i<NB_BINS_PER_SM; i++){
563 563 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i];
564 564 outputASM[ (asmComponent*NB_BINS_PER_SM) + i] = re;
565 565 }
566 566 }
567 567
568 568 void ASM_patch( float *inputASM, float *outputASM )
569 569 {
570 570 extractReImVectors( inputASM, outputASM, 1); // b1b2
571 571 extractReImVectors( inputASM, outputASM, 3 ); // b1b3
572 572 extractReImVectors( inputASM, outputASM, 5 ); // b1e1
573 573 extractReImVectors( inputASM, outputASM, 7 ); // b1e2
574 574 extractReImVectors( inputASM, outputASM, 10 ); // b2b3
575 575 extractReImVectors( inputASM, outputASM, 12 ); // b2e1
576 576 extractReImVectors( inputASM, outputASM, 14 ); // b2e2
577 577 extractReImVectors( inputASM, outputASM, 17 ); // b3e1
578 578 extractReImVectors( inputASM, outputASM, 19 ); // b3e2
579 579 extractReImVectors( inputASM, outputASM, 22 ); // e1e2
580 580
581 581 copyReVectors(inputASM, outputASM, 0 ); // b1b1
582 582 copyReVectors(inputASM, outputASM, 9 ); // b2b2
583 583 copyReVectors(inputASM, outputASM, 16); // b3b3
584 584 copyReVectors(inputASM, outputASM, 21); // e1e1
585 585 copyReVectors(inputASM, outputASM, 24); // e2e2
586 586 }
Show file before | Show file after | Hide comments
@@ -1,1118 +1,1133
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 case TC_SUBTYPE_RESET:
71 result = action_reset( &TC, queue_snd_id, time );
72 close_action( &TC, result, queue_snd_id );
73 break;
74 //
75 case TC_SUBTYPE_LOAD_COMM:
76 result = action_load_common_par( &TC );
77 close_action( &TC, result, queue_snd_id );
78 break;
79 //
80 case TC_SUBTYPE_LOAD_NORM:
81 result = action_load_normal_par( &TC, queue_snd_id, time );
82 close_action( &TC, result, queue_snd_id );
83 break;
84 //
85 case TC_SUBTYPE_LOAD_BURST:
86 result = action_load_burst_par( &TC, queue_snd_id, time );
87 close_action( &TC, result, queue_snd_id );
88 break;
89 //
90 case TC_SUBTYPE_LOAD_SBM1:
91 result = action_load_sbm1_par( &TC, queue_snd_id, time );
92 close_action( &TC, result, queue_snd_id );
93 break;
94 //
95 case TC_SUBTYPE_LOAD_SBM2:
96 result = action_load_sbm2_par( &TC, queue_snd_id, time );
97 close_action( &TC, result, queue_snd_id );
98 break;
99 //
100 case TC_SUBTYPE_DUMP:
101 result = action_dump_par( queue_snd_id );
102 close_action( &TC, result, queue_snd_id );
103 break;
104 //
105 case TC_SUBTYPE_ENTER:
106 result = action_enter_mode( &TC, queue_snd_id );
107 close_action( &TC, result, queue_snd_id );
108 break;
109 //
110 case TC_SUBTYPE_UPDT_INFO:
111 result = action_update_info( &TC, queue_snd_id );
112 close_action( &TC, result, queue_snd_id );
113 break;
114 //
115 case TC_SUBTYPE_EN_CAL:
116 result = action_enable_calibration( &TC, queue_snd_id, time );
117 close_action( &TC, result, queue_snd_id );
118 break;
119 //
120 case TC_SUBTYPE_DIS_CAL:
121 result = action_disable_calibration( &TC, queue_snd_id, time );
122 close_action( &TC, result, queue_snd_id );
123 break;
124 //
125 case TC_SUBTYPE_UPDT_TIME:
126 result = action_update_time( &TC );
127 close_action( &TC, result, queue_snd_id );
128 break;
129 //
130 default:
131 break;
70 case TC_SUBTYPE_RESET:
71 result = action_reset( &TC, queue_snd_id, time );
72 close_action( &TC, result, queue_snd_id );
73 break;
74 case TC_SUBTYPE_LOAD_COMM:
75 result = action_load_common_par( &TC );
76 close_action( &TC, result, queue_snd_id );
77 break;
78 case TC_SUBTYPE_LOAD_NORM:
79 result = action_load_normal_par( &TC, queue_snd_id, time );
80 close_action( &TC, result, queue_snd_id );
81 break;
82 case TC_SUBTYPE_LOAD_BURST:
83 result = action_load_burst_par( &TC, queue_snd_id, time );
84 close_action( &TC, result, queue_snd_id );
85 break;
86 case TC_SUBTYPE_LOAD_SBM1:
87 result = action_load_sbm1_par( &TC, queue_snd_id, time );
88 close_action( &TC, result, queue_snd_id );
89 break;
90 case TC_SUBTYPE_LOAD_SBM2:
91 result = action_load_sbm2_par( &TC, queue_snd_id, time );
92 close_action( &TC, result, queue_snd_id );
93 break;
94 case TC_SUBTYPE_DUMP:
95 result = action_dump_par( queue_snd_id );
96 close_action( &TC, result, queue_snd_id );
97 break;
98 case TC_SUBTYPE_ENTER:
99 result = action_enter_mode( &TC, queue_snd_id );
100 close_action( &TC, result, queue_snd_id );
101 break;
102 case TC_SUBTYPE_UPDT_INFO:
103 result = action_update_info( &TC, queue_snd_id );
104 close_action( &TC, result, queue_snd_id );
105 break;
106 case TC_SUBTYPE_EN_CAL:
107 result = action_enable_calibration( &TC, queue_snd_id, time );
108 close_action( &TC, result, queue_snd_id );
109 break;
110 case TC_SUBTYPE_DIS_CAL:
111 result = action_disable_calibration( &TC, queue_snd_id, time );
112 close_action( &TC, result, queue_snd_id );
113 break;
114 case TC_SUBTYPE_LOAD_K:
115 result = action_load_kcoefficients( &TC, queue_snd_id, time );
116 close_action( &TC, result, queue_snd_id );
117 break;
118 case TC_SUBTYPE_DUMP_K:
119 result = action_dump_kcoefficients( &TC, queue_snd_id, time );
120 close_action( &TC, result, queue_snd_id );
121 break;
122 case TC_SUBTYPE_LOAD_FBINS:
123 result = action_load_fbins_mask( &TC, queue_snd_id, time );
124 close_action( &TC, result, queue_snd_id );
125 break;
126 case TC_SUBTYPE_UPDT_TIME:
127 result = action_update_time( &TC );
128 close_action( &TC, result, queue_snd_id );
129 break;
130 default:
131 break;
132 132 }
133 133 }
134 134 }
135 135 }
136 136
137 137 //***********
138 138 // TC ACTIONS
139 139
140 140 int action_reset(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
141 141 {
142 142 /** This function executes specific actions when a TC_LFR_RESET TeleCommand has been received.
143 143 *
144 144 * @param TC points to the TeleCommand packet that is being processed
145 145 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
146 146 *
147 147 */
148 148
149 149 printf("this is the end!!!\n");
150 150 exit(0);
151 151 send_tm_lfr_tc_exe_not_implemented( TC, queue_id, time );
152 152 return LFR_DEFAULT;
153 153 }
154 154
155 155 int action_enter_mode(ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
156 156 {
157 157 /** This function executes specific actions when a TC_LFR_ENTER_MODE TeleCommand has been received.
158 158 *
159 159 * @param TC points to the TeleCommand packet that is being processed
160 160 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
161 161 *
162 162 */
163 163
164 164 rtems_status_code status;
165 165 unsigned char requestedMode;
166 166 unsigned int *transitionCoarseTime_ptr;
167 167 unsigned int transitionCoarseTime;
168 168 unsigned char * bytePosPtr;
169 169
170 printTaskID();
171
170 172 bytePosPtr = (unsigned char *) &TC->packetID;
171 173
172 174 requestedMode = bytePosPtr[ BYTE_POS_CP_MODE_LFR_SET ];
173 175 transitionCoarseTime_ptr = (unsigned int *) ( &bytePosPtr[ BYTE_POS_CP_LFR_ENTER_MODE_TIME ] );
174 176 transitionCoarseTime = (*transitionCoarseTime_ptr) & 0x7fffffff;
175 177
176 178 status = check_mode_value( requestedMode );
177 179
178 180 if ( status != LFR_SUCCESSFUL ) // the mode value is inconsistent
179 181 {
180 182 send_tm_lfr_tc_exe_inconsistent( TC, queue_id, BYTE_POS_CP_MODE_LFR_SET, requestedMode );
181 183 }
182 184 else // the mode value is consistent, check the transition
183 185 {
184 186 status = check_mode_transition(requestedMode);
185 187 if (status != LFR_SUCCESSFUL)
186 188 {
187 189 PRINTF("ERR *** in action_enter_mode *** check_mode_transition\n")
188 190 send_tm_lfr_tc_exe_not_executable( TC, queue_id );
189 191 }
190 192 }
191 193
192 194 if ( status == LFR_SUCCESSFUL ) // the transition is valid, enter the mode
193 195 {
194 196 status = check_transition_date( transitionCoarseTime );
195 197 if (status != LFR_SUCCESSFUL)
196 198 {
197 199 PRINTF("ERR *** in action_enter_mode *** check_transition_date\n")
198 200 send_tm_lfr_tc_exe_inconsistent( TC, queue_id,
199 201 BYTE_POS_CP_LFR_ENTER_MODE_TIME,
200 202 bytePosPtr[ BYTE_POS_CP_LFR_ENTER_MODE_TIME + 3 ] );
201 203 }
202 204 }
203 205
204 206 if ( status == LFR_SUCCESSFUL ) // the date is valid, enter the mode
205 207 {
206 208 PRINTF1("OK *** in action_enter_mode *** enter mode %d\n", requestedMode);
207 209 status = enter_mode( requestedMode, transitionCoarseTime );
208 210 }
209 211
210 212 return status;
211 213 }
212 214
213 215 int action_update_info(ccsdsTelecommandPacket_t *TC, rtems_id queue_id)
214 216 {
215 217 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
216 218 *
217 219 * @param TC points to the TeleCommand packet that is being processed
218 220 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
219 221 *
220 222 * @return LFR directive status code:
221 223 * - LFR_DEFAULT
222 224 * - LFR_SUCCESSFUL
223 225 *
224 226 */
225 227
226 228 unsigned int val;
227 229 int result;
228 230 unsigned int status;
229 231 unsigned char mode;
230 232 unsigned char * bytePosPtr;
231 233
232 234 bytePosPtr = (unsigned char *) &TC->packetID;
233 235
234 236 // check LFR mode
235 237 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET5 ] & 0x1e) >> 1;
236 238 status = check_update_info_hk_lfr_mode( mode );
237 239 if (status == LFR_SUCCESSFUL) // check TDS mode
238 240 {
239 241 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET6 ] & 0xf0) >> 4;
240 242 status = check_update_info_hk_tds_mode( mode );
241 243 }
242 244 if (status == LFR_SUCCESSFUL) // check THR mode
243 245 {
244 246 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET6 ] & 0x0f);
245 247 status = check_update_info_hk_thr_mode( mode );
246 248 }
247 249 if (status == LFR_SUCCESSFUL) // if the parameter check is successful
248 250 {
249 251 val = housekeeping_packet.hk_lfr_update_info_tc_cnt[0] * 256
250 252 + housekeeping_packet.hk_lfr_update_info_tc_cnt[1];
251 253 val++;
252 254 housekeeping_packet.hk_lfr_update_info_tc_cnt[0] = (unsigned char) (val >> 8);
253 255 housekeeping_packet.hk_lfr_update_info_tc_cnt[1] = (unsigned char) (val);
254 256 }
255 257
256 258 result = status;
257 259
258 260 return result;
259 261 }
260 262
261 263 int action_enable_calibration(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
262 264 {
263 265 /** This function executes specific actions when a TC_LFR_ENABLE_CALIBRATION TeleCommand has been received.
264 266 *
265 267 * @param TC points to the TeleCommand packet that is being processed
266 268 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
267 269 *
268 270 */
269 271
270 272 int result;
271 273
272 274 result = LFR_DEFAULT;
273 275
274 276 startCalibration();
275 277
276 278 result = LFR_SUCCESSFUL;
277 279
278 280 return result;
279 281 }
280 282
281 283 int action_disable_calibration(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
282 284 {
283 285 /** This function executes specific actions when a TC_LFR_DISABLE_CALIBRATION TeleCommand has been received.
284 286 *
285 287 * @param TC points to the TeleCommand packet that is being processed
286 288 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
287 289 *
288 290 */
289 291
290 292 int result;
291 293
292 294 result = LFR_DEFAULT;
293 295
294 296 stopCalibration();
295 297
296 298 result = LFR_SUCCESSFUL;
297 299
298 300 return result;
299 301 }
300 302
301 303 int action_update_time(ccsdsTelecommandPacket_t *TC)
302 304 {
303 305 /** This function executes specific actions when a TC_LFR_UPDATE_TIME TeleCommand has been received.
304 306 *
305 307 * @param TC points to the TeleCommand packet that is being processed
306 308 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
307 309 *
308 310 * @return LFR_SUCCESSFUL
309 311 *
310 312 */
311 313
312 314 unsigned int val;
313 315
314 316 time_management_regs->coarse_time_load = (TC->dataAndCRC[0] << 24)
315 317 + (TC->dataAndCRC[1] << 16)
316 318 + (TC->dataAndCRC[2] << 8)
317 319 + TC->dataAndCRC[3];
318 320
319 321 val = housekeeping_packet.hk_lfr_update_time_tc_cnt[0] * 256
320 322 + housekeeping_packet.hk_lfr_update_time_tc_cnt[1];
321 323 val++;
322 324 housekeeping_packet.hk_lfr_update_time_tc_cnt[0] = (unsigned char) (val >> 8);
323 325 housekeeping_packet.hk_lfr_update_time_tc_cnt[1] = (unsigned char) (val);
324 326
325 327 return LFR_SUCCESSFUL;
326 328 }
327 329
328 330 //*******************
329 331 // ENTERING THE MODES
330 332 int check_mode_value( unsigned char requestedMode )
331 333 {
332 334 int status;
333 335
334 336 if ( (requestedMode != LFR_MODE_STANDBY)
335 337 && (requestedMode != LFR_MODE_NORMAL) && (requestedMode != LFR_MODE_BURST)
336 338 && (requestedMode != LFR_MODE_SBM1) && (requestedMode != LFR_MODE_SBM2) )
337 339 {
338 340 status = LFR_DEFAULT;
339 341 }
340 342 else
341 343 {
342 344 status = LFR_SUCCESSFUL;
343 345 }
344 346
345 347 return status;
346 348 }
347 349
348 350 int check_mode_transition( unsigned char requestedMode )
349 351 {
350 352 /** This function checks the validity of the transition requested by the TC_LFR_ENTER_MODE.
351 353 *
352 354 * @param requestedMode is the mode requested by the TC_LFR_ENTER_MODE
353 355 *
354 356 * @return LFR directive status codes:
355 357 * - LFR_SUCCESSFUL - the transition is authorized
356 358 * - LFR_DEFAULT - the transition is not authorized
357 359 *
358 360 */
359 361
360 362 int status;
361 363
362 364 switch (requestedMode)
363 365 {
364 366 case LFR_MODE_STANDBY:
365 367 if ( lfrCurrentMode == LFR_MODE_STANDBY ) {
366 368 status = LFR_DEFAULT;
367 369 }
368 370 else
369 371 {
370 372 status = LFR_SUCCESSFUL;
371 373 }
372 374 break;
373 375 case LFR_MODE_NORMAL:
374 376 if ( lfrCurrentMode == LFR_MODE_NORMAL ) {
375 377 status = LFR_DEFAULT;
376 378 }
377 379 else {
378 380 status = LFR_SUCCESSFUL;
379 381 }
380 382 break;
381 383 case LFR_MODE_BURST:
382 384 if ( lfrCurrentMode == LFR_MODE_BURST ) {
383 385 status = LFR_DEFAULT;
384 386 }
385 387 else {
386 388 status = LFR_SUCCESSFUL;
387 389 }
388 390 break;
389 391 case LFR_MODE_SBM1:
390 392 if ( lfrCurrentMode == LFR_MODE_SBM1 ) {
391 393 status = LFR_DEFAULT;
392 394 }
393 395 else {
394 396 status = LFR_SUCCESSFUL;
395 397 }
396 398 break;
397 399 case LFR_MODE_SBM2:
398 400 if ( lfrCurrentMode == LFR_MODE_SBM2 ) {
399 401 status = LFR_DEFAULT;
400 402 }
401 403 else {
402 404 status = LFR_SUCCESSFUL;
403 405 }
404 406 break;
405 407 default:
406 408 status = LFR_DEFAULT;
407 409 break;
408 410 }
409 411
410 412 return status;
411 413 }
412 414
413 415 int check_transition_date( unsigned int transitionCoarseTime )
414 416 {
415 417 int status;
416 418 unsigned int localCoarseTime;
417 419 unsigned int deltaCoarseTime;
418 420
419 421 status = LFR_SUCCESSFUL;
420 422
421 423 if (transitionCoarseTime == 0) // transition time = 0 means an instant transition
422 424 {
423 425 status = LFR_SUCCESSFUL;
424 426 }
425 427 else
426 428 {
427 429 localCoarseTime = time_management_regs->coarse_time & 0x7fffffff;
428 430
429 431 PRINTF2("localTime = %x, transitionTime = %x\n", localCoarseTime, transitionCoarseTime)
430 432
431 433 if ( transitionCoarseTime <= localCoarseTime ) // SSS-CP-EQS-322
432 434 {
433 435 status = LFR_DEFAULT;
434 436 PRINTF("ERR *** in check_transition_date *** transitionCoarseTime <= localCoarseTime\n")
435 437 }
436 438
437 439 if (status == LFR_SUCCESSFUL)
438 440 {
439 441 deltaCoarseTime = transitionCoarseTime - localCoarseTime;
440 442 if ( deltaCoarseTime > 3 ) // SSS-CP-EQS-323
441 443 {
442 444 status = LFR_DEFAULT;
443 445 PRINTF1("ERR *** in check_transition_date *** deltaCoarseTime = %x\n", deltaCoarseTime)
444 446 }
445 447 }
446 448 }
447 449
448 450 return status;
449 451 }
450 452
451 453 int stop_current_mode( void )
452 454 {
453 455 /** This function stops the current mode by masking interrupt lines and suspending science tasks.
454 456 *
455 457 * @return RTEMS directive status codes:
456 458 * - RTEMS_SUCCESSFUL - task restarted successfully
457 459 * - RTEMS_INVALID_ID - task id invalid
458 460 * - RTEMS_ALREADY_SUSPENDED - task already suspended
459 461 *
460 462 */
461 463
462 464 rtems_status_code status;
463 465
464 466 status = RTEMS_SUCCESSFUL;
465 467
466 468 // (1) mask interruptions
467 469 LEON_Mask_interrupt( IRQ_WAVEFORM_PICKER ); // mask waveform picker interrupt
468 470 LEON_Mask_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
469 471
470 472 // (2) reset waveform picker registers
471 473 reset_wfp_burst_enable(); // reset burst and enable bits
472 474 reset_wfp_status(); // reset all the status bits
473 475
474 476 // (3) reset spectral matrices registers
475 477 set_sm_irq_onNewMatrix( 0 ); // stop the spectral matrices
476 478 reset_sm_status();
477 479
478 480 // reset lfr VHDL module
479 481 reset_lfr();
480 482
481 483 reset_extractSWF(); // reset the extractSWF flag to false
482 484
483 485 // (4) clear interruptions
484 486 LEON_Clear_interrupt( IRQ_WAVEFORM_PICKER ); // clear waveform picker interrupt
485 487 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
486 488
487 489 // <Spectral Matrices simulator>
488 490 LEON_Mask_interrupt( IRQ_SM_SIMULATOR ); // mask spectral matrix interrupt simulator
489 491 timer_stop( (gptimer_regs_t*) REGS_ADDR_GPTIMER, TIMER_SM_SIMULATOR );
490 492 LEON_Clear_interrupt( IRQ_SM_SIMULATOR ); // clear spectral matrix interrupt simulator
491 493 // </Spectral Matrices simulator>
492 494
493 495 // suspend several tasks
494 496 if (lfrCurrentMode != LFR_MODE_STANDBY) {
495 497 status = suspend_science_tasks();
496 498 }
497 499
498 500 if (status != RTEMS_SUCCESSFUL)
499 501 {
500 502 PRINTF1("in stop_current_mode *** in suspend_science_tasks *** ERR code: %d\n", status)
501 503 }
502 504
503 505 return status;
504 506 }
505 507
506 508 int enter_mode( unsigned char mode, unsigned int transitionCoarseTime )
507 509 {
508 510 /** This function is launched after a mode transition validation.
509 511 *
510 512 * @param mode is the mode in which LFR will be put.
511 513 *
512 514 * @return RTEMS directive status codes:
513 515 * - RTEMS_SUCCESSFUL - the mode has been entered successfully
514 516 * - RTEMS_NOT_SATISFIED - the mode has not been entered successfully
515 517 *
516 518 */
517 519
518 520 rtems_status_code status;
519 521
520 522 //**********************
521 523 // STOP THE CURRENT MODE
522 524 status = stop_current_mode();
523 525 if (status != RTEMS_SUCCESSFUL)
524 526 {
525 527 PRINTF1("ERR *** in enter_mode *** stop_current_mode with mode = %d\n", mode)
526 528 }
527 529
528 530 //*************************
529 531 // ENTER THE REQUESTED MODE
530 532 if ( (mode == LFR_MODE_NORMAL) || (mode == LFR_MODE_BURST)
531 533 || (mode == LFR_MODE_SBM1) || (mode == LFR_MODE_SBM2) )
532 534 {
533 535 #ifdef PRINT_TASK_STATISTICS
534 536 rtems_cpu_usage_reset();
535 537 maxCount = 0;
536 538 #endif
537 539 status = restart_science_tasks( mode );
538 540 launch_spectral_matrix( );
539 541 launch_waveform_picker( mode, transitionCoarseTime );
540 542 // launch_spectral_matrix_simu( );
541 543 }
542 544 else if ( mode == LFR_MODE_STANDBY )
543 545 {
544 546 #ifdef PRINT_TASK_STATISTICS
545 547 rtems_cpu_usage_report();
546 548 #endif
547 549
548 550 #ifdef PRINT_STACK_REPORT
549 551 PRINTF("stack report selected\n")
550 552 rtems_stack_checker_report_usage();
551 553 #endif
552 554 PRINTF1("maxCount = %d\n", maxCount)
553 555 }
554 556 else
555 557 {
556 558 status = RTEMS_UNSATISFIED;
557 559 }
558 560
559 561 if (status != RTEMS_SUCCESSFUL)
560 562 {
561 563 PRINTF1("ERR *** in enter_mode *** status = %d\n", status)
562 564 status = RTEMS_UNSATISFIED;
563 565 }
564 566
565 567 return status;
566 568 }
567 569
568 570 int restart_science_tasks(unsigned char lfrRequestedMode )
569 571 {
570 572 /** This function is used to restart all science tasks.
571 573 *
572 574 * @return RTEMS directive status codes:
573 575 * - RTEMS_SUCCESSFUL - task restarted successfully
574 576 * - RTEMS_INVALID_ID - task id invalid
575 577 * - RTEMS_INCORRECT_STATE - task never started
576 578 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
577 579 *
578 580 * Science tasks are AVF0, PRC0, WFRM, CWF3, CW2, CWF1
579 581 *
580 582 */
581 583
582 584 rtems_status_code status[10];
583 585 rtems_status_code ret;
584 586
585 587 ret = RTEMS_SUCCESSFUL;
586 588
587 589 status[0] = rtems_task_restart( Task_id[TASKID_AVF0], lfrRequestedMode );
588 590 if (status[0] != RTEMS_SUCCESSFUL)
589 591 {
590 592 PRINTF1("in restart_science_task *** AVF0 ERR %d\n", status[0])
591 593 }
592 594
593 595 status[1] = rtems_task_restart( Task_id[TASKID_PRC0], lfrRequestedMode );
594 596 if (status[1] != RTEMS_SUCCESSFUL)
595 597 {
596 598 PRINTF1("in restart_science_task *** PRC0 ERR %d\n", status[1])
597 599 }
598 600
599 601 status[2] = rtems_task_restart( Task_id[TASKID_WFRM],1 );
600 602 if (status[2] != RTEMS_SUCCESSFUL)
601 603 {
602 604 PRINTF1("in restart_science_task *** WFRM ERR %d\n", status[2])
603 605 }
604 606
605 607 status[3] = rtems_task_restart( Task_id[TASKID_CWF3],1 );
606 608 if (status[3] != RTEMS_SUCCESSFUL)
607 609 {
608 610 PRINTF1("in restart_science_task *** CWF3 ERR %d\n", status[3])
609 611 }
610 612
611 613 status[4] = rtems_task_restart( Task_id[TASKID_CWF2],1 );
612 614 if (status[4] != RTEMS_SUCCESSFUL)
613 615 {
614 616 PRINTF1("in restart_science_task *** CWF2 ERR %d\n", status[4])
615 617 }
616 618
617 619 status[5] = rtems_task_restart( Task_id[TASKID_CWF1],1 );
618 620 if (status[5] != RTEMS_SUCCESSFUL)
619 621 {
620 622 PRINTF1("in restart_science_task *** CWF1 ERR %d\n", status[5])
621 623 }
622 624
623 625 status[6] = rtems_task_restart( Task_id[TASKID_AVF1], lfrRequestedMode );
624 626 if (status[6] != RTEMS_SUCCESSFUL)
625 627 {
626 628 PRINTF1("in restart_science_task *** AVF1 ERR %d\n", status[6])
627 629 }
628 630
629 631 status[7] = rtems_task_restart( Task_id[TASKID_PRC1],lfrRequestedMode );
630 632 if (status[7] != RTEMS_SUCCESSFUL)
631 633 {
632 634 PRINTF1("in restart_science_task *** PRC1 ERR %d\n", status[7])
633 635 }
634 636
635 637 status[8] = rtems_task_restart( Task_id[TASKID_AVF2], 1 );
636 638 if (status[8] != RTEMS_SUCCESSFUL)
637 639 {
638 640 PRINTF1("in restart_science_task *** AVF2 ERR %d\n", status[8])
639 641 }
640 642
641 643 status[9] = rtems_task_restart( Task_id[TASKID_PRC2], 1 );
642 644 if (status[9] != RTEMS_SUCCESSFUL)
643 645 {
644 646 PRINTF1("in restart_science_task *** PRC2 ERR %d\n", status[9])
645 647 }
646 648
647 649 if ( (status[0] != RTEMS_SUCCESSFUL) || (status[1] != RTEMS_SUCCESSFUL) ||
648 650 (status[2] != RTEMS_SUCCESSFUL) || (status[3] != RTEMS_SUCCESSFUL) ||
649 651 (status[4] != RTEMS_SUCCESSFUL) || (status[5] != RTEMS_SUCCESSFUL) ||
650 652 (status[6] != RTEMS_SUCCESSFUL) || (status[7] != RTEMS_SUCCESSFUL) ||
651 653 (status[8] != RTEMS_SUCCESSFUL) || (status[9] != RTEMS_SUCCESSFUL) )
652 654 {
653 655 ret = RTEMS_UNSATISFIED;
654 656 }
655 657
656 658 return ret;
657 659 }
658 660
659 661 int suspend_science_tasks()
660 662 {
661 663 /** This function suspends the science tasks.
662 664 *
663 665 * @return RTEMS directive status codes:
664 666 * - RTEMS_SUCCESSFUL - task restarted successfully
665 667 * - RTEMS_INVALID_ID - task id invalid
666 668 * - RTEMS_ALREADY_SUSPENDED - task already suspended
667 669 *
668 670 */
669 671
670 672 rtems_status_code status;
671 673
674 printf("in suspend_science_tasks\n");
675 printTaskID();
676
672 677 status = rtems_task_suspend( Task_id[TASKID_AVF0] ); // suspend AVF0
673 678 if (status != RTEMS_SUCCESSFUL)
674 679 {
675 680 PRINTF1("in suspend_science_task *** AVF0 ERR %d\n", status)
676 681 }
677 682 if (status == RTEMS_SUCCESSFUL) // suspend PRC0
678 683 {
679 684 status = rtems_task_suspend( Task_id[TASKID_PRC0] );
680 685 if (status != RTEMS_SUCCESSFUL)
681 686 {
682 687 PRINTF1("in suspend_science_task *** PRC0 ERR %d\n", status)
683 688 }
684 689 }
685 690 if (status == RTEMS_SUCCESSFUL) // suspend AVF1
686 691 {
687 692 status = rtems_task_suspend( Task_id[TASKID_AVF1] );
688 693 if (status != RTEMS_SUCCESSFUL)
689 694 {
690 695 PRINTF1("in suspend_science_task *** AVF1 ERR %d\n", status)
691 696 }
692 697 }
693 698 if (status == RTEMS_SUCCESSFUL) // suspend PRC1
694 699 {
695 700 status = rtems_task_suspend( Task_id[TASKID_PRC1] );
696 701 if (status != RTEMS_SUCCESSFUL)
697 702 {
698 703 PRINTF1("in suspend_science_task *** PRC1 ERR %d\n", status)
699 704 }
700 705 }
701 706 if (status == RTEMS_SUCCESSFUL) // suspend AVF2
702 707 {
703 708 status = rtems_task_suspend( Task_id[TASKID_AVF2] );
704 709 if (status != RTEMS_SUCCESSFUL)
705 710 {
706 711 PRINTF1("in suspend_science_task *** AVF2 ERR %d\n", status)
707 712 }
708 713 }
709 714 if (status == RTEMS_SUCCESSFUL) // suspend PRC2
710 715 {
711 716 status = rtems_task_suspend( Task_id[TASKID_PRC2] );
712 717 if (status != RTEMS_SUCCESSFUL)
713 718 {
714 719 PRINTF1("in suspend_science_task *** PRC2 ERR %d\n", status)
715 720 }
716 721 }
717 722 if (status == RTEMS_SUCCESSFUL) // suspend WFRM
718 723 {
719 724 status = rtems_task_suspend( Task_id[TASKID_WFRM] );
720 725 if (status != RTEMS_SUCCESSFUL)
721 726 {
722 727 PRINTF1("in suspend_science_task *** WFRM ERR %d\n", status)
723 728 }
724 729 }
725 730 if (status == RTEMS_SUCCESSFUL) // suspend CWF3
726 731 {
727 732 status = rtems_task_suspend( Task_id[TASKID_CWF3] );
728 733 if (status != RTEMS_SUCCESSFUL)
729 734 {
730 735 PRINTF1("in suspend_science_task *** CWF3 ERR %d\n", status)
731 736 }
732 737 }
733 738 if (status == RTEMS_SUCCESSFUL) // suspend CWF2
734 739 {
735 740 status = rtems_task_suspend( Task_id[TASKID_CWF2] );
736 741 if (status != RTEMS_SUCCESSFUL)
737 742 {
738 743 PRINTF1("in suspend_science_task *** CWF2 ERR %d\n", status)
739 744 }
740 745 }
741 746 if (status == RTEMS_SUCCESSFUL) // suspend CWF1
742 747 {
743 748 status = rtems_task_suspend( Task_id[TASKID_CWF1] );
744 749 if (status != RTEMS_SUCCESSFUL)
745 750 {
746 751 PRINTF1("in suspend_science_task *** CWF1 ERR %d\n", status)
747 752 }
748 753 }
749 754
750 755 return status;
751 756 }
752 757
753 758 void launch_waveform_picker( unsigned char mode, unsigned int transitionCoarseTime )
754 759 {
755 760 WFP_reset_current_ring_nodes();
756 761
757 762 reset_waveform_picker_regs();
758 763
759 764 set_wfp_burst_enable_register( mode );
760 765
761 766 LEON_Clear_interrupt( IRQ_WAVEFORM_PICKER );
762 767 LEON_Unmask_interrupt( IRQ_WAVEFORM_PICKER );
763 768
764 769 if (transitionCoarseTime == 0)
765 770 {
766 771 waveform_picker_regs->start_date = time_management_regs->coarse_time;
767 772 }
768 773 else
769 774 {
770 775 waveform_picker_regs->start_date = transitionCoarseTime;
771 776 }
772 777
773 778 }
774 779
775 780 void launch_spectral_matrix( void )
776 781 {
777 782 SM_reset_current_ring_nodes();
778 783
779 784 reset_spectral_matrix_regs();
780 785
781 786 reset_nb_sm();
782 787
783 788 set_sm_irq_onNewMatrix( 1 );
784 789
785 790 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX );
786 791 LEON_Unmask_interrupt( IRQ_SPECTRAL_MATRIX );
787 792
788 793 }
789 794
790 795 void launch_spectral_matrix_simu( void )
791 796 {
792 797 SM_reset_current_ring_nodes();
793 798 reset_spectral_matrix_regs();
794 799 reset_nb_sm();
795 800
796 801 // Spectral Matrices simulator
797 802 timer_start( (gptimer_regs_t*) REGS_ADDR_GPTIMER, TIMER_SM_SIMULATOR );
798 803 LEON_Clear_interrupt( IRQ_SM_SIMULATOR );
799 804 LEON_Unmask_interrupt( IRQ_SM_SIMULATOR );
800 805 }
801 806
802 807 void set_sm_irq_onNewMatrix( unsigned char value )
803 808 {
804 809 if (value == 1)
805 810 {
806 811 spectral_matrix_regs->config = spectral_matrix_regs->config | 0x01;
807 812 }
808 813 else
809 814 {
810 815 spectral_matrix_regs->config = spectral_matrix_regs->config & 0xfffffffe; // 1110
811 816 }
812 817 }
813 818
814 819 void set_sm_irq_onError( unsigned char value )
815 820 {
816 821 if (value == 1)
817 822 {
818 823 spectral_matrix_regs->config = spectral_matrix_regs->config | 0x02;
819 824 }
820 825 else
821 826 {
822 827 spectral_matrix_regs->config = spectral_matrix_regs->config & 0xfffffffd; // 1101
823 828 }
824 829 }
825 830
826 831 //*****************************
827 832 // CONFIGURE CALIBRATION SIGNAL
828 833 void setCalibrationPrescaler( unsigned int prescaler )
829 834 {
830 835 // prescaling of the master clock (25 MHz)
831 836 // master clock is divided by 2^prescaler
832 837 time_management_regs->calPrescaler = prescaler;
833 838 }
834 839
835 840 void setCalibrationDivisor( unsigned int divisionFactor )
836 841 {
837 842 // division of the prescaled clock by the division factor
838 843 time_management_regs->calDivisor = divisionFactor;
839 844 }
840 845
841 846 void setCalibrationData( void ){
842 847 unsigned int k;
843 848 unsigned short data;
844 849 float val;
845 850 float f0;
846 851 float f1;
847 852 float fs;
848 853 float Ts;
849 854 float scaleFactor;
850 855
851 856 f0 = 625;
852 857 f1 = 10000;
853 858 fs = 160256.410;
854 859 Ts = 1. / fs;
855 860 scaleFactor = 0.125 / 0.000654; // 191, 500 mVpp, 2 sinus waves => 250 mVpp each, amplitude = 125 mV
856 861
857 862 time_management_regs->calDataPtr = 0x00;
858 863
859 864 // build the signal for the SCM calibration
860 865 for (k=0; k<256; k++)
861 866 {
862 867 val = sin( 2 * pi * f0 * k * Ts )
863 868 + sin( 2 * pi * f1 * k * Ts );
864 869 data = (unsigned short) ((val * scaleFactor) + 2048);
865 870 time_management_regs->calData = data & 0xfff;
866 871 }
867 872 }
868 873
869 874 void setCalibrationDataInterleaved( void ){
870 875 unsigned int k;
871 876 float val;
872 877 float f0;
873 878 float f1;
874 879 float fs;
875 880 float Ts;
876 881 unsigned short data[384];
877 882 unsigned char *dataPtr;
878 883
879 884 f0 = 625;
880 885 f1 = 10000;
881 886 fs = 240384.615;
882 887 Ts = 1. / fs;
883 888
884 889 time_management_regs->calDataPtr = 0x00;
885 890
886 891 // build the signal for the SCM calibration
887 892 for (k=0; k<384; k++)
888 893 {
889 894 val = sin( 2 * pi * f0 * k * Ts )
890 895 + sin( 2 * pi * f1 * k * Ts );
891 896 data[k] = (unsigned short) (val * 512 + 2048);
892 897 }
893 898
894 899 // write the signal in interleaved mode
895 900 for (k=0; k<128; k++)
896 901 {
897 902 dataPtr = (unsigned char*) &data[k*3 + 2];
898 903 time_management_regs->calData = (data[k*3] & 0xfff)
899 904 + ( (dataPtr[0] & 0x3f) << 12);
900 905 time_management_regs->calData = (data[k*3 + 1] & 0xfff)
901 906 + ( (dataPtr[1] & 0x3f) << 12);
902 907 }
903 908 }
904 909
905 910 void setCalibrationReload( bool state)
906 911 {
907 912 if (state == true)
908 913 {
909 914 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | 0x00000010; // [0001 0000]
910 915 }
911 916 else
912 917 {
913 918 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & 0xffffffef; // [1110 1111]
914 919 }
915 920 }
916 921
917 922 void setCalibrationEnable( bool state )
918 923 {
919 924 // this bit drives the multiplexer
920 925 if (state == true)
921 926 {
922 927 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | 0x00000040; // [0100 0000]
923 928 }
924 929 else
925 930 {
926 931 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & 0xffffffbf; // [1011 1111]
927 932 }
928 933 }
929 934
930 935 void setCalibrationInterleaved( bool state )
931 936 {
932 937 // this bit drives the multiplexer
933 938 if (state == true)
934 939 {
935 940 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | 0x00000020; // [0010 0000]
936 941 }
937 942 else
938 943 {
939 944 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & 0xffffffdf; // [1101 1111]
940 945 }
941 946 }
942 947
943 948 void startCalibration( void )
944 949 {
945 950 setCalibrationEnable( true );
946 951 setCalibrationReload( false );
947 952 }
948 953
949 954 void stopCalibration( void )
950 955 {
951 956 setCalibrationEnable( false );
952 957 setCalibrationReload( true );
953 958 }
954 959
955 960 void configureCalibration( bool interleaved )
956 961 {
957 962 stopCalibration();
958 963 if ( interleaved == true )
959 964 {
960 965 setCalibrationInterleaved( true );
961 966 setCalibrationPrescaler( 0 ); // 25 MHz => 25 000 000
962 967 setCalibrationDivisor( 26 ); // => 240 384
963 968 setCalibrationDataInterleaved();
964 969 }
965 970 else
966 971 {
967 972 setCalibrationPrescaler( 0 ); // 25 MHz => 25 000 000
968 973 setCalibrationDivisor( 38 ); // => 160 256 (39 - 1)
969 974 setCalibrationData();
970 975 }
971 976 }
972 977
973 978 //****************
974 979 // CLOSING ACTIONS
975 980 void update_last_TC_exe( ccsdsTelecommandPacket_t *TC, unsigned char * time )
976 981 {
977 982 /** This function is used to update the HK packets statistics after a successful TC execution.
978 983 *
979 984 * @param TC points to the TC being processed
980 985 * @param time is the time used to date the TC execution
981 986 *
982 987 */
983 988
984 989 unsigned int val;
985 990
986 991 housekeeping_packet.hk_lfr_last_exe_tc_id[0] = TC->packetID[0];
987 992 housekeeping_packet.hk_lfr_last_exe_tc_id[1] = TC->packetID[1];
988 993 housekeeping_packet.hk_lfr_last_exe_tc_type[0] = 0x00;
989 994 housekeeping_packet.hk_lfr_last_exe_tc_type[1] = TC->serviceType;
990 995 housekeeping_packet.hk_lfr_last_exe_tc_subtype[0] = 0x00;
991 996 housekeeping_packet.hk_lfr_last_exe_tc_subtype[1] = TC->serviceSubType;
992 997 housekeeping_packet.hk_lfr_last_exe_tc_time[0] = time[0];
993 998 housekeeping_packet.hk_lfr_last_exe_tc_time[1] = time[1];
994 999 housekeeping_packet.hk_lfr_last_exe_tc_time[2] = time[2];
995 1000 housekeeping_packet.hk_lfr_last_exe_tc_time[3] = time[3];
996 1001 housekeeping_packet.hk_lfr_last_exe_tc_time[4] = time[4];
997 1002 housekeeping_packet.hk_lfr_last_exe_tc_time[5] = time[5];
998 1003
999 1004 val = housekeeping_packet.hk_lfr_exe_tc_cnt[0] * 256 + housekeeping_packet.hk_lfr_exe_tc_cnt[1];
1000 1005 val++;
1001 1006 housekeeping_packet.hk_lfr_exe_tc_cnt[0] = (unsigned char) (val >> 8);
1002 1007 housekeeping_packet.hk_lfr_exe_tc_cnt[1] = (unsigned char) (val);
1003 1008 }
1004 1009
1005 1010 void update_last_TC_rej(ccsdsTelecommandPacket_t *TC, unsigned char * time )
1006 1011 {
1007 1012 /** This function is used to update the HK packets statistics after a TC rejection.
1008 1013 *
1009 1014 * @param TC points to the TC being processed
1010 1015 * @param time is the time used to date the TC rejection
1011 1016 *
1012 1017 */
1013 1018
1014 1019 unsigned int val;
1015 1020
1016 1021 housekeeping_packet.hk_lfr_last_rej_tc_id[0] = TC->packetID[0];
1017 1022 housekeeping_packet.hk_lfr_last_rej_tc_id[1] = TC->packetID[1];
1018 1023 housekeeping_packet.hk_lfr_last_rej_tc_type[0] = 0x00;
1019 1024 housekeeping_packet.hk_lfr_last_rej_tc_type[1] = TC->serviceType;
1020 1025 housekeeping_packet.hk_lfr_last_rej_tc_subtype[0] = 0x00;
1021 1026 housekeeping_packet.hk_lfr_last_rej_tc_subtype[1] = TC->serviceSubType;
1022 1027 housekeeping_packet.hk_lfr_last_rej_tc_time[0] = time[0];
1023 1028 housekeeping_packet.hk_lfr_last_rej_tc_time[1] = time[1];
1024 1029 housekeeping_packet.hk_lfr_last_rej_tc_time[2] = time[2];
1025 1030 housekeeping_packet.hk_lfr_last_rej_tc_time[3] = time[3];
1026 1031 housekeeping_packet.hk_lfr_last_rej_tc_time[4] = time[4];
1027 1032 housekeeping_packet.hk_lfr_last_rej_tc_time[5] = time[5];
1028 1033
1029 1034 val = housekeeping_packet.hk_lfr_rej_tc_cnt[0] * 256 + housekeeping_packet.hk_lfr_rej_tc_cnt[1];
1030 1035 val++;
1031 1036 housekeeping_packet.hk_lfr_rej_tc_cnt[0] = (unsigned char) (val >> 8);
1032 1037 housekeeping_packet.hk_lfr_rej_tc_cnt[1] = (unsigned char) (val);
1033 1038 }
1034 1039
1035 1040 void close_action(ccsdsTelecommandPacket_t *TC, int result, rtems_id queue_id )
1036 1041 {
1037 1042 /** This function is the last step of the TC execution workflow.
1038 1043 *
1039 1044 * @param TC points to the TC being processed
1040 1045 * @param result is the result of the TC execution (LFR_SUCCESSFUL / LFR_DEFAULT)
1041 1046 * @param queue_id is the id of the RTEMS message queue used to send TM packets
1042 1047 * @param time is the time used to date the TC execution
1043 1048 *
1044 1049 */
1045 1050
1046 1051 unsigned char requestedMode;
1047 1052
1048 1053 if (result == LFR_SUCCESSFUL)
1049 1054 {
1050 1055 if ( !( (TC->serviceType==TC_TYPE_TIME) & (TC->serviceSubType==TC_SUBTYPE_UPDT_TIME) )
1051 1056 &
1052 1057 !( (TC->serviceType==TC_TYPE_GEN) & (TC->serviceSubType==TC_SUBTYPE_UPDT_INFO))
1053 1058 )
1054 1059 {
1055 1060 send_tm_lfr_tc_exe_success( TC, queue_id );
1056 1061 }
1057 1062 if ( (TC->serviceType == TC_TYPE_GEN) & (TC->serviceSubType == TC_SUBTYPE_ENTER) )
1058 1063 {
1059 1064 //**********************************
1060 1065 // UPDATE THE LFRMODE LOCAL VARIABLE
1061 1066 requestedMode = TC->dataAndCRC[1];
1062 1067 housekeeping_packet.lfr_status_word[0] = (unsigned char) ((requestedMode << 4) + 0x0d);
1063 1068 updateLFRCurrentMode();
1064 1069 }
1065 1070 }
1066 1071 else if (result == LFR_EXE_ERROR)
1067 1072 {
1068 1073 send_tm_lfr_tc_exe_error( TC, queue_id );
1069 1074 }
1070 1075 }
1071 1076
1072 1077 //***************************
1073 1078 // Interrupt Service Routines
1074 1079 rtems_isr commutation_isr1( rtems_vector_number vector )
1075 1080 {
1076 1081 if (rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
1077 1082 printf("In commutation_isr1 *** Error sending event to DUMB\n");
1078 1083 }
1079 1084 }
1080 1085
1081 1086 rtems_isr commutation_isr2( rtems_vector_number vector )
1082 1087 {
1083 1088 if (rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
1084 1089 printf("In commutation_isr2 *** Error sending event to DUMB\n");
1085 1090 }
1086 1091 }
1087 1092
1088 1093 //****************
1089 1094 // OTHER FUNCTIONS
1090 1095 void updateLFRCurrentMode()
1091 1096 {
1092 1097 /** This function updates the value of the global variable lfrCurrentMode.
1093 1098 *
1094 1099 * lfrCurrentMode is a parameter used by several functions to know in which mode LFR is running.
1095 1100 *
1096 1101 */
1097 1102 // update the local value of lfrCurrentMode with the value contained in the housekeeping_packet structure
1098 1103 lfrCurrentMode = (housekeeping_packet.lfr_status_word[0] & 0xf0) >> 4;
1099 1104 }
1100 1105
1101 1106 void set_lfr_soft_reset( unsigned char value )
1102 1107 {
1103 1108 if (value == 1)
1104 1109 {
1105 1110 time_management_regs->ctrl = time_management_regs->ctrl | 0x00000004; // [0100]
1106 1111 }
1107 1112 else
1108 1113 {
1109 1114 time_management_regs->ctrl = time_management_regs->ctrl & 0xfffffffb; // [1011]
1110 1115 }
1111 1116 }
1112 1117
1113 1118 void reset_lfr( void )
1114 1119 {
1115 1120 set_lfr_soft_reset( 1 );
1116 1121
1117 1122 set_lfr_soft_reset( 0 );
1118 1123 }
1124
1125 void printTaskID( void )
1126 {
1127 unsigned int i;
1128
1129 for (i=0; i<20;i++)
1130 {
1131 printf("ID %d = %d\n", i, (unsigned int) Task_id[i]);
1132 }
1133 }
Show file before | Show file after | Hide comments
@@ -1,772 +1,826
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 int action_load_common_par(ccsdsTelecommandPacket_t *TC)
18 18 {
19 19 /** This function updates the LFR registers with the incoming common parameters.
20 20 *
21 21 * @param TC points to the TeleCommand packet that is being processed
22 22 *
23 23 *
24 24 */
25 25
26 26 parameter_dump_packet.unused0 = TC->dataAndCRC[0];
27 parameter_dump_packet.bw_sp0_sp1_r0_r1 = TC->dataAndCRC[1];
27 parameter_dump_packet.sy_lfr_common_parameters = TC->dataAndCRC[1];
28 28 set_wfp_data_shaping( );
29 29 return LFR_SUCCESSFUL;
30 30 }
31 31
32 32 int action_load_normal_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
33 33 {
34 34 /** This function updates the LFR registers with the incoming normal parameters.
35 35 *
36 36 * @param TC points to the TeleCommand packet that is being processed
37 37 * @param queue_id is the id of the queue which handles TM related to this execution step
38 38 *
39 39 */
40 40
41 41 int result;
42 42 int flag;
43 43 rtems_status_code status;
44 44
45 45 flag = LFR_SUCCESSFUL;
46 46
47 47 if ( (lfrCurrentMode == LFR_MODE_NORMAL) ||
48 48 (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) ) {
49 49 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
50 50 flag = LFR_DEFAULT;
51 51 }
52 52
53 53 // CHECK THE PARAMETERS SET CONSISTENCY
54 54 if (flag == LFR_SUCCESSFUL)
55 55 {
56 56 flag = check_common_par_consistency( TC, queue_id );
57 57 }
58 58
59 59 // SET THE PARAMETERS IF THEY ARE CONSISTENT
60 60 if (flag == LFR_SUCCESSFUL)
61 61 {
62 62 result = set_sy_lfr_n_swf_l( TC );
63 63 result = set_sy_lfr_n_swf_p( TC );
64 64 result = set_sy_lfr_n_bp_p0( TC );
65 65 result = set_sy_lfr_n_bp_p1( TC );
66 66 result = set_sy_lfr_n_asm_p( TC );
67 67 result = set_sy_lfr_n_cwf_long_f3( TC );
68 68 }
69 69
70 70 return flag;
71 71 }
72 72
73 73 int action_load_burst_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
74 74 {
75 75 /** This function updates the LFR registers with the incoming burst parameters.
76 76 *
77 77 * @param TC points to the TeleCommand packet that is being processed
78 78 * @param queue_id is the id of the queue which handles TM related to this execution step
79 79 *
80 80 */
81 81
82 82 int flag;
83 83 rtems_status_code status;
84 84 unsigned char sy_lfr_b_bp_p0;
85 85 unsigned char sy_lfr_b_bp_p1;
86 86 float aux;
87 87
88 88 flag = LFR_SUCCESSFUL;
89 89
90 90 if ( lfrCurrentMode == LFR_MODE_BURST ) {
91 91 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
92 92 flag = LFR_DEFAULT;
93 93 }
94 94
95 95 sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
96 96 sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
97 97
98 98 // sy_lfr_b_bp_p0
99 99 if (flag == LFR_SUCCESSFUL)
100 100 {
101 101 if (sy_lfr_b_bp_p0 < DEFAULT_SY_LFR_B_BP_P0 )
102 102 {
103 103 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P0+10, sy_lfr_b_bp_p0 );
104 104 flag = WRONG_APP_DATA;
105 105 }
106 106 }
107 107 // sy_lfr_b_bp_p1
108 108 if (flag == LFR_SUCCESSFUL)
109 109 {
110 110 if (sy_lfr_b_bp_p1 < DEFAULT_SY_LFR_B_BP_P1 )
111 111 {
112 112 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P1+10, sy_lfr_b_bp_p1 );
113 113 flag = WRONG_APP_DATA;
114 114 }
115 115 }
116 116 //****************************************************************
117 117 // check the consistency between sy_lfr_b_bp_p0 and sy_lfr_b_bp_p1
118 118 if (flag == LFR_SUCCESSFUL)
119 119 {
120 120 sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
121 121 sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
122 122 aux = ( (float ) sy_lfr_b_bp_p1 / sy_lfr_b_bp_p0 ) - floor(sy_lfr_b_bp_p1 / sy_lfr_b_bp_p0);
123 123 if (aux > FLOAT_EQUAL_ZERO)
124 124 {
125 125 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P0+10, sy_lfr_b_bp_p0 );
126 126 flag = LFR_DEFAULT;
127 127 }
128 128 }
129 129
130 130 // SET HTE PARAMETERS
131 131 if (flag == LFR_SUCCESSFUL)
132 132 {
133 133 flag = set_sy_lfr_b_bp_p0( TC );
134 134 flag = set_sy_lfr_b_bp_p1( TC );
135 135 }
136 136
137 137 return flag;
138 138 }
139 139
140 140 int action_load_sbm1_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
141 141 {
142 142 /** This function updates the LFR registers with the incoming sbm1 parameters.
143 143 *
144 144 * @param TC points to the TeleCommand packet that is being processed
145 145 * @param queue_id is the id of the queue which handles TM related to this execution step
146 146 *
147 147 */
148 148
149 149 int flag;
150 150 rtems_status_code status;
151 151 unsigned char sy_lfr_s1_bp_p0;
152 152 unsigned char sy_lfr_s1_bp_p1;
153 153 float aux;
154 154
155 155 flag = LFR_SUCCESSFUL;
156 156
157 157 if ( lfrCurrentMode == LFR_MODE_SBM1 ) {
158 158 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
159 159 flag = LFR_DEFAULT;
160 160 }
161 161
162 162 sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
163 163 sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
164 164
165 165 // sy_lfr_s1_bp_p0
166 166 if (flag == LFR_SUCCESSFUL)
167 167 {
168 168 if (sy_lfr_s1_bp_p0 < DEFAULT_SY_LFR_S1_BP_P0 )
169 169 {
170 170 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P0+10, sy_lfr_s1_bp_p0 );
171 171 flag = WRONG_APP_DATA;
172 172 }
173 173 }
174 174 // sy_lfr_s1_bp_p1
175 175 if (flag == LFR_SUCCESSFUL)
176 176 {
177 177 if (sy_lfr_s1_bp_p1 < DEFAULT_SY_LFR_S1_BP_P1 )
178 178 {
179 179 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P1+10, sy_lfr_s1_bp_p1 );
180 180 flag = WRONG_APP_DATA;
181 181 }
182 182 }
183 183 //******************************************************************
184 184 // check the consistency between sy_lfr_s1_bp_p0 and sy_lfr_s1_bp_p1
185 185 if (flag == LFR_SUCCESSFUL)
186 186 {
187 187 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));
188 188 if (aux > FLOAT_EQUAL_ZERO)
189 189 {
190 190 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P0+10, sy_lfr_s1_bp_p0 );
191 191 flag = LFR_DEFAULT;
192 192 }
193 193 }
194 194
195 195 // SET THE PARAMETERS
196 196 if (flag == LFR_SUCCESSFUL)
197 197 {
198 198 flag = set_sy_lfr_s1_bp_p0( TC );
199 199 flag = set_sy_lfr_s1_bp_p1( TC );
200 200 }
201 201
202 202 return flag;
203 203 }
204 204
205 205 int action_load_sbm2_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
206 206 {
207 207 /** This function updates the LFR registers with the incoming sbm2 parameters.
208 208 *
209 209 * @param TC points to the TeleCommand packet that is being processed
210 210 * @param queue_id is the id of the queue which handles TM related to this execution step
211 211 *
212 212 */
213 213
214 214 int flag;
215 215 rtems_status_code status;
216 216 unsigned char sy_lfr_s2_bp_p0;
217 217 unsigned char sy_lfr_s2_bp_p1;
218 218 float aux;
219 219
220 220 flag = LFR_SUCCESSFUL;
221 221
222 222 if ( lfrCurrentMode == LFR_MODE_SBM2 ) {
223 223 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
224 224 flag = LFR_DEFAULT;
225 225 }
226 226
227 227 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
228 228 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
229 229
230 230 // sy_lfr_s2_bp_p0
231 231 if (flag == LFR_SUCCESSFUL)
232 232 {
233 233 if (sy_lfr_s2_bp_p0 < DEFAULT_SY_LFR_S2_BP_P0 )
234 234 {
235 235 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0+10, sy_lfr_s2_bp_p0 );
236 236 flag = WRONG_APP_DATA;
237 237 }
238 238 }
239 239 // sy_lfr_s2_bp_p1
240 240 if (flag == LFR_SUCCESSFUL)
241 241 {
242 242 if (sy_lfr_s2_bp_p1 < DEFAULT_SY_LFR_S2_BP_P1 )
243 243 {
244 244 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P1+10, sy_lfr_s2_bp_p1 );
245 245 flag = WRONG_APP_DATA;
246 246 }
247 247 }
248 248 //******************************************************************
249 249 // check the consistency between sy_lfr_s2_bp_p0 and sy_lfr_s2_bp_p1
250 250 if (flag == LFR_SUCCESSFUL)
251 251 {
252 252 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
253 253 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
254 254 aux = ( (float ) sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0 ) - floor(sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0);
255 255 if (aux > FLOAT_EQUAL_ZERO)
256 256 {
257 257 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0+10, sy_lfr_s2_bp_p0 );
258 258 flag = LFR_DEFAULT;
259 259 }
260 260 }
261 261
262 262 // SET THE PARAMETERS
263 263 if (flag == LFR_SUCCESSFUL)
264 264 {
265 265 flag = set_sy_lfr_s2_bp_p0( TC );
266 266 flag = set_sy_lfr_s2_bp_p1( TC );
267 267 }
268 268
269 269 return flag;
270 270 }
271 271
272 int action_load_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
273 {
274 /** This function updates the LFR registers with the incoming sbm2 parameters.
275 *
276 * @param TC points to the TeleCommand packet that is being processed
277 * @param queue_id is the id of the queue which handles TM related to this execution step
278 *
279 */
280
281 int flag;
282
283 flag = LFR_DEFAULT;
284
285 send_tm_lfr_tc_exe_not_implemented( TC, queue_id, time );
286
287 return flag;
288 }
289
290 int action_load_fbins_mask(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
291 {
292 /** This function updates the LFR registers with the incoming sbm2 parameters.
293 *
294 * @param TC points to the TeleCommand packet that is being processed
295 * @param queue_id is the id of the queue which handles TM related to this execution step
296 *
297 */
298
299 int flag;
300
301 flag = LFR_DEFAULT;
302
303 send_tm_lfr_tc_exe_not_implemented( TC, queue_id, time );
304
305 return flag;
306 }
307
308 int action_dump_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
309 {
310 /** This function updates the LFR registers with the incoming sbm2 parameters.
311 *
312 * @param TC points to the TeleCommand packet that is being processed
313 * @param queue_id is the id of the queue which handles TM related to this execution step
314 *
315 */
316
317 int flag;
318
319 flag = LFR_DEFAULT;
320
321 send_tm_lfr_tc_exe_not_implemented( TC, queue_id, time );
322
323 return flag;
324 }
325
272 326 int action_dump_par( rtems_id queue_id )
273 327 {
274 328 /** This function dumps the LFR parameters by sending the appropriate TM packet to the dedicated RTEMS message queue.
275 329 *
276 330 * @param queue_id is the id of the queue which handles TM related to this execution step.
277 331 *
278 332 * @return RTEMS directive status codes:
279 333 * - RTEMS_SUCCESSFUL - message sent successfully
280 334 * - RTEMS_INVALID_ID - invalid queue id
281 335 * - RTEMS_INVALID_SIZE - invalid message size
282 336 * - RTEMS_INVALID_ADDRESS - buffer is NULL
283 337 * - RTEMS_UNSATISFIED - out of message buffers
284 338 * - RTEMS_TOO_MANY - queue s limit has been reached
285 339 *
286 340 */
287 341
288 342 int status;
289 343
290 344 // UPDATE TIME
291 345 parameter_dump_packet.packetSequenceControl[0] = (unsigned char) (sequenceCounterParameterDump >> 8);
292 346 parameter_dump_packet.packetSequenceControl[1] = (unsigned char) (sequenceCounterParameterDump );
293 347 increment_seq_counter( &sequenceCounterParameterDump );
294 348
295 349 parameter_dump_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
296 350 parameter_dump_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
297 351 parameter_dump_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
298 352 parameter_dump_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
299 353 parameter_dump_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
300 354 parameter_dump_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
301 355 // SEND DATA
302 356 status = rtems_message_queue_send( queue_id, &parameter_dump_packet,
303 357 PACKET_LENGTH_PARAMETER_DUMP + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
304 358 if (status != RTEMS_SUCCESSFUL) {
305 359 PRINTF1("in action_dump *** ERR sending packet, code %d", status)
306 360 }
307 361
308 362 return status;
309 363 }
310 364
311 365 //***********************
312 366 // NORMAL MODE PARAMETERS
313 367
314 368 int check_common_par_consistency( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
315 369 {
316 370 unsigned char msb;
317 371 unsigned char lsb;
318 372 int flag;
319 373 float aux;
320 374 rtems_status_code status;
321 375
322 376 unsigned int sy_lfr_n_swf_l;
323 377 unsigned int sy_lfr_n_swf_p;
324 378 unsigned int sy_lfr_n_asm_p;
325 379 unsigned char sy_lfr_n_bp_p0;
326 380 unsigned char sy_lfr_n_bp_p1;
327 381 unsigned char sy_lfr_n_cwf_long_f3;
328 382
329 383 flag = LFR_SUCCESSFUL;
330 384
331 385 //***************
332 386 // get parameters
333 387 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
334 388 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
335 389 sy_lfr_n_swf_l = msb * 256 + lsb;
336 390
337 391 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
338 392 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
339 393 sy_lfr_n_swf_p = msb * 256 + lsb;
340 394
341 395 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
342 396 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
343 397 sy_lfr_n_asm_p = msb * 256 + lsb;
344 398
345 399 sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
346 400
347 401 sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
348 402
349 403 sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
350 404
351 405 //******************
352 406 // check consistency
353 407 // sy_lfr_n_swf_l
354 408 if (sy_lfr_n_swf_l != 2048)
355 409 {
356 410 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_L+10, sy_lfr_n_swf_l );
357 411 flag = WRONG_APP_DATA;
358 412 }
359 413 // sy_lfr_n_swf_p
360 414 if (flag == LFR_SUCCESSFUL)
361 415 {
362 416 if ( sy_lfr_n_swf_p < 16 )
363 417 {
364 418 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_P+10, sy_lfr_n_swf_p );
365 419 flag = WRONG_APP_DATA;
366 420 }
367 421 }
368 422 // sy_lfr_n_bp_p0
369 423 if (flag == LFR_SUCCESSFUL)
370 424 {
371 425 if (sy_lfr_n_bp_p0 < DFLT_SY_LFR_N_BP_P0)
372 426 {
373 427 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P0+10, sy_lfr_n_bp_p0 );
374 428 flag = WRONG_APP_DATA;
375 429 }
376 430 }
377 431 // sy_lfr_n_asm_p
378 432 if (flag == LFR_SUCCESSFUL)
379 433 {
380 434 if (sy_lfr_n_asm_p == 0)
381 435 {
382 436 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P+10, sy_lfr_n_asm_p );
383 437 flag = WRONG_APP_DATA;
384 438 }
385 439 }
386 440 // sy_lfr_n_asm_p shall be a whole multiple of sy_lfr_n_bp_p0
387 441 if (flag == LFR_SUCCESSFUL)
388 442 {
389 443 aux = ( (float ) sy_lfr_n_asm_p / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_asm_p / sy_lfr_n_bp_p0);
390 444 if (aux > FLOAT_EQUAL_ZERO)
391 445 {
392 446 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P+10, sy_lfr_n_asm_p );
393 447 flag = WRONG_APP_DATA;
394 448 }
395 449 }
396 450 // sy_lfr_n_bp_p1
397 451 if (flag == LFR_SUCCESSFUL)
398 452 {
399 453 if (sy_lfr_n_bp_p1 < DFLT_SY_LFR_N_BP_P1)
400 454 {
401 455 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1+10, sy_lfr_n_bp_p1 );
402 456 flag = WRONG_APP_DATA;
403 457 }
404 458 }
405 459 // sy_lfr_n_bp_p1 shall be a whole multiple of sy_lfr_n_bp_p0
406 460 if (flag == LFR_SUCCESSFUL)
407 461 {
408 462 aux = ( (float ) sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0);
409 463 if (aux > FLOAT_EQUAL_ZERO)
410 464 {
411 465 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1+10, sy_lfr_n_bp_p1 );
412 466 flag = LFR_DEFAULT;
413 467 }
414 468 }
415 469 // sy_lfr_n_cwf_long_f3
416 470
417 471 return flag;
418 472 }
419 473
420 474 int set_sy_lfr_n_swf_l( ccsdsTelecommandPacket_t *TC )
421 475 {
422 476 /** This function sets the number of points of a snapshot (sy_lfr_n_swf_l).
423 477 *
424 478 * @param TC points to the TeleCommand packet that is being processed
425 479 * @param queue_id is the id of the queue which handles TM related to this execution step
426 480 *
427 481 */
428 482
429 483 int result;
430 484
431 485 result = LFR_SUCCESSFUL;
432 486
433 487 parameter_dump_packet.sy_lfr_n_swf_l[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
434 488 parameter_dump_packet.sy_lfr_n_swf_l[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
435 489
436 490 return result;
437 491 }
438 492
439 493 int set_sy_lfr_n_swf_p(ccsdsTelecommandPacket_t *TC )
440 494 {
441 495 /** This function sets the time between two snapshots, in s (sy_lfr_n_swf_p).
442 496 *
443 497 * @param TC points to the TeleCommand packet that is being processed
444 498 * @param queue_id is the id of the queue which handles TM related to this execution step
445 499 *
446 500 */
447 501
448 502 int result;
449 503
450 504 result = LFR_SUCCESSFUL;
451 505
452 506 parameter_dump_packet.sy_lfr_n_swf_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
453 507 parameter_dump_packet.sy_lfr_n_swf_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
454 508
455 509 return result;
456 510 }
457 511
458 512 int set_sy_lfr_n_asm_p( ccsdsTelecommandPacket_t *TC )
459 513 {
460 514 /** This function sets the time between two full spectral matrices transmission, in s (SY_LFR_N_ASM_P).
461 515 *
462 516 * @param TC points to the TeleCommand packet that is being processed
463 517 * @param queue_id is the id of the queue which handles TM related to this execution step
464 518 *
465 519 */
466 520
467 521 int result;
468 522
469 523 result = LFR_SUCCESSFUL;
470 524
471 525 parameter_dump_packet.sy_lfr_n_asm_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
472 526 parameter_dump_packet.sy_lfr_n_asm_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
473 527
474 528 return result;
475 529 }
476 530
477 531 int set_sy_lfr_n_bp_p0( ccsdsTelecommandPacket_t *TC )
478 532 {
479 533 /** This function sets the time between two basic parameter sets, in s (DFLT_SY_LFR_N_BP_P0).
480 534 *
481 535 * @param TC points to the TeleCommand packet that is being processed
482 536 * @param queue_id is the id of the queue which handles TM related to this execution step
483 537 *
484 538 */
485 539
486 540 int status;
487 541
488 542 status = LFR_SUCCESSFUL;
489 543
490 544 parameter_dump_packet.sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
491 545
492 546 return status;
493 547 }
494 548
495 549 int set_sy_lfr_n_bp_p1(ccsdsTelecommandPacket_t *TC )
496 550 {
497 551 /** This function sets the time between two basic parameter sets (autocorrelation + crosscorrelation), in s (sy_lfr_n_bp_p1).
498 552 *
499 553 * @param TC points to the TeleCommand packet that is being processed
500 554 * @param queue_id is the id of the queue which handles TM related to this execution step
501 555 *
502 556 */
503 557
504 558 int status;
505 559
506 560 status = LFR_SUCCESSFUL;
507 561
508 562 parameter_dump_packet.sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
509 563
510 564 return status;
511 565 }
512 566
513 567 int set_sy_lfr_n_cwf_long_f3(ccsdsTelecommandPacket_t *TC )
514 568 {
515 569 /** This function allows to switch from CWF_F3 packets to CWF_LONG_F3 packets.
516 570 *
517 571 * @param TC points to the TeleCommand packet that is being processed
518 572 * @param queue_id is the id of the queue which handles TM related to this execution step
519 573 *
520 574 */
521 575
522 576 int status;
523 577
524 578 status = LFR_SUCCESSFUL;
525 579
526 580 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
527 581
528 582 return status;
529 583 }
530 584
531 585 //**********************
532 586 // BURST MODE PARAMETERS
533 587 int set_sy_lfr_b_bp_p0(ccsdsTelecommandPacket_t *TC)
534 588 {
535 589 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P0).
536 590 *
537 591 * @param TC points to the TeleCommand packet that is being processed
538 592 * @param queue_id is the id of the queue which handles TM related to this execution step
539 593 *
540 594 */
541 595
542 596 int status;
543 597
544 598 status = LFR_SUCCESSFUL;
545 599
546 600 parameter_dump_packet.sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
547 601
548 602 return status;
549 603 }
550 604
551 605 int set_sy_lfr_b_bp_p1( ccsdsTelecommandPacket_t *TC )
552 606 {
553 607 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P1).
554 608 *
555 609 * @param TC points to the TeleCommand packet that is being processed
556 610 * @param queue_id is the id of the queue which handles TM related to this execution step
557 611 *
558 612 */
559 613
560 614 int status;
561 615
562 616 status = LFR_SUCCESSFUL;
563 617
564 618 parameter_dump_packet.sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
565 619
566 620 return status;
567 621 }
568 622
569 623 //*********************
570 624 // SBM1 MODE PARAMETERS
571 625 int set_sy_lfr_s1_bp_p0( ccsdsTelecommandPacket_t *TC )
572 626 {
573 627 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P0).
574 628 *
575 629 * @param TC points to the TeleCommand packet that is being processed
576 630 * @param queue_id is the id of the queue which handles TM related to this execution step
577 631 *
578 632 */
579 633
580 634 int status;
581 635
582 636 status = LFR_SUCCESSFUL;
583 637
584 638 parameter_dump_packet.sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
585 639
586 640 return status;
587 641 }
588 642
589 643 int set_sy_lfr_s1_bp_p1( ccsdsTelecommandPacket_t *TC )
590 644 {
591 645 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P1).
592 646 *
593 647 * @param TC points to the TeleCommand packet that is being processed
594 648 * @param queue_id is the id of the queue which handles TM related to this execution step
595 649 *
596 650 */
597 651
598 652 int status;
599 653
600 654 status = LFR_SUCCESSFUL;
601 655
602 656 parameter_dump_packet.sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
603 657
604 658 return status;
605 659 }
606 660
607 661 //*********************
608 662 // SBM2 MODE PARAMETERS
609 663 int set_sy_lfr_s2_bp_p0(ccsdsTelecommandPacket_t *TC)
610 664 {
611 665 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P0).
612 666 *
613 667 * @param TC points to the TeleCommand packet that is being processed
614 668 * @param queue_id is the id of the queue which handles TM related to this execution step
615 669 *
616 670 */
617 671
618 672 int status;
619 673
620 674 status = LFR_SUCCESSFUL;
621 675
622 676 parameter_dump_packet.sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
623 677
624 678 return status;
625 679 }
626 680
627 681 int set_sy_lfr_s2_bp_p1( ccsdsTelecommandPacket_t *TC )
628 682 {
629 683 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P1).
630 684 *
631 685 * @param TC points to the TeleCommand packet that is being processed
632 686 * @param queue_id is the id of the queue which handles TM related to this execution step
633 687 *
634 688 */
635 689
636 690 int status;
637 691
638 692 status = LFR_SUCCESSFUL;
639 693
640 694 parameter_dump_packet.sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
641 695
642 696 return status;
643 697 }
644 698
645 699
646 700 //*******************
647 701 // TC_LFR_UPDATE_INFO
648 702 unsigned int check_update_info_hk_lfr_mode( unsigned char mode )
649 703 {
650 704 unsigned int status;
651 705
652 706 if ( (mode == LFR_MODE_STANDBY) || (mode == LFR_MODE_NORMAL)
653 707 || (mode == LFR_MODE_BURST)
654 708 || (mode == LFR_MODE_SBM1) || (mode == LFR_MODE_SBM2))
655 709 {
656 710 status = LFR_SUCCESSFUL;
657 711 }
658 712 else
659 713 {
660 714 status = LFR_DEFAULT;
661 715 }
662 716
663 717 return status;
664 718 }
665 719
666 720 unsigned int check_update_info_hk_tds_mode( unsigned char mode )
667 721 {
668 722 unsigned int status;
669 723
670 724 if ( (mode == TDS_MODE_STANDBY) || (mode == TDS_MODE_NORMAL)
671 725 || (mode == TDS_MODE_BURST)
672 726 || (mode == TDS_MODE_SBM1) || (mode == TDS_MODE_SBM2)
673 727 || (mode == TDS_MODE_LFM))
674 728 {
675 729 status = LFR_SUCCESSFUL;
676 730 }
677 731 else
678 732 {
679 733 status = LFR_DEFAULT;
680 734 }
681 735
682 736 return status;
683 737 }
684 738
685 739 unsigned int check_update_info_hk_thr_mode( unsigned char mode )
686 740 {
687 741 unsigned int status;
688 742
689 743 if ( (mode == THR_MODE_STANDBY) || (mode == THR_MODE_NORMAL)
690 744 || (mode == THR_MODE_BURST))
691 745 {
692 746 status = LFR_SUCCESSFUL;
693 747 }
694 748 else
695 749 {
696 750 status = LFR_DEFAULT;
697 751 }
698 752
699 753 return status;
700 754 }
701 755
702 756 //**********
703 757 // init dump
704 758
705 759 void init_parameter_dump( void )
706 760 {
707 761 /** This function initialize the parameter_dump_packet global variable with default values.
708 762 *
709 763 */
710 764
711 765 parameter_dump_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
712 766 parameter_dump_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
713 767 parameter_dump_packet.reserved = CCSDS_RESERVED;
714 768 parameter_dump_packet.userApplication = CCSDS_USER_APP;
715 769 parameter_dump_packet.packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> 8);
716 770 parameter_dump_packet.packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;
717 771 parameter_dump_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
718 772 parameter_dump_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
719 773 parameter_dump_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_PARAMETER_DUMP >> 8);
720 774 parameter_dump_packet.packetLength[1] = (unsigned char) PACKET_LENGTH_PARAMETER_DUMP;
721 775 // DATA FIELD HEADER
722 776 parameter_dump_packet.spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
723 777 parameter_dump_packet.serviceType = TM_TYPE_PARAMETER_DUMP;
724 778 parameter_dump_packet.serviceSubType = TM_SUBTYPE_PARAMETER_DUMP;
725 779 parameter_dump_packet.destinationID = TM_DESTINATION_ID_GROUND;
726 780 parameter_dump_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
727 781 parameter_dump_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
728 782 parameter_dump_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
729 783 parameter_dump_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
730 784 parameter_dump_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
731 785 parameter_dump_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
732 786 parameter_dump_packet.sid = SID_PARAMETER_DUMP;
733 787
734 788 //******************
735 789 // COMMON PARAMETERS
736 790 parameter_dump_packet.unused0 = DEFAULT_SY_LFR_COMMON0;
737 parameter_dump_packet.bw_sp0_sp1_r0_r1 = DEFAULT_SY_LFR_COMMON1;
791 parameter_dump_packet.sy_lfr_common_parameters = DEFAULT_SY_LFR_COMMON1;
738 792
739 793 //******************
740 794 // NORMAL PARAMETERS
741 795 parameter_dump_packet.sy_lfr_n_swf_l[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_L >> 8);
742 796 parameter_dump_packet.sy_lfr_n_swf_l[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_L );
743 797 parameter_dump_packet.sy_lfr_n_swf_p[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_P >> 8);
744 798 parameter_dump_packet.sy_lfr_n_swf_p[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_P );
745 799 parameter_dump_packet.sy_lfr_n_asm_p[0] = (unsigned char) (DFLT_SY_LFR_N_ASM_P >> 8);
746 800 parameter_dump_packet.sy_lfr_n_asm_p[1] = (unsigned char) (DFLT_SY_LFR_N_ASM_P );
747 801 parameter_dump_packet.sy_lfr_n_bp_p0 = (unsigned char) DFLT_SY_LFR_N_BP_P0;
748 802 parameter_dump_packet.sy_lfr_n_bp_p1 = (unsigned char) DFLT_SY_LFR_N_BP_P1;
749 803 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = (unsigned char) DFLT_SY_LFR_N_CWF_LONG_F3;
750 804
751 805 //*****************
752 806 // BURST PARAMETERS
753 807 parameter_dump_packet.sy_lfr_b_bp_p0 = (unsigned char) DEFAULT_SY_LFR_B_BP_P0;
754 808 parameter_dump_packet.sy_lfr_b_bp_p1 = (unsigned char) DEFAULT_SY_LFR_B_BP_P1;
755 809
756 810 //****************
757 811 // SBM1 PARAMETERS
758 812 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
759 813 parameter_dump_packet.sy_lfr_s1_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S1_BP_P1;
760 814
761 815 //****************
762 816 // SBM2 PARAMETERS
763 817 parameter_dump_packet.sy_lfr_s2_bp_p0 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P0;
764 818 parameter_dump_packet.sy_lfr_s2_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P1;
765 819 }
766 820
767 821
768 822
769 823
770 824
771 825
772 826
Show file before | Show file after | Hide comments
@@ -1,1402 +1,1402
1 1 /** Functions and tasks related to waveform packet generation.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle waveforms, in snapshot or continuous format.\n
7 7 *
8 8 */
9 9
10 10 #include "wf_handler.h"
11 11
12 12 //***************
13 13 // waveform rings
14 14 // F0
15 15 ring_node waveform_ring_f0[NB_RING_NODES_F0];
16 16 ring_node *current_ring_node_f0;
17 17 ring_node *ring_node_to_send_swf_f0;
18 18 // F1
19 19 ring_node waveform_ring_f1[NB_RING_NODES_F1];
20 20 ring_node *current_ring_node_f1;
21 21 ring_node *ring_node_to_send_swf_f1;
22 22 ring_node *ring_node_to_send_cwf_f1;
23 23 // F2
24 24 ring_node waveform_ring_f2[NB_RING_NODES_F2];
25 25 ring_node *current_ring_node_f2;
26 26 ring_node *ring_node_to_send_swf_f2;
27 27 ring_node *ring_node_to_send_cwf_f2;
28 28 // F3
29 29 ring_node waveform_ring_f3[NB_RING_NODES_F3];
30 30 ring_node *current_ring_node_f3;
31 31 ring_node *ring_node_to_send_cwf_f3;
32 32 char wf_cont_f3_light[ (NB_SAMPLES_PER_SNAPSHOT) * NB_BYTES_CWF3_LIGHT_BLK ];
33 33
34 34 bool extractSWF = false;
35 35 bool swf_f0_ready = false;
36 36 bool swf_f1_ready = false;
37 37 bool swf_f2_ready = false;
38 38
39 39 int wf_snap_extracted[ (NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK) ];
40 40 ring_node ring_node_wf_snap_extracted;
41 41
42 42 //*********************
43 43 // Interrupt SubRoutine
44 44
45 45 ring_node * getRingNodeToSendCWF( unsigned char frequencyChannel)
46 46 {
47 47 ring_node *node;
48 48
49 49 node = NULL;
50 50 switch ( frequencyChannel ) {
51 51 case 1:
52 52 node = ring_node_to_send_cwf_f1;
53 53 break;
54 54 case 2:
55 55 node = ring_node_to_send_cwf_f2;
56 56 break;
57 57 case 3:
58 58 node = ring_node_to_send_cwf_f3;
59 59 break;
60 60 default:
61 61 break;
62 62 }
63 63
64 64 return node;
65 65 }
66 66
67 67 ring_node * getRingNodeToSendSWF( unsigned char frequencyChannel)
68 68 {
69 69 ring_node *node;
70 70
71 71 node = NULL;
72 72 switch ( frequencyChannel ) {
73 73 case 0:
74 74 node = ring_node_to_send_swf_f0;
75 75 break;
76 76 case 1:
77 77 node = ring_node_to_send_swf_f1;
78 78 break;
79 79 case 2:
80 80 node = ring_node_to_send_swf_f2;
81 81 break;
82 82 default:
83 83 break;
84 84 }
85 85
86 86 return node;
87 87 }
88 88
89 89 void reset_extractSWF( void )
90 90 {
91 91 extractSWF = false;
92 92 swf_f0_ready = false;
93 93 swf_f1_ready = false;
94 94 swf_f2_ready = false;
95 95 }
96 96
97 97 inline void waveforms_isr_f3( void )
98 98 {
99 99 rtems_status_code spare_status;
100 100
101 101 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_BURST) // in BURST the data are used to place v, e1 and e2 in the HK packet
102 102 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
103 103 { // in modes other than STANDBY and BURST, send the CWF_F3 data
104 104 //***
105 105 // F3
106 106 if ( (waveform_picker_regs->status & 0xc0) != 0x00 ) { // [1100 0000] check the f3 full bits
107 107 ring_node_to_send_cwf_f3 = current_ring_node_f3->previous;
108 108 current_ring_node_f3 = current_ring_node_f3->next;
109 109 if ((waveform_picker_regs->status & 0x40) == 0x40){ // [0100 0000] f3 buffer 0 is full
110 110 ring_node_to_send_cwf_f3->coarseTime = waveform_picker_regs->f3_0_coarse_time;
111 111 ring_node_to_send_cwf_f3->fineTime = waveform_picker_regs->f3_0_fine_time;
112 112 waveform_picker_regs->addr_data_f3_0 = current_ring_node_f3->buffer_address;
113 113 waveform_picker_regs->status = waveform_picker_regs->status & 0x00008840; // [1000 1000 0100 0000]
114 114 }
115 115 else if ((waveform_picker_regs->status & 0x80) == 0x80){ // [1000 0000] f3 buffer 1 is full
116 116 ring_node_to_send_cwf_f3->coarseTime = waveform_picker_regs->f3_1_coarse_time;
117 117 ring_node_to_send_cwf_f3->fineTime = waveform_picker_regs->f3_1_fine_time;
118 118 waveform_picker_regs->addr_data_f3_1 = current_ring_node_f3->buffer_address;
119 119 waveform_picker_regs->status = waveform_picker_regs->status & 0x00008880; // [1000 1000 1000 0000]
120 120 }
121 121 if (rtems_event_send( Task_id[TASKID_CWF3], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
122 122 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
123 123 }
124 124 }
125 125 }
126 126 }
127 127
128 128 inline void waveforms_isr_normal( void )
129 129 {
130 130 rtems_status_code status;
131 131
132 132 if ( ( (waveform_picker_regs->status & 0x30) != 0x00 ) // [0011 0000] check the f2 full bits
133 133 && ( (waveform_picker_regs->status & 0x0c) != 0x00 ) // [0000 1100] check the f1 full bits
134 134 && ( (waveform_picker_regs->status & 0x03) != 0x00 )) // [0000 0011] check the f0 full bits
135 135 {
136 136 //***
137 137 // F0
138 138 ring_node_to_send_swf_f0 = current_ring_node_f0->previous;
139 139 current_ring_node_f0 = current_ring_node_f0->next;
140 140 if ( (waveform_picker_regs->status & 0x01) == 0x01)
141 141 {
142 142
143 143 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_0_coarse_time;
144 144 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_0_fine_time;
145 145 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->buffer_address;
146 146 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001101; // [0001 0001 0000 0001]
147 147 }
148 148 else if ( (waveform_picker_regs->status & 0x02) == 0x02)
149 149 {
150 150 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_1_coarse_time;
151 151 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_1_fine_time;
152 152 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address;
153 153 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001102; // [0001 0001 0000 0010]
154 154 }
155 155
156 156 //***
157 157 // F1
158 158 ring_node_to_send_swf_f1 = current_ring_node_f1->previous;
159 159 current_ring_node_f1 = current_ring_node_f1->next;
160 160 if ( (waveform_picker_regs->status & 0x04) == 0x04)
161 161 {
162 162 ring_node_to_send_swf_f1->coarseTime = waveform_picker_regs->f1_0_coarse_time;
163 163 ring_node_to_send_swf_f1->fineTime = waveform_picker_regs->f1_0_fine_time;
164 164 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->buffer_address;
165 165 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002204; // [0010 0010 0000 0100] f1 bits = 0
166 166 }
167 167 else if ( (waveform_picker_regs->status & 0x08) == 0x08)
168 168 {
169 169 ring_node_to_send_swf_f1->coarseTime = waveform_picker_regs->f1_1_coarse_time;
170 170 ring_node_to_send_swf_f1->fineTime = waveform_picker_regs->f1_1_fine_time;
171 171 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address;
172 172 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002208; // [0010 0010 0000 1000] f1 bits = 0
173 173 }
174 174
175 175 //***
176 176 // F2
177 177 ring_node_to_send_swf_f2 = current_ring_node_f2->previous;
178 178 current_ring_node_f2 = current_ring_node_f2->next;
179 179 if ( (waveform_picker_regs->status & 0x10) == 0x10)
180 180 {
181 181 ring_node_to_send_swf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
182 182 ring_node_to_send_swf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
183 183 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
184 184 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
185 185 }
186 186 else if ( (waveform_picker_regs->status & 0x20) == 0x20)
187 187 {
188 188 ring_node_to_send_swf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
189 189 ring_node_to_send_swf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
190 190 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
191 191 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
192 192 }
193 193 //
194 194 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_NORMAL );
195 195 if ( status != RTEMS_SUCCESSFUL)
196 196 {
197 197 status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
198 198 }
199 199 }
200 200 }
201 201
202 202 inline void waveforms_isr_burst( void )
203 203 {
204 204 unsigned char status;
205 205 rtems_status_code spare_status;
206 206
207 207 status = (waveform_picker_regs->status & 0x30) >> 4; // [0011 0000] get the status bits for f2
208 208
209 209
210 210 switch(status)
211 211 {
212 212 case 1:
213 213 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
214 214 ring_node_to_send_cwf_f2->sid = SID_BURST_CWF_F2;
215 215 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
216 216 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
217 217 current_ring_node_f2 = current_ring_node_f2->next;
218 218 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
219 219 if (rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_BURST ) != RTEMS_SUCCESSFUL) {
220 220 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
221 221 }
222 222 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
223 223 break;
224 224 case 2:
225 225 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
226 226 ring_node_to_send_cwf_f2->sid = SID_BURST_CWF_F2;
227 227 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
228 228 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
229 229 current_ring_node_f2 = current_ring_node_f2->next;
230 230 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
231 231 if (rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_BURST ) != RTEMS_SUCCESSFUL) {
232 232 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
233 233 }
234 234 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
235 235 break;
236 236 default:
237 237 break;
238 238 }
239 239 }
240 240
241 241 inline void waveforms_isr_sbm1( void )
242 242 {
243 243 rtems_status_code status;
244 244
245 245 //***
246 246 // F1
247 247 if ( (waveform_picker_regs->status & 0x0c) != 0x00 ) { // [0000 1100] check the f1 full bits
248 248 // (1) change the receiving buffer for the waveform picker
249 249 ring_node_to_send_cwf_f1 = current_ring_node_f1->previous;
250 250 current_ring_node_f1 = current_ring_node_f1->next;
251 251 if ( (waveform_picker_regs->status & 0x04) == 0x04)
252 252 {
253 253 ring_node_to_send_cwf_f1->coarseTime = waveform_picker_regs->f1_0_coarse_time;
254 254 ring_node_to_send_cwf_f1->fineTime = waveform_picker_regs->f1_0_fine_time;
255 255 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->buffer_address;
256 256 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002204; // [0010 0010 0000 0100] f1 bits = 0
257 257 }
258 258 else if ( (waveform_picker_regs->status & 0x08) == 0x08)
259 259 {
260 260 ring_node_to_send_cwf_f1->coarseTime = waveform_picker_regs->f1_1_coarse_time;
261 261 ring_node_to_send_cwf_f1->fineTime = waveform_picker_regs->f1_1_fine_time;
262 262 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address;
263 263 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002208; // [0010 0010 0000 1000] f1 bits = 0
264 264 }
265 265 // (2) send an event for the the CWF1 task for transmission (and snapshot extraction if needed)
266 266 status = rtems_event_send( Task_id[TASKID_CWF1], RTEMS_EVENT_MODE_SBM1 );
267 267 }
268 268
269 269 //***
270 270 // F0
271 271 if ( (waveform_picker_regs->status & 0x03) != 0x00 ) { // [0000 0011] check the f0 full bits
272 272 swf_f0_ready = true;
273 273 // change f0 buffer
274 274 ring_node_to_send_swf_f0 = current_ring_node_f0->previous;
275 275 current_ring_node_f0 = current_ring_node_f0->next;
276 276 if ( (waveform_picker_regs->status & 0x01) == 0x01)
277 277 {
278 278
279 279 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_0_coarse_time;
280 280 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_0_fine_time;
281 281 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->buffer_address;
282 282 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001101; // [0001 0001 0000 0001]
283 283 }
284 284 else if ( (waveform_picker_regs->status & 0x02) == 0x02)
285 285 {
286 286 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_1_coarse_time;
287 287 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_1_fine_time;
288 288 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address;
289 289 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001102; // [0001 0001 0000 0010]
290 290 }
291 291 }
292 292
293 293 //***
294 294 // F2
295 295 if ( (waveform_picker_regs->status & 0x30) != 0x00 ) { // [0011 0000] check the f2 full bits
296 296 swf_f2_ready = true;
297 297 // change f2 buffer
298 298 ring_node_to_send_swf_f2 = current_ring_node_f2->previous;
299 299 current_ring_node_f2 = current_ring_node_f2->next;
300 300 if ( (waveform_picker_regs->status & 0x10) == 0x10)
301 301 {
302 302 ring_node_to_send_swf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
303 303 ring_node_to_send_swf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
304 304 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
305 305 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
306 306 }
307 307 else if ( (waveform_picker_regs->status & 0x20) == 0x20)
308 308 {
309 309 ring_node_to_send_swf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
310 310 ring_node_to_send_swf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
311 311 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
312 312 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
313 313 }
314 314 }
315 315 }
316 316
317 317 inline void waveforms_isr_sbm2( void )
318 318 {
319 319 rtems_status_code status;
320 320
321 321 //***
322 322 // F2
323 323 if ( (waveform_picker_regs->status & 0x30) != 0x00 ) { // [0011 0000] check the f2 full bit
324 324 // (1) change the receiving buffer for the waveform picker
325 325 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
326 326 ring_node_to_send_cwf_f2->sid = SID_SBM2_CWF_F2;
327 327 current_ring_node_f2 = current_ring_node_f2->next;
328 328 if ( (waveform_picker_regs->status & 0x10) == 0x10)
329 329 {
330 330 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
331 331 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
332 332 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
333 333 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004410; // [0100 0100 0001 0000]
334 334 }
335 335 else if ( (waveform_picker_regs->status & 0x20) == 0x20)
336 336 {
337 337 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
338 338 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
339 339 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
340 340 waveform_picker_regs->status = waveform_picker_regs->status & 0x00004420; // [0100 0100 0010 0000]
341 341 }
342 342 // (2) send an event for the waveforms transmission
343 343 status = rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_SBM2 );
344 344 }
345 345
346 346 //***
347 347 // F0
348 348 if ( (waveform_picker_regs->status & 0x03) != 0x00 ) { // [0000 0011] check the f0 full bit
349 349 swf_f0_ready = true;
350 350 // change f0 buffer
351 351 ring_node_to_send_swf_f0 = current_ring_node_f0->previous;
352 352 current_ring_node_f0 = current_ring_node_f0->next;
353 353 if ( (waveform_picker_regs->status & 0x01) == 0x01)
354 354 {
355 355
356 356 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_0_coarse_time;
357 357 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_0_fine_time;
358 358 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->buffer_address;
359 359 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001101; // [0001 0001 0000 0001]
360 360 }
361 361 else if ( (waveform_picker_regs->status & 0x02) == 0x02)
362 362 {
363 363 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_1_coarse_time;
364 364 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_1_fine_time;
365 365 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address;
366 366 waveform_picker_regs->status = waveform_picker_regs->status & 0x00001102; // [0001 0001 0000 0010]
367 367 }
368 368 }
369 369
370 370 //***
371 371 // F1
372 372 if ( (waveform_picker_regs->status & 0x0c) != 0x00 ) { // [0000 1100] check the f1 full bit
373 373 swf_f1_ready = true;
374 374 ring_node_to_send_swf_f1 = current_ring_node_f1->previous;
375 375 current_ring_node_f1 = current_ring_node_f1->next;
376 376 if ( (waveform_picker_regs->status & 0x04) == 0x04)
377 377 {
378 378 ring_node_to_send_swf_f1->coarseTime = waveform_picker_regs->f1_0_coarse_time;
379 379 ring_node_to_send_swf_f1->fineTime = waveform_picker_regs->f1_0_fine_time;
380 380 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->buffer_address;
381 381 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002204; // [0010 0010 0000 0100] f1 bits = 0
382 382 }
383 383 else if ( (waveform_picker_regs->status & 0x08) == 0x08)
384 384 {
385 385 ring_node_to_send_swf_f1->coarseTime = waveform_picker_regs->f1_1_coarse_time;
386 386 ring_node_to_send_swf_f1->fineTime = waveform_picker_regs->f1_1_fine_time;
387 387 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address;
388 388 waveform_picker_regs->status = waveform_picker_regs->status & 0x00002208; // [0010 0010 0000 1000] f1 bits = 0
389 389 }
390 390 }
391 391 }
392 392
393 393 rtems_isr waveforms_isr( rtems_vector_number vector )
394 394 {
395 395 /** This is the interrupt sub routine called by the waveform picker core.
396 396 *
397 397 * This ISR launch different actions depending mainly on two pieces of information:
398 398 * 1. the values read in the registers of the waveform picker.
399 399 * 2. the current LFR mode.
400 400 *
401 401 */
402 402
403 403 // STATUS
404 404 // new error error buffer full
405 405 // 15 14 13 12 11 10 9 8
406 406 // f3 f2 f1 f0 f3 f2 f1 f0
407 407 //
408 408 // ready buffer
409 409 // 7 6 5 4 3 2 1 0
410 410 // f3_1 f3_0 f2_1 f2_0 f1_1 f1_0 f0_1 f0_0
411 411
412 412 rtems_status_code spare_status;
413 413
414 414 waveforms_isr_f3();
415 415
416 416 if ( (waveform_picker_regs->status & 0xff00) != 0x00) // [1111 1111 0000 0000] check the error bits
417 417 {
418 418 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_10 );
419 419 }
420 420
421 421 switch(lfrCurrentMode)
422 422 {
423 423 //********
424 424 // STANDBY
425 425 case(LFR_MODE_STANDBY):
426 426 break;
427 427
428 428 //******
429 429 // NORMAL
430 430 case(LFR_MODE_NORMAL):
431 431 waveforms_isr_normal();
432 432 break;
433 433
434 434 //******
435 435 // BURST
436 436 case(LFR_MODE_BURST):
437 437 waveforms_isr_burst();
438 438 break;
439 439
440 440 //*****
441 441 // SBM1
442 442 case(LFR_MODE_SBM1):
443 443 waveforms_isr_sbm1();
444 444 break;
445 445
446 446 //*****
447 447 // SBM2
448 448 case(LFR_MODE_SBM2):
449 449 waveforms_isr_sbm2();
450 450 break;
451 451
452 452 //********
453 453 // DEFAULT
454 454 default:
455 455 break;
456 456 }
457 457 }
458 458
459 459 //************
460 460 // RTEMS TASKS
461 461
462 462 rtems_task wfrm_task(rtems_task_argument argument) //used with the waveform picker VHDL IP
463 463 {
464 464 /** This RTEMS task is dedicated to the transmission of snapshots of the NORMAL mode.
465 465 *
466 466 * @param unused is the starting argument of the RTEMS task
467 467 *
468 468 * The following data packets are sent by this task:
469 469 * - TM_LFR_SCIENCE_NORMAL_SWF_F0
470 470 * - TM_LFR_SCIENCE_NORMAL_SWF_F1
471 471 * - TM_LFR_SCIENCE_NORMAL_SWF_F2
472 472 *
473 473 */
474 474
475 475 rtems_event_set event_out;
476 476 rtems_id queue_id;
477 477 rtems_status_code status;
478 478 bool resynchronisationEngaged;
479 479 ring_node *ring_node_wf_snap_extracted_ptr;
480 480
481 481 ring_node_wf_snap_extracted_ptr = (ring_node *) &ring_node_wf_snap_extracted;
482 482
483 483 resynchronisationEngaged = false;
484 484
485 485 status = get_message_queue_id_send( &queue_id );
486 486 if (status != RTEMS_SUCCESSFUL)
487 487 {
488 488 PRINTF1("in WFRM *** ERR get_message_queue_id_send %d\n", status)
489 489 }
490 490
491 491 BOOT_PRINTF("in WFRM ***\n")
492 492
493 493 while(1){
494 494 // wait for an RTEMS_EVENT
495 495 rtems_event_receive(RTEMS_EVENT_MODE_NORMAL | RTEMS_EVENT_MODE_SBM1
496 496 | RTEMS_EVENT_MODE_SBM2 | RTEMS_EVENT_MODE_SBM2_WFRM,
497 497 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
498 498 if(resynchronisationEngaged == false)
499 499 { // engage resynchronisation
500 500 snapshot_resynchronization( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
501 501 resynchronisationEngaged = true;
502 502 }
503 503 else
504 504 { // reset delta_snapshot to the nominal value
505 505 PRINTF("no resynchronisation, reset delta_snapshot to the nominal value\n")
506 506 set_wfp_delta_snapshot();
507 507 resynchronisationEngaged = false;
508 508 }
509 509 //
510 510
511 511 if (event_out == RTEMS_EVENT_MODE_NORMAL)
512 512 {
513 513 DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_NORMAL\n")
514 514 ring_node_to_send_swf_f0->sid = SID_NORM_SWF_F0;
515 515 ring_node_to_send_swf_f1->sid = SID_NORM_SWF_F1;
516 516 ring_node_to_send_swf_f2->sid = SID_NORM_SWF_F2;
517 517 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f0, sizeof( ring_node* ) );
518 518 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f1, sizeof( ring_node* ) );
519 519 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f2, sizeof( ring_node* ) );
520 520 }
521 521 if (event_out == RTEMS_EVENT_MODE_SBM1)
522 522 {
523 523 DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_SBM1\n")
524 524 ring_node_to_send_swf_f0->sid = SID_NORM_SWF_F0;
525 525 ring_node_wf_snap_extracted_ptr->sid = SID_NORM_SWF_F1;
526 526 ring_node_to_send_swf_f2->sid = SID_NORM_SWF_F2;
527 527 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f0, sizeof( ring_node* ) );
528 528 status = rtems_message_queue_send( queue_id, &ring_node_wf_snap_extracted_ptr, sizeof( ring_node* ) );
529 529 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f2, sizeof( ring_node* ) );
530 530 }
531 531 if (event_out == RTEMS_EVENT_MODE_SBM2)
532 532 {
533 533 DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_SBM2\n")
534 534 ring_node_to_send_swf_f0->sid = SID_NORM_SWF_F0;
535 535 ring_node_to_send_swf_f1->sid = SID_NORM_SWF_F1;
536 536 ring_node_wf_snap_extracted_ptr->sid = SID_NORM_SWF_F2;
537 537 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f0, sizeof( ring_node* ) );
538 538 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f1, sizeof( ring_node* ) );
539 539 status = rtems_message_queue_send( queue_id, &ring_node_wf_snap_extracted_ptr, sizeof( ring_node* ) );
540 540 }
541 541 }
542 542 }
543 543
544 544 rtems_task cwf3_task(rtems_task_argument argument) //used with the waveform picker VHDL IP
545 545 {
546 546 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f3.
547 547 *
548 548 * @param unused is the starting argument of the RTEMS task
549 549 *
550 550 * The following data packet is sent by this task:
551 551 * - TM_LFR_SCIENCE_NORMAL_CWF_F3
552 552 *
553 553 */
554 554
555 555 rtems_event_set event_out;
556 556 rtems_id queue_id;
557 557 rtems_status_code status;
558 558 ring_node ring_node_cwf3_light;
559 559
560 560 status = get_message_queue_id_send( &queue_id );
561 561 if (status != RTEMS_SUCCESSFUL)
562 562 {
563 563 PRINTF1("in CWF3 *** ERR get_message_queue_id_send %d\n", status)
564 564 }
565 565
566 566 ring_node_to_send_cwf_f3->sid = SID_NORM_CWF_LONG_F3;
567 567
568 568 // init the ring_node_cwf3_light structure
569 569 ring_node_cwf3_light.buffer_address = (int) wf_cont_f3_light;
570 570 ring_node_cwf3_light.coarseTime = 0x00;
571 571 ring_node_cwf3_light.fineTime = 0x00;
572 572 ring_node_cwf3_light.next = NULL;
573 573 ring_node_cwf3_light.previous = NULL;
574 574 ring_node_cwf3_light.sid = SID_NORM_CWF_F3;
575 575 ring_node_cwf3_light.status = 0x00;
576 576
577 577 BOOT_PRINTF("in CWF3 ***\n")
578 578
579 579 while(1){
580 580 // wait for an RTEMS_EVENT
581 581 rtems_event_receive( RTEMS_EVENT_0,
582 582 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
583 583 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
584 584 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode==LFR_MODE_SBM2) )
585 585 {
586 586 if ( (parameter_dump_packet.sy_lfr_n_cwf_long_f3 & 0x01) == 0x01)
587 587 {
588 588 PRINTF("send CWF_LONG_F3\n")
589 589 ring_node_to_send_cwf_f3->sid = SID_NORM_CWF_LONG_F3;
590 590 status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf_f3, sizeof( ring_node* ) );
591 591 }
592 592 else
593 593 {
594 594 PRINTF("send CWF_F3 (light)\n")
595 595 send_waveform_CWF3_light( ring_node_to_send_cwf_f3, &ring_node_cwf3_light, queue_id );
596 596 }
597 597
598 598 }
599 599 else
600 600 {
601 601 PRINTF1("in CWF3 *** lfrCurrentMode is %d, no data will be sent\n", lfrCurrentMode)
602 602 }
603 603 }
604 604 }
605 605
606 606 rtems_task cwf2_task(rtems_task_argument argument) // ONLY USED IN BURST AND SBM2
607 607 {
608 608 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f2.
609 609 *
610 610 * @param unused is the starting argument of the RTEMS task
611 611 *
612 612 * The following data packet is sent by this function:
613 613 * - TM_LFR_SCIENCE_BURST_CWF_F2
614 614 * - TM_LFR_SCIENCE_SBM2_CWF_F2
615 615 *
616 616 */
617 617
618 618 rtems_event_set event_out;
619 619 rtems_id queue_id;
620 620 rtems_status_code status;
621 621 ring_node *ring_node_to_send;
622 622 unsigned long long int acquisitionTimeF0_asLong;
623 623
624 624 acquisitionTimeF0_asLong = 0x00;
625 625
626 626 status = get_message_queue_id_send( &queue_id );
627 627 if (status != RTEMS_SUCCESSFUL)
628 628 {
629 629 PRINTF1("in CWF2 *** ERR get_message_queue_id_send %d\n", status)
630 630 }
631 631
632 632 BOOT_PRINTF("in CWF2 ***\n")
633 633
634 634 while(1){
635 635 // wait for an RTEMS_EVENT
636 636 rtems_event_receive( RTEMS_EVENT_MODE_BURST | RTEMS_EVENT_MODE_SBM2,
637 637 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
638 638 ring_node_to_send = getRingNodeToSendCWF( 2 );
639 639 if (event_out == RTEMS_EVENT_MODE_BURST)
640 640 {
641 641 status = rtems_message_queue_send( queue_id, &ring_node_to_send, sizeof( ring_node* ) );
642 642 }
643 643 if (event_out == RTEMS_EVENT_MODE_SBM2)
644 644 {
645 645 status = rtems_message_queue_send( queue_id, &ring_node_to_send, sizeof( ring_node* ) );
646 646 // launch snapshot extraction if needed
647 647 if (extractSWF == true)
648 648 {
649 649 ring_node_to_send_swf_f2 = ring_node_to_send_cwf_f2;
650 650 // extract the snapshot
651 651 build_snapshot_from_ring( ring_node_to_send_swf_f2, 2, acquisitionTimeF0_asLong );
652 652 // send the snapshot when built
653 653 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_SBM2 );
654 654 extractSWF = false;
655 655 }
656 656 if (swf_f0_ready && swf_f1_ready)
657 657 {
658 658 extractSWF = true;
659 659 // record the acquition time of the fà snapshot to use to build the snapshot at f2
660 660 acquisitionTimeF0_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
661 661 swf_f0_ready = false;
662 662 swf_f1_ready = false;
663 663 }
664 664 }
665 665 }
666 666 }
667 667
668 668 rtems_task cwf1_task(rtems_task_argument argument) // ONLY USED IN SBM1
669 669 {
670 670 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f1.
671 671 *
672 672 * @param unused is the starting argument of the RTEMS task
673 673 *
674 674 * The following data packet is sent by this function:
675 675 * - TM_LFR_SCIENCE_SBM1_CWF_F1
676 676 *
677 677 */
678 678
679 679 rtems_event_set event_out;
680 680 rtems_id queue_id;
681 681 rtems_status_code status;
682 682
683 683 ring_node * ring_node_to_send_cwf;
684 684
685 685 status = get_message_queue_id_send( &queue_id );
686 686 if (status != RTEMS_SUCCESSFUL)
687 687 {
688 688 PRINTF1("in CWF1 *** ERR get_message_queue_id_send %d\n", status)
689 689 }
690 690
691 691 BOOT_PRINTF("in CWF1 ***\n")
692 692
693 693 while(1){
694 694 // wait for an RTEMS_EVENT
695 695 rtems_event_receive( RTEMS_EVENT_MODE_SBM1,
696 696 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
697 697 ring_node_to_send_cwf = getRingNodeToSendCWF( 1 );
698 698 ring_node_to_send_cwf_f1->sid = SID_SBM1_CWF_F1;
699 699 status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf, sizeof( ring_node* ) );
700 700 // launch snapshot extraction if needed
701 701 if (extractSWF == true)
702 702 {
703 703 ring_node_to_send_swf_f1 = ring_node_to_send_cwf;
704 704 // launch the snapshot extraction
705 705 status = rtems_event_send( Task_id[TASKID_SWBD], RTEMS_EVENT_MODE_SBM1 );
706 706 extractSWF = false;
707 707 }
708 708 if (swf_f0_ready == true)
709 709 {
710 710 extractSWF = true;
711 711 swf_f0_ready = false; // this step shall be executed only one time
712 712 }
713 713 if ((swf_f1_ready == true) && (swf_f2_ready == true)) // swf_f1 is ready after the extraction
714 714 {
715 715 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_SBM1 );
716 716 swf_f1_ready = false;
717 717 swf_f2_ready = false;
718 718 }
719 719 }
720 720 }
721 721
722 722 rtems_task swbd_task(rtems_task_argument argument)
723 723 {
724 724 /** This RTEMS task is dedicated to the building of snapshots from different continuous waveforms buffers.
725 725 *
726 726 * @param unused is the starting argument of the RTEMS task
727 727 *
728 728 */
729 729
730 730 rtems_event_set event_out;
731 731 unsigned long long int acquisitionTimeF0_asLong;
732 732
733 733 acquisitionTimeF0_asLong = 0x00;
734 734
735 735 BOOT_PRINTF("in SWBD ***\n")
736 736
737 737 while(1){
738 738 // wait for an RTEMS_EVENT
739 739 rtems_event_receive( RTEMS_EVENT_MODE_SBM1 | RTEMS_EVENT_MODE_SBM2,
740 740 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
741 741 if (event_out == RTEMS_EVENT_MODE_SBM1)
742 742 {
743 743 acquisitionTimeF0_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
744 744 build_snapshot_from_ring( ring_node_to_send_swf_f1, 1, acquisitionTimeF0_asLong );
745 745 swf_f1_ready = true; // the snapshot has been extracted and is ready to be sent
746 746 }
747 747 else
748 748 {
749 749 PRINTF1("in SWBD *** unexpected rtems event received %x\n", (int) event_out)
750 750 }
751 751 }
752 752 }
753 753
754 754 //******************
755 755 // general functions
756 756
757 757 void WFP_init_rings( void )
758 758 {
759 759 // F0 RING
760 760 init_ring( waveform_ring_f0, NB_RING_NODES_F0, wf_buffer_f0, WFRM_BUFFER );
761 761 // F1 RING
762 762 init_ring( waveform_ring_f1, NB_RING_NODES_F1, wf_buffer_f1, WFRM_BUFFER );
763 763 // F2 RING
764 764 init_ring( waveform_ring_f2, NB_RING_NODES_F2, wf_buffer_f2, WFRM_BUFFER );
765 765 // F3 RING
766 766 init_ring( waveform_ring_f3, NB_RING_NODES_F3, wf_buffer_f3, WFRM_BUFFER );
767 767
768 768 ring_node_wf_snap_extracted.buffer_address = (int) wf_snap_extracted;
769 769
770 770 DEBUG_PRINTF1("waveform_ring_f0 @%x\n", (unsigned int) waveform_ring_f0)
771 771 DEBUG_PRINTF1("waveform_ring_f1 @%x\n", (unsigned int) waveform_ring_f1)
772 772 DEBUG_PRINTF1("waveform_ring_f2 @%x\n", (unsigned int) waveform_ring_f2)
773 773 DEBUG_PRINTF1("waveform_ring_f3 @%x\n", (unsigned int) waveform_ring_f3)
774 774 DEBUG_PRINTF1("wf_buffer_f0 @%x\n", (unsigned int) wf_buffer_f0)
775 775 DEBUG_PRINTF1("wf_buffer_f1 @%x\n", (unsigned int) wf_buffer_f1)
776 776 DEBUG_PRINTF1("wf_buffer_f2 @%x\n", (unsigned int) wf_buffer_f2)
777 777 DEBUG_PRINTF1("wf_buffer_f3 @%x\n", (unsigned int) wf_buffer_f3)
778 778
779 779 }
780 780
781 781 void init_ring(ring_node ring[], unsigned char nbNodes, volatile int buffer[], unsigned int bufferSize )
782 782 {
783 783 unsigned char i;
784 784
785 785 //***************
786 786 // BUFFER ADDRESS
787 787 for(i=0; i<nbNodes; i++)
788 788 {
789 789 ring[i].coarseTime = 0x00;
790 790 ring[i].fineTime = 0x00;
791 791 ring[i].sid = 0x00;
792 792 ring[i].status = 0x00;
793 793 ring[i].buffer_address = (int) &buffer[ i * bufferSize ];
794 794 }
795 795
796 796 //*****
797 797 // NEXT
798 798 ring[ nbNodes - 1 ].next = (ring_node*) &ring[ 0 ];
799 799 for(i=0; i<nbNodes-1; i++)
800 800 {
801 801 ring[i].next = (ring_node*) &ring[ i + 1 ];
802 802 }
803 803
804 804 //*********
805 805 // PREVIOUS
806 806 ring[ 0 ].previous = (ring_node*) &ring[ nbNodes - 1 ];
807 807 for(i=1; i<nbNodes; i++)
808 808 {
809 809 ring[i].previous = (ring_node*) &ring[ i - 1 ];
810 810 }
811 811 }
812 812
813 813 void WFP_reset_current_ring_nodes( void )
814 814 {
815 815 current_ring_node_f0 = waveform_ring_f0[0].next;
816 816 current_ring_node_f1 = waveform_ring_f1[0].next;
817 817 current_ring_node_f2 = waveform_ring_f2[0].next;
818 818 current_ring_node_f3 = waveform_ring_f3[0].next;
819 819
820 820 ring_node_to_send_swf_f0 = waveform_ring_f0;
821 821 ring_node_to_send_swf_f1 = waveform_ring_f1;
822 822 ring_node_to_send_swf_f2 = waveform_ring_f2;
823 823
824 824 ring_node_to_send_cwf_f1 = waveform_ring_f1;
825 825 ring_node_to_send_cwf_f2 = waveform_ring_f2;
826 826 ring_node_to_send_cwf_f3 = waveform_ring_f3;
827 827 }
828 828
829 829 int send_waveform_CWF3_light( ring_node *ring_node_to_send, ring_node *ring_node_cwf3_light, rtems_id queue_id )
830 830 {
831 831 /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data.
832 832 *
833 833 * @param waveform points to the buffer containing the data that will be send.
834 834 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
835 835 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
836 836 * contain information to setup the transmission of the data packets.
837 837 *
838 838 * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer
839 839 * from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks.
840 840 *
841 841 */
842 842
843 843 unsigned int i;
844 844 int ret;
845 845 rtems_status_code status;
846 846
847 847 char *sample;
848 848 int *dataPtr;
849 849
850 850 ret = LFR_DEFAULT;
851 851
852 852 dataPtr = (int*) ring_node_to_send->buffer_address;
853 853
854 854 ring_node_cwf3_light->coarseTime = ring_node_to_send->coarseTime;
855 855 ring_node_cwf3_light->fineTime = ring_node_to_send->fineTime;
856 856
857 857 //**********************
858 858 // BUILD CWF3_light DATA
859 859 for ( i=0; i< NB_SAMPLES_PER_SNAPSHOT; i++)
860 860 {
861 861 sample = (char*) &dataPtr[ (i * NB_WORDS_SWF_BLK) ];
862 862 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) ] = sample[ 0 ];
863 863 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 1 ] = sample[ 1 ];
864 864 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 2 ] = sample[ 2 ];
865 865 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 3 ] = sample[ 3 ];
866 866 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 4 ] = sample[ 4 ];
867 867 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + 5 ] = sample[ 5 ];
868 868 }
869 869
870 870 // SEND PACKET
871 871 status = rtems_message_queue_send( queue_id, &ring_node_cwf3_light, sizeof( ring_node* ) );
872 872 if (status != RTEMS_SUCCESSFUL) {
873 873 printf("%d-%d, ERR %d\n", SID_NORM_CWF_F3, i, (int) status);
874 874 ret = LFR_DEFAULT;
875 875 }
876 876
877 877 return ret;
878 878 }
879 879
880 880 void compute_acquisition_time( unsigned int coarseTime, unsigned int fineTime,
881 881 unsigned int sid, unsigned char pa_lfr_pkt_nr, unsigned char * acquisitionTime )
882 882 {
883 883 unsigned long long int acquisitionTimeAsLong;
884 884 unsigned char localAcquisitionTime[6];
885 885 double deltaT;
886 886
887 887 deltaT = 0.;
888 888
889 889 localAcquisitionTime[0] = (unsigned char) ( coarseTime >> 24 );
890 890 localAcquisitionTime[1] = (unsigned char) ( coarseTime >> 16 );
891 891 localAcquisitionTime[2] = (unsigned char) ( coarseTime >> 8 );
892 892 localAcquisitionTime[3] = (unsigned char) ( coarseTime );
893 893 localAcquisitionTime[4] = (unsigned char) ( fineTime >> 8 );
894 894 localAcquisitionTime[5] = (unsigned char) ( fineTime );
895 895
896 896 acquisitionTimeAsLong = ( (unsigned long long int) localAcquisitionTime[0] << 40 )
897 897 + ( (unsigned long long int) localAcquisitionTime[1] << 32 )
898 898 + ( (unsigned long long int) localAcquisitionTime[2] << 24 )
899 899 + ( (unsigned long long int) localAcquisitionTime[3] << 16 )
900 900 + ( (unsigned long long int) localAcquisitionTime[4] << 8 )
901 901 + ( (unsigned long long int) localAcquisitionTime[5] );
902 902
903 903 switch( sid )
904 904 {
905 905 case SID_NORM_SWF_F0:
906 906 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * 65536. / 24576. ;
907 907 break;
908 908
909 909 case SID_NORM_SWF_F1:
910 910 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * 65536. / 4096. ;
911 911 break;
912 912
913 913 case SID_NORM_SWF_F2:
914 914 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * 65536. / 256. ;
915 915 break;
916 916
917 917 case SID_SBM1_CWF_F1:
918 918 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 4096. ;
919 919 break;
920 920
921 921 case SID_SBM2_CWF_F2:
922 922 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 256. ;
923 923 break;
924 924
925 925 case SID_BURST_CWF_F2:
926 926 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 256. ;
927 927 break;
928 928
929 929 case SID_NORM_CWF_F3:
930 930 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF_SHORT_F3 * 65536. / 16. ;
931 931 break;
932 932
933 933 case SID_NORM_CWF_LONG_F3:
934 934 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * 65536. / 16. ;
935 935 break;
936 936
937 937 default:
938 938 PRINTF1("in compute_acquisition_time *** ERR unexpected sid %d\n", sid)
939 939 deltaT = 0.;
940 940 break;
941 941 }
942 942
943 943 acquisitionTimeAsLong = acquisitionTimeAsLong + (unsigned long long int) deltaT;
944 944 //
945 945 acquisitionTime[0] = (unsigned char) (acquisitionTimeAsLong >> 40);
946 946 acquisitionTime[1] = (unsigned char) (acquisitionTimeAsLong >> 32);
947 947 acquisitionTime[2] = (unsigned char) (acquisitionTimeAsLong >> 24);
948 948 acquisitionTime[3] = (unsigned char) (acquisitionTimeAsLong >> 16);
949 949 acquisitionTime[4] = (unsigned char) (acquisitionTimeAsLong >> 8 );
950 950 acquisitionTime[5] = (unsigned char) (acquisitionTimeAsLong );
951 951
952 952 }
953 953
954 954 void build_snapshot_from_ring( ring_node *ring_node_to_send, unsigned char frequencyChannel, unsigned long long int acquisitionTimeF0_asLong )
955 955 {
956 956 unsigned int i;
957 957 unsigned long long int centerTime_asLong;
958 958 unsigned long long int acquisitionTime_asLong;
959 959 unsigned long long int bufferAcquisitionTime_asLong;
960 960 unsigned char *ptr1;
961 961 unsigned char *ptr2;
962 962 unsigned char *timeCharPtr;
963 963 unsigned char nb_ring_nodes;
964 964 unsigned long long int frequency_asLong;
965 965 unsigned long long int nbTicksPerSample_asLong;
966 966 unsigned long long int nbSamplesPart1_asLong;
967 967 unsigned long long int sampleOffset_asLong;
968 968
969 969 unsigned int deltaT_F0;
970 970 unsigned int deltaT_F1;
971 971 unsigned long long int deltaT_F2;
972 972
973 973 deltaT_F0 = 2731; // (2048. / 24576. / 2.) * 65536. = 2730.667;
974 974 deltaT_F1 = 16384; // (2048. / 4096. / 2.) * 65536. = 16384;
975 975 deltaT_F2 = 262144; // (2048. / 256. / 2.) * 65536. = 262144;
976 976 sampleOffset_asLong = 0x00;
977 977
978 978 // (1) get the f0 acquisition time => the value is passed in argument
979 979
980 980 // (2) compute the central reference time
981 981 centerTime_asLong = acquisitionTimeF0_asLong + deltaT_F0;
982 982
983 983 // (3) compute the acquisition time of the current snapshot
984 984 switch(frequencyChannel)
985 985 {
986 986 case 1: // 1 is for F1 = 4096 Hz
987 987 acquisitionTime_asLong = centerTime_asLong - deltaT_F1;
988 988 nb_ring_nodes = NB_RING_NODES_F1;
989 989 frequency_asLong = 4096;
990 990 nbTicksPerSample_asLong = 16; // 65536 / 4096;
991 991 break;
992 992 case 2: // 2 is for F2 = 256 Hz
993 993 acquisitionTime_asLong = centerTime_asLong - deltaT_F2;
994 994 nb_ring_nodes = NB_RING_NODES_F2;
995 995 frequency_asLong = 256;
996 996 nbTicksPerSample_asLong = 256; // 65536 / 256;
997 997 break;
998 998 default:
999 999 acquisitionTime_asLong = centerTime_asLong;
1000 1000 frequency_asLong = 256;
1001 1001 nbTicksPerSample_asLong = 256;
1002 1002 break;
1003 1003 }
1004 1004
1005 1005 //****************************************************************************
1006 1006 // (4) search the ring_node with the acquisition time <= acquisitionTime_asLong
1007 1007 for (i=0; i<nb_ring_nodes; i++)
1008 1008 {
1009 1009 PRINTF1("%d ... ", i)
1010 1010 bufferAcquisitionTime_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send->coarseTime );
1011 1011 if (bufferAcquisitionTime_asLong <= acquisitionTime_asLong)
1012 1012 {
1013 1013 PRINTF1("buffer found with acquisition time = %llx\n", bufferAcquisitionTime_asLong)
1014 1014 break;
1015 1015 }
1016 1016 ring_node_to_send = ring_node_to_send->previous;
1017 1017 }
1018 1018
1019 1019 // (5) compute the number of samples to take in the current buffer
1020 1020 sampleOffset_asLong = ((acquisitionTime_asLong - bufferAcquisitionTime_asLong) * frequency_asLong ) >> 16;
1021 1021 nbSamplesPart1_asLong = NB_SAMPLES_PER_SNAPSHOT - sampleOffset_asLong;
1022 1022 PRINTF2("sampleOffset_asLong = %lld, nbSamplesPart1_asLong = %lld\n", sampleOffset_asLong, nbSamplesPart1_asLong)
1023 1023
1024 1024 // (6) compute the final acquisition time
1025 1025 acquisitionTime_asLong = bufferAcquisitionTime_asLong +
1026 1026 sampleOffset_asLong * nbTicksPerSample_asLong;
1027 1027
1028 1028 // (7) copy the acquisition time at the beginning of the extrated snapshot
1029 1029 ptr1 = (unsigned char*) &acquisitionTime_asLong;
1030 1030 // fine time
1031 1031 ptr2 = (unsigned char*) &ring_node_wf_snap_extracted.fineTime;
1032 1032 ptr2[2] = ptr1[ 4 + 2 ];
1033 1033 ptr2[3] = ptr1[ 5 + 2 ];
1034 1034 // coarse time
1035 1035 ptr2 = (unsigned char*) &ring_node_wf_snap_extracted.coarseTime;
1036 1036 ptr2[0] = ptr1[ 0 + 2 ];
1037 1037 ptr2[1] = ptr1[ 1 + 2 ];
1038 1038 ptr2[2] = ptr1[ 2 + 2 ];
1039 1039 ptr2[3] = ptr1[ 3 + 2 ];
1040 1040
1041 1041 // re set the synchronization bit
1042 1042 timeCharPtr = (unsigned char*) &ring_node_to_send->coarseTime;
1043 1043 ptr2[0] = ptr2[0] | (timeCharPtr[0] & 0x80); // [1000 0000]
1044 1044
1045 1045 if ( (nbSamplesPart1_asLong >= NB_SAMPLES_PER_SNAPSHOT) | (nbSamplesPart1_asLong < 0) )
1046 1046 {
1047 1047 nbSamplesPart1_asLong = 0;
1048 1048 }
1049 1049 // copy the part 1 of the snapshot in the extracted buffer
1050 1050 for ( i = 0; i < (nbSamplesPart1_asLong * NB_WORDS_SWF_BLK); i++ )
1051 1051 {
1052 1052 wf_snap_extracted[i] =
1053 1053 ((int*) ring_node_to_send->buffer_address)[ i + (sampleOffset_asLong * NB_WORDS_SWF_BLK) ];
1054 1054 }
1055 1055 // copy the part 2 of the snapshot in the extracted buffer
1056 1056 ring_node_to_send = ring_node_to_send->next;
1057 1057 for ( i = (nbSamplesPart1_asLong * NB_WORDS_SWF_BLK); i < (NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK); i++ )
1058 1058 {
1059 1059 wf_snap_extracted[i] =
1060 1060 ((int*) ring_node_to_send->buffer_address)[ (i-(nbSamplesPart1_asLong * NB_WORDS_SWF_BLK)) ];
1061 1061 }
1062 1062 }
1063 1063
1064 1064 void snapshot_resynchronization( unsigned char *timePtr )
1065 1065 {
1066 1066 unsigned long long int acquisitionTime;
1067 1067 unsigned long long int centerTime;
1068 1068 unsigned long long int previousTick;
1069 1069 unsigned long long int nextTick;
1070 1070 unsigned long long int deltaPreviousTick;
1071 1071 unsigned long long int deltaNextTick;
1072 1072 unsigned int deltaTickInF2;
1073 1073 double deltaPrevious;
1074 1074 double deltaNext;
1075 1075
1076 1076 acquisitionTime = get_acquisition_time( timePtr );
1077 1077
1078 1078 // compute center time
1079 1079 centerTime = acquisitionTime + 2731; // (2048. / 24576. / 2.) * 65536. = 2730.667;
1080 1080 previousTick = centerTime - (centerTime & 0xffff);
1081 1081 nextTick = previousTick + 65536;
1082 1082
1083 1083 deltaPreviousTick = centerTime - previousTick;
1084 1084 deltaNextTick = nextTick - centerTime;
1085 1085
1086 1086 deltaPrevious = ((double) deltaPreviousTick) / 65536. * 1000.;
1087 1087 deltaNext = ((double) deltaNextTick) / 65536. * 1000.;
1088 1088
1089 1089 PRINTF2("delta previous = %f ms, delta next = %f ms\n", deltaPrevious, deltaNext)
1090 1090 PRINTF2("delta previous = %llu, delta next = %llu\n", deltaPreviousTick, deltaNextTick)
1091 1091
1092 1092 // which tick is the closest
1093 1093 if (deltaPreviousTick > deltaNextTick)
1094 1094 {
1095 1095 deltaTickInF2 = floor( (deltaNext * 256. / 1000.) ); // the division by 2 is important here
1096 1096 waveform_picker_regs->delta_snapshot = waveform_picker_regs->delta_snapshot + deltaTickInF2;
1097 1097 printf("correction of = + %u\n", deltaTickInF2);
1098 1098 }
1099 1099 else
1100 1100 {
1101 1101 deltaTickInF2 = floor( (deltaPrevious * 256. / 1000.) ); // the division by 2 is important here
1102 1102 waveform_picker_regs->delta_snapshot = waveform_picker_regs->delta_snapshot - deltaTickInF2;
1103 1103 printf("correction of = - %u\n", deltaTickInF2);
1104 1104 }
1105 1105 }
1106 1106
1107 1107 //**************
1108 1108 // wfp registers
1109 1109 void reset_wfp_burst_enable( void )
1110 1110 {
1111 1111 /** This function resets the waveform picker burst_enable register.
1112 1112 *
1113 1113 * The burst bits [f2 f1 f0] and the enable bits [f3 f2 f1 f0] are set to 0.
1114 1114 *
1115 1115 */
1116 1116
1117 1117 // [1000 000] burst f2, f1, f0 enable f3, f2, f1, f0
1118 1118 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable & 0x80;
1119 1119 }
1120 1120
1121 1121 void reset_wfp_status( void )
1122 1122 {
1123 1123 /** This function resets the waveform picker status register.
1124 1124 *
1125 1125 * All status bits are set to 0 [new_err full_err full].
1126 1126 *
1127 1127 */
1128 1128
1129 1129 waveform_picker_regs->status = 0xffff;
1130 1130 }
1131 1131
1132 1132 void reset_wfp_buffer_addresses( void )
1133 1133 {
1134 1134 // F0
1135 1135 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->previous->buffer_address; // 0x08
1136 1136 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address; // 0x0c
1137 1137 // F1
1138 1138 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->previous->buffer_address; // 0x10
1139 1139 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address; // 0x14
1140 1140 // F2
1141 1141 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->previous->buffer_address; // 0x18
1142 1142 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address; // 0x1c
1143 1143 // F3
1144 1144 waveform_picker_regs->addr_data_f3_0 = current_ring_node_f3->previous->buffer_address; // 0x20
1145 1145 waveform_picker_regs->addr_data_f3_1 = current_ring_node_f3->buffer_address; // 0x24
1146 1146 }
1147 1147
1148 1148 void reset_waveform_picker_regs( void )
1149 1149 {
1150 1150 /** This function resets the waveform picker module registers.
1151 1151 *
1152 1152 * The registers affected by this function are located at the following offset addresses:
1153 1153 * - 0x00 data_shaping
1154 1154 * - 0x04 run_burst_enable
1155 1155 * - 0x08 addr_data_f0
1156 1156 * - 0x0C addr_data_f1
1157 1157 * - 0x10 addr_data_f2
1158 1158 * - 0x14 addr_data_f3
1159 1159 * - 0x18 status
1160 1160 * - 0x1C delta_snapshot
1161 1161 * - 0x20 delta_f0
1162 1162 * - 0x24 delta_f0_2
1163 1163 * - 0x28 delta_f1
1164 1164 * - 0x2c delta_f2
1165 1165 * - 0x30 nb_data_by_buffer
1166 1166 * - 0x34 nb_snapshot_param
1167 1167 * - 0x38 start_date
1168 1168 * - 0x3c nb_word_in_buffer
1169 1169 *
1170 1170 */
1171 1171
1172 1172 set_wfp_data_shaping(); // 0x00 *** R1 R0 SP1 SP0 BW
1173 1173
1174 1174 reset_wfp_burst_enable(); // 0x04 *** [run *** burst f2, f1, f0 *** enable f3, f2, f1, f0 ]
1175 1175
1176 1176 reset_wfp_buffer_addresses();
1177 1177
1178 1178 reset_wfp_status(); // 0x18
1179 1179
1180 1180 set_wfp_delta_snapshot(); // 0x1c *** 300 s => 0x12bff
1181 1181
1182 1182 set_wfp_delta_f0_f0_2(); // 0x20, 0x24
1183 1183
1184 1184 set_wfp_delta_f1(); // 0x28
1185 1185
1186 1186 set_wfp_delta_f2(); // 0x2c
1187 1187
1188 1188 DEBUG_PRINTF1("delta_snapshot %x\n", waveform_picker_regs->delta_snapshot)
1189 1189 DEBUG_PRINTF1("delta_f0 %x\n", waveform_picker_regs->delta_f0)
1190 1190 DEBUG_PRINTF1("delta_f0_2 %x\n", waveform_picker_regs->delta_f0_2)
1191 1191 DEBUG_PRINTF1("delta_f1 %x\n", waveform_picker_regs->delta_f1)
1192 1192 DEBUG_PRINTF1("delta_f2 %x\n", waveform_picker_regs->delta_f2)
1193 1193 // 2688 = 8 * 336
1194 1194 waveform_picker_regs->nb_data_by_buffer = 0xa7f; // 0x30 *** 2688 - 1 => nb samples -1
1195 1195 waveform_picker_regs->snapshot_param = 0xa80; // 0x34 *** 2688 => nb samples
1196 1196 waveform_picker_regs->start_date = 0x7fffffff; // 0x38
1197 1197 //
1198 1198 // coarse time and fine time registers are not initialized, they are volatile
1199 1199 //
1200 1200 waveform_picker_regs->buffer_length = 0x1f8;// buffer length in burst = 3 * 2688 / 16 = 504 = 0x1f8
1201 1201 }
1202 1202
1203 1203 void set_wfp_data_shaping( void )
1204 1204 {
1205 1205 /** This function sets the data_shaping register of the waveform picker module.
1206 1206 *
1207 1207 * The value is read from one field of the parameter_dump_packet structure:\n
1208 1208 * bw_sp0_sp1_r0_r1
1209 1209 *
1210 1210 */
1211 1211
1212 1212 unsigned char data_shaping;
1213 1213
1214 1214 // get the parameters for the data shaping [BW SP0 SP1 R0 R1] in sy_lfr_common1 and configure the register
1215 1215 // waveform picker : [R1 R0 SP1 SP0 BW]
1216 1216
1217 data_shaping = parameter_dump_packet.bw_sp0_sp1_r0_r1;
1217 data_shaping = parameter_dump_packet.sy_lfr_common_parameters;
1218 1218
1219 1219 waveform_picker_regs->data_shaping =
1220 1220 ( (data_shaping & 0x10) >> 4 ) // BW
1221 1221 + ( (data_shaping & 0x08) >> 2 ) // SP0
1222 1222 + ( (data_shaping & 0x04) ) // SP1
1223 1223 + ( (data_shaping & 0x02) << 2 ) // R0
1224 1224 + ( (data_shaping & 0x01) << 4 ); // R1
1225 1225
1226 1226 // this is a temporary way to set R2, compatible with the release 2 of the flight software
1227 1227 waveform_picker_regs->data_shaping = waveform_picker_regs->data_shaping + ( (0x1) << 5 ); // R2
1228 1228 }
1229 1229
1230 1230 void set_wfp_burst_enable_register( unsigned char mode )
1231 1231 {
1232 1232 /** This function sets the waveform picker burst_enable register depending on the mode.
1233 1233 *
1234 1234 * @param mode is the LFR mode to launch.
1235 1235 *
1236 1236 * The burst bits shall be before the enable bits.
1237 1237 *
1238 1238 */
1239 1239
1240 1240 // [0000 0000] burst f2, f1, f0 enable f3 f2 f1 f0
1241 1241 // the burst bits shall be set first, before the enable bits
1242 1242 switch(mode) {
1243 1243 case(LFR_MODE_NORMAL):
1244 1244 waveform_picker_regs->run_burst_enable = 0x00; // [0000 0000] no burst enable
1245 1245 waveform_picker_regs->run_burst_enable = 0x0f; // [0000 1111] enable f3 f2 f1 f0
1246 1246 break;
1247 1247 case(LFR_MODE_BURST):
1248 1248 waveform_picker_regs->run_burst_enable = 0x40; // [0100 0000] f2 burst enabled
1249 1249 // waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x04; // [0100] enable f2
1250 1250 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x0c; // [1100] enable f3 AND f2
1251 1251 break;
1252 1252 case(LFR_MODE_SBM1):
1253 1253 waveform_picker_regs->run_burst_enable = 0x20; // [0010 0000] f1 burst enabled
1254 1254 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x0f; // [1111] enable f3 f2 f1 f0
1255 1255 break;
1256 1256 case(LFR_MODE_SBM2):
1257 1257 waveform_picker_regs->run_burst_enable = 0x40; // [0100 0000] f2 burst enabled
1258 1258 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x0f; // [1111] enable f3 f2 f1 f0
1259 1259 break;
1260 1260 default:
1261 1261 waveform_picker_regs->run_burst_enable = 0x00; // [0000 0000] no burst enabled, no waveform enabled
1262 1262 break;
1263 1263 }
1264 1264 }
1265 1265
1266 1266 void set_wfp_delta_snapshot( void )
1267 1267 {
1268 1268 /** This function sets the delta_snapshot register of the waveform picker module.
1269 1269 *
1270 1270 * The value is read from two (unsigned char) of the parameter_dump_packet structure:
1271 1271 * - sy_lfr_n_swf_p[0]
1272 1272 * - sy_lfr_n_swf_p[1]
1273 1273 *
1274 1274 */
1275 1275
1276 1276 unsigned int delta_snapshot;
1277 1277 unsigned int delta_snapshot_in_T2;
1278 1278
1279 1279 delta_snapshot = parameter_dump_packet.sy_lfr_n_swf_p[0]*256
1280 1280 + parameter_dump_packet.sy_lfr_n_swf_p[1];
1281 1281
1282 1282 delta_snapshot_in_T2 = delta_snapshot * 256;
1283 1283 waveform_picker_regs->delta_snapshot = delta_snapshot_in_T2 - 1; // max 4 bytes
1284 1284 }
1285 1285
1286 1286 void set_wfp_delta_f0_f0_2( void )
1287 1287 {
1288 1288 unsigned int delta_snapshot;
1289 1289 unsigned int nb_samples_per_snapshot;
1290 1290 float delta_f0_in_float;
1291 1291
1292 1292 delta_snapshot = waveform_picker_regs->delta_snapshot;
1293 1293 nb_samples_per_snapshot = parameter_dump_packet.sy_lfr_n_swf_l[0] * 256 + parameter_dump_packet.sy_lfr_n_swf_l[1];
1294 1294 delta_f0_in_float =nb_samples_per_snapshot / 2. * ( 1. / 256. - 1. / 24576.) * 256.;
1295 1295
1296 1296 waveform_picker_regs->delta_f0 = delta_snapshot - floor( delta_f0_in_float );
1297 1297 waveform_picker_regs->delta_f0_2 = 0x30; // 48 = 11 0000, max 7 bits
1298 1298 }
1299 1299
1300 1300 void set_wfp_delta_f1( void )
1301 1301 {
1302 1302 unsigned int delta_snapshot;
1303 1303 unsigned int nb_samples_per_snapshot;
1304 1304 float delta_f1_in_float;
1305 1305
1306 1306 delta_snapshot = waveform_picker_regs->delta_snapshot;
1307 1307 nb_samples_per_snapshot = parameter_dump_packet.sy_lfr_n_swf_l[0] * 256 + parameter_dump_packet.sy_lfr_n_swf_l[1];
1308 1308 delta_f1_in_float = nb_samples_per_snapshot / 2. * ( 1. / 256. - 1. / 4096.) * 256.;
1309 1309
1310 1310 waveform_picker_regs->delta_f1 = delta_snapshot - floor( delta_f1_in_float );
1311 1311 }
1312 1312
1313 1313 void set_wfp_delta_f2()
1314 1314 {
1315 1315 unsigned int delta_snapshot;
1316 1316 unsigned int nb_samples_per_snapshot;
1317 1317
1318 1318 delta_snapshot = waveform_picker_regs->delta_snapshot;
1319 1319 nb_samples_per_snapshot = parameter_dump_packet.sy_lfr_n_swf_l[0] * 256 + parameter_dump_packet.sy_lfr_n_swf_l[1];
1320 1320
1321 1321 waveform_picker_regs->delta_f2 = delta_snapshot - nb_samples_per_snapshot / 2;
1322 1322 }
1323 1323
1324 1324 //*****************
1325 1325 // local parameters
1326 1326
1327 1327 void increment_seq_counter_source_id( unsigned char *packet_sequence_control, unsigned int sid )
1328 1328 {
1329 1329 /** This function increments the parameter "sequence_cnt" depending on the sid passed in argument.
1330 1330 *
1331 1331 * @param packet_sequence_control is a pointer toward the parameter sequence_cnt to update.
1332 1332 * @param sid is the source identifier of the packet being updated.
1333 1333 *
1334 1334 * REQ-LFR-SRS-5240 / SSS-CP-FS-590
1335 1335 * The sequence counters shall wrap around from 2^14 to zero.
1336 1336 * The sequence counter shall start at zero at startup.
1337 1337 *
1338 1338 * REQ-LFR-SRS-5239 / SSS-CP-FS-580
1339 1339 * All TM_LFR_SCIENCE_ packets are sent to ground, i.e. destination id = 0
1340 1340 *
1341 1341 */
1342 1342
1343 1343 unsigned short *sequence_cnt;
1344 1344 unsigned short segmentation_grouping_flag;
1345 1345 unsigned short new_packet_sequence_control;
1346 1346 rtems_mode initial_mode_set;
1347 1347 rtems_mode current_mode_set;
1348 1348 rtems_status_code status;
1349 1349
1350 1350 //******************************************
1351 1351 // CHANGE THE MODE OF THE CALLING RTEMS TASK
1352 1352 status = rtems_task_mode( RTEMS_NO_PREEMPT, RTEMS_PREEMPT_MASK, &initial_mode_set );
1353 1353
1354 1354 if ( (sid == SID_NORM_SWF_F0) || (sid == SID_NORM_SWF_F1) || (sid == SID_NORM_SWF_F2)
1355 1355 || (sid == SID_NORM_CWF_F3) || (sid == SID_NORM_CWF_LONG_F3)
1356 1356 || (sid == SID_BURST_CWF_F2)
1357 1357 || (sid == SID_NORM_ASM_F0) || (sid == SID_NORM_ASM_F1) || (sid == SID_NORM_ASM_F2)
1358 1358 || (sid == SID_NORM_BP1_F0) || (sid == SID_NORM_BP1_F1) || (sid == SID_NORM_BP1_F2)
1359 1359 || (sid == SID_NORM_BP2_F0) || (sid == SID_NORM_BP2_F1) || (sid == SID_NORM_BP2_F2)
1360 1360 || (sid == SID_BURST_BP1_F0) || (sid == SID_BURST_BP2_F0)
1361 1361 || (sid == SID_BURST_BP1_F1) || (sid == SID_BURST_BP2_F1) )
1362 1362 {
1363 1363 sequence_cnt = (unsigned short *) &sequenceCounters_SCIENCE_NORMAL_BURST;
1364 1364 }
1365 1365 else if ( (sid ==SID_SBM1_CWF_F1) || (sid ==SID_SBM2_CWF_F2)
1366 1366 || (sid == SID_SBM1_BP1_F0) || (sid == SID_SBM1_BP2_F0)
1367 1367 || (sid == SID_SBM2_BP1_F0) || (sid == SID_SBM2_BP2_F0)
1368 1368 || (sid == SID_SBM2_BP1_F1) || (sid == SID_SBM2_BP2_F1) )
1369 1369 {
1370 1370 sequence_cnt = (unsigned short *) &sequenceCounters_SCIENCE_SBM1_SBM2;
1371 1371 }
1372 1372 else
1373 1373 {
1374 1374 sequence_cnt = (unsigned short *) NULL;
1375 1375 PRINTF1("in increment_seq_counter_source_id *** ERR apid_destid %d not known\n", sid)
1376 1376 }
1377 1377
1378 1378 if (sequence_cnt != NULL)
1379 1379 {
1380 1380 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
1381 1381 *sequence_cnt = (*sequence_cnt) & 0x3fff;
1382 1382
1383 1383 new_packet_sequence_control = segmentation_grouping_flag | (*sequence_cnt) ;
1384 1384
1385 1385 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> 8);
1386 1386 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
1387 1387
1388 1388 // increment the sequence counter
1389 1389 if ( *sequence_cnt < SEQ_CNT_MAX)
1390 1390 {
1391 1391 *sequence_cnt = *sequence_cnt + 1;
1392 1392 }
1393 1393 else
1394 1394 {
1395 1395 *sequence_cnt = 0;
1396 1396 }
1397 1397 }
1398 1398
1399 1399 //***********************************
1400 1400 // RESET THE MODE OF THE CALLING TASK
1401 1401 status = rtems_task_mode( initial_mode_set, RTEMS_PREEMPT_MASK, &current_mode_set );
1402 1402 }
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed, binary diff hidden
General Comments 0
You need to be logged in to leave comments. Login now