##// 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

R2 parameter added to TC_LFR_LOAD_COMMON_PAR...
paul -
r195:868116ca6c4a R3
parent child
Show More
Show file before | Show file after | Hide comments
@@ -1,2 +1,2
1 1 a586fe639ac179e95bdc150ebdbab0312f31dc30 LFR_basic-parameters
2 5467523e44cd6a627a81b156673a891f4d6b0017 header/lfr_common_headers
2 a806a190dcd72f71d336545073400d3cdaaa3119 header/lfr_common_headers
Show file before | Show file after | Hide comments
@@ -1,313 +1,324
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 40 unsigned char sy_lfr_common_parameters_spare;
41 41 unsigned char sy_lfr_common_parameters;
42 42 unsigned char acquisitionTime[6];
43 43 unsigned char pa_lfr_bp_blk_nr[2];
44 44 // SOURCE DATA
45 45 unsigned char data[ 780 ]; // MAX size is 26 bins * 30 Bytes [TM_LFR_SCIENCE_BURST_BP2_F1]
46 46 } bp_packet;
47 47
48 48 typedef struct
49 49 {
50 50 unsigned char targetLogicalAddress;
51 51 unsigned char protocolIdentifier;
52 52 unsigned char reserved;
53 53 unsigned char userApplication;
54 54 unsigned char packetID[2];
55 55 unsigned char packetSequenceControl[2];
56 56 unsigned char packetLength[2];
57 57 // DATA FIELD HEADER
58 58 unsigned char spare1_pusVersion_spare2;
59 59 unsigned char serviceType;
60 60 unsigned char serviceSubType;
61 61 unsigned char destinationID;
62 62 unsigned char time[6];
63 63 // AUXILIARY HEADER
64 64 unsigned char sid;
65 65 unsigned char biaStatusInfo;
66 66 unsigned char sy_lfr_common_parameters_spare;
67 67 unsigned char sy_lfr_common_parameters;
68 68 unsigned char acquisitionTime[6];
69 69 unsigned char source_data_spare;
70 70 unsigned char pa_lfr_bp_blk_nr[2];
71 71 // SOURCE DATA
72 72 unsigned char data[ 117 ]; // 13 bins * 9 Bytes only for TM_LFR_SCIENCE_NORMAL_BP1_F0 and F1
73 73 } bp_packet_with_spare;
74 74
75 75 typedef struct
76 76 {
77 77 ring_node_asm *norm;
78 78 ring_node_asm *burst_sbm;
79 79 rtems_event_set event;
80 80 unsigned int coarseTimeNORM;
81 81 unsigned int fineTimeNORM;
82 82 unsigned int coarseTimeSBM;
83 83 unsigned int fineTimeSBM;
84 84 } asm_msg;
85 85
86 86 extern volatile int sm_f0[ ];
87 87 extern volatile int sm_f1[ ];
88 88 extern volatile int sm_f2[ ];
89 89
90 90 // parameters
91 91 extern struct param_local_str param_local;
92 extern Packet_TM_LFR_PARAMETER_DUMP_t parameter_dump_packet;
92 93
93 94 // registers
94 95 extern time_management_regs_t *time_management_regs;
95 96 extern volatile spectral_matrix_regs_t *spectral_matrix_regs;
96 97
97 98 extern rtems_name misc_name[5];
98 99 extern rtems_id Task_id[20]; /* array of task ids */
99 100
100 101 //
101 102 ring_node * getRingNodeForAveraging( unsigned char frequencyChannel);
102 103 // ISR
103 104 rtems_isr spectral_matrices_isr( rtems_vector_number vector );
104 105 rtems_isr spectral_matrices_isr_simu( rtems_vector_number vector );
105 106
106 107 //******************
107 108 // Spectral Matrices
108 109 void reset_nb_sm( void );
109 110 // SM
110 111 void SM_init_rings( void );
111 112 void SM_reset_current_ring_nodes( void );
112 113 // ASM
113 114 void ASM_generic_init_ring(ring_node_asm *ring, unsigned char nbNodes );
114 115
115 116 //*****************
116 117 // Basic Parameters
117 118
118 119 void BP_reset_current_ring_nodes( void );
119 120 void BP_init_header(bp_packet *packet,
120 121 unsigned int apid, unsigned char sid,
121 122 unsigned int packetLength , unsigned char blkNr);
122 123 void BP_init_header_with_spare(bp_packet_with_spare *packet,
123 124 unsigned int apid, unsigned char sid,
124 125 unsigned int packetLength, unsigned char blkNr );
125 126 void BP_send( char *data,
126 127 rtems_id queue_id ,
127 128 unsigned int nbBytesToSend , unsigned int sid );
128 129
129 130 //******************
130 131 // general functions
131 132 void reset_sm_status( void );
132 133 void reset_spectral_matrix_regs( void );
133 134 void set_time(unsigned char *time, unsigned char *timeInBuffer );
134 135 unsigned long long int get_acquisition_time( unsigned char *timePtr );
135 136 unsigned char getSID( rtems_event_set event );
136 137
137 138 extern rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id );
138 139 extern rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id );
139 140
140 141 //***************************************
141 142 // DEFINITIONS OF STATIC INLINE FUNCTIONS
142 143 static inline void SM_average(float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
143 144 ring_node *ring_node_tab[],
144 145 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
145 146 asm_msg *msgForMATR );
147
146 148 static inline void SM_average_debug(float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
147 149 ring_node *ring_node_tab[],
148 150 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
149 151 asm_msg *msgForMATR );
150 152
151 153 void ASM_patch( float *inputASM, float *outputASM );
154
152 155 void extractReImVectors(float *inputASM, float *outputASM, unsigned int asmComponent );
153 156
154 157 static inline void ASM_reorganize_and_divide(float *averaged_spec_mat, float *averaged_spec_mat_reorganized,
155 158 float divider );
159
156 160 static inline void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat,
157 161 float divider,
158 162 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart);
163
159 164 static inline void ASM_convert(volatile float *input_matrix, char *output_matrix);
160 165
161 166 void SM_average( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
162 167 ring_node *ring_node_tab[],
163 168 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
164 169 asm_msg *msgForMATR )
165 170 {
166 171 float sum;
167 172 unsigned int i;
168 173
169 174 for(i=0; i<TOTAL_SIZE_SM; i++)
170 175 {
171 176 sum = ( (int *) (ring_node_tab[0]->buffer_address) ) [ i ]
172 177 + ( (int *) (ring_node_tab[1]->buffer_address) ) [ i ]
173 178 + ( (int *) (ring_node_tab[2]->buffer_address) ) [ i ]
174 179 + ( (int *) (ring_node_tab[3]->buffer_address) ) [ i ]
175 180 + ( (int *) (ring_node_tab[4]->buffer_address) ) [ i ]
176 181 + ( (int *) (ring_node_tab[5]->buffer_address) ) [ i ]
177 182 + ( (int *) (ring_node_tab[6]->buffer_address) ) [ i ]
178 183 + ( (int *) (ring_node_tab[7]->buffer_address) ) [ i ];
179 184
180 185 if ( (nbAverageNORM == 0) && (nbAverageSBM == 0) )
181 186 {
182 187 averaged_spec_mat_NORM[ i ] = sum;
183 188 averaged_spec_mat_SBM[ i ] = sum;
184 189 msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime;
185 190 msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime;
186 191 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
187 192 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
188 193 }
189 194 else if ( (nbAverageNORM != 0) && (nbAverageSBM != 0) )
190 195 {
191 196 averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum );
192 197 averaged_spec_mat_SBM[ i ] = ( averaged_spec_mat_SBM[ i ] + sum );
193 198 }
194 199 else if ( (nbAverageNORM != 0) && (nbAverageSBM == 0) )
195 200 {
196 201 averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum );
197 202 averaged_spec_mat_SBM[ i ] = sum;
198 203 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
199 204 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
200 205 }
201 206 else
202 207 {
203 208 PRINTF2("ERR *** in SM_average *** unexpected parameters %d %d\n", nbAverageNORM, nbAverageSBM)
204 209 }
205 210 }
206 211 }
207 212
208 213 void SM_average_debug( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
209 214 ring_node *ring_node_tab[],
210 215 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
211 216 asm_msg *msgForMATR )
212 217 {
213 218 float sum;
214 219 unsigned int i;
215 220
216 221 for(i=0; i<TOTAL_SIZE_SM; i++)
217 222 {
218 223 sum = ( (int *) (ring_node_tab[0]->buffer_address) ) [ i ];
219 224 averaged_spec_mat_NORM[ i ] = sum;
220 225 averaged_spec_mat_SBM[ i ] = sum;
221 226 msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime;
222 227 msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime;
223 228 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
224 229 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
225 230 }
226 231 }
227 232
228 233 void ASM_reorganize_and_divide( float *averaged_spec_mat, float *averaged_spec_mat_reorganized, float divider )
229 234 {
230 235 int frequencyBin;
231 236 int asmComponent;
232 237 unsigned int offsetASM;
233 238 unsigned int offsetASMReorganized;
234 239
235 240 // BUILD DATA
236 241 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
237 242 {
238 243 for( frequencyBin = 0; frequencyBin < NB_BINS_PER_SM; frequencyBin++ )
239 244 {
240 245 offsetASMReorganized =
241 246 frequencyBin * NB_VALUES_PER_SM
242 247 + asmComponent;
243 248 offsetASM =
244 249 asmComponent * NB_BINS_PER_SM
245 250 + frequencyBin;
246 251 averaged_spec_mat_reorganized[offsetASMReorganized ] =
247 252 averaged_spec_mat[ offsetASM ] / divider;
248 253 }
249 254 }
250 255 }
251 256
252 257 void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
253 258 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage, unsigned char ASMIndexStart )
254 259 {
255 260 int frequencyBin;
256 261 int asmComponent;
257 262 int offsetASM;
258 263 int offsetCompressed;
259 264 int k;
260 265
261 266 // BUILD DATA
262 267 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
263 268 {
264 269 for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
265 270 {
266 271 offsetCompressed = // NO TIME OFFSET
267 272 frequencyBin * NB_VALUES_PER_SM
268 273 + asmComponent;
269 274 offsetASM = // NO TIME OFFSET
270 275 asmComponent * NB_BINS_PER_SM
271 276 + ASMIndexStart
272 277 + frequencyBin * nbBinsToAverage;
273 278 compressed_spec_mat[ offsetCompressed ] = 0;
274 279 for ( k = 0; k < nbBinsToAverage; k++ )
275 280 {
276 281 compressed_spec_mat[offsetCompressed ] =
277 282 ( compressed_spec_mat[ offsetCompressed ]
278 283 + averaged_spec_mat[ offsetASM + k ] );
279 284 }
280 285 compressed_spec_mat[ offsetCompressed ] =
281 286 compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
282 287 }
283 288 }
284 289 }
285 290
286 291 void ASM_convert( volatile float *input_matrix, char *output_matrix)
287 292 {
288 293 unsigned int frequencyBin;
289 294 unsigned int asmComponent;
290 295 char * pt_char_input;
291 296 char * pt_char_output;
292 297 unsigned int offsetInput;
293 298 unsigned int offsetOutput;
294 299
295 300 pt_char_input = (char*) &input_matrix;
296 301 pt_char_output = (char*) &output_matrix;
297 302
298 303 // convert all other data
299 304 for( frequencyBin=0; frequencyBin<NB_BINS_PER_SM; frequencyBin++)
300 305 {
301 306 for ( asmComponent=0; asmComponent<NB_VALUES_PER_SM; asmComponent++)
302 307 {
303 308 offsetInput = (frequencyBin*NB_VALUES_PER_SM) + asmComponent ;
304 309 offsetOutput = 2 * ( (frequencyBin*NB_VALUES_PER_SM) + asmComponent ) ;
305 310 pt_char_input = (char*) &input_matrix [ offsetInput ];
306 311 pt_char_output = (char*) &output_matrix[ offsetOutput ];
307 312 pt_char_output[0] = pt_char_input[0]; // bits 31 downto 24 of the float
308 313 pt_char_output[1] = pt_char_input[1]; // bits 23 downto 16 of the float
309 314 }
310 315 }
311 316 }
312 317
318 void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat,
319 float divider,
320 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart);
321
322 int getFBinMask(int k);
323
313 324 #endif // FSW_PROCESSING_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -1,65 +1,70
1 1 #ifndef TC_LOAD_DUMP_PARAMETERS_H
2 2 #define TC_LOAD_DUMP_PARAMETERS_H
3 3
4 4 #include <rtems.h>
5 5 #include <stdio.h>
6 6
7 7 #include "fsw_params.h"
8 8 #include "wf_handler.h"
9 9 #include "tm_lfr_tc_exe.h"
10 10 #include "fsw_misc.h"
11 11 #include "basic_parameters_params.h"
12 #include "avf0_prc0.h"
12 13
13 14 #define FLOAT_EQUAL_ZERO 0.001
14 15
15 16 extern unsigned short sequenceCounterParameterDump;
16 17 extern float k_coeff_intercalib_f0_norm[ ];
17 18 extern float k_coeff_intercalib_f0_sbm[ ];
18 19 extern float k_coeff_intercalib_f1_norm[ ];
19 20 extern float k_coeff_intercalib_f1_sbm[ ];
20 21 extern float k_coeff_intercalib_f2[ ];
21 22
22 23 int action_load_common_par( ccsdsTelecommandPacket_t *TC );
23 24 int action_load_normal_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
24 25 int action_load_burst_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
25 26 int action_load_sbm1_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
26 27 int action_load_sbm2_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
27 28 int action_load_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
28 29 int action_load_fbins_mask(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
29 30 int action_dump_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
30 31 int action_dump_par(rtems_id queue_id );
31 32
32 33 // NORMAL
33 34 int check_common_par_consistency( ccsdsTelecommandPacket_t *TC, rtems_id queue_id );
34 35 int set_sy_lfr_n_swf_l( ccsdsTelecommandPacket_t *TC );
35 36 int set_sy_lfr_n_swf_p( ccsdsTelecommandPacket_t *TC );
36 37 int set_sy_lfr_n_asm_p( ccsdsTelecommandPacket_t *TC );
37 38 int set_sy_lfr_n_bp_p0( ccsdsTelecommandPacket_t *TC );
38 39 int set_sy_lfr_n_bp_p1( ccsdsTelecommandPacket_t *TC );
39 40 int set_sy_lfr_n_cwf_long_f3( ccsdsTelecommandPacket_t *TC );
40 41
41 42 // BURST
42 43 int set_sy_lfr_b_bp_p0( ccsdsTelecommandPacket_t *TC );
43 44 int set_sy_lfr_b_bp_p1( ccsdsTelecommandPacket_t *TC );
44 45
45 46 // SBM1
46 47 int set_sy_lfr_s1_bp_p0( ccsdsTelecommandPacket_t *TC );
47 48 int set_sy_lfr_s1_bp_p1( ccsdsTelecommandPacket_t *TC );
48 49
49 50 // SBM2
50 51 int set_sy_lfr_s2_bp_p0( ccsdsTelecommandPacket_t *TC );
51 52 int set_sy_lfr_s2_bp_p1( ccsdsTelecommandPacket_t *TC );
52 53
53 54 // TC_LFR_UPDATE_INFO
54 55 unsigned int check_update_info_hk_lfr_mode( unsigned char mode );
55 56 unsigned int check_update_info_hk_tds_mode( unsigned char mode );
56 57 unsigned int check_update_info_hk_thr_mode( unsigned char mode );
57 58
59 // FBINS_MASK
60 int set_sy_lfr_fbins( ccsdsTelecommandPacket_t *TC );
61
58 62 // KCOEFFICIENTS
59 63 int set_sy_lfr_kcoeff( ccsdsTelecommandPacket_t *TC );
64 void copyFloatByChar( unsigned char *destination, unsigned char *source );
60 65
61 66 void init_parameter_dump( void );
62 67 void init_kcoefficients_dump( void );
63 68 void init_kcoefficients_dump_packet( Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump, unsigned char pkt_nr, unsigned char blk_nr );
64 69
65 70 #endif // TC_LOAD_DUMP_PARAMETERS_H
Show file before | Show file after | Hide comments
@@ -1,14 +1,14
1 1 # LOAD FSW USING LINK 1
2 SpwPlugin0.StarDundeeSelectLinkNumber( 1 )
2 SpwPlugin0.StarDundeeSelectLinkNumber( 2 )
3 3
4 4 dsu3plugin0.openFile("/opt/DEV_PLE/FSW-qt/bin/fsw")
5 5 #dsu3plugin0.openFile("/opt/LFR/LFR-FSW/2.0.2.3/fsw")
6 6 dsu3plugin0.loadFile()
7 7
8 8 dsu3plugin0.run()
9 9
10 10 # START SENDING TIMECODES AT 1 Hz
11 11 SpwPlugin0.StarDundeeStartTimecodes( 1 )
12 12
13 13 # it is possible to change the time code frequency
14 14 #RMAPPlugin0.changeTimecodeFrequency(2)
Show file before | Show file after | Hide comments
@@ -1,30 +1,31
1 1 import time
2 2
3 3 proxy.loadSysDriver("SpwPlugin","SpwPlugin0")
4 4 SpwPlugin0.selectBridge("STAR-Dundee Spw USB Brick")
5 5
6 6 proxy.loadSysDriverToParent("dsu3plugin","SpwPlugin0")
7 7 proxy.loadSysDriverToParent("LFRControlPlugin","SpwPlugin0")
8 8
9 9 availableBrickCount = SpwPlugin0.StarDundeeGetAvailableBrickCount()
10 10 print str(availableBrickCount) + " SpaceWire brick(s) found"
11 11
12 12 SpwPlugin0.StarDundeeSelectBrick(1)
13 13 SpwPlugin0.StarDundeeSetBrickAsARouter(1)
14 SpwPlugin0.StarDundeeSelectLinkNumber( 2 )
14 15 SpwPlugin0.connectBridge()
15 16
16 17 #SpwPlugin0.TCPServerSetIP("127.0.0.1")
17 18 SpwPlugin0.TCPServerConnect()
18 19
19 20 # OPEN SPACEWIRE SERVER
20 21 #LFRControlPlugin0.SetSpwServerIP(129,104,27,164)
21 22 LFRControlPlugin0.TCPServerConnect()
22 23
23 24 # OPEN TM ECHO BRIDGE SERVER
24 25 LFRControlPlugin0.TMEchoBridgeOpenPort()
25 26
26 27 # START SENDING TIMECODES AT 1 Hz
27 28 SpwPlugin0.StarDundeeStartTimecodes( 1 )
28 29
29 30 # it is possible to change the time code frequency
30 31 #RMAPPlugin0.changeTimecodeFrequency(2)
Show file before | Show file after | Hide comments
@@ -1,814 +1,812
1 1 /** This is the RTEMS initialization module.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * This module contains two very different information:
7 7 * - specific instructions to configure the compilation of the RTEMS executive
8 8 * - functions related to the fligth softwre initialization, especially the INIT RTEMS task
9 9 *
10 10 */
11 11
12 12 //*************************
13 13 // GPL reminder to be added
14 14 //*************************
15 15
16 16 #include <rtems.h>
17 17
18 18 /* configuration information */
19 19
20 20 #define CONFIGURE_INIT
21 21
22 22 #include <bsp.h> /* for device driver prototypes */
23 23
24 24 /* configuration information */
25 25
26 26 #define CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
27 27 #define CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
28 28
29 29 #define CONFIGURE_MAXIMUM_TASKS 20
30 30 #define CONFIGURE_RTEMS_INIT_TASKS_TABLE
31 31 #define CONFIGURE_EXTRA_TASK_STACKS (3 * RTEMS_MINIMUM_STACK_SIZE)
32 32 #define CONFIGURE_LIBIO_MAXIMUM_FILE_DESCRIPTORS 32
33 33 #define CONFIGURE_INIT_TASK_PRIORITY 1 // instead of 100
34 34 #define CONFIGURE_INIT_TASK_MODE (RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT)
35 35 #define CONFIGURE_INIT_TASK_ATTRIBUTES (RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT)
36 36 #define CONFIGURE_MAXIMUM_DRIVERS 16
37 37 #define CONFIGURE_MAXIMUM_PERIODS 5
38 38 #define CONFIGURE_MAXIMUM_TIMERS 5 // STAT (1s), send SWF (0.3s), send CWF3 (1s)
39 39 #define CONFIGURE_MAXIMUM_MESSAGE_QUEUES 5
40 40 #ifdef PRINT_STACK_REPORT
41 41 #define CONFIGURE_STACK_CHECKER_ENABLED
42 42 #endif
43 43
44 44 #include <rtems/confdefs.h>
45 45
46 46 /* If --drvmgr was enabled during the configuration of the RTEMS kernel */
47 47 #ifdef RTEMS_DRVMGR_STARTUP
48 48 #ifdef LEON3
49 49 /* Add Timer and UART Driver */
50 50 #ifdef CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
51 51 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GPTIMER
52 52 #endif
53 53 #ifdef CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
54 54 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_APBUART
55 55 #endif
56 56 #endif
57 57 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GRSPW /* GRSPW Driver */
58 58 #include <drvmgr/drvmgr_confdefs.h>
59 59 #endif
60 60
61 61 #include "fsw_init.h"
62 62 #include "fsw_config.c"
63 63
64 64 void initCache()
65 65 {
66 66 // unsigned int cacheControlRegister;
67 67
68 68 // cacheControlRegister = getCacheControlRegister();
69 69 // printf("(0) cacheControlRegister = %x\n", cacheControlRegister);
70 70
71 71 enableInstructionCache();
72 72 enableDataCache();
73 73 enableInstructionBurstFetch();
74 74
75 75 // cacheControlRegister = getCacheControlRegister();
76 76 // printf("(1) cacheControlRegister = %x\n", cacheControlRegister);
77 77 }
78 78
79 79 rtems_task Init( rtems_task_argument ignored )
80 80 {
81 81 /** This is the RTEMS INIT taks, it the first task launched by the system.
82 82 *
83 83 * @param unused is the starting argument of the RTEMS task
84 84 *
85 85 * The INIT task create and run all other RTEMS tasks.
86 86 *
87 87 */
88 88
89 89 //***********
90 90 // INIT CACHE
91 91
92 92 unsigned char *vhdlVersion;
93 93
94 94 reset_lfr();
95 95
96 96 reset_local_time();
97 97
98 98 rtems_cpu_usage_reset();
99 99
100 100 rtems_status_code status;
101 101 rtems_status_code status_spw;
102 102 rtems_isr_entry old_isr_handler;
103 103
104 104 // UART settings
105 105 send_console_outputs_on_apbuart_port();
106 106 set_apbuart_scaler_reload_register(REGS_ADDR_APBUART, APBUART_SCALER_RELOAD_VALUE);
107 107 enable_apbuart_transmitter();
108 108
109 109 DEBUG_PRINTF("\n\n\n\n\nIn INIT *** Now the console is on port COM1\n")
110 110
111 111
112 112 PRINTF("\n\n\n\n\n")
113 113
114 114 initCache();
115 115
116 116 PRINTF("*************************\n")
117 117 PRINTF("** LFR Flight Software **\n")
118 118 PRINTF1("** %d.", SW_VERSION_N1)
119 119 PRINTF1("%d." , SW_VERSION_N2)
120 120 PRINTF1("%d." , SW_VERSION_N3)
121 121 PRINTF1("%d **\n", SW_VERSION_N4)
122 122
123 123 vhdlVersion = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
124 124 PRINTF("** VHDL **\n")
125 125 PRINTF1("** %d.", vhdlVersion[1])
126 126 PRINTF1("%d." , vhdlVersion[2])
127 127 PRINTF1("%d **\n", vhdlVersion[3])
128 128 PRINTF("*************************\n")
129 129 PRINTF("\n\n")
130 130
131 131 init_parameter_dump();
132 132 init_kcoefficients_dump();
133 133 init_local_mode_parameters();
134 134 init_housekeeping_parameters();
135 135 init_k_coefficients_f0();
136 136 init_k_coefficients_f1();
137 137 init_k_coefficients_f2();
138 138
139 139 // waveform picker initialization
140 140 WFP_init_rings(); // initialize the waveform rings
141 141 WFP_reset_current_ring_nodes();
142 142 reset_waveform_picker_regs();
143 143
144 144 // spectral matrices initialization
145 145 SM_init_rings(); // initialize spectral matrices rings
146 146 SM_reset_current_ring_nodes();
147 147 reset_spectral_matrix_regs();
148 148
149 149 // configure calibration
150 150 configureCalibration( false ); // true means interleaved mode, false is for normal mode
151 151
152 152 updateLFRCurrentMode();
153 153
154 154 BOOT_PRINTF1("in INIT *** lfrCurrentMode is %d\n", lfrCurrentMode)
155 155
156 156 create_names(); // create all names
157 157
158 158 status = create_message_queues(); // create message queues
159 159 if (status != RTEMS_SUCCESSFUL)
160 160 {
161 161 PRINTF1("in INIT *** ERR in create_message_queues, code %d", status)
162 162 }
163 163
164 164 status = create_all_tasks(); // create all tasks
165 165 if (status != RTEMS_SUCCESSFUL)
166 166 {
167 167 PRINTF1("in INIT *** ERR in create_all_tasks, code %d\n", status)
168 168 }
169 169
170 170 // **************************
171 171 // <SPACEWIRE INITIALIZATION>
172 172 grspw_timecode_callback = &timecode_irq_handler;
173 173
174 174 status_spw = spacewire_open_link(); // (1) open the link
175 175 if ( status_spw != RTEMS_SUCCESSFUL )
176 176 {
177 177 PRINTF1("in INIT *** ERR spacewire_open_link code %d\n", status_spw )
178 178 }
179 179
180 180 if ( status_spw == RTEMS_SUCCESSFUL ) // (2) configure the link
181 181 {
182 182 status_spw = spacewire_configure_link( fdSPW );
183 183 if ( status_spw != RTEMS_SUCCESSFUL )
184 184 {
185 185 PRINTF1("in INIT *** ERR spacewire_configure_link code %d\n", status_spw )
186 186 }
187 187 }
188 188
189 189 if ( status_spw == RTEMS_SUCCESSFUL) // (3) start the link
190 190 {
191 191 status_spw = spacewire_start_link( fdSPW );
192 192 if ( status_spw != RTEMS_SUCCESSFUL )
193 193 {
194 194 PRINTF1("in INIT *** ERR spacewire_start_link code %d\n", status_spw )
195 195 }
196 196 }
197 197 // </SPACEWIRE INITIALIZATION>
198 198 // ***************************
199 199
200 200 status = start_all_tasks(); // start all tasks
201 201 if (status != RTEMS_SUCCESSFUL)
202 202 {
203 203 PRINTF1("in INIT *** ERR in start_all_tasks, code %d", status)
204 204 }
205 205
206 206 // start RECV and SEND *AFTER* SpaceWire Initialization, due to the timeout of the start call during the initialization
207 207 status = start_recv_send_tasks();
208 208 if ( status != RTEMS_SUCCESSFUL )
209 209 {
210 210 PRINTF1("in INIT *** ERR start_recv_send_tasks code %d\n", status )
211 211 }
212 212
213 213 // suspend science tasks, they will be restarted later depending on the mode
214 214 status = suspend_science_tasks(); // suspend science tasks (not done in stop_current_mode if current mode = STANDBY)
215 215 if (status != RTEMS_SUCCESSFUL)
216 216 {
217 217 PRINTF1("in INIT *** in suspend_science_tasks *** ERR code: %d\n", status)
218 218 }
219 219
220 220 //******************************
221 221 // <SPECTRAL MATRICES SIMULATOR>
222 222 LEON_Mask_interrupt( IRQ_SM_SIMULATOR );
223 223 configure_timer((gptimer_regs_t*) REGS_ADDR_GPTIMER, TIMER_SM_SIMULATOR, CLKDIV_SM_SIMULATOR,
224 224 IRQ_SPARC_SM_SIMULATOR, spectral_matrices_isr_simu );
225 225 // </SPECTRAL MATRICES SIMULATOR>
226 226 //*******************************
227 227
228 228 // configure IRQ handling for the waveform picker unit
229 229 status = rtems_interrupt_catch( waveforms_isr,
230 230 IRQ_SPARC_WAVEFORM_PICKER,
231 231 &old_isr_handler) ;
232 232 // configure IRQ handling for the spectral matrices unit
233 233 status = rtems_interrupt_catch( spectral_matrices_isr,
234 234 IRQ_SPARC_SPECTRAL_MATRIX,
235 235 &old_isr_handler) ;
236 236
237 237 // if the spacewire link is not up then send an event to the SPIQ task for link recovery
238 238 if ( status_spw != RTEMS_SUCCESSFUL )
239 239 {
240 240 status = rtems_event_send( Task_id[TASKID_SPIQ], SPW_LINKERR_EVENT );
241 241 if ( status != RTEMS_SUCCESSFUL ) {
242 242 PRINTF1("in INIT *** ERR rtems_event_send to SPIQ code %d\n", status )
243 243 }
244 244 }
245 245
246 246 BOOT_PRINTF("delete INIT\n")
247 247
248 // test_TCH();
249
250 248 status = rtems_task_delete(RTEMS_SELF);
251 249
252 250 }
253 251
254 252 void init_local_mode_parameters( void )
255 253 {
256 254 /** This function initialize the param_local global variable with default values.
257 255 *
258 256 */
259 257
260 258 unsigned int i;
261 259
262 260 // LOCAL PARAMETERS
263 261
264 262 BOOT_PRINTF1("local_sbm1_nb_cwf_max %d \n", param_local.local_sbm1_nb_cwf_max)
265 263 BOOT_PRINTF1("local_sbm2_nb_cwf_max %d \n", param_local.local_sbm2_nb_cwf_max)
266 264 BOOT_PRINTF1("nb_interrupt_f0_MAX = %d\n", param_local.local_nb_interrupt_f0_MAX)
267 265
268 266 // init sequence counters
269 267
270 268 for(i = 0; i<SEQ_CNT_NB_DEST_ID; i++)
271 269 {
272 270 sequenceCounters_TC_EXE[i] = 0x00;
273 271 }
274 272 sequenceCounters_SCIENCE_NORMAL_BURST = 0x00;
275 273 sequenceCounters_SCIENCE_SBM1_SBM2 = 0x00;
276 274 sequenceCounterHK = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
277 275 sequenceCounterParameterDump = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
278 276 }
279 277
280 278 void reset_local_time( void )
281 279 {
282 280 time_management_regs->ctrl = time_management_regs->ctrl | 0x02; // [0010] software reset, coarse time = 0x80000000
283 281 }
284 282
285 283 void create_names( void ) // create all names for tasks and queues
286 284 {
287 285 /** This function creates all RTEMS names used in the software for tasks and queues.
288 286 *
289 287 * @return RTEMS directive status codes:
290 288 * - RTEMS_SUCCESSFUL - successful completion
291 289 *
292 290 */
293 291
294 292 // task names
295 293 Task_name[TASKID_RECV] = rtems_build_name( 'R', 'E', 'C', 'V' );
296 294 Task_name[TASKID_ACTN] = rtems_build_name( 'A', 'C', 'T', 'N' );
297 295 Task_name[TASKID_SPIQ] = rtems_build_name( 'S', 'P', 'I', 'Q' );
298 296 Task_name[TASKID_STAT] = rtems_build_name( 'S', 'T', 'A', 'T' );
299 297 Task_name[TASKID_AVF0] = rtems_build_name( 'A', 'V', 'F', '0' );
300 298 Task_name[TASKID_SWBD] = rtems_build_name( 'S', 'W', 'B', 'D' );
301 299 Task_name[TASKID_WFRM] = rtems_build_name( 'W', 'F', 'R', 'M' );
302 300 Task_name[TASKID_DUMB] = rtems_build_name( 'D', 'U', 'M', 'B' );
303 301 Task_name[TASKID_HOUS] = rtems_build_name( 'H', 'O', 'U', 'S' );
304 302 Task_name[TASKID_PRC0] = rtems_build_name( 'P', 'R', 'C', '0' );
305 303 Task_name[TASKID_CWF3] = rtems_build_name( 'C', 'W', 'F', '3' );
306 304 Task_name[TASKID_CWF2] = rtems_build_name( 'C', 'W', 'F', '2' );
307 305 Task_name[TASKID_CWF1] = rtems_build_name( 'C', 'W', 'F', '1' );
308 306 Task_name[TASKID_SEND] = rtems_build_name( 'S', 'E', 'N', 'D' );
309 307 Task_name[TASKID_WTDG] = rtems_build_name( 'W', 'T', 'D', 'G' );
310 308 Task_name[TASKID_AVF1] = rtems_build_name( 'A', 'V', 'F', '1' );
311 309 Task_name[TASKID_PRC1] = rtems_build_name( 'P', 'R', 'C', '1' );
312 310 Task_name[TASKID_AVF2] = rtems_build_name( 'A', 'V', 'F', '2' );
313 311 Task_name[TASKID_PRC2] = rtems_build_name( 'P', 'R', 'C', '2' );
314 312
315 313 // rate monotonic period names
316 314 name_hk_rate_monotonic = rtems_build_name( 'H', 'O', 'U', 'S' );
317 315
318 316 misc_name[QUEUE_RECV] = rtems_build_name( 'Q', '_', 'R', 'V' );
319 317 misc_name[QUEUE_SEND] = rtems_build_name( 'Q', '_', 'S', 'D' );
320 318 misc_name[QUEUE_PRC0] = rtems_build_name( 'Q', '_', 'P', '0' );
321 319 misc_name[QUEUE_PRC1] = rtems_build_name( 'Q', '_', 'P', '1' );
322 320 misc_name[QUEUE_PRC2] = rtems_build_name( 'Q', '_', 'P', '2' );
323 321 }
324 322
325 323 int create_all_tasks( void ) // create all tasks which run in the software
326 324 {
327 325 /** This function creates all RTEMS tasks used in the software.
328 326 *
329 327 * @return RTEMS directive status codes:
330 328 * - RTEMS_SUCCESSFUL - task created successfully
331 329 * - RTEMS_INVALID_ADDRESS - id is NULL
332 330 * - RTEMS_INVALID_NAME - invalid task name
333 331 * - RTEMS_INVALID_PRIORITY - invalid task priority
334 332 * - RTEMS_MP_NOT_CONFIGURED - multiprocessing not configured
335 333 * - RTEMS_TOO_MANY - too many tasks created
336 334 * - RTEMS_UNSATISFIED - not enough memory for stack/FP context
337 335 * - RTEMS_TOO_MANY - too many global objects
338 336 *
339 337 */
340 338
341 339 rtems_status_code status;
342 340
343 341 //**********
344 342 // SPACEWIRE
345 343 // RECV
346 344 status = rtems_task_create(
347 345 Task_name[TASKID_RECV], TASK_PRIORITY_RECV, RTEMS_MINIMUM_STACK_SIZE,
348 346 RTEMS_DEFAULT_MODES,
349 347 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_RECV]
350 348 );
351 349 if (status == RTEMS_SUCCESSFUL) // SEND
352 350 {
353 351 status = rtems_task_create(
354 352 Task_name[TASKID_SEND], TASK_PRIORITY_SEND, RTEMS_MINIMUM_STACK_SIZE * 2,
355 353 RTEMS_DEFAULT_MODES,
356 354 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SEND]
357 355 );
358 356 }
359 357 if (status == RTEMS_SUCCESSFUL) // WTDG
360 358 {
361 359 status = rtems_task_create(
362 360 Task_name[TASKID_WTDG], TASK_PRIORITY_WTDG, RTEMS_MINIMUM_STACK_SIZE,
363 361 RTEMS_DEFAULT_MODES,
364 362 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_WTDG]
365 363 );
366 364 }
367 365 if (status == RTEMS_SUCCESSFUL) // ACTN
368 366 {
369 367 status = rtems_task_create(
370 368 Task_name[TASKID_ACTN], TASK_PRIORITY_ACTN, RTEMS_MINIMUM_STACK_SIZE,
371 369 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
372 370 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_ACTN]
373 371 );
374 372 }
375 373 if (status == RTEMS_SUCCESSFUL) // SPIQ
376 374 {
377 375 status = rtems_task_create(
378 376 Task_name[TASKID_SPIQ], TASK_PRIORITY_SPIQ, RTEMS_MINIMUM_STACK_SIZE,
379 377 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
380 378 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_SPIQ]
381 379 );
382 380 }
383 381
384 382 //******************
385 383 // SPECTRAL MATRICES
386 384 if (status == RTEMS_SUCCESSFUL) // AVF0
387 385 {
388 386 status = rtems_task_create(
389 387 Task_name[TASKID_AVF0], TASK_PRIORITY_AVF0, RTEMS_MINIMUM_STACK_SIZE,
390 388 RTEMS_DEFAULT_MODES,
391 389 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF0]
392 390 );
393 391 }
394 392 if (status == RTEMS_SUCCESSFUL) // PRC0
395 393 {
396 394 status = rtems_task_create(
397 395 Task_name[TASKID_PRC0], TASK_PRIORITY_PRC0, RTEMS_MINIMUM_STACK_SIZE * 2,
398 396 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
399 397 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC0]
400 398 );
401 399 }
402 400 if (status == RTEMS_SUCCESSFUL) // AVF1
403 401 {
404 402 status = rtems_task_create(
405 403 Task_name[TASKID_AVF1], TASK_PRIORITY_AVF1, RTEMS_MINIMUM_STACK_SIZE,
406 404 RTEMS_DEFAULT_MODES,
407 405 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF1]
408 406 );
409 407 }
410 408 if (status == RTEMS_SUCCESSFUL) // PRC1
411 409 {
412 410 status = rtems_task_create(
413 411 Task_name[TASKID_PRC1], TASK_PRIORITY_PRC1, RTEMS_MINIMUM_STACK_SIZE * 2,
414 412 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
415 413 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC1]
416 414 );
417 415 }
418 416 if (status == RTEMS_SUCCESSFUL) // AVF2
419 417 {
420 418 status = rtems_task_create(
421 419 Task_name[TASKID_AVF2], TASK_PRIORITY_AVF2, RTEMS_MINIMUM_STACK_SIZE,
422 420 RTEMS_DEFAULT_MODES,
423 421 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF2]
424 422 );
425 423 }
426 424 if (status == RTEMS_SUCCESSFUL) // PRC2
427 425 {
428 426 status = rtems_task_create(
429 427 Task_name[TASKID_PRC2], TASK_PRIORITY_PRC2, RTEMS_MINIMUM_STACK_SIZE * 2,
430 428 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
431 429 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC2]
432 430 );
433 431 }
434 432
435 433 //****************
436 434 // WAVEFORM PICKER
437 435 if (status == RTEMS_SUCCESSFUL) // WFRM
438 436 {
439 437 status = rtems_task_create(
440 438 Task_name[TASKID_WFRM], TASK_PRIORITY_WFRM, RTEMS_MINIMUM_STACK_SIZE,
441 439 RTEMS_DEFAULT_MODES,
442 440 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_WFRM]
443 441 );
444 442 }
445 443 if (status == RTEMS_SUCCESSFUL) // CWF3
446 444 {
447 445 status = rtems_task_create(
448 446 Task_name[TASKID_CWF3], TASK_PRIORITY_CWF3, RTEMS_MINIMUM_STACK_SIZE,
449 447 RTEMS_DEFAULT_MODES,
450 448 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF3]
451 449 );
452 450 }
453 451 if (status == RTEMS_SUCCESSFUL) // CWF2
454 452 {
455 453 status = rtems_task_create(
456 454 Task_name[TASKID_CWF2], TASK_PRIORITY_CWF2, RTEMS_MINIMUM_STACK_SIZE,
457 455 RTEMS_DEFAULT_MODES,
458 456 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF2]
459 457 );
460 458 }
461 459 if (status == RTEMS_SUCCESSFUL) // CWF1
462 460 {
463 461 status = rtems_task_create(
464 462 Task_name[TASKID_CWF1], TASK_PRIORITY_CWF1, RTEMS_MINIMUM_STACK_SIZE,
465 463 RTEMS_DEFAULT_MODES,
466 464 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF1]
467 465 );
468 466 }
469 467 if (status == RTEMS_SUCCESSFUL) // SWBD
470 468 {
471 469 status = rtems_task_create(
472 470 Task_name[TASKID_SWBD], TASK_PRIORITY_SWBD, RTEMS_MINIMUM_STACK_SIZE,
473 471 RTEMS_DEFAULT_MODES,
474 472 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SWBD]
475 473 );
476 474 }
477 475
478 476 //*****
479 477 // MISC
480 478 if (status == RTEMS_SUCCESSFUL) // STAT
481 479 {
482 480 status = rtems_task_create(
483 481 Task_name[TASKID_STAT], TASK_PRIORITY_STAT, RTEMS_MINIMUM_STACK_SIZE,
484 482 RTEMS_DEFAULT_MODES,
485 483 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_STAT]
486 484 );
487 485 }
488 486 if (status == RTEMS_SUCCESSFUL) // DUMB
489 487 {
490 488 status = rtems_task_create(
491 489 Task_name[TASKID_DUMB], TASK_PRIORITY_DUMB, RTEMS_MINIMUM_STACK_SIZE,
492 490 RTEMS_DEFAULT_MODES,
493 491 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_DUMB]
494 492 );
495 493 }
496 494 if (status == RTEMS_SUCCESSFUL) // HOUS
497 495 {
498 496 status = rtems_task_create(
499 497 Task_name[TASKID_HOUS], TASK_PRIORITY_HOUS, RTEMS_MINIMUM_STACK_SIZE,
500 498 RTEMS_DEFAULT_MODES,
501 499 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_HOUS]
502 500 );
503 501 }
504 502
505 503 return status;
506 504 }
507 505
508 506 int start_recv_send_tasks( void )
509 507 {
510 508 rtems_status_code status;
511 509
512 510 status = rtems_task_start( Task_id[TASKID_RECV], recv_task, 1 );
513 511 if (status!=RTEMS_SUCCESSFUL) {
514 512 BOOT_PRINTF("in INIT *** Error starting TASK_RECV\n")
515 513 }
516 514
517 515 if (status == RTEMS_SUCCESSFUL) // SEND
518 516 {
519 517 status = rtems_task_start( Task_id[TASKID_SEND], send_task, 1 );
520 518 if (status!=RTEMS_SUCCESSFUL) {
521 519 BOOT_PRINTF("in INIT *** Error starting TASK_SEND\n")
522 520 }
523 521 }
524 522
525 523 return status;
526 524 }
527 525
528 526 int start_all_tasks( void ) // start all tasks except SEND RECV and HOUS
529 527 {
530 528 /** This function starts all RTEMS tasks used in the software.
531 529 *
532 530 * @return RTEMS directive status codes:
533 531 * - RTEMS_SUCCESSFUL - ask started successfully
534 532 * - RTEMS_INVALID_ADDRESS - invalid task entry point
535 533 * - RTEMS_INVALID_ID - invalid task id
536 534 * - RTEMS_INCORRECT_STATE - task not in the dormant state
537 535 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot start remote task
538 536 *
539 537 */
540 538 // starts all the tasks fot eh flight software
541 539
542 540 rtems_status_code status;
543 541
544 542 //**********
545 543 // SPACEWIRE
546 544 status = rtems_task_start( Task_id[TASKID_SPIQ], spiq_task, 1 );
547 545 if (status!=RTEMS_SUCCESSFUL) {
548 546 BOOT_PRINTF("in INIT *** Error starting TASK_SPIQ\n")
549 547 }
550 548
551 549 if (status == RTEMS_SUCCESSFUL) // WTDG
552 550 {
553 551 status = rtems_task_start( Task_id[TASKID_WTDG], wtdg_task, 1 );
554 552 if (status!=RTEMS_SUCCESSFUL) {
555 553 BOOT_PRINTF("in INIT *** Error starting TASK_WTDG\n")
556 554 }
557 555 }
558 556
559 557 if (status == RTEMS_SUCCESSFUL) // ACTN
560 558 {
561 559 status = rtems_task_start( Task_id[TASKID_ACTN], actn_task, 1 );
562 560 if (status!=RTEMS_SUCCESSFUL) {
563 561 BOOT_PRINTF("in INIT *** Error starting TASK_ACTN\n")
564 562 }
565 563 }
566 564
567 565 //******************
568 566 // SPECTRAL MATRICES
569 567 if (status == RTEMS_SUCCESSFUL) // AVF0
570 568 {
571 569 status = rtems_task_start( Task_id[TASKID_AVF0], avf0_task, LFR_MODE_STANDBY );
572 570 if (status!=RTEMS_SUCCESSFUL) {
573 571 BOOT_PRINTF("in INIT *** Error starting TASK_AVF0\n")
574 572 }
575 573 }
576 574 if (status == RTEMS_SUCCESSFUL) // PRC0
577 575 {
578 576 status = rtems_task_start( Task_id[TASKID_PRC0], prc0_task, LFR_MODE_STANDBY );
579 577 if (status!=RTEMS_SUCCESSFUL) {
580 578 BOOT_PRINTF("in INIT *** Error starting TASK_PRC0\n")
581 579 }
582 580 }
583 581 if (status == RTEMS_SUCCESSFUL) // AVF1
584 582 {
585 583 status = rtems_task_start( Task_id[TASKID_AVF1], avf1_task, LFR_MODE_STANDBY );
586 584 if (status!=RTEMS_SUCCESSFUL) {
587 585 BOOT_PRINTF("in INIT *** Error starting TASK_AVF1\n")
588 586 }
589 587 }
590 588 if (status == RTEMS_SUCCESSFUL) // PRC1
591 589 {
592 590 status = rtems_task_start( Task_id[TASKID_PRC1], prc1_task, LFR_MODE_STANDBY );
593 591 if (status!=RTEMS_SUCCESSFUL) {
594 592 BOOT_PRINTF("in INIT *** Error starting TASK_PRC1\n")
595 593 }
596 594 }
597 595 if (status == RTEMS_SUCCESSFUL) // AVF2
598 596 {
599 597 status = rtems_task_start( Task_id[TASKID_AVF2], avf2_task, 1 );
600 598 if (status!=RTEMS_SUCCESSFUL) {
601 599 BOOT_PRINTF("in INIT *** Error starting TASK_AVF2\n")
602 600 }
603 601 }
604 602 if (status == RTEMS_SUCCESSFUL) // PRC2
605 603 {
606 604 status = rtems_task_start( Task_id[TASKID_PRC2], prc2_task, 1 );
607 605 if (status!=RTEMS_SUCCESSFUL) {
608 606 BOOT_PRINTF("in INIT *** Error starting TASK_PRC2\n")
609 607 }
610 608 }
611 609
612 610 //****************
613 611 // WAVEFORM PICKER
614 612 if (status == RTEMS_SUCCESSFUL) // WFRM
615 613 {
616 614 status = rtems_task_start( Task_id[TASKID_WFRM], wfrm_task, 1 );
617 615 if (status!=RTEMS_SUCCESSFUL) {
618 616 BOOT_PRINTF("in INIT *** Error starting TASK_WFRM\n")
619 617 }
620 618 }
621 619 if (status == RTEMS_SUCCESSFUL) // CWF3
622 620 {
623 621 status = rtems_task_start( Task_id[TASKID_CWF3], cwf3_task, 1 );
624 622 if (status!=RTEMS_SUCCESSFUL) {
625 623 BOOT_PRINTF("in INIT *** Error starting TASK_CWF3\n")
626 624 }
627 625 }
628 626 if (status == RTEMS_SUCCESSFUL) // CWF2
629 627 {
630 628 status = rtems_task_start( Task_id[TASKID_CWF2], cwf2_task, 1 );
631 629 if (status!=RTEMS_SUCCESSFUL) {
632 630 BOOT_PRINTF("in INIT *** Error starting TASK_CWF2\n")
633 631 }
634 632 }
635 633 if (status == RTEMS_SUCCESSFUL) // CWF1
636 634 {
637 635 status = rtems_task_start( Task_id[TASKID_CWF1], cwf1_task, 1 );
638 636 if (status!=RTEMS_SUCCESSFUL) {
639 637 BOOT_PRINTF("in INIT *** Error starting TASK_CWF1\n")
640 638 }
641 639 }
642 640 if (status == RTEMS_SUCCESSFUL) // SWBD
643 641 {
644 642 status = rtems_task_start( Task_id[TASKID_SWBD], swbd_task, 1 );
645 643 if (status!=RTEMS_SUCCESSFUL) {
646 644 BOOT_PRINTF("in INIT *** Error starting TASK_SWBD\n")
647 645 }
648 646 }
649 647
650 648 //*****
651 649 // MISC
652 650 if (status == RTEMS_SUCCESSFUL) // HOUS
653 651 {
654 652 status = rtems_task_start( Task_id[TASKID_HOUS], hous_task, 1 );
655 653 if (status!=RTEMS_SUCCESSFUL) {
656 654 BOOT_PRINTF("in INIT *** Error starting TASK_HOUS\n")
657 655 }
658 656 }
659 657 if (status == RTEMS_SUCCESSFUL) // DUMB
660 658 {
661 659 status = rtems_task_start( Task_id[TASKID_DUMB], dumb_task, 1 );
662 660 if (status!=RTEMS_SUCCESSFUL) {
663 661 BOOT_PRINTF("in INIT *** Error starting TASK_DUMB\n")
664 662 }
665 663 }
666 664 if (status == RTEMS_SUCCESSFUL) // STAT
667 665 {
668 666 status = rtems_task_start( Task_id[TASKID_STAT], stat_task, 1 );
669 667 if (status!=RTEMS_SUCCESSFUL) {
670 668 BOOT_PRINTF("in INIT *** Error starting TASK_STAT\n")
671 669 }
672 670 }
673 671
674 672 return status;
675 673 }
676 674
677 675 rtems_status_code create_message_queues( void ) // create the two message queues used in the software
678 676 {
679 677 rtems_status_code status_recv;
680 678 rtems_status_code status_send;
681 679 rtems_status_code status_q_p0;
682 680 rtems_status_code status_q_p1;
683 681 rtems_status_code status_q_p2;
684 682 rtems_status_code ret;
685 683 rtems_id queue_id;
686 684
687 685 //****************************************
688 686 // create the queue for handling valid TCs
689 687 status_recv = rtems_message_queue_create( misc_name[QUEUE_RECV],
690 688 MSG_QUEUE_COUNT_RECV, CCSDS_TC_PKT_MAX_SIZE,
691 689 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
692 690 if ( status_recv != RTEMS_SUCCESSFUL ) {
693 691 PRINTF1("in create_message_queues *** ERR creating QUEU queue, %d\n", status_recv)
694 692 }
695 693
696 694 //************************************************
697 695 // create the queue for handling TM packet sending
698 696 status_send = rtems_message_queue_create( misc_name[QUEUE_SEND],
699 697 MSG_QUEUE_COUNT_SEND, MSG_QUEUE_SIZE_SEND,
700 698 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
701 699 if ( status_send != RTEMS_SUCCESSFUL ) {
702 700 PRINTF1("in create_message_queues *** ERR creating PKTS queue, %d\n", status_send)
703 701 }
704 702
705 703 //*****************************************************************************
706 704 // create the queue for handling averaged spectral matrices for processing @ f0
707 705 status_q_p0 = rtems_message_queue_create( misc_name[QUEUE_PRC0],
708 706 MSG_QUEUE_COUNT_PRC0, MSG_QUEUE_SIZE_PRC0,
709 707 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
710 708 if ( status_q_p0 != RTEMS_SUCCESSFUL ) {
711 709 PRINTF1("in create_message_queues *** ERR creating Q_P0 queue, %d\n", status_q_p0)
712 710 }
713 711
714 712 //*****************************************************************************
715 713 // create the queue for handling averaged spectral matrices for processing @ f1
716 714 status_q_p1 = rtems_message_queue_create( misc_name[QUEUE_PRC1],
717 715 MSG_QUEUE_COUNT_PRC1, MSG_QUEUE_SIZE_PRC1,
718 716 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
719 717 if ( status_q_p1 != RTEMS_SUCCESSFUL ) {
720 718 PRINTF1("in create_message_queues *** ERR creating Q_P1 queue, %d\n", status_q_p1)
721 719 }
722 720
723 721 //*****************************************************************************
724 722 // create the queue for handling averaged spectral matrices for processing @ f2
725 723 status_q_p2 = rtems_message_queue_create( misc_name[QUEUE_PRC2],
726 724 MSG_QUEUE_COUNT_PRC2, MSG_QUEUE_SIZE_PRC2,
727 725 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
728 726 if ( status_q_p2 != RTEMS_SUCCESSFUL ) {
729 727 PRINTF1("in create_message_queues *** ERR creating Q_P2 queue, %d\n", status_q_p2)
730 728 }
731 729
732 730 if ( status_recv != RTEMS_SUCCESSFUL )
733 731 {
734 732 ret = status_recv;
735 733 }
736 734 else if( status_send != RTEMS_SUCCESSFUL )
737 735 {
738 736 ret = status_send;
739 737 }
740 738 else if( status_q_p0 != RTEMS_SUCCESSFUL )
741 739 {
742 740 ret = status_q_p0;
743 741 }
744 742 else if( status_q_p1 != RTEMS_SUCCESSFUL )
745 743 {
746 744 ret = status_q_p1;
747 745 }
748 746 else
749 747 {
750 748 ret = status_q_p2;
751 749 }
752 750
753 751 return ret;
754 752 }
755 753
756 754 rtems_status_code get_message_queue_id_send( rtems_id *queue_id )
757 755 {
758 756 rtems_status_code status;
759 757 rtems_name queue_name;
760 758
761 759 queue_name = rtems_build_name( 'Q', '_', 'S', 'D' );
762 760
763 761 status = rtems_message_queue_ident( queue_name, 0, queue_id );
764 762
765 763 return status;
766 764 }
767 765
768 766 rtems_status_code get_message_queue_id_recv( rtems_id *queue_id )
769 767 {
770 768 rtems_status_code status;
771 769 rtems_name queue_name;
772 770
773 771 queue_name = rtems_build_name( 'Q', '_', 'R', 'V' );
774 772
775 773 status = rtems_message_queue_ident( queue_name, 0, queue_id );
776 774
777 775 return status;
778 776 }
779 777
780 778 rtems_status_code get_message_queue_id_prc0( rtems_id *queue_id )
781 779 {
782 780 rtems_status_code status;
783 781 rtems_name queue_name;
784 782
785 783 queue_name = rtems_build_name( 'Q', '_', 'P', '0' );
786 784
787 785 status = rtems_message_queue_ident( queue_name, 0, queue_id );
788 786
789 787 return status;
790 788 }
791 789
792 790 rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id )
793 791 {
794 792 rtems_status_code status;
795 793 rtems_name queue_name;
796 794
797 795 queue_name = rtems_build_name( 'Q', '_', 'P', '1' );
798 796
799 797 status = rtems_message_queue_ident( queue_name, 0, queue_id );
800 798
801 799 return status;
802 800 }
803 801
804 802 rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id )
805 803 {
806 804 rtems_status_code status;
807 805 rtems_name queue_name;
808 806
809 807 queue_name = rtems_build_name( 'Q', '_', 'P', '2' );
810 808
811 809 status = rtems_message_queue_ident( queue_name, 0, queue_id );
812 810
813 811 return status;
814 812 }
Show file before | Show file after | Hide comments
@@ -1,511 +1,513
1 1 /** General usage functions and RTEMS tasks.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 */
7 7
8 8 #include "fsw_misc.h"
9 9
10 10 void configure_timer(gptimer_regs_t *gptimer_regs, unsigned char timer, unsigned int clock_divider,
11 11 unsigned char interrupt_level, rtems_isr (*timer_isr)() )
12 12 {
13 13 /** This function configures a GPTIMER timer instantiated in the VHDL design.
14 14 *
15 15 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
16 16 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
17 17 * @param clock_divider is the divider of the 1 MHz clock that will be configured.
18 18 * @param interrupt_level is the interrupt level that the timer drives.
19 19 * @param timer_isr is the interrupt subroutine that will be attached to the IRQ driven by the timer.
20 20 *
21 21 * Interrupt levels are described in the SPARC documentation sparcv8.pdf p.76
22 22 *
23 23 */
24 24
25 25 rtems_status_code status;
26 26 rtems_isr_entry old_isr_handler;
27 27
28 28 gptimer_regs->timer[timer].ctrl = 0x00; // reset the control register
29 29
30 30 status = rtems_interrupt_catch( timer_isr, interrupt_level, &old_isr_handler) ; // see sparcv8.pdf p.76 for interrupt levels
31 31 if (status!=RTEMS_SUCCESSFUL)
32 32 {
33 33 PRINTF("in configure_timer *** ERR rtems_interrupt_catch\n")
34 34 }
35 35
36 36 timer_set_clock_divider( gptimer_regs, timer, clock_divider);
37 37 }
38 38
39 39 void timer_start(gptimer_regs_t *gptimer_regs, unsigned char timer)
40 40 {
41 41 /** This function starts a GPTIMER timer.
42 42 *
43 43 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
44 44 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
45 45 *
46 46 */
47 47
48 48 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000010; // clear pending IRQ if any
49 49 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000004; // LD load value from the reload register
50 50 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000001; // EN enable the timer
51 51 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000002; // RS restart
52 52 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000008; // IE interrupt enable
53 53 }
54 54
55 55 void timer_stop(gptimer_regs_t *gptimer_regs, unsigned char timer)
56 56 {
57 57 /** This function stops a GPTIMER timer.
58 58 *
59 59 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
60 60 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
61 61 *
62 62 */
63 63
64 64 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl & 0xfffffffe; // EN enable the timer
65 65 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl & 0xffffffef; // IE interrupt enable
66 66 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000010; // clear pending IRQ if any
67 67 }
68 68
69 69 void timer_set_clock_divider(gptimer_regs_t *gptimer_regs, unsigned char timer, unsigned int clock_divider)
70 70 {
71 71 /** This function sets the clock divider of a GPTIMER timer.
72 72 *
73 73 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
74 74 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
75 75 * @param clock_divider is the divider of the 1 MHz clock that will be configured.
76 76 *
77 77 */
78 78
79 79 gptimer_regs->timer[timer].reload = clock_divider; // base clock frequency is 1 MHz
80 80 }
81 81
82 82 int send_console_outputs_on_apbuart_port( void ) // Send the console outputs on the apbuart port
83 83 {
84 84 struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) REGS_ADDR_APBUART;
85 85
86 86 apbuart_regs->ctrl = APBUART_CTRL_REG_MASK_TE;
87 87
88 88 return 0;
89 89 }
90 90
91 91 int enable_apbuart_transmitter( void ) // set the bit 1, TE Transmitter Enable to 1 in the APBUART control register
92 92 {
93 93 struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) REGS_ADDR_APBUART;
94 94
95 95 apbuart_regs->ctrl = apbuart_regs->ctrl | APBUART_CTRL_REG_MASK_TE;
96 96
97 97 return 0;
98 98 }
99 99
100 100 void set_apbuart_scaler_reload_register(unsigned int regs, unsigned int value)
101 101 {
102 102 /** This function sets the scaler reload register of the apbuart module
103 103 *
104 104 * @param regs is the address of the apbuart registers in memory
105 105 * @param value is the value that will be stored in the scaler register
106 106 *
107 107 * The value shall be set by the software to get data on the serial interface.
108 108 *
109 109 */
110 110
111 111 struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) regs;
112 112
113 113 apbuart_regs->scaler = value;
114 114 BOOT_PRINTF1("OK *** apbuart port scaler reload register set to 0x%x\n", value)
115 115 }
116 116
117 117 //************
118 118 // RTEMS TASKS
119 119
120 120 rtems_task stat_task(rtems_task_argument argument)
121 121 {
122 122 int i;
123 123 int j;
124 124 i = 0;
125 125 j = 0;
126 126 BOOT_PRINTF("in STAT *** \n")
127 127 while(1){
128 128 rtems_task_wake_after(1000);
129 129 PRINTF1("%d\n", j)
130 130 if (i == CPU_USAGE_REPORT_PERIOD) {
131 131 // #ifdef PRINT_TASK_STATISTICS
132 132 // rtems_cpu_usage_report();
133 133 // rtems_cpu_usage_reset();
134 134 // #endif
135 135 i = 0;
136 136 }
137 137 else i++;
138 138 j++;
139 139 }
140 140 }
141 141
142 142 rtems_task hous_task(rtems_task_argument argument)
143 143 {
144 144 rtems_status_code status;
145 145 rtems_status_code spare_status;
146 146 rtems_id queue_id;
147 147 rtems_rate_monotonic_period_status period_status;
148 148
149 149 status = get_message_queue_id_send( &queue_id );
150 150 if (status != RTEMS_SUCCESSFUL)
151 151 {
152 152 PRINTF1("in HOUS *** ERR get_message_queue_id_send %d\n", status)
153 153 }
154 154
155 155 BOOT_PRINTF("in HOUS ***\n")
156 156
157 157 if (rtems_rate_monotonic_ident( name_hk_rate_monotonic, &HK_id) != RTEMS_SUCCESSFUL) {
158 158 status = rtems_rate_monotonic_create( name_hk_rate_monotonic, &HK_id );
159 159 if( status != RTEMS_SUCCESSFUL ) {
160 160 PRINTF1( "rtems_rate_monotonic_create failed with status of %d\n", status )
161 161 }
162 162 }
163 163
164 164 housekeeping_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
165 165 housekeeping_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
166 166 housekeeping_packet.reserved = DEFAULT_RESERVED;
167 167 housekeeping_packet.userApplication = CCSDS_USER_APP;
168 168 housekeeping_packet.packetID[0] = (unsigned char) (APID_TM_HK >> 8);
169 169 housekeeping_packet.packetID[1] = (unsigned char) (APID_TM_HK);
170 170 housekeeping_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
171 171 housekeeping_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
172 172 housekeeping_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_HK >> 8);
173 173 housekeeping_packet.packetLength[1] = (unsigned char) (PACKET_LENGTH_HK );
174 174 housekeeping_packet.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
175 175 housekeeping_packet.serviceType = TM_TYPE_HK;
176 176 housekeeping_packet.serviceSubType = TM_SUBTYPE_HK;
177 177 housekeeping_packet.destinationID = TM_DESTINATION_ID_GROUND;
178 178 housekeeping_packet.sid = SID_HK;
179 179
180 180 status = rtems_rate_monotonic_cancel(HK_id);
181 181 if( status != RTEMS_SUCCESSFUL ) {
182 182 PRINTF1( "ERR *** in HOUS *** rtems_rate_monotonic_cancel(HK_id) ***code: %d\n", status )
183 183 }
184 184 else {
185 185 DEBUG_PRINTF("OK *** in HOUS *** rtems_rate_monotonic_cancel(HK_id)\n")
186 186 }
187 187
188 188 // startup phase
189 189 status = rtems_rate_monotonic_period( HK_id, SY_LFR_TIME_SYN_TIMEOUT_in_ticks );
190 190 status = rtems_rate_monotonic_get_status( HK_id, &period_status );
191 191 DEBUG_PRINTF1("startup HK, HK_id status = %d\n", period_status.state)
192 192 while(period_status.state != RATE_MONOTONIC_EXPIRED ) // after SY_LFR_TIME_SYN_TIMEOUT ms, starts HK anyway
193 193 {
194 194 if ((time_management_regs->coarse_time & 0x80000000) == 0x00000000) // check time synchronization
195 195 {
196 196 break; // break if LFR is synchronized
197 197 }
198 198 else
199 199 {
200 200 status = rtems_rate_monotonic_get_status( HK_id, &period_status );
201 201 // sched_yield();
202 202 status = rtems_task_wake_after( 10 ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 100 ms = 10 * 10 ms
203 203 }
204 204 }
205 205 status = rtems_rate_monotonic_cancel(HK_id);
206 206 DEBUG_PRINTF1("startup HK, HK_id status = %d\n", period_status.state)
207 207
208 208 while(1){ // launch the rate monotonic task
209 209 status = rtems_rate_monotonic_period( HK_id, HK_PERIOD );
210 210 if ( status != RTEMS_SUCCESSFUL ) {
211 211 PRINTF1( "in HOUS *** ERR period: %d\n", status);
212 212 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_6 );
213 213 }
214 214 else {
215 215 housekeeping_packet.packetSequenceControl[0] = (unsigned char) (sequenceCounterHK >> 8);
216 216 housekeeping_packet.packetSequenceControl[1] = (unsigned char) (sequenceCounterHK );
217 217 increment_seq_counter( &sequenceCounterHK );
218 218
219 219 housekeeping_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
220 220 housekeeping_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
221 221 housekeeping_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
222 222 housekeeping_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
223 223 housekeeping_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
224 224 housekeeping_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
225 225
226 226 spacewire_update_statistics();
227 227
228 housekeeping_packet.sy_lfr_common_parameters_spare = parameter_dump_packet.sy_lfr_common_parameters_spare;
229 housekeeping_packet.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
228 230 get_temperatures( housekeeping_packet.hk_lfr_temp_scm );
229 231 get_v_e1_e2_f3( housekeeping_packet.hk_lfr_sc_v_f3 );
230 232 get_cpu_load( (unsigned char *) &housekeeping_packet.hk_lfr_cpu_load );
231 233
232 234 // SEND PACKET
233 235 status = rtems_message_queue_send( queue_id, &housekeeping_packet,
234 236 PACKET_LENGTH_HK + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
235 237 if (status != RTEMS_SUCCESSFUL) {
236 238 PRINTF1("in HOUS *** ERR send: %d\n", status)
237 239 }
238 240 }
239 241 }
240 242
241 243 PRINTF("in HOUS *** deleting task\n")
242 244
243 245 status = rtems_task_delete( RTEMS_SELF ); // should not return
244 246 printf( "rtems_task_delete returned with status of %d.\n", status );
245 247 return;
246 248 }
247 249
248 250 rtems_task dumb_task( rtems_task_argument unused )
249 251 {
250 252 /** This RTEMS taks is used to print messages without affecting the general behaviour of the software.
251 253 *
252 254 * @param unused is the starting argument of the RTEMS task
253 255 *
254 256 * The DUMB taks waits for RTEMS events and print messages depending on the incoming events.
255 257 *
256 258 */
257 259
258 260 unsigned int i;
259 261 unsigned int intEventOut;
260 262 unsigned int coarse_time = 0;
261 263 unsigned int fine_time = 0;
262 264 rtems_event_set event_out;
263 265
264 266 char *DumbMessages[12] = {"in DUMB *** default", // RTEMS_EVENT_0
265 267 "in DUMB *** timecode_irq_handler", // RTEMS_EVENT_1
266 268 "in DUMB *** f3 buffer changed", // RTEMS_EVENT_2
267 269 "in DUMB *** in SMIQ *** Error sending event to AVF0", // RTEMS_EVENT_3
268 270 "in DUMB *** spectral_matrices_isr *** Error sending event to SMIQ", // RTEMS_EVENT_4
269 271 "in DUMB *** waveforms_simulator_isr", // RTEMS_EVENT_5
270 272 "VHDL SM *** two buffers f0 ready", // RTEMS_EVENT_6
271 273 "ready for dump", // RTEMS_EVENT_7
272 274 "VHDL ERR *** spectral matrix", // RTEMS_EVENT_8
273 275 "tick", // RTEMS_EVENT_9
274 276 "VHDL ERR *** waveform picker", // RTEMS_EVENT_10
275 277 "VHDL ERR *** unexpected ready matrix values" // RTEMS_EVENT_11
276 278 };
277 279
278 280 BOOT_PRINTF("in DUMB *** \n")
279 281
280 282 while(1){
281 283 rtems_event_receive(RTEMS_EVENT_0 | RTEMS_EVENT_1 | RTEMS_EVENT_2 | RTEMS_EVENT_3
282 284 | RTEMS_EVENT_4 | RTEMS_EVENT_5 | RTEMS_EVENT_6 | RTEMS_EVENT_7
283 285 | RTEMS_EVENT_8 | RTEMS_EVENT_9,
284 286 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT
285 287 intEventOut = (unsigned int) event_out;
286 288 for ( i=0; i<32; i++)
287 289 {
288 290 if ( ((intEventOut >> i) & 0x0001) != 0)
289 291 {
290 292 coarse_time = time_management_regs->coarse_time;
291 293 fine_time = time_management_regs->fine_time;
292 294 printf("in DUMB *** coarse: %x, fine: %x, %s\n", coarse_time, fine_time, DumbMessages[i]);
293 295 if (i==8)
294 296 {
295 297 }
296 298 if (i==10)
297 299 {
298 300 }
299 301 }
300 302 }
301 303 }
302 304 }
303 305
304 306 //*****************************
305 307 // init housekeeping parameters
306 308
307 309 void init_housekeeping_parameters( void )
308 310 {
309 311 /** This function initialize the housekeeping_packet global variable with default values.
310 312 *
311 313 */
312 314
313 315 unsigned int i = 0;
314 316 unsigned char *parameters;
315 317
316 318 parameters = (unsigned char*) &housekeeping_packet.lfr_status_word;
317 319 for(i = 0; i< SIZE_HK_PARAMETERS; i++)
318 320 {
319 321 parameters[i] = 0x00;
320 322 }
321 323 // init status word
322 324 housekeeping_packet.lfr_status_word[0] = DEFAULT_STATUS_WORD_BYTE0;
323 325 housekeeping_packet.lfr_status_word[1] = DEFAULT_STATUS_WORD_BYTE1;
324 326 // init software version
325 327 housekeeping_packet.lfr_sw_version[0] = SW_VERSION_N1;
326 328 housekeeping_packet.lfr_sw_version[1] = SW_VERSION_N2;
327 329 housekeeping_packet.lfr_sw_version[2] = SW_VERSION_N3;
328 330 housekeeping_packet.lfr_sw_version[3] = SW_VERSION_N4;
329 331 // init fpga version
330 332 parameters = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
331 333 housekeeping_packet.lfr_fpga_version[0] = parameters[1]; // n1
332 334 housekeeping_packet.lfr_fpga_version[1] = parameters[2]; // n2
333 335 housekeeping_packet.lfr_fpga_version[2] = parameters[3]; // n3
334 336 }
335 337
336 338 void increment_seq_counter( unsigned short *packetSequenceControl )
337 339 {
338 340 /** This function increment the sequence counter passes in argument.
339 341 *
340 342 * The increment does not affect the grouping flag. In case of an overflow, the counter is reset to 0.
341 343 *
342 344 */
343 345
344 346 unsigned short segmentation_grouping_flag;
345 347 unsigned short sequence_cnt;
346 348
347 349 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << 8; // keep bits 7 downto 6
348 350 sequence_cnt = (*packetSequenceControl) & 0x3fff; // [0011 1111 1111 1111]
349 351
350 352 if ( sequence_cnt < SEQ_CNT_MAX)
351 353 {
352 354 sequence_cnt = sequence_cnt + 1;
353 355 }
354 356 else
355 357 {
356 358 sequence_cnt = 0;
357 359 }
358 360
359 361 *packetSequenceControl = segmentation_grouping_flag | sequence_cnt ;
360 362 }
361 363
362 364 void getTime( unsigned char *time)
363 365 {
364 366 /** This function write the current local time in the time buffer passed in argument.
365 367 *
366 368 */
367 369
368 370 time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
369 371 time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
370 372 time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
371 373 time[3] = (unsigned char) (time_management_regs->coarse_time);
372 374 time[4] = (unsigned char) (time_management_regs->fine_time>>8);
373 375 time[5] = (unsigned char) (time_management_regs->fine_time);
374 376 }
375 377
376 378 unsigned long long int getTimeAsUnsignedLongLongInt( )
377 379 {
378 380 /** This function write the current local time in the time buffer passed in argument.
379 381 *
380 382 */
381 383 unsigned long long int time;
382 384
383 385 time = ( (unsigned long long int) (time_management_regs->coarse_time & 0x7fffffff) << 16 )
384 386 + time_management_regs->fine_time;
385 387
386 388 return time;
387 389 }
388 390
389 391 void send_dumb_hk( void )
390 392 {
391 393 Packet_TM_LFR_HK_t dummy_hk_packet;
392 394 unsigned char *parameters;
393 395 unsigned int i;
394 396 rtems_id queue_id;
395 397
396 398 dummy_hk_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
397 399 dummy_hk_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
398 400 dummy_hk_packet.reserved = DEFAULT_RESERVED;
399 401 dummy_hk_packet.userApplication = CCSDS_USER_APP;
400 402 dummy_hk_packet.packetID[0] = (unsigned char) (APID_TM_HK >> 8);
401 403 dummy_hk_packet.packetID[1] = (unsigned char) (APID_TM_HK);
402 404 dummy_hk_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
403 405 dummy_hk_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
404 406 dummy_hk_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_HK >> 8);
405 407 dummy_hk_packet.packetLength[1] = (unsigned char) (PACKET_LENGTH_HK );
406 408 dummy_hk_packet.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
407 409 dummy_hk_packet.serviceType = TM_TYPE_HK;
408 410 dummy_hk_packet.serviceSubType = TM_SUBTYPE_HK;
409 411 dummy_hk_packet.destinationID = TM_DESTINATION_ID_GROUND;
410 412 dummy_hk_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
411 413 dummy_hk_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
412 414 dummy_hk_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
413 415 dummy_hk_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
414 416 dummy_hk_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
415 417 dummy_hk_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
416 418 dummy_hk_packet.sid = SID_HK;
417 419
418 420 // init status word
419 421 dummy_hk_packet.lfr_status_word[0] = 0xff;
420 422 dummy_hk_packet.lfr_status_word[1] = 0xff;
421 423 // init software version
422 424 dummy_hk_packet.lfr_sw_version[0] = SW_VERSION_N1;
423 425 dummy_hk_packet.lfr_sw_version[1] = SW_VERSION_N2;
424 426 dummy_hk_packet.lfr_sw_version[2] = SW_VERSION_N3;
425 427 dummy_hk_packet.lfr_sw_version[3] = SW_VERSION_N4;
426 428 // init fpga version
427 429 parameters = (unsigned char *) (REGS_ADDR_WAVEFORM_PICKER + 0xb0);
428 430 dummy_hk_packet.lfr_fpga_version[0] = parameters[1]; // n1
429 431 dummy_hk_packet.lfr_fpga_version[1] = parameters[2]; // n2
430 432 dummy_hk_packet.lfr_fpga_version[2] = parameters[3]; // n3
431 433
432 434 parameters = (unsigned char *) &dummy_hk_packet.hk_lfr_cpu_load;
433 435
434 436 for (i=0; i<100; i++)
435 437 {
436 438 parameters[i] = 0xff;
437 439 }
438 440
439 441 get_message_queue_id_send( &queue_id );
440 442
441 443 rtems_message_queue_send( queue_id, &dummy_hk_packet,
442 444 PACKET_LENGTH_HK + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
443 445 }
444 446
445 447 void get_temperatures( unsigned char *temperatures )
446 448 {
447 449 unsigned char* temp_scm_ptr;
448 450 unsigned char* temp_pcb_ptr;
449 451 unsigned char* temp_fpga_ptr;
450 452
451 453 // SEL1 SEL0
452 454 // 0 0 => PCB
453 455 // 0 1 => FPGA
454 456 // 1 0 => SCM
455 457
456 458 temp_scm_ptr = (unsigned char *) &time_management_regs->temp_scm;
457 459 temp_pcb_ptr = (unsigned char *) &time_management_regs->temp_pcb;
458 460 temp_fpga_ptr = (unsigned char *) &time_management_regs->temp_fpga;
459 461
460 462 temperatures[0] = temp_scm_ptr[2];
461 463 temperatures[1] = temp_scm_ptr[3];
462 464 temperatures[2] = temp_pcb_ptr[2];
463 465 temperatures[3] = temp_pcb_ptr[3];
464 466 temperatures[4] = temp_fpga_ptr[2];
465 467 temperatures[5] = temp_fpga_ptr[3];
466 468 }
467 469
468 470 void get_v_e1_e2_f3( unsigned char *spacecraft_potential )
469 471 {
470 472 unsigned char* v_ptr;
471 473 unsigned char* e1_ptr;
472 474 unsigned char* e2_ptr;
473 475
474 476 v_ptr = (unsigned char *) &waveform_picker_regs->v;
475 477 e1_ptr = (unsigned char *) &waveform_picker_regs->e1;
476 478 e2_ptr = (unsigned char *) &waveform_picker_regs->e2;
477 479
478 480 spacecraft_potential[0] = v_ptr[2];
479 481 spacecraft_potential[1] = v_ptr[3];
480 482 spacecraft_potential[2] = e1_ptr[2];
481 483 spacecraft_potential[3] = e1_ptr[3];
482 484 spacecraft_potential[4] = e2_ptr[2];
483 485 spacecraft_potential[5] = e2_ptr[3];
484 486 }
485 487
486 488 void get_cpu_load( unsigned char *resource_statistics )
487 489 {
488 490 unsigned char cpu_load;
489 491
490 492 cpu_load = lfr_rtems_cpu_usage_report();
491 493
492 494 // HK_LFR_CPU_LOAD
493 495 resource_statistics[0] = cpu_load;
494 496
495 497 // HK_LFR_CPU_LOAD_MAX
496 498 if (cpu_load > resource_statistics[1])
497 499 {
498 500 resource_statistics[1] = cpu_load;
499 501 }
500 502
501 503 // CPU_LOAD_AVE
502 504 resource_statistics[2] = 0;
503 505
504 506 #ifndef PRINT_TASK_STATISTICS
505 507 rtems_cpu_usage_reset();
506 508 #endif
507 509
508 510 }
509 511
510 512
511 513
Show file before | Show file after | Hide comments
@@ -1,586 +1,651
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 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 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 }
587
588 void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
589 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage, unsigned char ASMIndexStart )
590 {
591 //*************
592 // input format
593 // component0[0 .. 127] component1[0 .. 127] .. component24[0 .. 127]
594 //**************
595 // output format
596 // matr0[0 .. 24] matr1[0 .. 24] .. matr127[0 .. 24]
597 //************
598 // compression
599 // matr0[0 .. 24] matr1[0 .. 24] .. matr11[0 .. 24] => f0 NORM
600 // matr0[0 .. 24] matr1[0 .. 24] .. matr22[0 .. 24] => f0 BURST, SBM
601
602 int frequencyBin;
603 int asmComponent;
604 int offsetASM;
605 int offsetCompressed;
606 int offsetFBin;
607 int fBinMask;
608 int k;
609
610 // BUILD DATA
611 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
612 {
613 for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
614 {
615 offsetCompressed = // NO TIME OFFSET
616 frequencyBin * NB_VALUES_PER_SM
617 + asmComponent;
618 offsetASM = // NO TIME OFFSET
619 asmComponent * NB_BINS_PER_SM
620 + ASMIndexStart
621 + frequencyBin * nbBinsToAverage;
622 offsetFBin = ASMIndexStart
623 + frequencyBin * nbBinsToAverage;
624 compressed_spec_mat[ offsetCompressed ] = 0;
625 for ( k = 0; k < nbBinsToAverage; k++ )
626 {
627 fBinMask = getFBinMask( offsetFBin + k );
628 compressed_spec_mat[offsetCompressed ] =
629 ( compressed_spec_mat[ offsetCompressed ]
630 + averaged_spec_mat[ offsetASM + k ] * fBinMask );
631 }
632 compressed_spec_mat[ offsetCompressed ] =
633 compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
634 }
635 }
636
637 }
638
639 int getFBinMask( int index )
640 {
641 unsigned int indexInChar;
642 unsigned int indexInTheChar;
643 int fbin;
644
645 indexInChar = index >> 3;
646 indexInTheChar = index - indexInChar * 8;
647
648 fbin = (int) ((parameter_dump_packet.sy_lfr_fbins_f0_word1[ NB_BYTES_PER_FREQ_MASK - 1 - indexInChar] >> indexInTheChar) & 0x1);
649
650 return fbin;
651 }
Show file before | Show file after | Hide comments
@@ -1,1092 +1,1120
1 1 /** Functions to load and dump parameters in the LFR registers.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle TC related to parameter loading and dumping.\n
7 7 * TC_LFR_LOAD_COMMON_PAR\n
8 8 * TC_LFR_LOAD_NORMAL_PAR\n
9 9 * TC_LFR_LOAD_BURST_PAR\n
10 10 * TC_LFR_LOAD_SBM1_PAR\n
11 11 * TC_LFR_LOAD_SBM2_PAR\n
12 12 *
13 13 */
14 14
15 15 #include "tc_load_dump_parameters.h"
16 16
17 17 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t kcoefficients_dump_1;
18 18 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t kcoefficients_dump_2;
19 19 ring_node kcoefficient_node_1;
20 20 ring_node kcoefficient_node_2;
21 21
22 22 int action_load_common_par(ccsdsTelecommandPacket_t *TC)
23 23 {
24 24 /** This function updates the LFR registers with the incoming common parameters.
25 25 *
26 26 * @param TC points to the TeleCommand packet that is being processed
27 27 *
28 28 *
29 29 */
30 30
31 parameter_dump_packet.unused0 = TC->dataAndCRC[0];
31 parameter_dump_packet.sy_lfr_common_parameters_spare = TC->dataAndCRC[0];
32 32 parameter_dump_packet.sy_lfr_common_parameters = TC->dataAndCRC[1];
33 33 set_wfp_data_shaping( );
34 34 return LFR_SUCCESSFUL;
35 35 }
36 36
37 37 int action_load_normal_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
38 38 {
39 39 /** This function updates the LFR registers with the incoming normal parameters.
40 40 *
41 41 * @param TC points to the TeleCommand packet that is being processed
42 42 * @param queue_id is the id of the queue which handles TM related to this execution step
43 43 *
44 44 */
45 45
46 46 int result;
47 47 int flag;
48 48 rtems_status_code status;
49 49
50 50 flag = LFR_SUCCESSFUL;
51 51
52 52 if ( (lfrCurrentMode == LFR_MODE_NORMAL) ||
53 53 (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) ) {
54 54 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
55 55 flag = LFR_DEFAULT;
56 56 }
57 57
58 58 // CHECK THE PARAMETERS SET CONSISTENCY
59 59 if (flag == LFR_SUCCESSFUL)
60 60 {
61 61 flag = check_common_par_consistency( TC, queue_id );
62 62 }
63 63
64 64 // SET THE PARAMETERS IF THEY ARE CONSISTENT
65 65 if (flag == LFR_SUCCESSFUL)
66 66 {
67 67 result = set_sy_lfr_n_swf_l( TC );
68 68 result = set_sy_lfr_n_swf_p( TC );
69 69 result = set_sy_lfr_n_bp_p0( TC );
70 70 result = set_sy_lfr_n_bp_p1( TC );
71 71 result = set_sy_lfr_n_asm_p( TC );
72 72 result = set_sy_lfr_n_cwf_long_f3( TC );
73 73 }
74 74
75 75 return flag;
76 76 }
77 77
78 78 int action_load_burst_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
79 79 {
80 80 /** This function updates the LFR registers with the incoming burst parameters.
81 81 *
82 82 * @param TC points to the TeleCommand packet that is being processed
83 83 * @param queue_id is the id of the queue which handles TM related to this execution step
84 84 *
85 85 */
86 86
87 87 int flag;
88 88 rtems_status_code status;
89 89 unsigned char sy_lfr_b_bp_p0;
90 90 unsigned char sy_lfr_b_bp_p1;
91 91 float aux;
92 92
93 93 flag = LFR_SUCCESSFUL;
94 94
95 95 if ( lfrCurrentMode == LFR_MODE_BURST ) {
96 96 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
97 97 flag = LFR_DEFAULT;
98 98 }
99 99
100 100 sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
101 101 sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
102 102
103 103 // sy_lfr_b_bp_p0
104 104 if (flag == LFR_SUCCESSFUL)
105 105 {
106 106 if (sy_lfr_b_bp_p0 < DEFAULT_SY_LFR_B_BP_P0 )
107 107 {
108 108 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P0+10, sy_lfr_b_bp_p0 );
109 109 flag = WRONG_APP_DATA;
110 110 }
111 111 }
112 112 // sy_lfr_b_bp_p1
113 113 if (flag == LFR_SUCCESSFUL)
114 114 {
115 115 if (sy_lfr_b_bp_p1 < DEFAULT_SY_LFR_B_BP_P1 )
116 116 {
117 117 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P1+10, sy_lfr_b_bp_p1 );
118 118 flag = WRONG_APP_DATA;
119 119 }
120 120 }
121 121 //****************************************************************
122 122 // check the consistency between sy_lfr_b_bp_p0 and sy_lfr_b_bp_p1
123 123 if (flag == LFR_SUCCESSFUL)
124 124 {
125 125 sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
126 126 sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
127 127 aux = ( (float ) sy_lfr_b_bp_p1 / sy_lfr_b_bp_p0 ) - floor(sy_lfr_b_bp_p1 / sy_lfr_b_bp_p0);
128 128 if (aux > FLOAT_EQUAL_ZERO)
129 129 {
130 130 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P0+10, sy_lfr_b_bp_p0 );
131 131 flag = LFR_DEFAULT;
132 132 }
133 133 }
134 134
135 135 // SET HTE PARAMETERS
136 136 if (flag == LFR_SUCCESSFUL)
137 137 {
138 138 flag = set_sy_lfr_b_bp_p0( TC );
139 139 flag = set_sy_lfr_b_bp_p1( TC );
140 140 }
141 141
142 142 return flag;
143 143 }
144 144
145 145 int action_load_sbm1_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
146 146 {
147 147 /** This function updates the LFR registers with the incoming sbm1 parameters.
148 148 *
149 149 * @param TC points to the TeleCommand packet that is being processed
150 150 * @param queue_id is the id of the queue which handles TM related to this execution step
151 151 *
152 152 */
153 153
154 154 int flag;
155 155 rtems_status_code status;
156 156 unsigned char sy_lfr_s1_bp_p0;
157 157 unsigned char sy_lfr_s1_bp_p1;
158 158 float aux;
159 159
160 160 flag = LFR_SUCCESSFUL;
161 161
162 162 if ( lfrCurrentMode == LFR_MODE_SBM1 ) {
163 163 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
164 164 flag = LFR_DEFAULT;
165 165 }
166 166
167 167 sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
168 168 sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
169 169
170 170 // sy_lfr_s1_bp_p0
171 171 if (flag == LFR_SUCCESSFUL)
172 172 {
173 173 if (sy_lfr_s1_bp_p0 < DEFAULT_SY_LFR_S1_BP_P0 )
174 174 {
175 175 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P0+10, sy_lfr_s1_bp_p0 );
176 176 flag = WRONG_APP_DATA;
177 177 }
178 178 }
179 179 // sy_lfr_s1_bp_p1
180 180 if (flag == LFR_SUCCESSFUL)
181 181 {
182 182 if (sy_lfr_s1_bp_p1 < DEFAULT_SY_LFR_S1_BP_P1 )
183 183 {
184 184 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P1+10, sy_lfr_s1_bp_p1 );
185 185 flag = WRONG_APP_DATA;
186 186 }
187 187 }
188 188 //******************************************************************
189 189 // check the consistency between sy_lfr_s1_bp_p0 and sy_lfr_s1_bp_p1
190 190 if (flag == LFR_SUCCESSFUL)
191 191 {
192 192 aux = ( (float ) sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0*0.25) ) - floor(sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0*0.25));
193 193 if (aux > FLOAT_EQUAL_ZERO)
194 194 {
195 195 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P0+10, sy_lfr_s1_bp_p0 );
196 196 flag = LFR_DEFAULT;
197 197 }
198 198 }
199 199
200 200 // SET THE PARAMETERS
201 201 if (flag == LFR_SUCCESSFUL)
202 202 {
203 203 flag = set_sy_lfr_s1_bp_p0( TC );
204 204 flag = set_sy_lfr_s1_bp_p1( TC );
205 205 }
206 206
207 207 return flag;
208 208 }
209 209
210 210 int action_load_sbm2_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
211 211 {
212 212 /** This function updates the LFR registers with the incoming sbm2 parameters.
213 213 *
214 214 * @param TC points to the TeleCommand packet that is being processed
215 215 * @param queue_id is the id of the queue which handles TM related to this execution step
216 216 *
217 217 */
218 218
219 219 int flag;
220 220 rtems_status_code status;
221 221 unsigned char sy_lfr_s2_bp_p0;
222 222 unsigned char sy_lfr_s2_bp_p1;
223 223 float aux;
224 224
225 225 flag = LFR_SUCCESSFUL;
226 226
227 227 if ( lfrCurrentMode == LFR_MODE_SBM2 ) {
228 228 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
229 229 flag = LFR_DEFAULT;
230 230 }
231 231
232 232 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
233 233 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
234 234
235 235 // sy_lfr_s2_bp_p0
236 236 if (flag == LFR_SUCCESSFUL)
237 237 {
238 238 if (sy_lfr_s2_bp_p0 < DEFAULT_SY_LFR_S2_BP_P0 )
239 239 {
240 240 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0+10, sy_lfr_s2_bp_p0 );
241 241 flag = WRONG_APP_DATA;
242 242 }
243 243 }
244 244 // sy_lfr_s2_bp_p1
245 245 if (flag == LFR_SUCCESSFUL)
246 246 {
247 247 if (sy_lfr_s2_bp_p1 < DEFAULT_SY_LFR_S2_BP_P1 )
248 248 {
249 249 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P1+10, sy_lfr_s2_bp_p1 );
250 250 flag = WRONG_APP_DATA;
251 251 }
252 252 }
253 253 //******************************************************************
254 254 // check the consistency between sy_lfr_s2_bp_p0 and sy_lfr_s2_bp_p1
255 255 if (flag == LFR_SUCCESSFUL)
256 256 {
257 257 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
258 258 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
259 259 aux = ( (float ) sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0 ) - floor(sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0);
260 260 if (aux > FLOAT_EQUAL_ZERO)
261 261 {
262 262 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0+10, sy_lfr_s2_bp_p0 );
263 263 flag = LFR_DEFAULT;
264 264 }
265 265 }
266 266
267 267 // SET THE PARAMETERS
268 268 if (flag == LFR_SUCCESSFUL)
269 269 {
270 270 flag = set_sy_lfr_s2_bp_p0( TC );
271 271 flag = set_sy_lfr_s2_bp_p1( TC );
272 272 }
273 273
274 274 return flag;
275 275 }
276 276
277 277 int action_load_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
278 278 {
279 279 /** This function updates the LFR registers with the incoming sbm2 parameters.
280 280 *
281 281 * @param TC points to the TeleCommand packet that is being processed
282 282 * @param queue_id is the id of the queue which handles TM related to this execution step
283 283 *
284 284 */
285 285
286 286 int flag;
287 287
288 288 flag = LFR_DEFAULT;
289 289
290 290 flag = set_sy_lfr_kcoeff( TC );
291 291
292 292 return flag;
293 293 }
294 294
295 295 int action_load_fbins_mask(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
296 296 {
297 297 /** This function updates the LFR registers with the incoming sbm2 parameters.
298 298 *
299 299 * @param TC points to the TeleCommand packet that is being processed
300 300 * @param queue_id is the id of the queue which handles TM related to this execution step
301 301 *
302 302 */
303 303
304 304 int flag;
305 305
306 306 flag = LFR_DEFAULT;
307 307
308 send_tm_lfr_tc_exe_not_implemented( TC, queue_id, time );
308 flag = set_sy_lfr_fbins( TC );
309 309
310 310 return flag;
311 311 }
312 312
313 313 void printKCoefficients(unsigned int freq, unsigned int bin, float *k_coeff)
314 314 {
315 315 printf("freq = %d *** bin = %d *** (0) %f *** (1) %f *** (2) %f *** (3) %f *** (4) %f\n",
316 316 freq,
317 317 bin,
318 318 k_coeff[ (bin*NB_K_COEFF_PER_BIN) + 0 ],
319 319 k_coeff[ (bin*NB_K_COEFF_PER_BIN) + 1 ],
320 320 k_coeff[ (bin*NB_K_COEFF_PER_BIN) + 2 ],
321 321 k_coeff[ (bin*NB_K_COEFF_PER_BIN) + 3 ],
322 322 k_coeff[ (bin*NB_K_COEFF_PER_BIN) + 4 ]);
323 323 }
324 324
325 325 int action_dump_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
326 326 {
327 327 /** This function updates the LFR registers with the incoming sbm2 parameters.
328 328 *
329 329 * @param TC points to the TeleCommand packet that is being processed
330 330 * @param queue_id is the id of the queue which handles TM related to this execution step
331 331 *
332 332 */
333 333
334 334 unsigned int address;
335 335 rtems_status_code status;
336 336 unsigned int freq;
337 337 unsigned int bin;
338 338 unsigned int coeff;
339 339 unsigned char *kCoeffPtr;
340 340 unsigned char *kCoeffDumpPtr;
341 341
342 342 // for each sy_lfr_kcoeff_frequency there is 32 kcoeff
343 343 // F0 => 11 bins
344 344 // F1 => 13 bins
345 345 // F2 => 12 bins
346 346 // 36 bins to dump in two packets (30 bins max per packet)
347 347
348 348 //*********
349 349 // PACKET 1
350 350 // 11 F0 bins, 13 F1 bins and 6 F2 bins
351 351 kcoefficients_dump_1.packetSequenceControl[0] = (unsigned char) (sequenceCounterParameterDump >> 8);
352 352 kcoefficients_dump_1.packetSequenceControl[1] = (unsigned char) (sequenceCounterParameterDump );
353 353 increment_seq_counter( &sequenceCounterParameterDump );
354 354 for( freq=0;
355 355 freq<NB_BINS_COMPRESSED_SM_F0;
356 356 freq++ )
357 357 {
358 358 kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1] = freq;
359 359 bin = freq;
360 printKCoefficients( freq, bin, k_coeff_intercalib_f0_norm);
360 // printKCoefficients( freq, bin, k_coeff_intercalib_f0_norm);
361 361 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
362 362 {
363 363 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
364 364 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f0_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
365 kCoeffDumpPtr[0] = kCoeffPtr[0];
366 kCoeffDumpPtr[1] = kCoeffPtr[1];
367 kCoeffDumpPtr[2] = kCoeffPtr[2];
368 kCoeffDumpPtr[3] = kCoeffPtr[3];
365 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
369 366 }
370 367 }
371 368 for( freq=NB_BINS_COMPRESSED_SM_F0;
372 369 freq<(NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1);
373 370 freq++ )
374 371 {
375 372 kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1 ] = freq;
376 373 bin = freq - NB_BINS_COMPRESSED_SM_F0;
377 printKCoefficients( freq, bin, k_coeff_intercalib_f1_norm);
374 // printKCoefficients( freq, bin, k_coeff_intercalib_f1_norm);
378 375 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
379 376 {
380 377 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
381 378 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f1_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
382 kCoeffDumpPtr[0] = kCoeffPtr[0];
383 kCoeffDumpPtr[1] = kCoeffPtr[1];
384 kCoeffDumpPtr[2] = kCoeffPtr[2];
385 kCoeffDumpPtr[3] = kCoeffPtr[3];
379 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
386 380 }
387 381 }
388 382 for( freq=(NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1);
389 383 freq<(NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1+6);
390 384 freq++ )
391 385 {
392 386 kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1 ] = freq;
393 387 bin = freq - (NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1);
394 printKCoefficients( freq, bin, k_coeff_intercalib_f2);
388 // printKCoefficients( freq, bin, k_coeff_intercalib_f2);
395 389 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
396 390 {
397 391 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
398 392 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
399 kCoeffDumpPtr[0] = kCoeffPtr[0];
400 kCoeffDumpPtr[1] = kCoeffPtr[1];
401 kCoeffDumpPtr[2] = kCoeffPtr[2];
402 kCoeffDumpPtr[3] = kCoeffPtr[3];
393 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
403 394 }
404 395 }
405 396 kcoefficients_dump_1.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
406 397 kcoefficients_dump_1.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
407 398 kcoefficients_dump_1.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
408 399 kcoefficients_dump_1.time[3] = (unsigned char) (time_management_regs->coarse_time);
409 400 kcoefficients_dump_1.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
410 401 kcoefficients_dump_1.time[5] = (unsigned char) (time_management_regs->fine_time);
411 402 // SEND DATA
412 403 kcoefficient_node_1.status = 1;
413 404 address = (unsigned int) &kcoefficient_node_1;
414 405 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
415 406 if (status != RTEMS_SUCCESSFUL) {
416 407 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 1 , code %d", status)
417 408 }
418 409
419 410 //********
420 411 // PACKET 2
421 412 // 6 F2 bins
422 413 kcoefficients_dump_2.packetSequenceControl[0] = (unsigned char) (sequenceCounterParameterDump >> 8);
423 414 kcoefficients_dump_2.packetSequenceControl[1] = (unsigned char) (sequenceCounterParameterDump );
424 415 increment_seq_counter( &sequenceCounterParameterDump );
425 416 for( freq=0; freq<6; freq++ )
426 417 {
427 418 kcoefficients_dump_2.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1 ] = NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 + 6 + freq;
428 419 bin = freq + 6;
429 printKCoefficients( freq, bin, k_coeff_intercalib_f2);
420 // printKCoefficients( freq, bin, k_coeff_intercalib_f2);
430 421 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
431 422 {
432 423 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_2.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
433 424 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
434 kCoeffDumpPtr[0] = kCoeffPtr[0];
435 kCoeffDumpPtr[1] = kCoeffPtr[1];
436 kCoeffDumpPtr[2] = kCoeffPtr[2];
437 kCoeffDumpPtr[3] = kCoeffPtr[3];
425 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
438 426 }
439 427 }
440 428 kcoefficients_dump_2.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
441 429 kcoefficients_dump_2.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
442 430 kcoefficients_dump_2.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
443 431 kcoefficients_dump_2.time[3] = (unsigned char) (time_management_regs->coarse_time);
444 432 kcoefficients_dump_2.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
445 433 kcoefficients_dump_2.time[5] = (unsigned char) (time_management_regs->fine_time);
446 434 // SEND DATA
447 435 kcoefficient_node_2.status = 1;
448 436 address = (unsigned int) &kcoefficient_node_2;
449 437 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
450 438 if (status != RTEMS_SUCCESSFUL) {
451 439 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 2, code %d", status)
452 440 }
453 441
454 442 return status;
455 443 }
456 444
457 445 int action_dump_par( rtems_id queue_id )
458 446 {
459 447 /** This function dumps the LFR parameters by sending the appropriate TM packet to the dedicated RTEMS message queue.
460 448 *
461 449 * @param queue_id is the id of the queue which handles TM related to this execution step.
462 450 *
463 451 * @return RTEMS directive status codes:
464 452 * - RTEMS_SUCCESSFUL - message sent successfully
465 453 * - RTEMS_INVALID_ID - invalid queue id
466 454 * - RTEMS_INVALID_SIZE - invalid message size
467 455 * - RTEMS_INVALID_ADDRESS - buffer is NULL
468 456 * - RTEMS_UNSATISFIED - out of message buffers
469 457 * - RTEMS_TOO_MANY - queue s limit has been reached
470 458 *
471 459 */
472 460
473 461 int status;
474 462
475 463 // UPDATE TIME
476 464 parameter_dump_packet.packetSequenceControl[0] = (unsigned char) (sequenceCounterParameterDump >> 8);
477 465 parameter_dump_packet.packetSequenceControl[1] = (unsigned char) (sequenceCounterParameterDump );
478 466 increment_seq_counter( &sequenceCounterParameterDump );
479 467
480 468 parameter_dump_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
481 469 parameter_dump_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
482 470 parameter_dump_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
483 471 parameter_dump_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
484 472 parameter_dump_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
485 473 parameter_dump_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
486 474 // SEND DATA
487 475 status = rtems_message_queue_send( queue_id, &parameter_dump_packet,
488 476 PACKET_LENGTH_PARAMETER_DUMP + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
489 477 if (status != RTEMS_SUCCESSFUL) {
490 478 PRINTF1("in action_dump *** ERR sending packet, code %d", status)
491 479 }
492 480
493 481 return status;
494 482 }
495 483
496 484 //***********************
497 485 // NORMAL MODE PARAMETERS
498 486
499 487 int check_common_par_consistency( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
500 488 {
501 489 unsigned char msb;
502 490 unsigned char lsb;
503 491 int flag;
504 492 float aux;
505 493 rtems_status_code status;
506 494
507 495 unsigned int sy_lfr_n_swf_l;
508 496 unsigned int sy_lfr_n_swf_p;
509 497 unsigned int sy_lfr_n_asm_p;
510 498 unsigned char sy_lfr_n_bp_p0;
511 499 unsigned char sy_lfr_n_bp_p1;
512 500 unsigned char sy_lfr_n_cwf_long_f3;
513 501
514 502 flag = LFR_SUCCESSFUL;
515 503
516 504 //***************
517 505 // get parameters
518 506 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
519 507 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
520 508 sy_lfr_n_swf_l = msb * 256 + lsb;
521 509
522 510 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
523 511 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
524 512 sy_lfr_n_swf_p = msb * 256 + lsb;
525 513
526 514 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
527 515 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
528 516 sy_lfr_n_asm_p = msb * 256 + lsb;
529 517
530 518 sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
531 519
532 520 sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
533 521
534 522 sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
535 523
536 524 //******************
537 525 // check consistency
538 526 // sy_lfr_n_swf_l
539 527 if (sy_lfr_n_swf_l != 2048)
540 528 {
541 529 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_L+10, sy_lfr_n_swf_l );
542 530 flag = WRONG_APP_DATA;
543 531 }
544 532 // sy_lfr_n_swf_p
545 533 if (flag == LFR_SUCCESSFUL)
546 534 {
547 535 if ( sy_lfr_n_swf_p < 16 )
548 536 {
549 537 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_P+10, sy_lfr_n_swf_p );
550 538 flag = WRONG_APP_DATA;
551 539 }
552 540 }
553 541 // sy_lfr_n_bp_p0
554 542 if (flag == LFR_SUCCESSFUL)
555 543 {
556 544 if (sy_lfr_n_bp_p0 < DFLT_SY_LFR_N_BP_P0)
557 545 {
558 546 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P0+10, sy_lfr_n_bp_p0 );
559 547 flag = WRONG_APP_DATA;
560 548 }
561 549 }
562 550 // sy_lfr_n_asm_p
563 551 if (flag == LFR_SUCCESSFUL)
564 552 {
565 553 if (sy_lfr_n_asm_p == 0)
566 554 {
567 555 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P+10, sy_lfr_n_asm_p );
568 556 flag = WRONG_APP_DATA;
569 557 }
570 558 }
571 559 // sy_lfr_n_asm_p shall be a whole multiple of sy_lfr_n_bp_p0
572 560 if (flag == LFR_SUCCESSFUL)
573 561 {
574 562 aux = ( (float ) sy_lfr_n_asm_p / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_asm_p / sy_lfr_n_bp_p0);
575 563 if (aux > FLOAT_EQUAL_ZERO)
576 564 {
577 565 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P+10, sy_lfr_n_asm_p );
578 566 flag = WRONG_APP_DATA;
579 567 }
580 568 }
581 569 // sy_lfr_n_bp_p1
582 570 if (flag == LFR_SUCCESSFUL)
583 571 {
584 572 if (sy_lfr_n_bp_p1 < DFLT_SY_LFR_N_BP_P1)
585 573 {
586 574 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1+10, sy_lfr_n_bp_p1 );
587 575 flag = WRONG_APP_DATA;
588 576 }
589 577 }
590 578 // sy_lfr_n_bp_p1 shall be a whole multiple of sy_lfr_n_bp_p0
591 579 if (flag == LFR_SUCCESSFUL)
592 580 {
593 581 aux = ( (float ) sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0);
594 582 if (aux > FLOAT_EQUAL_ZERO)
595 583 {
596 584 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1+10, sy_lfr_n_bp_p1 );
597 585 flag = LFR_DEFAULT;
598 586 }
599 587 }
600 588 // sy_lfr_n_cwf_long_f3
601 589
602 590 return flag;
603 591 }
604 592
605 593 int set_sy_lfr_n_swf_l( ccsdsTelecommandPacket_t *TC )
606 594 {
607 595 /** This function sets the number of points of a snapshot (sy_lfr_n_swf_l).
608 596 *
609 597 * @param TC points to the TeleCommand packet that is being processed
610 598 * @param queue_id is the id of the queue which handles TM related to this execution step
611 599 *
612 600 */
613 601
614 602 int result;
615 603
616 604 result = LFR_SUCCESSFUL;
617 605
618 606 parameter_dump_packet.sy_lfr_n_swf_l[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
619 607 parameter_dump_packet.sy_lfr_n_swf_l[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
620 608
621 609 return result;
622 610 }
623 611
624 612 int set_sy_lfr_n_swf_p(ccsdsTelecommandPacket_t *TC )
625 613 {
626 614 /** This function sets the time between two snapshots, in s (sy_lfr_n_swf_p).
627 615 *
628 616 * @param TC points to the TeleCommand packet that is being processed
629 617 * @param queue_id is the id of the queue which handles TM related to this execution step
630 618 *
631 619 */
632 620
633 621 int result;
634 622
635 623 result = LFR_SUCCESSFUL;
636 624
637 625 parameter_dump_packet.sy_lfr_n_swf_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
638 626 parameter_dump_packet.sy_lfr_n_swf_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
639 627
640 628 return result;
641 629 }
642 630
643 631 int set_sy_lfr_n_asm_p( ccsdsTelecommandPacket_t *TC )
644 632 {
645 633 /** This function sets the time between two full spectral matrices transmission, in s (SY_LFR_N_ASM_P).
646 634 *
647 635 * @param TC points to the TeleCommand packet that is being processed
648 636 * @param queue_id is the id of the queue which handles TM related to this execution step
649 637 *
650 638 */
651 639
652 640 int result;
653 641
654 642 result = LFR_SUCCESSFUL;
655 643
656 644 parameter_dump_packet.sy_lfr_n_asm_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
657 645 parameter_dump_packet.sy_lfr_n_asm_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
658 646
659 647 return result;
660 648 }
661 649
662 650 int set_sy_lfr_n_bp_p0( ccsdsTelecommandPacket_t *TC )
663 651 {
664 652 /** This function sets the time between two basic parameter sets, in s (DFLT_SY_LFR_N_BP_P0).
665 653 *
666 654 * @param TC points to the TeleCommand packet that is being processed
667 655 * @param queue_id is the id of the queue which handles TM related to this execution step
668 656 *
669 657 */
670 658
671 659 int status;
672 660
673 661 status = LFR_SUCCESSFUL;
674 662
675 663 parameter_dump_packet.sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
676 664
677 665 return status;
678 666 }
679 667
680 668 int set_sy_lfr_n_bp_p1(ccsdsTelecommandPacket_t *TC )
681 669 {
682 670 /** This function sets the time between two basic parameter sets (autocorrelation + crosscorrelation), in s (sy_lfr_n_bp_p1).
683 671 *
684 672 * @param TC points to the TeleCommand packet that is being processed
685 673 * @param queue_id is the id of the queue which handles TM related to this execution step
686 674 *
687 675 */
688 676
689 677 int status;
690 678
691 679 status = LFR_SUCCESSFUL;
692 680
693 681 parameter_dump_packet.sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
694 682
695 683 return status;
696 684 }
697 685
698 686 int set_sy_lfr_n_cwf_long_f3(ccsdsTelecommandPacket_t *TC )
699 687 {
700 688 /** This function allows to switch from CWF_F3 packets to CWF_LONG_F3 packets.
701 689 *
702 690 * @param TC points to the TeleCommand packet that is being processed
703 691 * @param queue_id is the id of the queue which handles TM related to this execution step
704 692 *
705 693 */
706 694
707 695 int status;
708 696
709 697 status = LFR_SUCCESSFUL;
710 698
711 699 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
712 700
713 701 return status;
714 702 }
715 703
716 704 //**********************
717 705 // BURST MODE PARAMETERS
718 706 int set_sy_lfr_b_bp_p0(ccsdsTelecommandPacket_t *TC)
719 707 {
720 708 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P0).
721 709 *
722 710 * @param TC points to the TeleCommand packet that is being processed
723 711 * @param queue_id is the id of the queue which handles TM related to this execution step
724 712 *
725 713 */
726 714
727 715 int status;
728 716
729 717 status = LFR_SUCCESSFUL;
730 718
731 719 parameter_dump_packet.sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
732 720
733 721 return status;
734 722 }
735 723
736 724 int set_sy_lfr_b_bp_p1( ccsdsTelecommandPacket_t *TC )
737 725 {
738 726 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P1).
739 727 *
740 728 * @param TC points to the TeleCommand packet that is being processed
741 729 * @param queue_id is the id of the queue which handles TM related to this execution step
742 730 *
743 731 */
744 732
745 733 int status;
746 734
747 735 status = LFR_SUCCESSFUL;
748 736
749 737 parameter_dump_packet.sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
750 738
751 739 return status;
752 740 }
753 741
754 742 //*********************
755 743 // SBM1 MODE PARAMETERS
756 744 int set_sy_lfr_s1_bp_p0( ccsdsTelecommandPacket_t *TC )
757 745 {
758 746 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P0).
759 747 *
760 748 * @param TC points to the TeleCommand packet that is being processed
761 749 * @param queue_id is the id of the queue which handles TM related to this execution step
762 750 *
763 751 */
764 752
765 753 int status;
766 754
767 755 status = LFR_SUCCESSFUL;
768 756
769 757 parameter_dump_packet.sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
770 758
771 759 return status;
772 760 }
773 761
774 762 int set_sy_lfr_s1_bp_p1( ccsdsTelecommandPacket_t *TC )
775 763 {
776 764 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P1).
777 765 *
778 766 * @param TC points to the TeleCommand packet that is being processed
779 767 * @param queue_id is the id of the queue which handles TM related to this execution step
780 768 *
781 769 */
782 770
783 771 int status;
784 772
785 773 status = LFR_SUCCESSFUL;
786 774
787 775 parameter_dump_packet.sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
788 776
789 777 return status;
790 778 }
791 779
792 780 //*********************
793 781 // SBM2 MODE PARAMETERS
794 782 int set_sy_lfr_s2_bp_p0(ccsdsTelecommandPacket_t *TC)
795 783 {
796 784 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P0).
797 785 *
798 786 * @param TC points to the TeleCommand packet that is being processed
799 787 * @param queue_id is the id of the queue which handles TM related to this execution step
800 788 *
801 789 */
802 790
803 791 int status;
804 792
805 793 status = LFR_SUCCESSFUL;
806 794
807 795 parameter_dump_packet.sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
808 796
809 797 return status;
810 798 }
811 799
812 800 int set_sy_lfr_s2_bp_p1( ccsdsTelecommandPacket_t *TC )
813 801 {
814 802 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P1).
815 803 *
816 804 * @param TC points to the TeleCommand packet that is being processed
817 805 * @param queue_id is the id of the queue which handles TM related to this execution step
818 806 *
819 807 */
820 808
821 809 int status;
822 810
823 811 status = LFR_SUCCESSFUL;
824 812
825 813 parameter_dump_packet.sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
826 814
827 815 return status;
828 816 }
829 817
830 818 //*******************
831 819 // TC_LFR_UPDATE_INFO
832 820 unsigned int check_update_info_hk_lfr_mode( unsigned char mode )
833 821 {
834 822 unsigned int status;
835 823
836 824 if ( (mode == LFR_MODE_STANDBY) || (mode == LFR_MODE_NORMAL)
837 825 || (mode == LFR_MODE_BURST)
838 826 || (mode == LFR_MODE_SBM1) || (mode == LFR_MODE_SBM2))
839 827 {
840 828 status = LFR_SUCCESSFUL;
841 829 }
842 830 else
843 831 {
844 832 status = LFR_DEFAULT;
845 833 }
846 834
847 835 return status;
848 836 }
849 837
850 838 unsigned int check_update_info_hk_tds_mode( unsigned char mode )
851 839 {
852 840 unsigned int status;
853 841
854 842 if ( (mode == TDS_MODE_STANDBY) || (mode == TDS_MODE_NORMAL)
855 843 || (mode == TDS_MODE_BURST)
856 844 || (mode == TDS_MODE_SBM1) || (mode == TDS_MODE_SBM2)
857 845 || (mode == TDS_MODE_LFM))
858 846 {
859 847 status = LFR_SUCCESSFUL;
860 848 }
861 849 else
862 850 {
863 851 status = LFR_DEFAULT;
864 852 }
865 853
866 854 return status;
867 855 }
868 856
869 857 unsigned int check_update_info_hk_thr_mode( unsigned char mode )
870 858 {
871 859 unsigned int status;
872 860
873 861 if ( (mode == THR_MODE_STANDBY) || (mode == THR_MODE_NORMAL)
874 862 || (mode == THR_MODE_BURST))
875 863 {
876 864 status = LFR_SUCCESSFUL;
877 865 }
878 866 else
879 867 {
880 868 status = LFR_DEFAULT;
881 869 }
882 870
883 871 return status;
884 872 }
885 873
874 //***********
875 // FBINS MASK
876
877 int set_sy_lfr_fbins( ccsdsTelecommandPacket_t *TC )
878 {
879 int status;
880 unsigned int k;
881 unsigned char *fbins_mask_dump;
882 unsigned char *fbins_mask_TC;
883
884 status = LFR_SUCCESSFUL;
885
886 fbins_mask_dump = parameter_dump_packet.sy_lfr_fbins_f0_word1;
887 fbins_mask_TC = TC->dataAndCRC;
888
889 for (k=0; k < NB_FBINS_MASKS * NB_BYTES_PER_FBINS_MASK; k++)
890 {
891 fbins_mask_dump[k] = fbins_mask_TC[k];
892 }
893 for (k=0; k < NB_FBINS_MASKS; k++)
894 {
895 unsigned char *auxPtr;
896 auxPtr = &parameter_dump_packet.sy_lfr_fbins_f0_word1[k*NB_BYTES_PER_FBINS_MASK];
897 printf("%x %x %x %x\n", auxPtr[0], auxPtr[1], auxPtr[2], auxPtr[3]);
898 }
899
900
901 return status;
902 }
903
886 904 //**************
887 905 // KCOEFFICIENTS
888 906 int set_sy_lfr_kcoeff( ccsdsTelecommandPacket_t *TC )
889 907 {
890 908 unsigned int i;
891 909 unsigned short sy_lfr_kcoeff_frequency;
892 910 unsigned short bin;
893 911 unsigned short *freqPtr;
894 912 float *kcoeffPtr_norm;
895 913 float *kcoeffPtr_sbm;
896 914 int status;
897 915 unsigned char *kcoeffLoadPtr;
898 916 unsigned char *kcoeffNormPtr;
899 917
900 918 status = LFR_SUCCESSFUL;
901 919
902 920 kcoeffPtr_norm = NULL;
903 921 kcoeffPtr_sbm = NULL;
904 922 bin = 0;
905 923
906 924 freqPtr = (unsigned short *) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY];
907 925 sy_lfr_kcoeff_frequency = *freqPtr;
908 926
909 927 if ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM )
910 928 {
911 929 PRINTF1("ERR *** in set_sy_lfr_kcoeff_frequency *** sy_lfr_kcoeff_frequency = %d\n", sy_lfr_kcoeff_frequency)
912 930 }
913 931 else
914 932 {
915 933 if ( ( sy_lfr_kcoeff_frequency >= 0 )
916 934 && ( sy_lfr_kcoeff_frequency < NB_BINS_COMPRESSED_SM_F0 ) )
917 935 {
918 936 kcoeffPtr_norm = k_coeff_intercalib_f0_norm;
919 937 kcoeffPtr_sbm = k_coeff_intercalib_f0_sbm;
920 938 bin = sy_lfr_kcoeff_frequency;
921 939 }
922 940 else if ( ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM_F0 )
923 941 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) ) )
924 942 {
925 943 kcoeffPtr_norm = k_coeff_intercalib_f1_norm;
926 944 kcoeffPtr_sbm = k_coeff_intercalib_f1_sbm;
927 945 bin = sy_lfr_kcoeff_frequency - NB_BINS_COMPRESSED_SM_F0;
928 946 }
929 947 else if ( ( sy_lfr_kcoeff_frequency >= (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) )
930 948 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 + NB_BINS_COMPRESSED_SM_F2) ) )
931 949 {
932 950 kcoeffPtr_norm = k_coeff_intercalib_f2;
933 951 kcoeffPtr_sbm = NULL;
934 952 bin = sy_lfr_kcoeff_frequency - (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1);
935 953 }
936 954 }
937 955
938 956 if (kcoeffPtr_norm != NULL )
939 957 {
940 958 printf("freq = %d, bin = %d\n", sy_lfr_kcoeff_frequency, bin);
941 959 for (i=0; i<NB_K_COEFF_PER_BIN; i++)
942 960 {
943 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + NB_BYTES_PER_FLOAT * i];
961 // destination
944 962 kcoeffNormPtr = (unsigned char*) &kcoeffPtr_norm[ (bin * NB_K_COEFF_PER_BIN) + i ];
945 kcoeffNormPtr[0] = kcoeffLoadPtr[0];
946 kcoeffNormPtr[1] = kcoeffLoadPtr[1];
947 kcoeffNormPtr[2] = kcoeffLoadPtr[2];
948 kcoeffNormPtr[3] = kcoeffLoadPtr[3];
949 printf("kcoeffPtr: %x %x %x %x *** %f \n",
950 kcoeffLoadPtr[0],
951 kcoeffLoadPtr[1],
952 kcoeffLoadPtr[2],
953 kcoeffLoadPtr[3],
954 kcoeffPtr_norm[ (bin * NB_K_COEFF_PER_BIN) + i ]);
963 // source
964 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + NB_BYTES_PER_FLOAT * i];
965 copyFloatByChar( kcoeffNormPtr, kcoeffLoadPtr );
955 966 }
956 967 }
957 968
958 969 return status;
959 970 }
960 971
972 void copyFloatByChar( unsigned char *destination, unsigned char *source )
973 {
974 destination[0] = source[0];
975 destination[1] = source[1];
976 destination[2] = source[2];
977 destination[3] = source[3];
978 }
979
961 980 //**********
962 981 // init dump
963 982
964 983 void init_parameter_dump( void )
965 984 {
966 985 /** This function initialize the parameter_dump_packet global variable with default values.
967 986 *
968 987 */
969 988
989 unsigned int k;
990
970 991 parameter_dump_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
971 992 parameter_dump_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
972 993 parameter_dump_packet.reserved = CCSDS_RESERVED;
973 994 parameter_dump_packet.userApplication = CCSDS_USER_APP;
974 995 parameter_dump_packet.packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> 8);
975 996 parameter_dump_packet.packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;
976 997 parameter_dump_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
977 998 parameter_dump_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
978 999 parameter_dump_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_PARAMETER_DUMP >> 8);
979 1000 parameter_dump_packet.packetLength[1] = (unsigned char) PACKET_LENGTH_PARAMETER_DUMP;
980 1001 // DATA FIELD HEADER
981 1002 parameter_dump_packet.spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
982 1003 parameter_dump_packet.serviceType = TM_TYPE_PARAMETER_DUMP;
983 1004 parameter_dump_packet.serviceSubType = TM_SUBTYPE_PARAMETER_DUMP;
984 1005 parameter_dump_packet.destinationID = TM_DESTINATION_ID_GROUND;
985 1006 parameter_dump_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
986 1007 parameter_dump_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
987 1008 parameter_dump_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
988 1009 parameter_dump_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
989 1010 parameter_dump_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
990 1011 parameter_dump_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
991 1012 parameter_dump_packet.sid = SID_PARAMETER_DUMP;
992 1013
993 1014 //******************
994 1015 // COMMON PARAMETERS
995 parameter_dump_packet.unused0 = DEFAULT_SY_LFR_COMMON0;
1016 parameter_dump_packet.sy_lfr_common_parameters_spare = DEFAULT_SY_LFR_COMMON0;
996 1017 parameter_dump_packet.sy_lfr_common_parameters = DEFAULT_SY_LFR_COMMON1;
997 1018
998 1019 //******************
999 1020 // NORMAL PARAMETERS
1000 1021 parameter_dump_packet.sy_lfr_n_swf_l[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_L >> 8);
1001 1022 parameter_dump_packet.sy_lfr_n_swf_l[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_L );
1002 1023 parameter_dump_packet.sy_lfr_n_swf_p[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_P >> 8);
1003 1024 parameter_dump_packet.sy_lfr_n_swf_p[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_P );
1004 1025 parameter_dump_packet.sy_lfr_n_asm_p[0] = (unsigned char) (DFLT_SY_LFR_N_ASM_P >> 8);
1005 1026 parameter_dump_packet.sy_lfr_n_asm_p[1] = (unsigned char) (DFLT_SY_LFR_N_ASM_P );
1006 1027 parameter_dump_packet.sy_lfr_n_bp_p0 = (unsigned char) DFLT_SY_LFR_N_BP_P0;
1007 1028 parameter_dump_packet.sy_lfr_n_bp_p1 = (unsigned char) DFLT_SY_LFR_N_BP_P1;
1008 1029 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = (unsigned char) DFLT_SY_LFR_N_CWF_LONG_F3;
1009 1030
1010 1031 //*****************
1011 1032 // BURST PARAMETERS
1012 1033 parameter_dump_packet.sy_lfr_b_bp_p0 = (unsigned char) DEFAULT_SY_LFR_B_BP_P0;
1013 1034 parameter_dump_packet.sy_lfr_b_bp_p1 = (unsigned char) DEFAULT_SY_LFR_B_BP_P1;
1014 1035
1015 1036 //****************
1016 1037 // SBM1 PARAMETERS
1017 1038 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
1018 1039 parameter_dump_packet.sy_lfr_s1_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S1_BP_P1;
1019 1040
1020 1041 //****************
1021 1042 // SBM2 PARAMETERS
1022 1043 parameter_dump_packet.sy_lfr_s2_bp_p0 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P0;
1023 1044 parameter_dump_packet.sy_lfr_s2_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P1;
1045
1046 //************
1047 // FBINS MASKS
1048 for (k=0; k < NB_FBINS_MASKS * NB_BYTES_PER_FBINS_MASK; k++)
1049 {
1050 parameter_dump_packet.sy_lfr_fbins_f0_word1[k] = 0xff;
1051 }
1024 1052 }
1025 1053
1026 1054 void init_kcoefficients_dump( void )
1027 1055 {
1028 1056 init_kcoefficients_dump_packet( &kcoefficients_dump_1, 1, 30 );
1029 1057 init_kcoefficients_dump_packet( &kcoefficients_dump_2, 2, 6 );
1030 1058
1031 1059 kcoefficient_node_1.previous = NULL;
1032 1060 kcoefficient_node_1.next = NULL;
1033 1061 kcoefficient_node_1.sid = TM_CODE_K_DUMP;
1034 1062 kcoefficient_node_1.coarseTime = 0x00;
1035 1063 kcoefficient_node_1.fineTime = 0x00;
1036 1064 kcoefficient_node_1.buffer_address = (int) &kcoefficients_dump_1;
1037 1065 kcoefficient_node_1.status = 0x00;
1038 1066
1039 1067 kcoefficient_node_2.previous = NULL;
1040 1068 kcoefficient_node_2.next = NULL;
1041 1069 kcoefficient_node_2.sid = TM_CODE_K_DUMP;
1042 1070 kcoefficient_node_2.coarseTime = 0x00;
1043 1071 kcoefficient_node_2.fineTime = 0x00;
1044 1072 kcoefficient_node_2.buffer_address = (int) &kcoefficients_dump_2;
1045 1073 kcoefficient_node_2.status = 0x00;
1046 1074 }
1047 1075
1048 1076 void init_kcoefficients_dump_packet( Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump, unsigned char pkt_nr, unsigned char blk_nr )
1049 1077 {
1050 1078 unsigned int k;
1051 1079 unsigned int packetLength;
1052 1080
1053 1081 packetLength = blk_nr * 130 + 20 - CCSDS_TC_TM_PACKET_OFFSET; // 4 bytes for the CCSDS header
1054 1082
1055 1083 kcoefficients_dump->targetLogicalAddress = CCSDS_DESTINATION_ID;
1056 1084 kcoefficients_dump->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1057 1085 kcoefficients_dump->reserved = CCSDS_RESERVED;
1058 1086 kcoefficients_dump->userApplication = CCSDS_USER_APP;
1059 1087 kcoefficients_dump->packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> 8);;
1060 1088 kcoefficients_dump->packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;;
1061 1089 kcoefficients_dump->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1062 1090 kcoefficients_dump->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1063 1091 kcoefficients_dump->packetLength[0] = (unsigned char) (packetLength >> 8);
1064 1092 kcoefficients_dump->packetLength[1] = (unsigned char) packetLength;
1065 1093 // DATA FIELD HEADER
1066 1094 kcoefficients_dump->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1067 1095 kcoefficients_dump->serviceType = TM_TYPE_K_DUMP;
1068 1096 kcoefficients_dump->serviceSubType = TM_SUBTYPE_K_DUMP;
1069 1097 kcoefficients_dump->destinationID= TM_DESTINATION_ID_GROUND;
1070 1098 kcoefficients_dump->time[0] = 0x00;
1071 1099 kcoefficients_dump->time[1] = 0x00;
1072 1100 kcoefficients_dump->time[2] = 0x00;
1073 1101 kcoefficients_dump->time[3] = 0x00;
1074 1102 kcoefficients_dump->time[4] = 0x00;
1075 1103 kcoefficients_dump->time[5] = 0x00;
1076 1104 kcoefficients_dump->sid = SID_K_DUMP;
1077 1105
1078 1106 kcoefficients_dump->pkt_cnt = 2;
1079 1107 kcoefficients_dump->pkt_nr = pkt_nr;
1080 1108 kcoefficients_dump->blk_nr = blk_nr;
1081 1109
1082 1110 //******************
1083 1111 // SOURCE DATA repeated N times with N in [0 .. PA_LFR_KCOEFF_BLK_NR]
1084 1112 // one blk is 2 + 4 * 32 = 130 bytes, 30 blks max in one packet (30 * 130 = 3900)
1085 1113 for (k=0; k<3900; k++)
1086 1114 {
1087 1115 kcoefficients_dump->kcoeff_blks[k] = 0x00;
1088 1116 }
1089 1117 }
1090 1118
1091 1119
1092 1120
Show file before | Show file after | Hide comments
@@ -1,1402 +1,1400
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 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 + ( (data_shaping & 0x01) << 4 ); // R1
1225
1226 // this is a temporary way to set R2, compatible with the release 2 of the flight software
1227 waveform_picker_regs->data_shaping = waveform_picker_regs->data_shaping + ( (0x1) << 5 ); // R2
1224 + ( (data_shaping & 0x01) << 4 ) // R1
1225 + ( (data_shaping & 0x01) << 5 ); // R2
1228 1226 }
1229 1227
1230 1228 void set_wfp_burst_enable_register( unsigned char mode )
1231 1229 {
1232 1230 /** This function sets the waveform picker burst_enable register depending on the mode.
1233 1231 *
1234 1232 * @param mode is the LFR mode to launch.
1235 1233 *
1236 1234 * The burst bits shall be before the enable bits.
1237 1235 *
1238 1236 */
1239 1237
1240 1238 // [0000 0000] burst f2, f1, f0 enable f3 f2 f1 f0
1241 1239 // the burst bits shall be set first, before the enable bits
1242 1240 switch(mode) {
1243 1241 case(LFR_MODE_NORMAL):
1244 1242 waveform_picker_regs->run_burst_enable = 0x00; // [0000 0000] no burst enable
1245 1243 waveform_picker_regs->run_burst_enable = 0x0f; // [0000 1111] enable f3 f2 f1 f0
1246 1244 break;
1247 1245 case(LFR_MODE_BURST):
1248 1246 waveform_picker_regs->run_burst_enable = 0x40; // [0100 0000] f2 burst enabled
1249 1247 // waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x04; // [0100] enable f2
1250 1248 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x0c; // [1100] enable f3 AND f2
1251 1249 break;
1252 1250 case(LFR_MODE_SBM1):
1253 1251 waveform_picker_regs->run_burst_enable = 0x20; // [0010 0000] f1 burst enabled
1254 1252 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x0f; // [1111] enable f3 f2 f1 f0
1255 1253 break;
1256 1254 case(LFR_MODE_SBM2):
1257 1255 waveform_picker_regs->run_burst_enable = 0x40; // [0100 0000] f2 burst enabled
1258 1256 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | 0x0f; // [1111] enable f3 f2 f1 f0
1259 1257 break;
1260 1258 default:
1261 1259 waveform_picker_regs->run_burst_enable = 0x00; // [0000 0000] no burst enabled, no waveform enabled
1262 1260 break;
1263 1261 }
1264 1262 }
1265 1263
1266 1264 void set_wfp_delta_snapshot( void )
1267 1265 {
1268 1266 /** This function sets the delta_snapshot register of the waveform picker module.
1269 1267 *
1270 1268 * The value is read from two (unsigned char) of the parameter_dump_packet structure:
1271 1269 * - sy_lfr_n_swf_p[0]
1272 1270 * - sy_lfr_n_swf_p[1]
1273 1271 *
1274 1272 */
1275 1273
1276 1274 unsigned int delta_snapshot;
1277 1275 unsigned int delta_snapshot_in_T2;
1278 1276
1279 1277 delta_snapshot = parameter_dump_packet.sy_lfr_n_swf_p[0]*256
1280 1278 + parameter_dump_packet.sy_lfr_n_swf_p[1];
1281 1279
1282 1280 delta_snapshot_in_T2 = delta_snapshot * 256;
1283 1281 waveform_picker_regs->delta_snapshot = delta_snapshot_in_T2 - 1; // max 4 bytes
1284 1282 }
1285 1283
1286 1284 void set_wfp_delta_f0_f0_2( void )
1287 1285 {
1288 1286 unsigned int delta_snapshot;
1289 1287 unsigned int nb_samples_per_snapshot;
1290 1288 float delta_f0_in_float;
1291 1289
1292 1290 delta_snapshot = waveform_picker_regs->delta_snapshot;
1293 1291 nb_samples_per_snapshot = parameter_dump_packet.sy_lfr_n_swf_l[0] * 256 + parameter_dump_packet.sy_lfr_n_swf_l[1];
1294 1292 delta_f0_in_float =nb_samples_per_snapshot / 2. * ( 1. / 256. - 1. / 24576.) * 256.;
1295 1293
1296 1294 waveform_picker_regs->delta_f0 = delta_snapshot - floor( delta_f0_in_float );
1297 1295 waveform_picker_regs->delta_f0_2 = 0x30; // 48 = 11 0000, max 7 bits
1298 1296 }
1299 1297
1300 1298 void set_wfp_delta_f1( void )
1301 1299 {
1302 1300 unsigned int delta_snapshot;
1303 1301 unsigned int nb_samples_per_snapshot;
1304 1302 float delta_f1_in_float;
1305 1303
1306 1304 delta_snapshot = waveform_picker_regs->delta_snapshot;
1307 1305 nb_samples_per_snapshot = parameter_dump_packet.sy_lfr_n_swf_l[0] * 256 + parameter_dump_packet.sy_lfr_n_swf_l[1];
1308 1306 delta_f1_in_float = nb_samples_per_snapshot / 2. * ( 1. / 256. - 1. / 4096.) * 256.;
1309 1307
1310 1308 waveform_picker_regs->delta_f1 = delta_snapshot - floor( delta_f1_in_float );
1311 1309 }
1312 1310
1313 1311 void set_wfp_delta_f2()
1314 1312 {
1315 1313 unsigned int delta_snapshot;
1316 1314 unsigned int nb_samples_per_snapshot;
1317 1315
1318 1316 delta_snapshot = waveform_picker_regs->delta_snapshot;
1319 1317 nb_samples_per_snapshot = parameter_dump_packet.sy_lfr_n_swf_l[0] * 256 + parameter_dump_packet.sy_lfr_n_swf_l[1];
1320 1318
1321 1319 waveform_picker_regs->delta_f2 = delta_snapshot - nb_samples_per_snapshot / 2;
1322 1320 }
1323 1321
1324 1322 //*****************
1325 1323 // local parameters
1326 1324
1327 1325 void increment_seq_counter_source_id( unsigned char *packet_sequence_control, unsigned int sid )
1328 1326 {
1329 1327 /** This function increments the parameter "sequence_cnt" depending on the sid passed in argument.
1330 1328 *
1331 1329 * @param packet_sequence_control is a pointer toward the parameter sequence_cnt to update.
1332 1330 * @param sid is the source identifier of the packet being updated.
1333 1331 *
1334 1332 * REQ-LFR-SRS-5240 / SSS-CP-FS-590
1335 1333 * The sequence counters shall wrap around from 2^14 to zero.
1336 1334 * The sequence counter shall start at zero at startup.
1337 1335 *
1338 1336 * REQ-LFR-SRS-5239 / SSS-CP-FS-580
1339 1337 * All TM_LFR_SCIENCE_ packets are sent to ground, i.e. destination id = 0
1340 1338 *
1341 1339 */
1342 1340
1343 1341 unsigned short *sequence_cnt;
1344 1342 unsigned short segmentation_grouping_flag;
1345 1343 unsigned short new_packet_sequence_control;
1346 1344 rtems_mode initial_mode_set;
1347 1345 rtems_mode current_mode_set;
1348 1346 rtems_status_code status;
1349 1347
1350 1348 //******************************************
1351 1349 // CHANGE THE MODE OF THE CALLING RTEMS TASK
1352 1350 status = rtems_task_mode( RTEMS_NO_PREEMPT, RTEMS_PREEMPT_MASK, &initial_mode_set );
1353 1351
1354 1352 if ( (sid == SID_NORM_SWF_F0) || (sid == SID_NORM_SWF_F1) || (sid == SID_NORM_SWF_F2)
1355 1353 || (sid == SID_NORM_CWF_F3) || (sid == SID_NORM_CWF_LONG_F3)
1356 1354 || (sid == SID_BURST_CWF_F2)
1357 1355 || (sid == SID_NORM_ASM_F0) || (sid == SID_NORM_ASM_F1) || (sid == SID_NORM_ASM_F2)
1358 1356 || (sid == SID_NORM_BP1_F0) || (sid == SID_NORM_BP1_F1) || (sid == SID_NORM_BP1_F2)
1359 1357 || (sid == SID_NORM_BP2_F0) || (sid == SID_NORM_BP2_F1) || (sid == SID_NORM_BP2_F2)
1360 1358 || (sid == SID_BURST_BP1_F0) || (sid == SID_BURST_BP2_F0)
1361 1359 || (sid == SID_BURST_BP1_F1) || (sid == SID_BURST_BP2_F1) )
1362 1360 {
1363 1361 sequence_cnt = (unsigned short *) &sequenceCounters_SCIENCE_NORMAL_BURST;
1364 1362 }
1365 1363 else if ( (sid ==SID_SBM1_CWF_F1) || (sid ==SID_SBM2_CWF_F2)
1366 1364 || (sid == SID_SBM1_BP1_F0) || (sid == SID_SBM1_BP2_F0)
1367 1365 || (sid == SID_SBM2_BP1_F0) || (sid == SID_SBM2_BP2_F0)
1368 1366 || (sid == SID_SBM2_BP1_F1) || (sid == SID_SBM2_BP2_F1) )
1369 1367 {
1370 1368 sequence_cnt = (unsigned short *) &sequenceCounters_SCIENCE_SBM1_SBM2;
1371 1369 }
1372 1370 else
1373 1371 {
1374 1372 sequence_cnt = (unsigned short *) NULL;
1375 1373 PRINTF1("in increment_seq_counter_source_id *** ERR apid_destid %d not known\n", sid)
1376 1374 }
1377 1375
1378 1376 if (sequence_cnt != NULL)
1379 1377 {
1380 1378 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
1381 1379 *sequence_cnt = (*sequence_cnt) & 0x3fff;
1382 1380
1383 1381 new_packet_sequence_control = segmentation_grouping_flag | (*sequence_cnt) ;
1384 1382
1385 1383 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> 8);
1386 1384 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
1387 1385
1388 1386 // increment the sequence counter
1389 1387 if ( *sequence_cnt < SEQ_CNT_MAX)
1390 1388 {
1391 1389 *sequence_cnt = *sequence_cnt + 1;
1392 1390 }
1393 1391 else
1394 1392 {
1395 1393 *sequence_cnt = 0;
1396 1394 }
1397 1395 }
1398 1396
1399 1397 //***********************************
1400 1398 // RESET THE MODE OF THE CALLING TASK
1401 1399 status = rtems_task_mode( initial_mode_set, RTEMS_PREEMPT_MASK, &current_mode_set );
1402 1400 }
General Comments 0
You need to be logged in to leave comments. Login now