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

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

minor updates
paul -
r285:bc5036e4a86c R3_plus draft
parent child
Show More
Show file before | Show file after | Hide comments
@@ -1,82 +1,82
1 1 #ifndef TC_LOAD_DUMP_PARAMETERS_H
2 2 #define TC_LOAD_DUMP_PARAMETERS_H
3 3
4 4 #include <rtems.h>
5 5 #include <stdio.h>
6 6
7 7 #include "fsw_params.h"
8 8 #include "wf_handler.h"
9 9 #include "tm_lfr_tc_exe.h"
10 10 #include "fsw_misc.h"
11 11 #include "basic_parameters_params.h"
12 12 #include "avf0_prc0.h"
13 13
14 14 #define FLOAT_EQUAL_ZERO 0.001
15 15
16 16 extern unsigned short sequenceCounterParameterDump;
17 17 extern unsigned short sequenceCounters_TM_DUMP[];
18 18 extern float k_coeff_intercalib_f0_norm[ ];
19 19 extern float k_coeff_intercalib_f0_sbm[ ];
20 20 extern float k_coeff_intercalib_f1_norm[ ];
21 21 extern float k_coeff_intercalib_f1_sbm[ ];
22 22 extern float k_coeff_intercalib_f2[ ];
23 23 extern unsigned char rw_fbins_mask_f0[16];
24 24 extern unsigned char rw_fbins_mask_f1[16];
25 25 extern unsigned char rw_fbins_mask_f2[16];
26 26
27 27 int action_load_common_par( ccsdsTelecommandPacket_t *TC );
28 28 int action_load_normal_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
29 29 int action_load_burst_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
30 30 int action_load_sbm1_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
31 31 int action_load_sbm2_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
32 32 int action_load_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
33 33 int action_load_fbins_mask(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
34 34 int action_load_filter_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
35 35 int action_dump_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
36 36 int action_dump_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id );
37 37
38 38 // NORMAL
39 39 int check_normal_par_consistency( ccsdsTelecommandPacket_t *TC, rtems_id queue_id );
40 40 int set_sy_lfr_n_swf_l( ccsdsTelecommandPacket_t *TC );
41 41 int set_sy_lfr_n_swf_p( ccsdsTelecommandPacket_t *TC );
42 42 int set_sy_lfr_n_asm_p( ccsdsTelecommandPacket_t *TC );
43 43 int set_sy_lfr_n_bp_p0( ccsdsTelecommandPacket_t *TC );
44 44 int set_sy_lfr_n_bp_p1( ccsdsTelecommandPacket_t *TC );
45 45 int set_sy_lfr_n_cwf_long_f3( ccsdsTelecommandPacket_t *TC );
46 46
47 47 // BURST
48 48 int set_sy_lfr_b_bp_p0( ccsdsTelecommandPacket_t *TC );
49 49 int set_sy_lfr_b_bp_p1( ccsdsTelecommandPacket_t *TC );
50 50
51 51 // SBM1
52 52 int set_sy_lfr_s1_bp_p0( ccsdsTelecommandPacket_t *TC );
53 53 int set_sy_lfr_s1_bp_p1( ccsdsTelecommandPacket_t *TC );
54 54
55 55 // SBM2
56 56 int set_sy_lfr_s2_bp_p0( ccsdsTelecommandPacket_t *TC );
57 57 int set_sy_lfr_s2_bp_p1( ccsdsTelecommandPacket_t *TC );
58 58
59 59 // TC_LFR_UPDATE_INFO
60 60 unsigned int check_update_info_hk_lfr_mode( unsigned char mode );
61 61 unsigned int check_update_info_hk_tds_mode( unsigned char mode );
62 62 unsigned int check_update_info_hk_thr_mode( unsigned char mode );
63 63 void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC );
64 64 void build_rw_fbins_mask( unsigned int channel );
65 65 void build_rw_fbins_masks();
66 66
67 67 // FBINS_MASK
68 68 int set_sy_lfr_fbins( ccsdsTelecommandPacket_t *TC );
69 69
70 70 // TC_LFR_LOAD_PARS_FILTER_PAR
71 int check_sy_lfr_pas_filter_parameters( ccsdsTelecommandPacket_t *TC, rtems_id queue_id );
71 int check_sy_lfr_filter_parameters( ccsdsTelecommandPacket_t *TC, rtems_id queue_id );
72 72
73 73 // KCOEFFICIENTS
74 74 int set_sy_lfr_kcoeff(ccsdsTelecommandPacket_t *TC , rtems_id queue_id);
75 75 void copyFloatByChar( unsigned char *destination, unsigned char *source );
76 76
77 77 void init_parameter_dump( void );
78 78 void init_kcoefficients_dump( void );
79 79 void init_kcoefficients_dump_packet( Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump, unsigned char pkt_nr, unsigned char blk_nr );
80 80 void increment_seq_counter_destination_id_dump( unsigned char *packet_sequence_control, unsigned char destination_id );
81 81
82 82 #endif // TC_LOAD_DUMP_PARAMETERS_H
Show file before | Show file after | Hide comments
@@ -1,1641 +1,1641
1 1 /** Functions and tasks related to TeleCommand handling.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle TeleCommands:\n
7 7 * action launching\n
8 8 * TC parsing\n
9 9 * ...
10 10 *
11 11 */
12 12
13 13 #include "tc_handler.h"
14 14 #include "math.h"
15 15
16 16 //***********
17 17 // RTEMS TASK
18 18
19 19 rtems_task actn_task( rtems_task_argument unused )
20 20 {
21 21 /** This RTEMS task is responsible for launching actions upton the reception of valid TeleCommands.
22 22 *
23 23 * @param unused is the starting argument of the RTEMS task
24 24 *
25 25 * The ACTN task waits for data coming from an RTEMS msesage queue. When data arrives, it launches specific actions depending
26 26 * on the incoming TeleCommand.
27 27 *
28 28 */
29 29
30 30 int result;
31 31 rtems_status_code status; // RTEMS status code
32 32 ccsdsTelecommandPacket_t TC; // TC sent to the ACTN task
33 33 size_t size; // size of the incoming TC packet
34 34 unsigned char subtype; // subtype of the current TC packet
35 35 unsigned char time[6];
36 36 rtems_id queue_rcv_id;
37 37 rtems_id queue_snd_id;
38 38
39 39 status = get_message_queue_id_recv( &queue_rcv_id );
40 40 if (status != RTEMS_SUCCESSFUL)
41 41 {
42 42 PRINTF1("in ACTN *** ERR get_message_queue_id_recv %d\n", status)
43 43 }
44 44
45 45 status = get_message_queue_id_send( &queue_snd_id );
46 46 if (status != RTEMS_SUCCESSFUL)
47 47 {
48 48 PRINTF1("in ACTN *** ERR get_message_queue_id_send %d\n", status)
49 49 }
50 50
51 51 result = LFR_SUCCESSFUL;
52 52 subtype = 0; // subtype of the current TC packet
53 53
54 BOOT_PRINTF("in ACTN *** \n")
54 BOOT_PRINTF("in ACTN *** \n");
55 55
56 56 while(1)
57 57 {
58 58 status = rtems_message_queue_receive( queue_rcv_id, (char*) &TC, &size,
59 59 RTEMS_WAIT, RTEMS_NO_TIMEOUT);
60 60 getTime( time ); // set time to the current time
61 61 if (status!=RTEMS_SUCCESSFUL)
62 62 {
63 63 PRINTF1("ERR *** in task ACTN *** error receiving a message, code %d \n", status)
64 64 }
65 65 else
66 66 {
67 67 subtype = TC.serviceSubType;
68 68 switch(subtype)
69 69 {
70 70 case TC_SUBTYPE_RESET:
71 71 result = action_reset( &TC, queue_snd_id, time );
72 72 close_action( &TC, result, queue_snd_id );
73 73 break;
74 74 case TC_SUBTYPE_LOAD_COMM:
75 75 result = action_load_common_par( &TC );
76 76 close_action( &TC, result, queue_snd_id );
77 77 break;
78 78 case TC_SUBTYPE_LOAD_NORM:
79 79 result = action_load_normal_par( &TC, queue_snd_id, time );
80 80 close_action( &TC, result, queue_snd_id );
81 81 break;
82 82 case TC_SUBTYPE_LOAD_BURST:
83 83 result = action_load_burst_par( &TC, queue_snd_id, time );
84 84 close_action( &TC, result, queue_snd_id );
85 85 break;
86 86 case TC_SUBTYPE_LOAD_SBM1:
87 87 result = action_load_sbm1_par( &TC, queue_snd_id, time );
88 88 close_action( &TC, result, queue_snd_id );
89 89 break;
90 90 case TC_SUBTYPE_LOAD_SBM2:
91 91 result = action_load_sbm2_par( &TC, queue_snd_id, time );
92 92 close_action( &TC, result, queue_snd_id );
93 93 break;
94 94 case TC_SUBTYPE_DUMP:
95 95 result = action_dump_par( &TC, queue_snd_id );
96 96 close_action( &TC, result, queue_snd_id );
97 97 break;
98 98 case TC_SUBTYPE_ENTER:
99 99 result = action_enter_mode( &TC, queue_snd_id );
100 100 close_action( &TC, result, queue_snd_id );
101 101 break;
102 102 case TC_SUBTYPE_UPDT_INFO:
103 103 result = action_update_info( &TC, queue_snd_id );
104 104 close_action( &TC, result, queue_snd_id );
105 105 break;
106 106 case TC_SUBTYPE_EN_CAL:
107 107 result = action_enable_calibration( &TC, queue_snd_id, time );
108 108 close_action( &TC, result, queue_snd_id );
109 109 break;
110 110 case TC_SUBTYPE_DIS_CAL:
111 111 result = action_disable_calibration( &TC, queue_snd_id, time );
112 112 close_action( &TC, result, queue_snd_id );
113 113 break;
114 114 case TC_SUBTYPE_LOAD_K:
115 115 result = action_load_kcoefficients( &TC, queue_snd_id, time );
116 116 close_action( &TC, result, queue_snd_id );
117 117 break;
118 118 case TC_SUBTYPE_DUMP_K:
119 119 result = action_dump_kcoefficients( &TC, queue_snd_id, time );
120 120 close_action( &TC, result, queue_snd_id );
121 121 break;
122 122 case TC_SUBTYPE_LOAD_FBINS:
123 123 result = action_load_fbins_mask( &TC, queue_snd_id, time );
124 124 close_action( &TC, result, queue_snd_id );
125 125 break;
126 126 case TC_SUBTYPE_LOAD_FILTER_PAR:
127 127 result = action_load_filter_par( &TC, queue_snd_id, time );
128 128 close_action( &TC, result, queue_snd_id );
129 129 break;
130 130 case TC_SUBTYPE_UPDT_TIME:
131 131 result = action_update_time( &TC );
132 132 close_action( &TC, result, queue_snd_id );
133 133 break;
134 134 default:
135 135 break;
136 136 }
137 137 }
138 138 }
139 139 }
140 140
141 141 //***********
142 142 // TC ACTIONS
143 143
144 144 int action_reset(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
145 145 {
146 146 /** This function executes specific actions when a TC_LFR_RESET TeleCommand has been received.
147 147 *
148 148 * @param TC points to the TeleCommand packet that is being processed
149 149 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
150 150 *
151 151 */
152 152
153 153 PRINTF("this is the end!!!\n");
154 154 exit(0);
155 155
156 156 send_tm_lfr_tc_exe_not_implemented( TC, queue_id, time );
157 157
158 158 return LFR_DEFAULT;
159 159 }
160 160
161 161 int action_enter_mode(ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
162 162 {
163 163 /** This function executes specific actions when a TC_LFR_ENTER_MODE TeleCommand has been received.
164 164 *
165 165 * @param TC points to the TeleCommand packet that is being processed
166 166 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
167 167 *
168 168 */
169 169
170 170 rtems_status_code status;
171 171 unsigned char requestedMode;
172 172 unsigned int *transitionCoarseTime_ptr;
173 173 unsigned int transitionCoarseTime;
174 174 unsigned char * bytePosPtr;
175 175
176 176 bytePosPtr = (unsigned char *) &TC->packetID;
177 177
178 178 requestedMode = bytePosPtr[ BYTE_POS_CP_MODE_LFR_SET ];
179 179 transitionCoarseTime_ptr = (unsigned int *) ( &bytePosPtr[ BYTE_POS_CP_LFR_ENTER_MODE_TIME ] );
180 180 transitionCoarseTime = (*transitionCoarseTime_ptr) & 0x7fffffff;
181 181
182 182 status = check_mode_value( requestedMode );
183 183
184 184 if ( status != LFR_SUCCESSFUL ) // the mode value is inconsistent
185 185 {
186 186 send_tm_lfr_tc_exe_inconsistent( TC, queue_id, BYTE_POS_CP_MODE_LFR_SET, requestedMode );
187 187 }
188 188
189 189 else // the mode value is valid, check the transition
190 190 {
191 191 status = check_mode_transition(requestedMode);
192 192 if (status != LFR_SUCCESSFUL)
193 193 {
194 194 PRINTF("ERR *** in action_enter_mode *** check_mode_transition\n")
195 195 send_tm_lfr_tc_exe_not_executable( TC, queue_id );
196 196 }
197 197 }
198 198
199 199 if ( status == LFR_SUCCESSFUL ) // the transition is valid, check the date
200 200 {
201 201 status = check_transition_date( transitionCoarseTime );
202 202 if (status != LFR_SUCCESSFUL)
203 203 {
204 204 PRINTF("ERR *** in action_enter_mode *** check_transition_date\n");
205 205 send_tm_lfr_tc_exe_not_executable(TC, queue_id );
206 206 }
207 207 }
208 208
209 209 if ( status == LFR_SUCCESSFUL ) // the date is valid, enter the mode
210 210 {
211 211 PRINTF1("OK *** in action_enter_mode *** enter mode %d\n", requestedMode);
212 212
213 213 switch(requestedMode)
214 214 {
215 215 case LFR_MODE_STANDBY:
216 216 status = enter_mode_standby();
217 217 break;
218 218 case LFR_MODE_NORMAL:
219 219 status = enter_mode_normal( transitionCoarseTime );
220 220 break;
221 221 case LFR_MODE_BURST:
222 222 status = enter_mode_burst( transitionCoarseTime );
223 223 break;
224 224 case LFR_MODE_SBM1:
225 225 status = enter_mode_sbm1( transitionCoarseTime );
226 226 break;
227 227 case LFR_MODE_SBM2:
228 228 status = enter_mode_sbm2( transitionCoarseTime );
229 229 break;
230 230 default:
231 231 break;
232 232 }
233 233
234 234 if (status != RTEMS_SUCCESSFUL)
235 235 {
236 236 status = LFR_EXE_ERROR;
237 237 }
238 238 }
239 239
240 240 return status;
241 241 }
242 242
243 243 int action_update_info(ccsdsTelecommandPacket_t *TC, rtems_id queue_id)
244 244 {
245 245 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
246 246 *
247 247 * @param TC points to the TeleCommand packet that is being processed
248 248 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
249 249 *
250 250 * @return LFR directive status code:
251 251 * - LFR_DEFAULT
252 252 * - LFR_SUCCESSFUL
253 253 *
254 254 */
255 255
256 256 unsigned int val;
257 257 int result;
258 258 unsigned int status;
259 259 unsigned char mode;
260 260 unsigned char * bytePosPtr;
261 261
262 262 bytePosPtr = (unsigned char *) &TC->packetID;
263 263
264 264 // check LFR mode
265 265 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET5 ] & 0x1e) >> 1;
266 266 status = check_update_info_hk_lfr_mode( mode );
267 267 if (status == LFR_SUCCESSFUL) // check TDS mode
268 268 {
269 269 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET6 ] & 0xf0) >> 4;
270 270 status = check_update_info_hk_tds_mode( mode );
271 271 }
272 272 if (status == LFR_SUCCESSFUL) // check THR mode
273 273 {
274 274 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET6 ] & 0x0f);
275 275 status = check_update_info_hk_thr_mode( mode );
276 276 }
277 277 if (status == LFR_SUCCESSFUL) // if the parameter check is successful
278 278 {
279 279 val = housekeeping_packet.hk_lfr_update_info_tc_cnt[0] * 256
280 280 + housekeeping_packet.hk_lfr_update_info_tc_cnt[1];
281 281 val++;
282 282 housekeeping_packet.hk_lfr_update_info_tc_cnt[0] = (unsigned char) (val >> 8);
283 283 housekeeping_packet.hk_lfr_update_info_tc_cnt[1] = (unsigned char) (val);
284 284 }
285 285
286 286 // pa_bia_status_info
287 287 // => pa_bia_mode_mux_set 3 bits
288 288 // => pa_bia_mode_hv_enabled 1 bit
289 289 // => pa_bia_mode_bias1_enabled 1 bit
290 290 // => pa_bia_mode_bias2_enabled 1 bit
291 291 // => pa_bia_mode_bias3_enabled 1 bit
292 292 // => pa_bia_on_off (cp_dpu_bias_on_off)
293 293 pa_bia_status_info = bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET2 ] & 0xfe; // [1111 1110]
294 294 pa_bia_status_info = pa_bia_status_info
295 295 | (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET1 ] & 0x1);
296 296
297 297 // REACTION_WHEELS_FREQUENCY, copy the incoming parameters in the local variable (to be copied in HK packets)
298 298 cp_rpw_sc_rw_f_flags = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW_F_FLAGS ];
299 299 getReactionWheelsFrequencies( TC );
300 300 build_rw_fbins_masks();
301 301
302 302 result = status;
303 303
304 304 return result;
305 305 }
306 306
307 307 int action_enable_calibration(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
308 308 {
309 309 /** This function executes specific actions when a TC_LFR_ENABLE_CALIBRATION TeleCommand has been received.
310 310 *
311 311 * @param TC points to the TeleCommand packet that is being processed
312 312 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
313 313 *
314 314 */
315 315
316 316 int result;
317 317
318 318 result = LFR_DEFAULT;
319 319
320 320 setCalibration( true );
321 321
322 322 result = LFR_SUCCESSFUL;
323 323
324 324 return result;
325 325 }
326 326
327 327 int action_disable_calibration(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
328 328 {
329 329 /** This function executes specific actions when a TC_LFR_DISABLE_CALIBRATION TeleCommand has been received.
330 330 *
331 331 * @param TC points to the TeleCommand packet that is being processed
332 332 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
333 333 *
334 334 */
335 335
336 336 int result;
337 337
338 338 result = LFR_DEFAULT;
339 339
340 340 setCalibration( false );
341 341
342 342 result = LFR_SUCCESSFUL;
343 343
344 344 return result;
345 345 }
346 346
347 347 int action_update_time(ccsdsTelecommandPacket_t *TC)
348 348 {
349 349 /** This function executes specific actions when a TC_LFR_UPDATE_TIME TeleCommand has been received.
350 350 *
351 351 * @param TC points to the TeleCommand packet that is being processed
352 352 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
353 353 *
354 354 * @return LFR_SUCCESSFUL
355 355 *
356 356 */
357 357
358 358 unsigned int val;
359 359
360 360 time_management_regs->coarse_time_load = (TC->dataAndCRC[0] << 24)
361 361 + (TC->dataAndCRC[1] << 16)
362 362 + (TC->dataAndCRC[2] << 8)
363 363 + TC->dataAndCRC[3];
364 364
365 365 val = housekeeping_packet.hk_lfr_update_time_tc_cnt[0] * 256
366 366 + housekeeping_packet.hk_lfr_update_time_tc_cnt[1];
367 367 val++;
368 368 housekeeping_packet.hk_lfr_update_time_tc_cnt[0] = (unsigned char) (val >> 8);
369 369 housekeeping_packet.hk_lfr_update_time_tc_cnt[1] = (unsigned char) (val);
370 370
371 371 oneTcLfrUpdateTimeReceived = 1;
372 372
373 373 return LFR_SUCCESSFUL;
374 374 }
375 375
376 376 //*******************
377 377 // ENTERING THE MODES
378 378 int check_mode_value( unsigned char requestedMode )
379 379 {
380 380 int status;
381 381
382 382 if ( (requestedMode != LFR_MODE_STANDBY)
383 383 && (requestedMode != LFR_MODE_NORMAL) && (requestedMode != LFR_MODE_BURST)
384 384 && (requestedMode != LFR_MODE_SBM1) && (requestedMode != LFR_MODE_SBM2) )
385 385 {
386 386 status = LFR_DEFAULT;
387 387 }
388 388 else
389 389 {
390 390 status = LFR_SUCCESSFUL;
391 391 }
392 392
393 393 return status;
394 394 }
395 395
396 396 int check_mode_transition( unsigned char requestedMode )
397 397 {
398 398 /** This function checks the validity of the transition requested by the TC_LFR_ENTER_MODE.
399 399 *
400 400 * @param requestedMode is the mode requested by the TC_LFR_ENTER_MODE
401 401 *
402 402 * @return LFR directive status codes:
403 403 * - LFR_SUCCESSFUL - the transition is authorized
404 404 * - LFR_DEFAULT - the transition is not authorized
405 405 *
406 406 */
407 407
408 408 int status;
409 409
410 410 switch (requestedMode)
411 411 {
412 412 case LFR_MODE_STANDBY:
413 413 if ( lfrCurrentMode == LFR_MODE_STANDBY ) {
414 414 status = LFR_DEFAULT;
415 415 }
416 416 else
417 417 {
418 418 status = LFR_SUCCESSFUL;
419 419 }
420 420 break;
421 421 case LFR_MODE_NORMAL:
422 422 if ( lfrCurrentMode == LFR_MODE_NORMAL ) {
423 423 status = LFR_DEFAULT;
424 424 }
425 425 else {
426 426 status = LFR_SUCCESSFUL;
427 427 }
428 428 break;
429 429 case LFR_MODE_BURST:
430 430 if ( lfrCurrentMode == LFR_MODE_BURST ) {
431 431 status = LFR_DEFAULT;
432 432 }
433 433 else {
434 434 status = LFR_SUCCESSFUL;
435 435 }
436 436 break;
437 437 case LFR_MODE_SBM1:
438 438 if ( lfrCurrentMode == LFR_MODE_SBM1 ) {
439 439 status = LFR_DEFAULT;
440 440 }
441 441 else {
442 442 status = LFR_SUCCESSFUL;
443 443 }
444 444 break;
445 445 case LFR_MODE_SBM2:
446 446 if ( lfrCurrentMode == LFR_MODE_SBM2 ) {
447 447 status = LFR_DEFAULT;
448 448 }
449 449 else {
450 450 status = LFR_SUCCESSFUL;
451 451 }
452 452 break;
453 453 default:
454 454 status = LFR_DEFAULT;
455 455 break;
456 456 }
457 457
458 458 return status;
459 459 }
460 460
461 461 void update_last_valid_transition_date( unsigned int transitionCoarseTime )
462 462 {
463 463 if (transitionCoarseTime == 0)
464 464 {
465 465 lastValidEnterModeTime = time_management_regs->coarse_time + 1;
466 466 PRINTF1("lastValidEnterModeTime = 0x%x (transitionCoarseTime = 0 => coarse_time+1)\n", lastValidEnterModeTime);
467 467 }
468 468 else
469 469 {
470 470 lastValidEnterModeTime = transitionCoarseTime;
471 471 PRINTF1("lastValidEnterModeTime = 0x%x\n", transitionCoarseTime);
472 472 }
473 473 }
474 474
475 475 int check_transition_date( unsigned int transitionCoarseTime )
476 476 {
477 477 int status;
478 478 unsigned int localCoarseTime;
479 479 unsigned int deltaCoarseTime;
480 480
481 481 status = LFR_SUCCESSFUL;
482 482
483 483 if (transitionCoarseTime == 0) // transition time = 0 means an instant transition
484 484 {
485 485 status = LFR_SUCCESSFUL;
486 486 }
487 487 else
488 488 {
489 489 localCoarseTime = time_management_regs->coarse_time & 0x7fffffff;
490 490
491 491 PRINTF2("localTime = %x, transitionTime = %x\n", localCoarseTime, transitionCoarseTime);
492 492
493 493 if ( transitionCoarseTime <= localCoarseTime ) // SSS-CP-EQS-322
494 494 {
495 495 status = LFR_DEFAULT;
496 496 PRINTF("ERR *** in check_transition_date *** transitionCoarseTime <= localCoarseTime\n");
497 497 }
498 498
499 499 if (status == LFR_SUCCESSFUL)
500 500 {
501 501 deltaCoarseTime = transitionCoarseTime - localCoarseTime;
502 502 if ( deltaCoarseTime > 3 ) // SSS-CP-EQS-323
503 503 {
504 504 status = LFR_DEFAULT;
505 505 PRINTF1("ERR *** in check_transition_date *** deltaCoarseTime = %x\n", deltaCoarseTime)
506 506 }
507 507 }
508 508 }
509 509
510 510 return status;
511 511 }
512 512
513 513 int restart_asm_activities( unsigned char lfrRequestedMode )
514 514 {
515 515 rtems_status_code status;
516 516
517 517 status = stop_spectral_matrices();
518 518
519 519 thisIsAnASMRestart = 1;
520 520
521 521 status = restart_asm_tasks( lfrRequestedMode );
522 522
523 523 launch_spectral_matrix();
524 524
525 525 return status;
526 526 }
527 527
528 528 int stop_spectral_matrices( void )
529 529 {
530 530 /** This function stops and restarts the current mode average spectral matrices activities.
531 531 *
532 532 * @return RTEMS directive status codes:
533 533 * - RTEMS_SUCCESSFUL - task restarted successfully
534 534 * - RTEMS_INVALID_ID - task id invalid
535 535 * - RTEMS_ALREADY_SUSPENDED - task already suspended
536 536 *
537 537 */
538 538
539 539 rtems_status_code status;
540 540
541 541 status = RTEMS_SUCCESSFUL;
542 542
543 543 // (1) mask interruptions
544 544 LEON_Mask_interrupt( IRQ_SPECTRAL_MATRIX ); // mask spectral matrix interrupt
545 545
546 546 // (2) reset spectral matrices registers
547 547 set_sm_irq_onNewMatrix( 0 ); // stop the spectral matrices
548 548 reset_sm_status();
549 549
550 550 // (3) clear interruptions
551 551 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
552 552
553 553 // suspend several tasks
554 554 if (lfrCurrentMode != LFR_MODE_STANDBY) {
555 555 status = suspend_asm_tasks();
556 556 }
557 557
558 558 if (status != RTEMS_SUCCESSFUL)
559 559 {
560 560 PRINTF1("in stop_current_mode *** in suspend_science_tasks *** ERR code: %d\n", status)
561 561 }
562 562
563 563 return status;
564 564 }
565 565
566 566 int stop_current_mode( void )
567 567 {
568 568 /** This function stops the current mode by masking interrupt lines and suspending science tasks.
569 569 *
570 570 * @return RTEMS directive status codes:
571 571 * - RTEMS_SUCCESSFUL - task restarted successfully
572 572 * - RTEMS_INVALID_ID - task id invalid
573 573 * - RTEMS_ALREADY_SUSPENDED - task already suspended
574 574 *
575 575 */
576 576
577 577 rtems_status_code status;
578 578
579 579 status = RTEMS_SUCCESSFUL;
580 580
581 581 // (1) mask interruptions
582 582 LEON_Mask_interrupt( IRQ_WAVEFORM_PICKER ); // mask waveform picker interrupt
583 583 LEON_Mask_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
584 584
585 585 // (2) reset waveform picker registers
586 586 reset_wfp_burst_enable(); // reset burst and enable bits
587 587 reset_wfp_status(); // reset all the status bits
588 588
589 589 // (3) reset spectral matrices registers
590 590 set_sm_irq_onNewMatrix( 0 ); // stop the spectral matrices
591 591 reset_sm_status();
592 592
593 593 // reset lfr VHDL module
594 594 reset_lfr();
595 595
596 596 reset_extractSWF(); // reset the extractSWF flag to false
597 597
598 598 // (4) clear interruptions
599 599 LEON_Clear_interrupt( IRQ_WAVEFORM_PICKER ); // clear waveform picker interrupt
600 600 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
601 601
602 602 // suspend several tasks
603 603 if (lfrCurrentMode != LFR_MODE_STANDBY) {
604 604 status = suspend_science_tasks();
605 605 }
606 606
607 607 if (status != RTEMS_SUCCESSFUL)
608 608 {
609 609 PRINTF1("in stop_current_mode *** in suspend_science_tasks *** ERR code: %d\n", status)
610 610 }
611 611
612 612 return status;
613 613 }
614 614
615 615 int enter_mode_standby( void )
616 616 {
617 617 /** This function is used to put LFR in the STANDBY mode.
618 618 *
619 619 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
620 620 *
621 621 * @return RTEMS directive status codes:
622 622 * - RTEMS_SUCCESSFUL - task restarted successfully
623 623 * - RTEMS_INVALID_ID - task id invalid
624 624 * - RTEMS_INCORRECT_STATE - task never started
625 625 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
626 626 *
627 627 * The STANDBY mode does not depends on a specific transition date, the effect of the TC_LFR_ENTER_MODE
628 628 * is immediate.
629 629 *
630 630 */
631 631
632 632 int status;
633 633
634 634 status = stop_current_mode(); // STOP THE CURRENT MODE
635 635
636 636 #ifdef PRINT_TASK_STATISTICS
637 637 rtems_cpu_usage_report();
638 638 #endif
639 639
640 640 #ifdef PRINT_STACK_REPORT
641 641 PRINTF("stack report selected\n")
642 642 rtems_stack_checker_report_usage();
643 643 #endif
644 644
645 645 return status;
646 646 }
647 647
648 648 int enter_mode_normal( unsigned int transitionCoarseTime )
649 649 {
650 650 /** This function is used to start the NORMAL mode.
651 651 *
652 652 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
653 653 *
654 654 * @return RTEMS directive status codes:
655 655 * - RTEMS_SUCCESSFUL - task restarted successfully
656 656 * - RTEMS_INVALID_ID - task id invalid
657 657 * - RTEMS_INCORRECT_STATE - task never started
658 658 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
659 659 *
660 660 * The way the NORMAL mode is started depends on the LFR current mode. If LFR is in SBM1 or SBM2,
661 661 * the snapshots are not restarted, only ASM, BP and CWF data generation are affected.
662 662 *
663 663 */
664 664
665 665 int status;
666 666
667 667 #ifdef PRINT_TASK_STATISTICS
668 668 rtems_cpu_usage_reset();
669 669 #endif
670 670
671 671 status = RTEMS_UNSATISFIED;
672 672
673 673 switch( lfrCurrentMode )
674 674 {
675 675 case LFR_MODE_STANDBY:
676 676 status = restart_science_tasks( LFR_MODE_NORMAL ); // restart science tasks
677 677 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
678 678 {
679 679 launch_spectral_matrix( );
680 680 launch_waveform_picker( LFR_MODE_NORMAL, transitionCoarseTime );
681 681 }
682 682 break;
683 683 case LFR_MODE_BURST:
684 684 status = stop_current_mode(); // stop the current mode
685 685 status = restart_science_tasks( LFR_MODE_NORMAL ); // restart the science tasks
686 686 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
687 687 {
688 688 launch_spectral_matrix( );
689 689 launch_waveform_picker( LFR_MODE_NORMAL, transitionCoarseTime );
690 690 }
691 691 break;
692 692 case LFR_MODE_SBM1:
693 693 status = restart_asm_activities( LFR_MODE_NORMAL ); // this is necessary to restart ASM tasks to update the parameters
694 694 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
695 695 update_last_valid_transition_date( transitionCoarseTime );
696 696 break;
697 697 case LFR_MODE_SBM2:
698 698 status = restart_asm_activities( LFR_MODE_NORMAL ); // this is necessary to restart ASM tasks to update the parameters
699 699 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
700 700 update_last_valid_transition_date( transitionCoarseTime );
701 701 break;
702 702 default:
703 703 break;
704 704 }
705 705
706 706 if (status != RTEMS_SUCCESSFUL)
707 707 {
708 708 PRINTF1("ERR *** in enter_mode_normal *** status = %d\n", status)
709 709 status = RTEMS_UNSATISFIED;
710 710 }
711 711
712 712 return status;
713 713 }
714 714
715 715 int enter_mode_burst( unsigned int transitionCoarseTime )
716 716 {
717 717 /** This function is used to start the BURST mode.
718 718 *
719 719 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
720 720 *
721 721 * @return RTEMS directive status codes:
722 722 * - RTEMS_SUCCESSFUL - task restarted successfully
723 723 * - RTEMS_INVALID_ID - task id invalid
724 724 * - RTEMS_INCORRECT_STATE - task never started
725 725 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
726 726 *
727 727 * The way the BURST mode is started does not depend on the LFR current mode.
728 728 *
729 729 */
730 730
731 731
732 732 int status;
733 733
734 734 #ifdef PRINT_TASK_STATISTICS
735 735 rtems_cpu_usage_reset();
736 736 #endif
737 737
738 738 status = stop_current_mode(); // stop the current mode
739 739 status = restart_science_tasks( LFR_MODE_BURST ); // restart the science tasks
740 740 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
741 741 {
742 742 launch_spectral_matrix( );
743 743 launch_waveform_picker( LFR_MODE_BURST, transitionCoarseTime );
744 744 }
745 745
746 746 if (status != RTEMS_SUCCESSFUL)
747 747 {
748 748 PRINTF1("ERR *** in enter_mode_burst *** status = %d\n", status)
749 749 status = RTEMS_UNSATISFIED;
750 750 }
751 751
752 752 return status;
753 753 }
754 754
755 755 int enter_mode_sbm1( unsigned int transitionCoarseTime )
756 756 {
757 757 /** This function is used to start the SBM1 mode.
758 758 *
759 759 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
760 760 *
761 761 * @return RTEMS directive status codes:
762 762 * - RTEMS_SUCCESSFUL - task restarted successfully
763 763 * - RTEMS_INVALID_ID - task id invalid
764 764 * - RTEMS_INCORRECT_STATE - task never started
765 765 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
766 766 *
767 767 * The way the SBM1 mode is started depends on the LFR current mode. If LFR is in NORMAL or SBM2,
768 768 * the snapshots are not restarted, only ASM, BP and CWF data generation are affected. In other
769 769 * cases, the acquisition is completely restarted.
770 770 *
771 771 */
772 772
773 773 int status;
774 774
775 775 #ifdef PRINT_TASK_STATISTICS
776 776 rtems_cpu_usage_reset();
777 777 #endif
778 778
779 779 status = RTEMS_UNSATISFIED;
780 780
781 781 switch( lfrCurrentMode )
782 782 {
783 783 case LFR_MODE_STANDBY:
784 784 status = restart_science_tasks( LFR_MODE_SBM1 ); // restart science tasks
785 785 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
786 786 {
787 787 launch_spectral_matrix( );
788 788 launch_waveform_picker( LFR_MODE_SBM1, transitionCoarseTime );
789 789 }
790 790 break;
791 791 case LFR_MODE_NORMAL: // lfrCurrentMode will be updated after the execution of close_action
792 792 status = restart_asm_activities( LFR_MODE_SBM1 );
793 793 status = LFR_SUCCESSFUL;
794 794 update_last_valid_transition_date( transitionCoarseTime );
795 795 break;
796 796 case LFR_MODE_BURST:
797 797 status = stop_current_mode(); // stop the current mode
798 798 status = restart_science_tasks( LFR_MODE_SBM1 ); // restart the science tasks
799 799 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
800 800 {
801 801 launch_spectral_matrix( );
802 802 launch_waveform_picker( LFR_MODE_SBM1, transitionCoarseTime );
803 803 }
804 804 break;
805 805 case LFR_MODE_SBM2:
806 806 status = restart_asm_activities( LFR_MODE_SBM1 );
807 807 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
808 808 update_last_valid_transition_date( transitionCoarseTime );
809 809 break;
810 810 default:
811 811 break;
812 812 }
813 813
814 814 if (status != RTEMS_SUCCESSFUL)
815 815 {
816 816 PRINTF1("ERR *** in enter_mode_sbm1 *** status = %d\n", status);
817 817 status = RTEMS_UNSATISFIED;
818 818 }
819 819
820 820 return status;
821 821 }
822 822
823 823 int enter_mode_sbm2( unsigned int transitionCoarseTime )
824 824 {
825 825 /** This function is used to start the SBM2 mode.
826 826 *
827 827 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
828 828 *
829 829 * @return RTEMS directive status codes:
830 830 * - RTEMS_SUCCESSFUL - task restarted successfully
831 831 * - RTEMS_INVALID_ID - task id invalid
832 832 * - RTEMS_INCORRECT_STATE - task never started
833 833 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
834 834 *
835 835 * The way the SBM2 mode is started depends on the LFR current mode. If LFR is in NORMAL or SBM1,
836 836 * the snapshots are not restarted, only ASM, BP and CWF data generation are affected. In other
837 837 * cases, the acquisition is completely restarted.
838 838 *
839 839 */
840 840
841 841 int status;
842 842
843 843 #ifdef PRINT_TASK_STATISTICS
844 844 rtems_cpu_usage_reset();
845 845 #endif
846 846
847 847 status = RTEMS_UNSATISFIED;
848 848
849 849 switch( lfrCurrentMode )
850 850 {
851 851 case LFR_MODE_STANDBY:
852 852 status = restart_science_tasks( LFR_MODE_SBM2 ); // restart science tasks
853 853 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
854 854 {
855 855 launch_spectral_matrix( );
856 856 launch_waveform_picker( LFR_MODE_SBM2, transitionCoarseTime );
857 857 }
858 858 break;
859 859 case LFR_MODE_NORMAL:
860 860 status = restart_asm_activities( LFR_MODE_SBM2 );
861 861 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
862 862 update_last_valid_transition_date( transitionCoarseTime );
863 863 break;
864 864 case LFR_MODE_BURST:
865 865 status = stop_current_mode(); // stop the current mode
866 866 status = restart_science_tasks( LFR_MODE_SBM2 ); // restart the science tasks
867 867 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
868 868 {
869 869 launch_spectral_matrix( );
870 870 launch_waveform_picker( LFR_MODE_SBM2, transitionCoarseTime );
871 871 }
872 872 break;
873 873 case LFR_MODE_SBM1:
874 874 status = restart_asm_activities( LFR_MODE_SBM2 );
875 875 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
876 876 update_last_valid_transition_date( transitionCoarseTime );
877 877 break;
878 878 default:
879 879 break;
880 880 }
881 881
882 882 if (status != RTEMS_SUCCESSFUL)
883 883 {
884 884 PRINTF1("ERR *** in enter_mode_sbm2 *** status = %d\n", status)
885 885 status = RTEMS_UNSATISFIED;
886 886 }
887 887
888 888 return status;
889 889 }
890 890
891 891 int restart_science_tasks( unsigned char lfrRequestedMode )
892 892 {
893 893 /** This function is used to restart all science tasks.
894 894 *
895 895 * @return RTEMS directive status codes:
896 896 * - RTEMS_SUCCESSFUL - task restarted successfully
897 897 * - RTEMS_INVALID_ID - task id invalid
898 898 * - RTEMS_INCORRECT_STATE - task never started
899 899 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
900 900 *
901 901 * Science tasks are AVF0, PRC0, WFRM, CWF3, CW2, CWF1
902 902 *
903 903 */
904 904
905 905 rtems_status_code status[10];
906 906 rtems_status_code ret;
907 907
908 908 ret = RTEMS_SUCCESSFUL;
909 909
910 910 status[0] = rtems_task_restart( Task_id[TASKID_AVF0], lfrRequestedMode );
911 911 if (status[0] != RTEMS_SUCCESSFUL)
912 912 {
913 913 PRINTF1("in restart_science_task *** AVF0 ERR %d\n", status[0])
914 914 }
915 915
916 916 status[1] = rtems_task_restart( Task_id[TASKID_PRC0], lfrRequestedMode );
917 917 if (status[1] != RTEMS_SUCCESSFUL)
918 918 {
919 919 PRINTF1("in restart_science_task *** PRC0 ERR %d\n", status[1])
920 920 }
921 921
922 922 status[2] = rtems_task_restart( Task_id[TASKID_WFRM],1 );
923 923 if (status[2] != RTEMS_SUCCESSFUL)
924 924 {
925 925 PRINTF1("in restart_science_task *** WFRM ERR %d\n", status[2])
926 926 }
927 927
928 928 status[3] = rtems_task_restart( Task_id[TASKID_CWF3],1 );
929 929 if (status[3] != RTEMS_SUCCESSFUL)
930 930 {
931 931 PRINTF1("in restart_science_task *** CWF3 ERR %d\n", status[3])
932 932 }
933 933
934 934 status[4] = rtems_task_restart( Task_id[TASKID_CWF2],1 );
935 935 if (status[4] != RTEMS_SUCCESSFUL)
936 936 {
937 937 PRINTF1("in restart_science_task *** CWF2 ERR %d\n", status[4])
938 938 }
939 939
940 940 status[5] = rtems_task_restart( Task_id[TASKID_CWF1],1 );
941 941 if (status[5] != RTEMS_SUCCESSFUL)
942 942 {
943 943 PRINTF1("in restart_science_task *** CWF1 ERR %d\n", status[5])
944 944 }
945 945
946 946 status[6] = rtems_task_restart( Task_id[TASKID_AVF1], lfrRequestedMode );
947 947 if (status[6] != RTEMS_SUCCESSFUL)
948 948 {
949 949 PRINTF1("in restart_science_task *** AVF1 ERR %d\n", status[6])
950 950 }
951 951
952 952 status[7] = rtems_task_restart( Task_id[TASKID_PRC1],lfrRequestedMode );
953 953 if (status[7] != RTEMS_SUCCESSFUL)
954 954 {
955 955 PRINTF1("in restart_science_task *** PRC1 ERR %d\n", status[7])
956 956 }
957 957
958 958 status[8] = rtems_task_restart( Task_id[TASKID_AVF2], 1 );
959 959 if (status[8] != RTEMS_SUCCESSFUL)
960 960 {
961 961 PRINTF1("in restart_science_task *** AVF2 ERR %d\n", status[8])
962 962 }
963 963
964 964 status[9] = rtems_task_restart( Task_id[TASKID_PRC2], 1 );
965 965 if (status[9] != RTEMS_SUCCESSFUL)
966 966 {
967 967 PRINTF1("in restart_science_task *** PRC2 ERR %d\n", status[9])
968 968 }
969 969
970 970 if ( (status[0] != RTEMS_SUCCESSFUL) || (status[1] != RTEMS_SUCCESSFUL) ||
971 971 (status[2] != RTEMS_SUCCESSFUL) || (status[3] != RTEMS_SUCCESSFUL) ||
972 972 (status[4] != RTEMS_SUCCESSFUL) || (status[5] != RTEMS_SUCCESSFUL) ||
973 973 (status[6] != RTEMS_SUCCESSFUL) || (status[7] != RTEMS_SUCCESSFUL) ||
974 974 (status[8] != RTEMS_SUCCESSFUL) || (status[9] != RTEMS_SUCCESSFUL) )
975 975 {
976 976 ret = RTEMS_UNSATISFIED;
977 977 }
978 978
979 979 return ret;
980 980 }
981 981
982 982 int restart_asm_tasks( unsigned char lfrRequestedMode )
983 983 {
984 984 /** This function is used to restart average spectral matrices tasks.
985 985 *
986 986 * @return RTEMS directive status codes:
987 987 * - RTEMS_SUCCESSFUL - task restarted successfully
988 988 * - RTEMS_INVALID_ID - task id invalid
989 989 * - RTEMS_INCORRECT_STATE - task never started
990 990 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
991 991 *
992 992 * ASM tasks are AVF0, PRC0, AVF1, PRC1, AVF2 and PRC2
993 993 *
994 994 */
995 995
996 996 rtems_status_code status[6];
997 997 rtems_status_code ret;
998 998
999 999 ret = RTEMS_SUCCESSFUL;
1000 1000
1001 1001 status[0] = rtems_task_restart( Task_id[TASKID_AVF0], lfrRequestedMode );
1002 1002 if (status[0] != RTEMS_SUCCESSFUL)
1003 1003 {
1004 1004 PRINTF1("in restart_science_task *** AVF0 ERR %d\n", status[0])
1005 1005 }
1006 1006
1007 1007 status[1] = rtems_task_restart( Task_id[TASKID_PRC0], lfrRequestedMode );
1008 1008 if (status[1] != RTEMS_SUCCESSFUL)
1009 1009 {
1010 1010 PRINTF1("in restart_science_task *** PRC0 ERR %d\n", status[1])
1011 1011 }
1012 1012
1013 1013 status[2] = rtems_task_restart( Task_id[TASKID_AVF1], lfrRequestedMode );
1014 1014 if (status[2] != RTEMS_SUCCESSFUL)
1015 1015 {
1016 1016 PRINTF1("in restart_science_task *** AVF1 ERR %d\n", status[2])
1017 1017 }
1018 1018
1019 1019 status[3] = rtems_task_restart( Task_id[TASKID_PRC1],lfrRequestedMode );
1020 1020 if (status[3] != RTEMS_SUCCESSFUL)
1021 1021 {
1022 1022 PRINTF1("in restart_science_task *** PRC1 ERR %d\n", status[3])
1023 1023 }
1024 1024
1025 1025 status[4] = rtems_task_restart( Task_id[TASKID_AVF2], 1 );
1026 1026 if (status[4] != RTEMS_SUCCESSFUL)
1027 1027 {
1028 1028 PRINTF1("in restart_science_task *** AVF2 ERR %d\n", status[4])
1029 1029 }
1030 1030
1031 1031 status[5] = rtems_task_restart( Task_id[TASKID_PRC2], 1 );
1032 1032 if (status[5] != RTEMS_SUCCESSFUL)
1033 1033 {
1034 1034 PRINTF1("in restart_science_task *** PRC2 ERR %d\n", status[5])
1035 1035 }
1036 1036
1037 1037 if ( (status[0] != RTEMS_SUCCESSFUL) || (status[1] != RTEMS_SUCCESSFUL) ||
1038 1038 (status[2] != RTEMS_SUCCESSFUL) || (status[3] != RTEMS_SUCCESSFUL) ||
1039 1039 (status[4] != RTEMS_SUCCESSFUL) || (status[5] != RTEMS_SUCCESSFUL) )
1040 1040 {
1041 1041 ret = RTEMS_UNSATISFIED;
1042 1042 }
1043 1043
1044 1044 return ret;
1045 1045 }
1046 1046
1047 1047 int suspend_science_tasks( void )
1048 1048 {
1049 1049 /** This function suspends the science tasks.
1050 1050 *
1051 1051 * @return RTEMS directive status codes:
1052 1052 * - RTEMS_SUCCESSFUL - task restarted successfully
1053 1053 * - RTEMS_INVALID_ID - task id invalid
1054 1054 * - RTEMS_ALREADY_SUSPENDED - task already suspended
1055 1055 *
1056 1056 */
1057 1057
1058 1058 rtems_status_code status;
1059 1059
1060 1060 PRINTF("in suspend_science_tasks\n")
1061 1061
1062 1062 status = rtems_task_suspend( Task_id[TASKID_AVF0] ); // suspend AVF0
1063 1063 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1064 1064 {
1065 1065 PRINTF1("in suspend_science_task *** AVF0 ERR %d\n", status)
1066 1066 }
1067 1067 else
1068 1068 {
1069 1069 status = RTEMS_SUCCESSFUL;
1070 1070 }
1071 1071 if (status == RTEMS_SUCCESSFUL) // suspend PRC0
1072 1072 {
1073 1073 status = rtems_task_suspend( Task_id[TASKID_PRC0] );
1074 1074 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1075 1075 {
1076 1076 PRINTF1("in suspend_science_task *** PRC0 ERR %d\n", status)
1077 1077 }
1078 1078 else
1079 1079 {
1080 1080 status = RTEMS_SUCCESSFUL;
1081 1081 }
1082 1082 }
1083 1083 if (status == RTEMS_SUCCESSFUL) // suspend AVF1
1084 1084 {
1085 1085 status = rtems_task_suspend( Task_id[TASKID_AVF1] );
1086 1086 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1087 1087 {
1088 1088 PRINTF1("in suspend_science_task *** AVF1 ERR %d\n", status)
1089 1089 }
1090 1090 else
1091 1091 {
1092 1092 status = RTEMS_SUCCESSFUL;
1093 1093 }
1094 1094 }
1095 1095 if (status == RTEMS_SUCCESSFUL) // suspend PRC1
1096 1096 {
1097 1097 status = rtems_task_suspend( Task_id[TASKID_PRC1] );
1098 1098 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1099 1099 {
1100 1100 PRINTF1("in suspend_science_task *** PRC1 ERR %d\n", status)
1101 1101 }
1102 1102 else
1103 1103 {
1104 1104 status = RTEMS_SUCCESSFUL;
1105 1105 }
1106 1106 }
1107 1107 if (status == RTEMS_SUCCESSFUL) // suspend AVF2
1108 1108 {
1109 1109 status = rtems_task_suspend( Task_id[TASKID_AVF2] );
1110 1110 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1111 1111 {
1112 1112 PRINTF1("in suspend_science_task *** AVF2 ERR %d\n", status)
1113 1113 }
1114 1114 else
1115 1115 {
1116 1116 status = RTEMS_SUCCESSFUL;
1117 1117 }
1118 1118 }
1119 1119 if (status == RTEMS_SUCCESSFUL) // suspend PRC2
1120 1120 {
1121 1121 status = rtems_task_suspend( Task_id[TASKID_PRC2] );
1122 1122 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1123 1123 {
1124 1124 PRINTF1("in suspend_science_task *** PRC2 ERR %d\n", status)
1125 1125 }
1126 1126 else
1127 1127 {
1128 1128 status = RTEMS_SUCCESSFUL;
1129 1129 }
1130 1130 }
1131 1131 if (status == RTEMS_SUCCESSFUL) // suspend WFRM
1132 1132 {
1133 1133 status = rtems_task_suspend( Task_id[TASKID_WFRM] );
1134 1134 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1135 1135 {
1136 1136 PRINTF1("in suspend_science_task *** WFRM ERR %d\n", status)
1137 1137 }
1138 1138 else
1139 1139 {
1140 1140 status = RTEMS_SUCCESSFUL;
1141 1141 }
1142 1142 }
1143 1143 if (status == RTEMS_SUCCESSFUL) // suspend CWF3
1144 1144 {
1145 1145 status = rtems_task_suspend( Task_id[TASKID_CWF3] );
1146 1146 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1147 1147 {
1148 1148 PRINTF1("in suspend_science_task *** CWF3 ERR %d\n", status)
1149 1149 }
1150 1150 else
1151 1151 {
1152 1152 status = RTEMS_SUCCESSFUL;
1153 1153 }
1154 1154 }
1155 1155 if (status == RTEMS_SUCCESSFUL) // suspend CWF2
1156 1156 {
1157 1157 status = rtems_task_suspend( Task_id[TASKID_CWF2] );
1158 1158 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1159 1159 {
1160 1160 PRINTF1("in suspend_science_task *** CWF2 ERR %d\n", status)
1161 1161 }
1162 1162 else
1163 1163 {
1164 1164 status = RTEMS_SUCCESSFUL;
1165 1165 }
1166 1166 }
1167 1167 if (status == RTEMS_SUCCESSFUL) // suspend CWF1
1168 1168 {
1169 1169 status = rtems_task_suspend( Task_id[TASKID_CWF1] );
1170 1170 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1171 1171 {
1172 1172 PRINTF1("in suspend_science_task *** CWF1 ERR %d\n", status)
1173 1173 }
1174 1174 else
1175 1175 {
1176 1176 status = RTEMS_SUCCESSFUL;
1177 1177 }
1178 1178 }
1179 1179
1180 1180 return status;
1181 1181 }
1182 1182
1183 1183 int suspend_asm_tasks( void )
1184 1184 {
1185 1185 /** This function suspends the science tasks.
1186 1186 *
1187 1187 * @return RTEMS directive status codes:
1188 1188 * - RTEMS_SUCCESSFUL - task restarted successfully
1189 1189 * - RTEMS_INVALID_ID - task id invalid
1190 1190 * - RTEMS_ALREADY_SUSPENDED - task already suspended
1191 1191 *
1192 1192 */
1193 1193
1194 1194 rtems_status_code status;
1195 1195
1196 1196 PRINTF("in suspend_science_tasks\n")
1197 1197
1198 1198 status = rtems_task_suspend( Task_id[TASKID_AVF0] ); // suspend AVF0
1199 1199 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1200 1200 {
1201 1201 PRINTF1("in suspend_science_task *** AVF0 ERR %d\n", status)
1202 1202 }
1203 1203 else
1204 1204 {
1205 1205 status = RTEMS_SUCCESSFUL;
1206 1206 }
1207 1207
1208 1208 if (status == RTEMS_SUCCESSFUL) // suspend PRC0
1209 1209 {
1210 1210 status = rtems_task_suspend( Task_id[TASKID_PRC0] );
1211 1211 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1212 1212 {
1213 1213 PRINTF1("in suspend_science_task *** PRC0 ERR %d\n", status)
1214 1214 }
1215 1215 else
1216 1216 {
1217 1217 status = RTEMS_SUCCESSFUL;
1218 1218 }
1219 1219 }
1220 1220
1221 1221 if (status == RTEMS_SUCCESSFUL) // suspend AVF1
1222 1222 {
1223 1223 status = rtems_task_suspend( Task_id[TASKID_AVF1] );
1224 1224 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1225 1225 {
1226 1226 PRINTF1("in suspend_science_task *** AVF1 ERR %d\n", status)
1227 1227 }
1228 1228 else
1229 1229 {
1230 1230 status = RTEMS_SUCCESSFUL;
1231 1231 }
1232 1232 }
1233 1233
1234 1234 if (status == RTEMS_SUCCESSFUL) // suspend PRC1
1235 1235 {
1236 1236 status = rtems_task_suspend( Task_id[TASKID_PRC1] );
1237 1237 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1238 1238 {
1239 1239 PRINTF1("in suspend_science_task *** PRC1 ERR %d\n", status)
1240 1240 }
1241 1241 else
1242 1242 {
1243 1243 status = RTEMS_SUCCESSFUL;
1244 1244 }
1245 1245 }
1246 1246
1247 1247 if (status == RTEMS_SUCCESSFUL) // suspend AVF2
1248 1248 {
1249 1249 status = rtems_task_suspend( Task_id[TASKID_AVF2] );
1250 1250 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1251 1251 {
1252 1252 PRINTF1("in suspend_science_task *** AVF2 ERR %d\n", status)
1253 1253 }
1254 1254 else
1255 1255 {
1256 1256 status = RTEMS_SUCCESSFUL;
1257 1257 }
1258 1258 }
1259 1259
1260 1260 if (status == RTEMS_SUCCESSFUL) // suspend PRC2
1261 1261 {
1262 1262 status = rtems_task_suspend( Task_id[TASKID_PRC2] );
1263 1263 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1264 1264 {
1265 1265 PRINTF1("in suspend_science_task *** PRC2 ERR %d\n", status)
1266 1266 }
1267 1267 else
1268 1268 {
1269 1269 status = RTEMS_SUCCESSFUL;
1270 1270 }
1271 1271 }
1272 1272
1273 1273 return status;
1274 1274 }
1275 1275
1276 1276 void launch_waveform_picker( unsigned char mode, unsigned int transitionCoarseTime )
1277 1277 {
1278 1278
1279 1279 WFP_reset_current_ring_nodes();
1280 1280
1281 1281 reset_waveform_picker_regs();
1282 1282
1283 1283 set_wfp_burst_enable_register( mode );
1284 1284
1285 1285 LEON_Clear_interrupt( IRQ_WAVEFORM_PICKER );
1286 1286 LEON_Unmask_interrupt( IRQ_WAVEFORM_PICKER );
1287 1287
1288 1288 if (transitionCoarseTime == 0)
1289 1289 {
1290 1290 // instant transition means transition on the next valid date
1291 1291 // this is mandatory to have a good snapshot period and a good correction of the snapshot period
1292 1292 waveform_picker_regs->start_date = time_management_regs->coarse_time + 1;
1293 1293 }
1294 1294 else
1295 1295 {
1296 1296 waveform_picker_regs->start_date = transitionCoarseTime;
1297 1297 }
1298 1298
1299 1299 update_last_valid_transition_date(waveform_picker_regs->start_date);
1300 1300
1301 1301 }
1302 1302
1303 1303 void launch_spectral_matrix( void )
1304 1304 {
1305 1305 SM_reset_current_ring_nodes();
1306 1306
1307 1307 reset_spectral_matrix_regs();
1308 1308
1309 1309 reset_nb_sm();
1310 1310
1311 1311 set_sm_irq_onNewMatrix( 1 );
1312 1312
1313 1313 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX );
1314 1314 LEON_Unmask_interrupt( IRQ_SPECTRAL_MATRIX );
1315 1315
1316 1316 }
1317 1317
1318 1318 void set_sm_irq_onNewMatrix( unsigned char value )
1319 1319 {
1320 1320 if (value == 1)
1321 1321 {
1322 1322 spectral_matrix_regs->config = spectral_matrix_regs->config | 0x01;
1323 1323 }
1324 1324 else
1325 1325 {
1326 1326 spectral_matrix_regs->config = spectral_matrix_regs->config & 0xfffffffe; // 1110
1327 1327 }
1328 1328 }
1329 1329
1330 1330 void set_sm_irq_onError( unsigned char value )
1331 1331 {
1332 1332 if (value == 1)
1333 1333 {
1334 1334 spectral_matrix_regs->config = spectral_matrix_regs->config | 0x02;
1335 1335 }
1336 1336 else
1337 1337 {
1338 1338 spectral_matrix_regs->config = spectral_matrix_regs->config & 0xfffffffd; // 1101
1339 1339 }
1340 1340 }
1341 1341
1342 1342 //*****************************
1343 1343 // CONFIGURE CALIBRATION SIGNAL
1344 1344 void setCalibrationPrescaler( unsigned int prescaler )
1345 1345 {
1346 1346 // prescaling of the master clock (25 MHz)
1347 1347 // master clock is divided by 2^prescaler
1348 1348 time_management_regs->calPrescaler = prescaler;
1349 1349 }
1350 1350
1351 1351 void setCalibrationDivisor( unsigned int divisionFactor )
1352 1352 {
1353 1353 // division of the prescaled clock by the division factor
1354 1354 time_management_regs->calDivisor = divisionFactor;
1355 1355 }
1356 1356
1357 1357 void setCalibrationData( void ){
1358 1358 unsigned int k;
1359 1359 unsigned short data;
1360 1360 float val;
1361 1361 float f0;
1362 1362 float f1;
1363 1363 float fs;
1364 1364 float Ts;
1365 1365 float scaleFactor;
1366 1366
1367 1367 f0 = 625;
1368 1368 f1 = 10000;
1369 1369 fs = 160256.410;
1370 1370 Ts = 1. / fs;
1371 1371 scaleFactor = 0.250 / 0.000654; // 191, 500 mVpp, 2 sinus waves => 500 mVpp each, amplitude = 250 mV
1372 1372
1373 1373 time_management_regs->calDataPtr = 0x00;
1374 1374
1375 1375 // build the signal for the SCM calibration
1376 1376 for (k=0; k<256; k++)
1377 1377 {
1378 1378 val = sin( 2 * pi * f0 * k * Ts )
1379 1379 + sin( 2 * pi * f1 * k * Ts );
1380 1380 data = (unsigned short) ((val * scaleFactor) + 2048);
1381 1381 time_management_regs->calData = data & 0xfff;
1382 1382 }
1383 1383 }
1384 1384
1385 1385 void setCalibrationDataInterleaved( void ){
1386 1386 unsigned int k;
1387 1387 float val;
1388 1388 float f0;
1389 1389 float f1;
1390 1390 float fs;
1391 1391 float Ts;
1392 1392 unsigned short data[384];
1393 1393 unsigned char *dataPtr;
1394 1394
1395 1395 f0 = 625;
1396 1396 f1 = 10000;
1397 1397 fs = 240384.615;
1398 1398 Ts = 1. / fs;
1399 1399
1400 1400 time_management_regs->calDataPtr = 0x00;
1401 1401
1402 1402 // build the signal for the SCM calibration
1403 1403 for (k=0; k<384; k++)
1404 1404 {
1405 1405 val = sin( 2 * pi * f0 * k * Ts )
1406 1406 + sin( 2 * pi * f1 * k * Ts );
1407 1407 data[k] = (unsigned short) (val * 512 + 2048);
1408 1408 }
1409 1409
1410 1410 // write the signal in interleaved mode
1411 1411 for (k=0; k<128; k++)
1412 1412 {
1413 1413 dataPtr = (unsigned char*) &data[k*3 + 2];
1414 1414 time_management_regs->calData = (data[k*3] & 0xfff)
1415 1415 + ( (dataPtr[0] & 0x3f) << 12);
1416 1416 time_management_regs->calData = (data[k*3 + 1] & 0xfff)
1417 1417 + ( (dataPtr[1] & 0x3f) << 12);
1418 1418 }
1419 1419 }
1420 1420
1421 1421 void setCalibrationReload( bool state)
1422 1422 {
1423 1423 if (state == true)
1424 1424 {
1425 1425 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | 0x00000010; // [0001 0000]
1426 1426 }
1427 1427 else
1428 1428 {
1429 1429 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & 0xffffffef; // [1110 1111]
1430 1430 }
1431 1431 }
1432 1432
1433 1433 void setCalibrationEnable( bool state )
1434 1434 {
1435 1435 // this bit drives the multiplexer
1436 1436 if (state == true)
1437 1437 {
1438 1438 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | 0x00000040; // [0100 0000]
1439 1439 }
1440 1440 else
1441 1441 {
1442 1442 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & 0xffffffbf; // [1011 1111]
1443 1443 }
1444 1444 }
1445 1445
1446 1446 void setCalibrationInterleaved( bool state )
1447 1447 {
1448 1448 // this bit drives the multiplexer
1449 1449 if (state == true)
1450 1450 {
1451 1451 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | 0x00000020; // [0010 0000]
1452 1452 }
1453 1453 else
1454 1454 {
1455 1455 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & 0xffffffdf; // [1101 1111]
1456 1456 }
1457 1457 }
1458 1458
1459 1459 void setCalibration( bool state )
1460 1460 {
1461 1461 if (state == true)
1462 1462 {
1463 1463 setCalibrationEnable( true );
1464 1464 setCalibrationReload( false );
1465 1465 set_hk_lfr_calib_enable( true );
1466 1466 }
1467 1467 else
1468 1468 {
1469 1469 setCalibrationEnable( false );
1470 1470 setCalibrationReload( true );
1471 1471 set_hk_lfr_calib_enable( false );
1472 1472 }
1473 1473 }
1474 1474
1475 1475 void configureCalibration( bool interleaved )
1476 1476 {
1477 1477 setCalibration( false );
1478 1478 if ( interleaved == true )
1479 1479 {
1480 1480 setCalibrationInterleaved( true );
1481 1481 setCalibrationPrescaler( 0 ); // 25 MHz => 25 000 000
1482 1482 setCalibrationDivisor( 26 ); // => 240 384
1483 1483 setCalibrationDataInterleaved();
1484 1484 }
1485 1485 else
1486 1486 {
1487 1487 setCalibrationPrescaler( 0 ); // 25 MHz => 25 000 000
1488 1488 setCalibrationDivisor( 38 ); // => 160 256 (39 - 1)
1489 1489 setCalibrationData();
1490 1490 }
1491 1491 }
1492 1492
1493 1493 //****************
1494 1494 // CLOSING ACTIONS
1495 1495 void update_last_TC_exe( ccsdsTelecommandPacket_t *TC, unsigned char * time )
1496 1496 {
1497 1497 /** This function is used to update the HK packets statistics after a successful TC execution.
1498 1498 *
1499 1499 * @param TC points to the TC being processed
1500 1500 * @param time is the time used to date the TC execution
1501 1501 *
1502 1502 */
1503 1503
1504 1504 unsigned int val;
1505 1505
1506 1506 housekeeping_packet.hk_lfr_last_exe_tc_id[0] = TC->packetID[0];
1507 1507 housekeeping_packet.hk_lfr_last_exe_tc_id[1] = TC->packetID[1];
1508 1508 housekeeping_packet.hk_lfr_last_exe_tc_type[0] = 0x00;
1509 1509 housekeeping_packet.hk_lfr_last_exe_tc_type[1] = TC->serviceType;
1510 1510 housekeeping_packet.hk_lfr_last_exe_tc_subtype[0] = 0x00;
1511 1511 housekeeping_packet.hk_lfr_last_exe_tc_subtype[1] = TC->serviceSubType;
1512 1512 housekeeping_packet.hk_lfr_last_exe_tc_time[0] = time[0];
1513 1513 housekeeping_packet.hk_lfr_last_exe_tc_time[1] = time[1];
1514 1514 housekeeping_packet.hk_lfr_last_exe_tc_time[2] = time[2];
1515 1515 housekeeping_packet.hk_lfr_last_exe_tc_time[3] = time[3];
1516 1516 housekeeping_packet.hk_lfr_last_exe_tc_time[4] = time[4];
1517 1517 housekeeping_packet.hk_lfr_last_exe_tc_time[5] = time[5];
1518 1518
1519 1519 val = housekeeping_packet.hk_lfr_exe_tc_cnt[0] * 256 + housekeeping_packet.hk_lfr_exe_tc_cnt[1];
1520 1520 val++;
1521 1521 housekeeping_packet.hk_lfr_exe_tc_cnt[0] = (unsigned char) (val >> 8);
1522 1522 housekeeping_packet.hk_lfr_exe_tc_cnt[1] = (unsigned char) (val);
1523 1523 }
1524 1524
1525 1525 void update_last_TC_rej(ccsdsTelecommandPacket_t *TC, unsigned char * time )
1526 1526 {
1527 1527 /** This function is used to update the HK packets statistics after a TC rejection.
1528 1528 *
1529 1529 * @param TC points to the TC being processed
1530 1530 * @param time is the time used to date the TC rejection
1531 1531 *
1532 1532 */
1533 1533
1534 1534 unsigned int val;
1535 1535
1536 1536 housekeeping_packet.hk_lfr_last_rej_tc_id[0] = TC->packetID[0];
1537 1537 housekeeping_packet.hk_lfr_last_rej_tc_id[1] = TC->packetID[1];
1538 1538 housekeeping_packet.hk_lfr_last_rej_tc_type[0] = 0x00;
1539 1539 housekeeping_packet.hk_lfr_last_rej_tc_type[1] = TC->serviceType;
1540 1540 housekeeping_packet.hk_lfr_last_rej_tc_subtype[0] = 0x00;
1541 1541 housekeeping_packet.hk_lfr_last_rej_tc_subtype[1] = TC->serviceSubType;
1542 1542 housekeeping_packet.hk_lfr_last_rej_tc_time[0] = time[0];
1543 1543 housekeeping_packet.hk_lfr_last_rej_tc_time[1] = time[1];
1544 1544 housekeeping_packet.hk_lfr_last_rej_tc_time[2] = time[2];
1545 1545 housekeeping_packet.hk_lfr_last_rej_tc_time[3] = time[3];
1546 1546 housekeeping_packet.hk_lfr_last_rej_tc_time[4] = time[4];
1547 1547 housekeeping_packet.hk_lfr_last_rej_tc_time[5] = time[5];
1548 1548
1549 1549 val = housekeeping_packet.hk_lfr_rej_tc_cnt[0] * 256 + housekeeping_packet.hk_lfr_rej_tc_cnt[1];
1550 1550 val++;
1551 1551 housekeeping_packet.hk_lfr_rej_tc_cnt[0] = (unsigned char) (val >> 8);
1552 1552 housekeeping_packet.hk_lfr_rej_tc_cnt[1] = (unsigned char) (val);
1553 1553 }
1554 1554
1555 1555 void close_action(ccsdsTelecommandPacket_t *TC, int result, rtems_id queue_id )
1556 1556 {
1557 1557 /** This function is the last step of the TC execution workflow.
1558 1558 *
1559 1559 * @param TC points to the TC being processed
1560 1560 * @param result is the result of the TC execution (LFR_SUCCESSFUL / LFR_DEFAULT)
1561 1561 * @param queue_id is the id of the RTEMS message queue used to send TM packets
1562 1562 * @param time is the time used to date the TC execution
1563 1563 *
1564 1564 */
1565 1565
1566 1566 unsigned char requestedMode;
1567 1567
1568 1568 if (result == LFR_SUCCESSFUL)
1569 1569 {
1570 1570 if ( !( (TC->serviceType==TC_TYPE_TIME) & (TC->serviceSubType==TC_SUBTYPE_UPDT_TIME) )
1571 1571 &
1572 1572 !( (TC->serviceType==TC_TYPE_GEN) & (TC->serviceSubType==TC_SUBTYPE_UPDT_INFO))
1573 1573 )
1574 1574 {
1575 1575 send_tm_lfr_tc_exe_success( TC, queue_id );
1576 1576 }
1577 1577 if ( (TC->serviceType == TC_TYPE_GEN) & (TC->serviceSubType == TC_SUBTYPE_ENTER) )
1578 1578 {
1579 1579 //**********************************
1580 1580 // UPDATE THE LFRMODE LOCAL VARIABLE
1581 1581 requestedMode = TC->dataAndCRC[1];
1582 1582 updateLFRCurrentMode( requestedMode );
1583 1583 }
1584 1584 }
1585 1585 else if (result == LFR_EXE_ERROR)
1586 1586 {
1587 1587 send_tm_lfr_tc_exe_error( TC, queue_id );
1588 1588 }
1589 1589 }
1590 1590
1591 1591 //***************************
1592 1592 // Interrupt Service Routines
1593 1593 rtems_isr commutation_isr1( rtems_vector_number vector )
1594 1594 {
1595 1595 if (rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
1596 1596 PRINTF("In commutation_isr1 *** Error sending event to DUMB\n")
1597 1597 }
1598 1598 }
1599 1599
1600 1600 rtems_isr commutation_isr2( rtems_vector_number vector )
1601 1601 {
1602 1602 if (rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
1603 1603 PRINTF("In commutation_isr2 *** Error sending event to DUMB\n")
1604 1604 }
1605 1605 }
1606 1606
1607 1607 //****************
1608 1608 // OTHER FUNCTIONS
1609 1609 void updateLFRCurrentMode( unsigned char requestedMode )
1610 1610 {
1611 1611 /** This function updates the value of the global variable lfrCurrentMode.
1612 1612 *
1613 1613 * lfrCurrentMode is a parameter used by several functions to know in which mode LFR is running.
1614 1614 *
1615 1615 */
1616 1616
1617 1617 // update the local value of lfrCurrentMode with the value contained in the housekeeping_packet structure
1618 1618 housekeeping_packet.lfr_status_word[0] = (unsigned char) ((requestedMode << 4) + 0x0d);
1619 1619 lfrCurrentMode = requestedMode;
1620 1620 }
1621 1621
1622 1622 void set_lfr_soft_reset( unsigned char value )
1623 1623 {
1624 1624 if (value == 1)
1625 1625 {
1626 1626 time_management_regs->ctrl = time_management_regs->ctrl | 0x00000004; // [0100]
1627 1627 }
1628 1628 else
1629 1629 {
1630 1630 time_management_regs->ctrl = time_management_regs->ctrl & 0xfffffffb; // [1011]
1631 1631 }
1632 1632 }
1633 1633
1634 1634 void reset_lfr( void )
1635 1635 {
1636 1636 set_lfr_soft_reset( 1 );
1637 1637
1638 1638 set_lfr_soft_reset( 0 );
1639 1639
1640 1640 set_hk_lfr_sc_potential_flag( true );
1641 1641 }
Show file before | Show file after | Hide comments
@@ -1,1430 +1,1466
1 1 /** Functions to load and dump parameters in the LFR registers.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle TC related to parameter loading and dumping.\n
7 7 * TC_LFR_LOAD_COMMON_PAR\n
8 8 * TC_LFR_LOAD_NORMAL_PAR\n
9 9 * TC_LFR_LOAD_BURST_PAR\n
10 10 * TC_LFR_LOAD_SBM1_PAR\n
11 11 * TC_LFR_LOAD_SBM2_PAR\n
12 12 *
13 13 */
14 14
15 15 #include "tc_load_dump_parameters.h"
16 16
17 17 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t kcoefficients_dump_1;
18 18 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t kcoefficients_dump_2;
19 19 ring_node kcoefficient_node_1;
20 20 ring_node kcoefficient_node_2;
21 21
22 22 int action_load_common_par(ccsdsTelecommandPacket_t *TC)
23 23 {
24 24 /** This function updates the LFR registers with the incoming common parameters.
25 25 *
26 26 * @param TC points to the TeleCommand packet that is being processed
27 27 *
28 28 *
29 29 */
30 30
31 31 parameter_dump_packet.sy_lfr_common_parameters_spare = TC->dataAndCRC[0];
32 32 parameter_dump_packet.sy_lfr_common_parameters = TC->dataAndCRC[1];
33 33 set_wfp_data_shaping( );
34 34 return LFR_SUCCESSFUL;
35 35 }
36 36
37 37 int action_load_normal_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
38 38 {
39 39 /** This function updates the LFR registers with the incoming normal parameters.
40 40 *
41 41 * @param TC points to the TeleCommand packet that is being processed
42 42 * @param queue_id is the id of the queue which handles TM related to this execution step
43 43 *
44 44 */
45 45
46 46 int result;
47 47 int flag;
48 48 rtems_status_code status;
49 49
50 50 flag = LFR_SUCCESSFUL;
51 51
52 52 if ( (lfrCurrentMode == LFR_MODE_NORMAL) ||
53 53 (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) ) {
54 54 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
55 55 flag = LFR_DEFAULT;
56 56 }
57 57
58 58 // CHECK THE PARAMETERS SET CONSISTENCY
59 59 if (flag == LFR_SUCCESSFUL)
60 60 {
61 61 flag = check_normal_par_consistency( TC, queue_id );
62 62 }
63 63
64 64 // SET THE PARAMETERS IF THEY ARE CONSISTENT
65 65 if (flag == LFR_SUCCESSFUL)
66 66 {
67 67 result = set_sy_lfr_n_swf_l( TC );
68 68 result = set_sy_lfr_n_swf_p( TC );
69 69 result = set_sy_lfr_n_bp_p0( TC );
70 70 result = set_sy_lfr_n_bp_p1( TC );
71 71 result = set_sy_lfr_n_asm_p( TC );
72 72 result = set_sy_lfr_n_cwf_long_f3( TC );
73 73 }
74 74
75 75 return flag;
76 76 }
77 77
78 78 int action_load_burst_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
79 79 {
80 80 /** This function updates the LFR registers with the incoming burst parameters.
81 81 *
82 82 * @param TC points to the TeleCommand packet that is being processed
83 83 * @param queue_id is the id of the queue which handles TM related to this execution step
84 84 *
85 85 */
86 86
87 87 int flag;
88 88 rtems_status_code status;
89 89 unsigned char sy_lfr_b_bp_p0;
90 90 unsigned char sy_lfr_b_bp_p1;
91 91 float aux;
92 92
93 93 flag = LFR_SUCCESSFUL;
94 94
95 95 if ( lfrCurrentMode == LFR_MODE_BURST ) {
96 96 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
97 97 flag = LFR_DEFAULT;
98 98 }
99 99
100 100 sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
101 101 sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
102 102
103 103 // sy_lfr_b_bp_p0 shall not be lower than its default value
104 104 if (flag == LFR_SUCCESSFUL)
105 105 {
106 106 if (sy_lfr_b_bp_p0 < DEFAULT_SY_LFR_B_BP_P0 )
107 107 {
108 108 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P0+10, sy_lfr_b_bp_p0 );
109 109 flag = WRONG_APP_DATA;
110 110 }
111 111 }
112 112 // sy_lfr_b_bp_p1 shall not be lower than its default value
113 113 if (flag == LFR_SUCCESSFUL)
114 114 {
115 115 if (sy_lfr_b_bp_p1 < DEFAULT_SY_LFR_B_BP_P1 )
116 116 {
117 117 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P1+10, sy_lfr_b_bp_p1 );
118 118 flag = WRONG_APP_DATA;
119 119 }
120 120 }
121 121 //****************************************************************
122 122 // check the consistency between sy_lfr_b_bp_p0 and sy_lfr_b_bp_p1
123 123 if (flag == LFR_SUCCESSFUL)
124 124 {
125 125 sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
126 126 sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
127 127 aux = ( (float ) sy_lfr_b_bp_p1 / sy_lfr_b_bp_p0 ) - floor(sy_lfr_b_bp_p1 / sy_lfr_b_bp_p0);
128 128 if (aux > FLOAT_EQUAL_ZERO)
129 129 {
130 130 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P0+10, sy_lfr_b_bp_p0 );
131 131 flag = LFR_DEFAULT;
132 132 }
133 133 }
134 134
135 135 // SET THE PARAMETERS
136 136 if (flag == LFR_SUCCESSFUL)
137 137 {
138 138 flag = set_sy_lfr_b_bp_p0( TC );
139 139 flag = set_sy_lfr_b_bp_p1( TC );
140 140 }
141 141
142 142 return flag;
143 143 }
144 144
145 145 int action_load_sbm1_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
146 146 {
147 147 /** This function updates the LFR registers with the incoming sbm1 parameters.
148 148 *
149 149 * @param TC points to the TeleCommand packet that is being processed
150 150 * @param queue_id is the id of the queue which handles TM related to this execution step
151 151 *
152 152 */
153 153
154 154 int flag;
155 155 rtems_status_code status;
156 156 unsigned char sy_lfr_s1_bp_p0;
157 157 unsigned char sy_lfr_s1_bp_p1;
158 158 float aux;
159 159
160 160 flag = LFR_SUCCESSFUL;
161 161
162 162 if ( lfrCurrentMode == LFR_MODE_SBM1 ) {
163 163 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
164 164 flag = LFR_DEFAULT;
165 165 }
166 166
167 167 sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
168 168 sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
169 169
170 170 // sy_lfr_s1_bp_p0
171 171 if (flag == LFR_SUCCESSFUL)
172 172 {
173 173 if (sy_lfr_s1_bp_p0 < DEFAULT_SY_LFR_S1_BP_P0 )
174 174 {
175 175 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P0+10, sy_lfr_s1_bp_p0 );
176 176 flag = WRONG_APP_DATA;
177 177 }
178 178 }
179 179 // sy_lfr_s1_bp_p1
180 180 if (flag == LFR_SUCCESSFUL)
181 181 {
182 182 if (sy_lfr_s1_bp_p1 < DEFAULT_SY_LFR_S1_BP_P1 )
183 183 {
184 184 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P1+10, sy_lfr_s1_bp_p1 );
185 185 flag = WRONG_APP_DATA;
186 186 }
187 187 }
188 188 //******************************************************************
189 189 // check the consistency between sy_lfr_s1_bp_p0 and sy_lfr_s1_bp_p1
190 190 if (flag == LFR_SUCCESSFUL)
191 191 {
192 192 aux = ( (float ) sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0*0.25) ) - floor(sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0*0.25));
193 193 if (aux > FLOAT_EQUAL_ZERO)
194 194 {
195 195 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P0+10, sy_lfr_s1_bp_p0 );
196 196 flag = LFR_DEFAULT;
197 197 }
198 198 }
199 199
200 200 // SET THE PARAMETERS
201 201 if (flag == LFR_SUCCESSFUL)
202 202 {
203 203 flag = set_sy_lfr_s1_bp_p0( TC );
204 204 flag = set_sy_lfr_s1_bp_p1( TC );
205 205 }
206 206
207 207 return flag;
208 208 }
209 209
210 210 int action_load_sbm2_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
211 211 {
212 212 /** This function updates the LFR registers with the incoming sbm2 parameters.
213 213 *
214 214 * @param TC points to the TeleCommand packet that is being processed
215 215 * @param queue_id is the id of the queue which handles TM related to this execution step
216 216 *
217 217 */
218 218
219 219 int flag;
220 220 rtems_status_code status;
221 221 unsigned char sy_lfr_s2_bp_p0;
222 222 unsigned char sy_lfr_s2_bp_p1;
223 223 float aux;
224 224
225 225 flag = LFR_SUCCESSFUL;
226 226
227 227 if ( lfrCurrentMode == LFR_MODE_SBM2 ) {
228 228 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
229 229 flag = LFR_DEFAULT;
230 230 }
231 231
232 232 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
233 233 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
234 234
235 235 // sy_lfr_s2_bp_p0
236 236 if (flag == LFR_SUCCESSFUL)
237 237 {
238 238 if (sy_lfr_s2_bp_p0 < DEFAULT_SY_LFR_S2_BP_P0 )
239 239 {
240 240 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0+10, sy_lfr_s2_bp_p0 );
241 241 flag = WRONG_APP_DATA;
242 242 }
243 243 }
244 244 // sy_lfr_s2_bp_p1
245 245 if (flag == LFR_SUCCESSFUL)
246 246 {
247 247 if (sy_lfr_s2_bp_p1 < DEFAULT_SY_LFR_S2_BP_P1 )
248 248 {
249 249 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P1+10, sy_lfr_s2_bp_p1 );
250 250 flag = WRONG_APP_DATA;
251 251 }
252 252 }
253 253 //******************************************************************
254 254 // check the consistency between sy_lfr_s2_bp_p0 and sy_lfr_s2_bp_p1
255 255 if (flag == LFR_SUCCESSFUL)
256 256 {
257 257 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
258 258 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
259 259 aux = ( (float ) sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0 ) - floor(sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0);
260 260 if (aux > FLOAT_EQUAL_ZERO)
261 261 {
262 262 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0+10, sy_lfr_s2_bp_p0 );
263 263 flag = LFR_DEFAULT;
264 264 }
265 265 }
266 266
267 267 // SET THE PARAMETERS
268 268 if (flag == LFR_SUCCESSFUL)
269 269 {
270 270 flag = set_sy_lfr_s2_bp_p0( TC );
271 271 flag = set_sy_lfr_s2_bp_p1( TC );
272 272 }
273 273
274 274 return flag;
275 275 }
276 276
277 277 int action_load_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
278 278 {
279 279 /** This function updates the LFR registers with the incoming sbm2 parameters.
280 280 *
281 281 * @param TC points to the TeleCommand packet that is being processed
282 282 * @param queue_id is the id of the queue which handles TM related to this execution step
283 283 *
284 284 */
285 285
286 286 int flag;
287 287
288 288 flag = LFR_DEFAULT;
289 289
290 290 flag = set_sy_lfr_kcoeff( TC, queue_id );
291 291
292 292 return flag;
293 293 }
294 294
295 295 int action_load_fbins_mask(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
296 296 {
297 297 /** This function updates the LFR registers with the incoming sbm2 parameters.
298 298 *
299 299 * @param TC points to the TeleCommand packet that is being processed
300 300 * @param queue_id is the id of the queue which handles TM related to this execution step
301 301 *
302 302 */
303 303
304 304 int flag;
305 305
306 306 flag = LFR_DEFAULT;
307 307
308 308 flag = set_sy_lfr_fbins( TC );
309 309
310 310 return flag;
311 311 }
312 312
313 313 int action_load_filter_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
314 314 {
315 315 /** This function updates the LFR registers with the incoming sbm2 parameters.
316 316 *
317 317 * @param TC points to the TeleCommand packet that is being processed
318 318 * @param queue_id is the id of the queue which handles TM related to this execution step
319 319 *
320 320 */
321 321
322 322 int flag;
323 323
324 324 flag = LFR_DEFAULT;
325 325
326 flag = check_sy_lfr_pas_filter_parameters( TC, queue_id );
326 flag = check_sy_lfr_filter_parameters( TC, queue_id );
327 327
328 328 if (flag == LFR_SUCCESSFUL)
329 329 {
330 330 parameter_dump_packet.spare_sy_lfr_pas_filter_enabled = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED ];
331 331 parameter_dump_packet.sy_lfr_pas_filter_modulus = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS ];
332 332 parameter_dump_packet.sy_lfr_pas_filter_tbad[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + 0 ];
333 333 parameter_dump_packet.sy_lfr_pas_filter_tbad[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + 1 ];
334 334 parameter_dump_packet.sy_lfr_pas_filter_tbad[2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + 2 ];
335 335 parameter_dump_packet.sy_lfr_pas_filter_tbad[3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + 3 ];
336 336 parameter_dump_packet.sy_lfr_pas_filter_offset = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET ];
337 337 parameter_dump_packet.sy_lfr_pas_filter_shift[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + 0 ];
338 338 parameter_dump_packet.sy_lfr_pas_filter_shift[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + 1 ];
339 339 parameter_dump_packet.sy_lfr_pas_filter_shift[2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + 2 ];
340 340 parameter_dump_packet.sy_lfr_pas_filter_shift[3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + 3 ];
341 341 parameter_dump_packet.sy_lfr_sc_rw_delta_f[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + 0 ];
342 342 parameter_dump_packet.sy_lfr_sc_rw_delta_f[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + 1 ];
343 343 parameter_dump_packet.sy_lfr_sc_rw_delta_f[2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + 2 ];
344 344 parameter_dump_packet.sy_lfr_sc_rw_delta_f[3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + 3 ];
345 345 }
346 346
347 347 return flag;
348 348 }
349 349
350 350 int action_dump_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
351 351 {
352 352 /** This function updates the LFR registers with the incoming sbm2 parameters.
353 353 *
354 354 * @param TC points to the TeleCommand packet that is being processed
355 355 * @param queue_id is the id of the queue which handles TM related to this execution step
356 356 *
357 357 */
358 358
359 359 unsigned int address;
360 360 rtems_status_code status;
361 361 unsigned int freq;
362 362 unsigned int bin;
363 363 unsigned int coeff;
364 364 unsigned char *kCoeffPtr;
365 365 unsigned char *kCoeffDumpPtr;
366 366
367 367 // for each sy_lfr_kcoeff_frequency there is 32 kcoeff
368 368 // F0 => 11 bins
369 369 // F1 => 13 bins
370 370 // F2 => 12 bins
371 371 // 36 bins to dump in two packets (30 bins max per packet)
372 372
373 373 //*********
374 374 // PACKET 1
375 375 // 11 F0 bins, 13 F1 bins and 6 F2 bins
376 376 kcoefficients_dump_1.destinationID = TC->sourceID;
377 377 increment_seq_counter_destination_id_dump( kcoefficients_dump_1.packetSequenceControl, TC->sourceID );
378 378 for( freq=0;
379 379 freq<NB_BINS_COMPRESSED_SM_F0;
380 380 freq++ )
381 381 {
382 382 kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1] = freq;
383 383 bin = freq;
384 384 // printKCoefficients( freq, bin, k_coeff_intercalib_f0_norm);
385 385 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
386 386 {
387 387 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
388 388 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f0_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
389 389 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
390 390 }
391 391 }
392 392 for( freq=NB_BINS_COMPRESSED_SM_F0;
393 393 freq<(NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1);
394 394 freq++ )
395 395 {
396 396 kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1 ] = freq;
397 397 bin = freq - NB_BINS_COMPRESSED_SM_F0;
398 398 // printKCoefficients( freq, bin, k_coeff_intercalib_f1_norm);
399 399 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
400 400 {
401 401 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
402 402 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f1_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
403 403 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
404 404 }
405 405 }
406 406 for( freq=(NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1);
407 407 freq<(NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1+6);
408 408 freq++ )
409 409 {
410 410 kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1 ] = freq;
411 411 bin = freq - (NB_BINS_COMPRESSED_SM_F0+NB_BINS_COMPRESSED_SM_F1);
412 412 // printKCoefficients( freq, bin, k_coeff_intercalib_f2);
413 413 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
414 414 {
415 415 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
416 416 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
417 417 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
418 418 }
419 419 }
420 420 kcoefficients_dump_1.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
421 421 kcoefficients_dump_1.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
422 422 kcoefficients_dump_1.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
423 423 kcoefficients_dump_1.time[3] = (unsigned char) (time_management_regs->coarse_time);
424 424 kcoefficients_dump_1.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
425 425 kcoefficients_dump_1.time[5] = (unsigned char) (time_management_regs->fine_time);
426 426 // SEND DATA
427 427 kcoefficient_node_1.status = 1;
428 428 address = (unsigned int) &kcoefficient_node_1;
429 429 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
430 430 if (status != RTEMS_SUCCESSFUL) {
431 431 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 1 , code %d", status)
432 432 }
433 433
434 434 //********
435 435 // PACKET 2
436 436 // 6 F2 bins
437 437 kcoefficients_dump_2.destinationID = TC->sourceID;
438 438 increment_seq_counter_destination_id_dump( kcoefficients_dump_2.packetSequenceControl, TC->sourceID );
439 439 for( freq=0; freq<6; freq++ )
440 440 {
441 441 kcoefficients_dump_2.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + 1 ] = NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 + 6 + freq;
442 442 bin = freq + 6;
443 443 // printKCoefficients( freq, bin, k_coeff_intercalib_f2);
444 444 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
445 445 {
446 446 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_2.kcoeff_blks[ freq*KCOEFF_BLK_SIZE + coeff*NB_BYTES_PER_FLOAT + 2 ]; // 2 for the kcoeff_frequency
447 447 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
448 448 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
449 449 }
450 450 }
451 451 kcoefficients_dump_2.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
452 452 kcoefficients_dump_2.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
453 453 kcoefficients_dump_2.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
454 454 kcoefficients_dump_2.time[3] = (unsigned char) (time_management_regs->coarse_time);
455 455 kcoefficients_dump_2.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
456 456 kcoefficients_dump_2.time[5] = (unsigned char) (time_management_regs->fine_time);
457 457 // SEND DATA
458 458 kcoefficient_node_2.status = 1;
459 459 address = (unsigned int) &kcoefficient_node_2;
460 460 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
461 461 if (status != RTEMS_SUCCESSFUL) {
462 462 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 2, code %d", status)
463 463 }
464 464
465 465 return status;
466 466 }
467 467
468 468 int action_dump_par( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
469 469 {
470 470 /** This function dumps the LFR parameters by sending the appropriate TM packet to the dedicated RTEMS message queue.
471 471 *
472 472 * @param queue_id is the id of the queue which handles TM related to this execution step.
473 473 *
474 474 * @return RTEMS directive status codes:
475 475 * - RTEMS_SUCCESSFUL - message sent successfully
476 476 * - RTEMS_INVALID_ID - invalid queue id
477 477 * - RTEMS_INVALID_SIZE - invalid message size
478 478 * - RTEMS_INVALID_ADDRESS - buffer is NULL
479 479 * - RTEMS_UNSATISFIED - out of message buffers
480 480 * - RTEMS_TOO_MANY - queue s limit has been reached
481 481 *
482 482 */
483 483
484 484 int status;
485 485
486 486 increment_seq_counter_destination_id_dump( parameter_dump_packet.packetSequenceControl, TC->sourceID );
487 487 parameter_dump_packet.destinationID = TC->sourceID;
488 488
489 489 // UPDATE TIME
490 490 parameter_dump_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
491 491 parameter_dump_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
492 492 parameter_dump_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
493 493 parameter_dump_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
494 494 parameter_dump_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
495 495 parameter_dump_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
496 496 // SEND DATA
497 497 status = rtems_message_queue_send( queue_id, &parameter_dump_packet,
498 498 PACKET_LENGTH_PARAMETER_DUMP + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
499 499 if (status != RTEMS_SUCCESSFUL) {
500 500 PRINTF1("in action_dump *** ERR sending packet, code %d", status)
501 501 }
502 502
503 503 return status;
504 504 }
505 505
506 506 //***********************
507 507 // NORMAL MODE PARAMETERS
508 508
509 509 int check_normal_par_consistency( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
510 510 {
511 511 unsigned char msb;
512 512 unsigned char lsb;
513 513 int flag;
514 514 float aux;
515 515 rtems_status_code status;
516 516
517 517 unsigned int sy_lfr_n_swf_l;
518 518 unsigned int sy_lfr_n_swf_p;
519 519 unsigned int sy_lfr_n_asm_p;
520 520 unsigned char sy_lfr_n_bp_p0;
521 521 unsigned char sy_lfr_n_bp_p1;
522 522 unsigned char sy_lfr_n_cwf_long_f3;
523 523
524 524 flag = LFR_SUCCESSFUL;
525 525
526 526 //***************
527 527 // get parameters
528 528 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
529 529 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
530 530 sy_lfr_n_swf_l = msb * 256 + lsb;
531 531
532 532 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
533 533 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
534 534 sy_lfr_n_swf_p = msb * 256 + lsb;
535 535
536 536 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
537 537 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
538 538 sy_lfr_n_asm_p = msb * 256 + lsb;
539 539
540 540 sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
541 541
542 542 sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
543 543
544 544 sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
545 545
546 546 //******************
547 547 // check consistency
548 548 // sy_lfr_n_swf_l
549 549 if (sy_lfr_n_swf_l != 2048)
550 550 {
551 551 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_L+10, sy_lfr_n_swf_l );
552 552 flag = WRONG_APP_DATA;
553 553 }
554 554 // sy_lfr_n_swf_p
555 555 if (flag == LFR_SUCCESSFUL)
556 556 {
557 557 if ( sy_lfr_n_swf_p < 22 )
558 558 {
559 559 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_P+10, sy_lfr_n_swf_p );
560 560 flag = WRONG_APP_DATA;
561 561 }
562 562 }
563 563 // sy_lfr_n_bp_p0
564 564 if (flag == LFR_SUCCESSFUL)
565 565 {
566 566 if (sy_lfr_n_bp_p0 < DFLT_SY_LFR_N_BP_P0)
567 567 {
568 568 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P0+10, sy_lfr_n_bp_p0 );
569 569 flag = WRONG_APP_DATA;
570 570 }
571 571 }
572 572 // sy_lfr_n_asm_p
573 573 if (flag == LFR_SUCCESSFUL)
574 574 {
575 575 if (sy_lfr_n_asm_p == 0)
576 576 {
577 577 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P+10, sy_lfr_n_asm_p );
578 578 flag = WRONG_APP_DATA;
579 579 }
580 580 }
581 581 // sy_lfr_n_asm_p shall be a whole multiple of sy_lfr_n_bp_p0
582 582 if (flag == LFR_SUCCESSFUL)
583 583 {
584 584 aux = ( (float ) sy_lfr_n_asm_p / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_asm_p / sy_lfr_n_bp_p0);
585 585 if (aux > FLOAT_EQUAL_ZERO)
586 586 {
587 587 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P+10, sy_lfr_n_asm_p );
588 588 flag = WRONG_APP_DATA;
589 589 }
590 590 }
591 591 // sy_lfr_n_bp_p1
592 592 if (flag == LFR_SUCCESSFUL)
593 593 {
594 594 if (sy_lfr_n_bp_p1 < DFLT_SY_LFR_N_BP_P1)
595 595 {
596 596 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1+10, sy_lfr_n_bp_p1 );
597 597 flag = WRONG_APP_DATA;
598 598 }
599 599 }
600 600 // sy_lfr_n_bp_p1 shall be a whole multiple of sy_lfr_n_bp_p0
601 601 if (flag == LFR_SUCCESSFUL)
602 602 {
603 603 aux = ( (float ) sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0);
604 604 if (aux > FLOAT_EQUAL_ZERO)
605 605 {
606 606 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1+10, sy_lfr_n_bp_p1 );
607 607 flag = LFR_DEFAULT;
608 608 }
609 609 }
610 610 // sy_lfr_n_cwf_long_f3
611 611
612 612 return flag;
613 613 }
614 614
615 615 int set_sy_lfr_n_swf_l( ccsdsTelecommandPacket_t *TC )
616 616 {
617 617 /** This function sets the number of points of a snapshot (sy_lfr_n_swf_l).
618 618 *
619 619 * @param TC points to the TeleCommand packet that is being processed
620 620 * @param queue_id is the id of the queue which handles TM related to this execution step
621 621 *
622 622 */
623 623
624 624 int result;
625 625
626 626 result = LFR_SUCCESSFUL;
627 627
628 628 parameter_dump_packet.sy_lfr_n_swf_l[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
629 629 parameter_dump_packet.sy_lfr_n_swf_l[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
630 630
631 631 return result;
632 632 }
633 633
634 634 int set_sy_lfr_n_swf_p(ccsdsTelecommandPacket_t *TC )
635 635 {
636 636 /** This function sets the time between two snapshots, in s (sy_lfr_n_swf_p).
637 637 *
638 638 * @param TC points to the TeleCommand packet that is being processed
639 639 * @param queue_id is the id of the queue which handles TM related to this execution step
640 640 *
641 641 */
642 642
643 643 int result;
644 644
645 645 result = LFR_SUCCESSFUL;
646 646
647 647 parameter_dump_packet.sy_lfr_n_swf_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
648 648 parameter_dump_packet.sy_lfr_n_swf_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
649 649
650 650 return result;
651 651 }
652 652
653 653 int set_sy_lfr_n_asm_p( ccsdsTelecommandPacket_t *TC )
654 654 {
655 655 /** This function sets the time between two full spectral matrices transmission, in s (SY_LFR_N_ASM_P).
656 656 *
657 657 * @param TC points to the TeleCommand packet that is being processed
658 658 * @param queue_id is the id of the queue which handles TM related to this execution step
659 659 *
660 660 */
661 661
662 662 int result;
663 663
664 664 result = LFR_SUCCESSFUL;
665 665
666 666 parameter_dump_packet.sy_lfr_n_asm_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
667 667 parameter_dump_packet.sy_lfr_n_asm_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
668 668
669 669 return result;
670 670 }
671 671
672 672 int set_sy_lfr_n_bp_p0( ccsdsTelecommandPacket_t *TC )
673 673 {
674 674 /** This function sets the time between two basic parameter sets, in s (DFLT_SY_LFR_N_BP_P0).
675 675 *
676 676 * @param TC points to the TeleCommand packet that is being processed
677 677 * @param queue_id is the id of the queue which handles TM related to this execution step
678 678 *
679 679 */
680 680
681 681 int status;
682 682
683 683 status = LFR_SUCCESSFUL;
684 684
685 685 parameter_dump_packet.sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
686 686
687 687 return status;
688 688 }
689 689
690 690 int set_sy_lfr_n_bp_p1(ccsdsTelecommandPacket_t *TC )
691 691 {
692 692 /** This function sets the time between two basic parameter sets (autocorrelation + crosscorrelation), in s (sy_lfr_n_bp_p1).
693 693 *
694 694 * @param TC points to the TeleCommand packet that is being processed
695 695 * @param queue_id is the id of the queue which handles TM related to this execution step
696 696 *
697 697 */
698 698
699 699 int status;
700 700
701 701 status = LFR_SUCCESSFUL;
702 702
703 703 parameter_dump_packet.sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
704 704
705 705 return status;
706 706 }
707 707
708 708 int set_sy_lfr_n_cwf_long_f3(ccsdsTelecommandPacket_t *TC )
709 709 {
710 710 /** This function allows to switch from CWF_F3 packets to CWF_LONG_F3 packets.
711 711 *
712 712 * @param TC points to the TeleCommand packet that is being processed
713 713 * @param queue_id is the id of the queue which handles TM related to this execution step
714 714 *
715 715 */
716 716
717 717 int status;
718 718
719 719 status = LFR_SUCCESSFUL;
720 720
721 721 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
722 722
723 723 return status;
724 724 }
725 725
726 726 //**********************
727 727 // BURST MODE PARAMETERS
728 728 int set_sy_lfr_b_bp_p0(ccsdsTelecommandPacket_t *TC)
729 729 {
730 730 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P0).
731 731 *
732 732 * @param TC points to the TeleCommand packet that is being processed
733 733 * @param queue_id is the id of the queue which handles TM related to this execution step
734 734 *
735 735 */
736 736
737 737 int status;
738 738
739 739 status = LFR_SUCCESSFUL;
740 740
741 741 parameter_dump_packet.sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
742 742
743 743 return status;
744 744 }
745 745
746 746 int set_sy_lfr_b_bp_p1( ccsdsTelecommandPacket_t *TC )
747 747 {
748 748 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P1).
749 749 *
750 750 * @param TC points to the TeleCommand packet that is being processed
751 751 * @param queue_id is the id of the queue which handles TM related to this execution step
752 752 *
753 753 */
754 754
755 755 int status;
756 756
757 757 status = LFR_SUCCESSFUL;
758 758
759 759 parameter_dump_packet.sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
760 760
761 761 return status;
762 762 }
763 763
764 764 //*********************
765 765 // SBM1 MODE PARAMETERS
766 766 int set_sy_lfr_s1_bp_p0( ccsdsTelecommandPacket_t *TC )
767 767 {
768 768 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P0).
769 769 *
770 770 * @param TC points to the TeleCommand packet that is being processed
771 771 * @param queue_id is the id of the queue which handles TM related to this execution step
772 772 *
773 773 */
774 774
775 775 int status;
776 776
777 777 status = LFR_SUCCESSFUL;
778 778
779 779 parameter_dump_packet.sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
780 780
781 781 return status;
782 782 }
783 783
784 784 int set_sy_lfr_s1_bp_p1( ccsdsTelecommandPacket_t *TC )
785 785 {
786 786 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P1).
787 787 *
788 788 * @param TC points to the TeleCommand packet that is being processed
789 789 * @param queue_id is the id of the queue which handles TM related to this execution step
790 790 *
791 791 */
792 792
793 793 int status;
794 794
795 795 status = LFR_SUCCESSFUL;
796 796
797 797 parameter_dump_packet.sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
798 798
799 799 return status;
800 800 }
801 801
802 802 //*********************
803 803 // SBM2 MODE PARAMETERS
804 804 int set_sy_lfr_s2_bp_p0( ccsdsTelecommandPacket_t *TC )
805 805 {
806 806 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P0).
807 807 *
808 808 * @param TC points to the TeleCommand packet that is being processed
809 809 * @param queue_id is the id of the queue which handles TM related to this execution step
810 810 *
811 811 */
812 812
813 813 int status;
814 814
815 815 status = LFR_SUCCESSFUL;
816 816
817 817 parameter_dump_packet.sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
818 818
819 819 return status;
820 820 }
821 821
822 822 int set_sy_lfr_s2_bp_p1( ccsdsTelecommandPacket_t *TC )
823 823 {
824 824 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P1).
825 825 *
826 826 * @param TC points to the TeleCommand packet that is being processed
827 827 * @param queue_id is the id of the queue which handles TM related to this execution step
828 828 *
829 829 */
830 830
831 831 int status;
832 832
833 833 status = LFR_SUCCESSFUL;
834 834
835 835 parameter_dump_packet.sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
836 836
837 837 return status;
838 838 }
839 839
840 840 //*******************
841 841 // TC_LFR_UPDATE_INFO
842 842 unsigned int check_update_info_hk_lfr_mode( unsigned char mode )
843 843 {
844 844 unsigned int status;
845 845
846 846 if ( (mode == LFR_MODE_STANDBY) || (mode == LFR_MODE_NORMAL)
847 847 || (mode == LFR_MODE_BURST)
848 848 || (mode == LFR_MODE_SBM1) || (mode == LFR_MODE_SBM2))
849 849 {
850 850 status = LFR_SUCCESSFUL;
851 851 }
852 852 else
853 853 {
854 854 status = LFR_DEFAULT;
855 855 }
856 856
857 857 return status;
858 858 }
859 859
860 860 unsigned int check_update_info_hk_tds_mode( unsigned char mode )
861 861 {
862 862 unsigned int status;
863 863
864 864 if ( (mode == TDS_MODE_STANDBY) || (mode == TDS_MODE_NORMAL)
865 865 || (mode == TDS_MODE_BURST)
866 866 || (mode == TDS_MODE_SBM1) || (mode == TDS_MODE_SBM2)
867 867 || (mode == TDS_MODE_LFM))
868 868 {
869 869 status = LFR_SUCCESSFUL;
870 870 }
871 871 else
872 872 {
873 873 status = LFR_DEFAULT;
874 874 }
875 875
876 876 return status;
877 877 }
878 878
879 879 unsigned int check_update_info_hk_thr_mode( unsigned char mode )
880 880 {
881 881 unsigned int status;
882 882
883 883 if ( (mode == THR_MODE_STANDBY) || (mode == THR_MODE_NORMAL)
884 884 || (mode == THR_MODE_BURST))
885 885 {
886 886 status = LFR_SUCCESSFUL;
887 887 }
888 888 else
889 889 {
890 890 status = LFR_DEFAULT;
891 891 }
892 892
893 893 return status;
894 894 }
895 895
896 896 void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC )
897 897 {
898 898 /** This function get the reaction wheels frequencies in the incoming TC_LFR_UPDATE_INFO and copy the values locally.
899 899 *
900 900 * @param TC points to the TeleCommand packet that is being processed
901 901 *
902 902 */
903 903
904 904 unsigned char * bytePosPtr; // pointer to the beginning of the incoming TC packet
905 905 unsigned char * floatPtr; // pointer to the Most Significant Byte of the considered float
906 906
907 907 bytePosPtr = (unsigned char *) &TC->packetID;
908 908
909 909 // cp_rpw_sc_rw1_f1
910 floatPtr = (unsigned char *) &cp_rpw_sc_rw1_f1;
911 floatPtr[0] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1 ];
912 floatPtr[1] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1 + 1 ];
913 floatPtr[2] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1 + 2 ];
914 floatPtr[3] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1 + 3 ];
910 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw1_f1,
911 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1 ] );
912
915 913 // cp_rpw_sc_rw1_f2
916 floatPtr = (unsigned char *) &cp_rpw_sc_rw1_f2;
917 floatPtr[0] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2 ];
918 floatPtr[1] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2 + 1 ];
919 floatPtr[2] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2 + 2 ];
920 floatPtr[3] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2 + 3 ];
914 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw1_f2,
915 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2 ] );
916
921 917 // cp_rpw_sc_rw2_f1
922 floatPtr = (unsigned char *) &cp_rpw_sc_rw2_f1;
923 floatPtr[0] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1 ];
924 floatPtr[1] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1 + 1 ];
925 floatPtr[2] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1 + 2 ];
926 floatPtr[3] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1 + 3 ];
918 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw2_f1,
919 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1 ] );
920
927 921 // cp_rpw_sc_rw2_f2
928 floatPtr = (unsigned char *) &cp_rpw_sc_rw2_f2;
929 floatPtr[0] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2 ];
930 floatPtr[1] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2 + 1 ];
931 floatPtr[2] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2 + 2 ];
932 floatPtr[3] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2 + 3 ];
922 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw2_f2,
923 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2 ] );
924
933 925 // cp_rpw_sc_rw3_f1
934 floatPtr = (unsigned char *) &cp_rpw_sc_rw3_f1;
935 floatPtr[0] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1 ];
936 floatPtr[1] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1 + 1 ];
937 floatPtr[2] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1 + 2 ];
938 floatPtr[3] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1 + 3 ];
926 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw3_f1,
927 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1 ] );
928
939 929 // cp_rpw_sc_rw3_f2
940 floatPtr = (unsigned char *) &cp_rpw_sc_rw3_f2;
941 floatPtr[0] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2 ];
942 floatPtr[1] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2 + 1 ];
943 floatPtr[2] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2 + 2 ];
944 floatPtr[3] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2 + 3 ];
930 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw3_f2,
931 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2 ] );
932
945 933 // cp_rpw_sc_rw4_f1
946 floatPtr = (unsigned char *) &cp_rpw_sc_rw4_f1;
947 floatPtr[0] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1 ];
948 floatPtr[1] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1 + 1 ];
949 floatPtr[2] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1 + 2 ];
950 floatPtr[3] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1 + 3 ];
934 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw4_f1,
935 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1 ] );
936
951 937 // cp_rpw_sc_rw4_f2
952 floatPtr = (unsigned char *) &cp_rpw_sc_rw4_f2;
953 floatPtr[0] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 ];
954 floatPtr[1] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 + 1 ];
955 floatPtr[2] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 + 2 ];
956 floatPtr[3] = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 + 3 ];
938 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw4_f2,
939 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 ] );
957 940 }
958 941
959 942 void setFBinMask( unsigned char *fbins_mask, float freq, unsigned char deltaFreq, unsigned char flag )
960 943 {
944 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
945 *
946 * @param fbins_mask
947 * @param freq
948 * @param deltaFreq
949 * @param flag [true] filtering enabled [false] filtering disabled
950 *
951 * @return void
952 *
953 */
954
961 955 unsigned int fBelow;
962 956
963 957 // compute the index of the frequency immediately below the reaction wheel frequency
964 958 fBelow = (unsigned int) ( floor( ((double) cp_rpw_sc_rw1_f1) / ((double) deltaFreq)) );
965 959
966 960 if (fBelow < 127) // if fbelow is greater than 127 or equal to 127, this means that the reaction wheel frequency is outside the frequency range
967 961 {
968 962 if (flag == 1)
969 963 {
970 964 // rw_fbins_mask[k] = (1 << fBelow) | (1 << fAbove);
971 965 }
972 966 }
973 967 }
974 968
975 969 void build_rw_fbins_mask( unsigned int channel )
976 970 {
977 971 unsigned char rw_fbins_mask[16];
978 972 unsigned char *maskPtr;
979 973 double deltaF;
980 974 unsigned k;
981 975
982 976 k = 0;
983 977
984 978 switch (channel)
985 979 {
986 980 case 0:
987 981 maskPtr = rw_fbins_mask_f0;
988 982 deltaF = 96.;
989 983 break;
990 984 case 1:
991 985 maskPtr = rw_fbins_mask_f1;
992 986 deltaF = 16.;
993 987 break;
994 988 case 2:
995 989 maskPtr = rw_fbins_mask_f2;
996 990 deltaF = 1.;
997 991 break;
998 992 default:
999 993 break;
1000 994 }
1001 995
1002 996 for (k = 0; k < 16; k++)
1003 997 {
1004 998 rw_fbins_mask[k] = 0x00;
1005 999 }
1006 1000
1007 1001 // RW1 F1
1008 // setFBinMask( rw_fbins_mask, fBelow );
1002 setFBinMask( rw_fbins_mask, fBelow );
1009 1003
1010 1004 // RW1 F2
1011 1005
1012 1006 // RW2 F1
1013 1007
1014 1008 // RW2 F2
1015 1009
1016 1010 // RW3 F1
1017 1011
1018 1012 // RW3 F2
1019 1013
1020 1014 // RW4 F1
1021 1015
1022 1016 // RW4 F2
1023 1017
1024 1018
1025 1019 // update the value of the fbins related to reaction wheels frequency filtering
1026 1020 for (k = 0; k < 16; k++)
1027 1021 {
1028 1022 maskPtr[k] = rw_fbins_mask[k];
1029 1023 }
1030 1024 }
1031 1025
1032 1026 void build_rw_fbins_masks()
1033 1027 {
1034 1028 build_rw_fbins_mask( 0 );
1035 1029 build_rw_fbins_mask( 1 );
1036 1030 build_rw_fbins_mask( 2 );
1037 1031 }
1038 1032
1039 1033 //***********
1040 1034 // FBINS MASK
1041 1035
1042 1036 int set_sy_lfr_fbins( ccsdsTelecommandPacket_t *TC )
1043 1037 {
1044 1038 int status;
1045 1039 unsigned int k;
1046 1040 unsigned char *fbins_mask_dump;
1047 1041 unsigned char *fbins_mask_TC;
1048 1042
1049 1043 status = LFR_SUCCESSFUL;
1050 1044
1051 1045 fbins_mask_dump = parameter_dump_packet.sy_lfr_fbins_f0_word1;
1052 1046 fbins_mask_TC = TC->dataAndCRC;
1053 1047
1054 1048 for (k=0; k < NB_FBINS_MASKS * NB_BYTES_PER_FBINS_MASK; k++)
1055 1049 {
1056 1050 fbins_mask_dump[k] = fbins_mask_TC[k];
1057 1051 }
1058 1052 for (k=0; k < NB_FBINS_MASKS; k++)
1059 1053 {
1060 1054 unsigned char *auxPtr;
1061 1055 auxPtr = &parameter_dump_packet.sy_lfr_fbins_f0_word1[k*NB_BYTES_PER_FBINS_MASK];
1062 1056 }
1063 1057
1064 1058
1065 1059 return status;
1066 1060 }
1067 1061
1068 1062 //***************************
1069 1063 // TC_LFR_LOAD_PAS_FILTER_PAR
1070 1064
1071 int check_sy_lfr_pas_filter_parameters( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
1065 int check_sy_lfr_filter_parameters( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
1072 1066 {
1073 1067 int flag;
1074 1068 rtems_status_code status;
1075 1069
1076 1070 unsigned char sy_lfr_pas_filter_enabled;
1077 1071 unsigned char sy_lfr_pas_filter_modulus;
1078 1072 float sy_lfr_pas_filter_tbad;
1079 1073 unsigned char sy_lfr_pas_filter_offset;
1080 float sy_lfr_pas_filtershift;
1074 float sy_lfr_pas_filter_shift;
1081 1075 float sy_lfr_sc_rw_delta_f;
1076 char *parPtr;
1082 1077
1083 1078 flag = LFR_SUCCESSFUL;
1079 sy_lfr_pas_filter_tbad = 0.0;
1080 sy_lfr_pas_filter_shift = 0.0;
1081 sy_lfr_sc_rw_delta_f = 0.0;
1082 parPtr = NULL;
1084 1083
1085 1084 //***************
1086 1085 // get parameters
1087 1086 sy_lfr_pas_filter_enabled = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED ] & 0x01; // [0000 0001]
1088 1087 sy_lfr_pas_filter_modulus = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS ];
1089
1088 copyFloatByChar(
1089 (char*) &sy_lfr_pas_filter_tbad,
1090 (char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD ]
1091 );
1090 1092 sy_lfr_pas_filter_offset = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET ];
1093 copyFloatByChar(
1094 (char*) &sy_lfr_pas_filter_shift,
1095 (char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT ]
1096 );
1097 copyFloatByChar(
1098 (char*) &sy_lfr_sc_rw_delta_f,
1099 (char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F ]
1100 );
1091 1101
1092 1102 //******************
1093 // check consistency
1103 // CHECK CONSISTENCY
1104
1105 //**************************
1094 1106 // sy_lfr_pas_filter_enabled
1107 // nothing to check, value is 0 or 1
1108
1109 //**************************
1095 1110 // sy_lfr_pas_filter_modulus
1096 1111 if ( (sy_lfr_pas_filter_modulus < 4) || (sy_lfr_pas_filter_modulus > 8) )
1097 1112 {
1098 1113 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS+10, sy_lfr_pas_filter_modulus );
1099 1114 flag = WRONG_APP_DATA;
1100 1115 }
1116
1117 //***********************
1101 1118 // sy_lfr_pas_filter_tbad
1119 if ( (sy_lfr_pas_filter_tbad < 0.0) || (sy_lfr_pas_filter_tbad > 4.0) )
1120 {
1121 parPtr = (char*) &sy_lfr_pas_filter_tbad;
1122 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD+10, parPtr[3] );
1123 flag = WRONG_APP_DATA;
1124 }
1125
1126 //*************************
1102 1127 // sy_lfr_pas_filter_offset
1103 1128 if (flag == LFR_SUCCESSFUL)
1104 1129 {
1105 if (sy_lfr_pas_filter_offset > 7)
1130 if ( (sy_lfr_pas_filter_offset < 0) || (sy_lfr_pas_filter_offset > 7) )
1106 1131 {
1107 1132 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET+10, sy_lfr_pas_filter_offset );
1108 1133 flag = WRONG_APP_DATA;
1109 1134 }
1110 1135 }
1111 // sy_lfr_pas_filtershift
1136
1137 //************************
1138 // sy_lfr_pas_filter_shift
1139 if ( (sy_lfr_pas_filter_shift < 0.0) || (sy_lfr_pas_filter_shift > 1.0) )
1140 {
1141 parPtr = (char*) &sy_lfr_pas_filter_shift;
1142 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT+10, parPtr[3] );
1143 flag = WRONG_APP_DATA;
1144 }
1145
1146 //*********************
1112 1147 // sy_lfr_sc_rw_delta_f
1148 // nothing to check, no default value in the ICD
1113 1149
1114 1150 return flag;
1115 1151 }
1116 1152
1117 1153 //**************
1118 1154 // KCOEFFICIENTS
1119 1155 int set_sy_lfr_kcoeff( ccsdsTelecommandPacket_t *TC,rtems_id queue_id )
1120 1156 {
1121 1157 unsigned int kcoeff;
1122 1158 unsigned short sy_lfr_kcoeff_frequency;
1123 1159 unsigned short bin;
1124 1160 unsigned short *freqPtr;
1125 1161 float *kcoeffPtr_norm;
1126 1162 float *kcoeffPtr_sbm;
1127 1163 int status;
1128 1164 unsigned char *kcoeffLoadPtr;
1129 1165 unsigned char *kcoeffNormPtr;
1130 1166 unsigned char *kcoeffSbmPtr_a;
1131 1167 unsigned char *kcoeffSbmPtr_b;
1132 1168
1133 1169 status = LFR_SUCCESSFUL;
1134 1170
1135 1171 kcoeffPtr_norm = NULL;
1136 1172 kcoeffPtr_sbm = NULL;
1137 1173 bin = 0;
1138 1174
1139 1175 freqPtr = (unsigned short *) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY];
1140 1176 sy_lfr_kcoeff_frequency = *freqPtr;
1141 1177
1142 1178 if ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM )
1143 1179 {
1144 1180 PRINTF1("ERR *** in set_sy_lfr_kcoeff_frequency *** sy_lfr_kcoeff_frequency = %d\n", sy_lfr_kcoeff_frequency)
1145 1181 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + 10 + 1,
1146 1182 TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + 1] ); // +1 to get the LSB instead of the MSB
1147 1183 status = LFR_DEFAULT;
1148 1184 }
1149 1185 else
1150 1186 {
1151 1187 if ( ( sy_lfr_kcoeff_frequency >= 0 )
1152 1188 && ( sy_lfr_kcoeff_frequency < NB_BINS_COMPRESSED_SM_F0 ) )
1153 1189 {
1154 1190 kcoeffPtr_norm = k_coeff_intercalib_f0_norm;
1155 1191 kcoeffPtr_sbm = k_coeff_intercalib_f0_sbm;
1156 1192 bin = sy_lfr_kcoeff_frequency;
1157 1193 }
1158 1194 else if ( ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM_F0 )
1159 1195 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) ) )
1160 1196 {
1161 1197 kcoeffPtr_norm = k_coeff_intercalib_f1_norm;
1162 1198 kcoeffPtr_sbm = k_coeff_intercalib_f1_sbm;
1163 1199 bin = sy_lfr_kcoeff_frequency - NB_BINS_COMPRESSED_SM_F0;
1164 1200 }
1165 1201 else if ( ( sy_lfr_kcoeff_frequency >= (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) )
1166 1202 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 + NB_BINS_COMPRESSED_SM_F2) ) )
1167 1203 {
1168 1204 kcoeffPtr_norm = k_coeff_intercalib_f2;
1169 1205 kcoeffPtr_sbm = NULL;
1170 1206 bin = sy_lfr_kcoeff_frequency - (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1);
1171 1207 }
1172 1208 }
1173 1209
1174 1210 if (kcoeffPtr_norm != NULL ) // update K coefficient for NORMAL data products
1175 1211 {
1176 1212 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
1177 1213 {
1178 1214 // destination
1179 1215 kcoeffNormPtr = (unsigned char*) &kcoeffPtr_norm[ (bin * NB_K_COEFF_PER_BIN) + kcoeff ];
1180 1216 // source
1181 1217 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + NB_BYTES_PER_FLOAT * kcoeff];
1182 1218 // copy source to destination
1183 1219 copyFloatByChar( kcoeffNormPtr, kcoeffLoadPtr );
1184 1220 }
1185 1221 }
1186 1222
1187 1223 if (kcoeffPtr_sbm != NULL ) // update K coefficient for SBM data products
1188 1224 {
1189 1225 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
1190 1226 {
1191 1227 // destination
1192 1228 kcoeffSbmPtr_a= (unsigned char*) &kcoeffPtr_sbm[ ( (bin * NB_K_COEFF_PER_BIN) + kcoeff) * 2 ];
1193 1229 kcoeffSbmPtr_b= (unsigned char*) &kcoeffPtr_sbm[ ( (bin * NB_K_COEFF_PER_BIN) + kcoeff) * 2 + 1 ];
1194 1230 // source
1195 1231 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + NB_BYTES_PER_FLOAT * kcoeff];
1196 1232 // copy source to destination
1197 1233 copyFloatByChar( kcoeffSbmPtr_a, kcoeffLoadPtr );
1198 1234 copyFloatByChar( kcoeffSbmPtr_b, kcoeffLoadPtr );
1199 1235 }
1200 1236 }
1201 1237
1202 1238 // print_k_coeff();
1203 1239
1204 1240 return status;
1205 1241 }
1206 1242
1207 1243 void copyFloatByChar( unsigned char *destination, unsigned char *source )
1208 1244 {
1209 1245 destination[0] = source[0];
1210 1246 destination[1] = source[1];
1211 1247 destination[2] = source[2];
1212 1248 destination[3] = source[3];
1213 1249 }
1214 1250
1215 1251 //**********
1216 1252 // init dump
1217 1253
1218 1254 void init_parameter_dump( void )
1219 1255 {
1220 1256 /** This function initialize the parameter_dump_packet global variable with default values.
1221 1257 *
1222 1258 */
1223 1259
1224 1260 unsigned int k;
1225 1261
1226 1262 parameter_dump_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
1227 1263 parameter_dump_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
1228 1264 parameter_dump_packet.reserved = CCSDS_RESERVED;
1229 1265 parameter_dump_packet.userApplication = CCSDS_USER_APP;
1230 1266 parameter_dump_packet.packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> 8);
1231 1267 parameter_dump_packet.packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;
1232 1268 parameter_dump_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1233 1269 parameter_dump_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1234 1270 parameter_dump_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_PARAMETER_DUMP >> 8);
1235 1271 parameter_dump_packet.packetLength[1] = (unsigned char) PACKET_LENGTH_PARAMETER_DUMP;
1236 1272 // DATA FIELD HEADER
1237 1273 parameter_dump_packet.spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1238 1274 parameter_dump_packet.serviceType = TM_TYPE_PARAMETER_DUMP;
1239 1275 parameter_dump_packet.serviceSubType = TM_SUBTYPE_PARAMETER_DUMP;
1240 1276 parameter_dump_packet.destinationID = TM_DESTINATION_ID_GROUND;
1241 1277 parameter_dump_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
1242 1278 parameter_dump_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
1243 1279 parameter_dump_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
1244 1280 parameter_dump_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
1245 1281 parameter_dump_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
1246 1282 parameter_dump_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
1247 1283 parameter_dump_packet.sid = SID_PARAMETER_DUMP;
1248 1284
1249 1285 //******************
1250 1286 // COMMON PARAMETERS
1251 1287 parameter_dump_packet.sy_lfr_common_parameters_spare = DEFAULT_SY_LFR_COMMON0;
1252 1288 parameter_dump_packet.sy_lfr_common_parameters = DEFAULT_SY_LFR_COMMON1;
1253 1289
1254 1290 //******************
1255 1291 // NORMAL PARAMETERS
1256 1292 parameter_dump_packet.sy_lfr_n_swf_l[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_L >> 8);
1257 1293 parameter_dump_packet.sy_lfr_n_swf_l[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_L );
1258 1294 parameter_dump_packet.sy_lfr_n_swf_p[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_P >> 8);
1259 1295 parameter_dump_packet.sy_lfr_n_swf_p[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_P );
1260 1296 parameter_dump_packet.sy_lfr_n_asm_p[0] = (unsigned char) (DFLT_SY_LFR_N_ASM_P >> 8);
1261 1297 parameter_dump_packet.sy_lfr_n_asm_p[1] = (unsigned char) (DFLT_SY_LFR_N_ASM_P );
1262 1298 parameter_dump_packet.sy_lfr_n_bp_p0 = (unsigned char) DFLT_SY_LFR_N_BP_P0;
1263 1299 parameter_dump_packet.sy_lfr_n_bp_p1 = (unsigned char) DFLT_SY_LFR_N_BP_P1;
1264 1300 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = (unsigned char) DFLT_SY_LFR_N_CWF_LONG_F3;
1265 1301
1266 1302 //*****************
1267 1303 // BURST PARAMETERS
1268 1304 parameter_dump_packet.sy_lfr_b_bp_p0 = (unsigned char) DEFAULT_SY_LFR_B_BP_P0;
1269 1305 parameter_dump_packet.sy_lfr_b_bp_p1 = (unsigned char) DEFAULT_SY_LFR_B_BP_P1;
1270 1306
1271 1307 //****************
1272 1308 // SBM1 PARAMETERS
1273 1309 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
1274 1310 parameter_dump_packet.sy_lfr_s1_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S1_BP_P1;
1275 1311
1276 1312 //****************
1277 1313 // SBM2 PARAMETERS
1278 1314 parameter_dump_packet.sy_lfr_s2_bp_p0 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P0;
1279 1315 parameter_dump_packet.sy_lfr_s2_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P1;
1280 1316
1281 1317 //************
1282 1318 // FBINS MASKS
1283 1319 for (k=0; k < NB_FBINS_MASKS * NB_BYTES_PER_FBINS_MASK; k++)
1284 1320 {
1285 1321 parameter_dump_packet.sy_lfr_fbins_f0_word1[k] = 0xff;
1286 1322 }
1287 1323 }
1288 1324
1289 1325 void init_kcoefficients_dump( void )
1290 1326 {
1291 1327 init_kcoefficients_dump_packet( &kcoefficients_dump_1, 1, 30 );
1292 1328 init_kcoefficients_dump_packet( &kcoefficients_dump_2, 2, 6 );
1293 1329
1294 1330 kcoefficient_node_1.previous = NULL;
1295 1331 kcoefficient_node_1.next = NULL;
1296 1332 kcoefficient_node_1.sid = TM_CODE_K_DUMP;
1297 1333 kcoefficient_node_1.coarseTime = 0x00;
1298 1334 kcoefficient_node_1.fineTime = 0x00;
1299 1335 kcoefficient_node_1.buffer_address = (int) &kcoefficients_dump_1;
1300 1336 kcoefficient_node_1.status = 0x00;
1301 1337
1302 1338 kcoefficient_node_2.previous = NULL;
1303 1339 kcoefficient_node_2.next = NULL;
1304 1340 kcoefficient_node_2.sid = TM_CODE_K_DUMP;
1305 1341 kcoefficient_node_2.coarseTime = 0x00;
1306 1342 kcoefficient_node_2.fineTime = 0x00;
1307 1343 kcoefficient_node_2.buffer_address = (int) &kcoefficients_dump_2;
1308 1344 kcoefficient_node_2.status = 0x00;
1309 1345 }
1310 1346
1311 1347 void init_kcoefficients_dump_packet( Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump, unsigned char pkt_nr, unsigned char blk_nr )
1312 1348 {
1313 1349 unsigned int k;
1314 1350 unsigned int packetLength;
1315 1351
1316 1352 packetLength = blk_nr * 130 + 20 - CCSDS_TC_TM_PACKET_OFFSET; // 4 bytes for the CCSDS header
1317 1353
1318 1354 kcoefficients_dump->targetLogicalAddress = CCSDS_DESTINATION_ID;
1319 1355 kcoefficients_dump->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1320 1356 kcoefficients_dump->reserved = CCSDS_RESERVED;
1321 1357 kcoefficients_dump->userApplication = CCSDS_USER_APP;
1322 1358 kcoefficients_dump->packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> 8);;
1323 1359 kcoefficients_dump->packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;;
1324 1360 kcoefficients_dump->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1325 1361 kcoefficients_dump->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1326 1362 kcoefficients_dump->packetLength[0] = (unsigned char) (packetLength >> 8);
1327 1363 kcoefficients_dump->packetLength[1] = (unsigned char) packetLength;
1328 1364 // DATA FIELD HEADER
1329 1365 kcoefficients_dump->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1330 1366 kcoefficients_dump->serviceType = TM_TYPE_K_DUMP;
1331 1367 kcoefficients_dump->serviceSubType = TM_SUBTYPE_K_DUMP;
1332 1368 kcoefficients_dump->destinationID= TM_DESTINATION_ID_GROUND;
1333 1369 kcoefficients_dump->time[0] = 0x00;
1334 1370 kcoefficients_dump->time[1] = 0x00;
1335 1371 kcoefficients_dump->time[2] = 0x00;
1336 1372 kcoefficients_dump->time[3] = 0x00;
1337 1373 kcoefficients_dump->time[4] = 0x00;
1338 1374 kcoefficients_dump->time[5] = 0x00;
1339 1375 kcoefficients_dump->sid = SID_K_DUMP;
1340 1376
1341 1377 kcoefficients_dump->pkt_cnt = 2;
1342 1378 kcoefficients_dump->pkt_nr = pkt_nr;
1343 1379 kcoefficients_dump->blk_nr = blk_nr;
1344 1380
1345 1381 //******************
1346 1382 // SOURCE DATA repeated N times with N in [0 .. PA_LFR_KCOEFF_BLK_NR]
1347 1383 // one blk is 2 + 4 * 32 = 130 bytes, 30 blks max in one packet (30 * 130 = 3900)
1348 1384 for (k=0; k<3900; k++)
1349 1385 {
1350 1386 kcoefficients_dump->kcoeff_blks[k] = 0x00;
1351 1387 }
1352 1388 }
1353 1389
1354 1390 void increment_seq_counter_destination_id_dump( unsigned char *packet_sequence_control, unsigned char destination_id )
1355 1391 {
1356 1392 /** This function increment the packet sequence control parameter of a TC, depending on its destination ID.
1357 1393 *
1358 1394 * @param packet_sequence_control points to the packet sequence control which will be incremented
1359 1395 * @param destination_id is the destination ID of the TM, there is one counter by destination ID
1360 1396 *
1361 1397 * If the destination ID is not known, a dedicated counter is incremented.
1362 1398 *
1363 1399 */
1364 1400
1365 1401 unsigned short sequence_cnt;
1366 1402 unsigned short segmentation_grouping_flag;
1367 1403 unsigned short new_packet_sequence_control;
1368 1404 unsigned char i;
1369 1405
1370 1406 switch (destination_id)
1371 1407 {
1372 1408 case SID_TC_GROUND:
1373 1409 i = GROUND;
1374 1410 break;
1375 1411 case SID_TC_MISSION_TIMELINE:
1376 1412 i = MISSION_TIMELINE;
1377 1413 break;
1378 1414 case SID_TC_TC_SEQUENCES:
1379 1415 i = TC_SEQUENCES;
1380 1416 break;
1381 1417 case SID_TC_RECOVERY_ACTION_CMD:
1382 1418 i = RECOVERY_ACTION_CMD;
1383 1419 break;
1384 1420 case SID_TC_BACKUP_MISSION_TIMELINE:
1385 1421 i = BACKUP_MISSION_TIMELINE;
1386 1422 break;
1387 1423 case SID_TC_DIRECT_CMD:
1388 1424 i = DIRECT_CMD;
1389 1425 break;
1390 1426 case SID_TC_SPARE_GRD_SRC1:
1391 1427 i = SPARE_GRD_SRC1;
1392 1428 break;
1393 1429 case SID_TC_SPARE_GRD_SRC2:
1394 1430 i = SPARE_GRD_SRC2;
1395 1431 break;
1396 1432 case SID_TC_OBCP:
1397 1433 i = OBCP;
1398 1434 break;
1399 1435 case SID_TC_SYSTEM_CONTROL:
1400 1436 i = SYSTEM_CONTROL;
1401 1437 break;
1402 1438 case SID_TC_AOCS:
1403 1439 i = AOCS;
1404 1440 break;
1405 1441 case SID_TC_RPW_INTERNAL:
1406 1442 i = RPW_INTERNAL;
1407 1443 break;
1408 1444 default:
1409 1445 i = GROUND;
1410 1446 break;
1411 1447 }
1412 1448
1413 1449 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
1414 1450 sequence_cnt = sequenceCounters_TM_DUMP[ i ] & 0x3fff;
1415 1451
1416 1452 new_packet_sequence_control = segmentation_grouping_flag | sequence_cnt ;
1417 1453
1418 1454 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> 8);
1419 1455 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
1420 1456
1421 1457 // increment the sequence counter
1422 1458 if ( sequenceCounters_TM_DUMP[ i ] < SEQ_CNT_MAX )
1423 1459 {
1424 1460 sequenceCounters_TM_DUMP[ i ] = sequenceCounters_TM_DUMP[ i ] + 1;
1425 1461 }
1426 1462 else
1427 1463 {
1428 1464 sequenceCounters_TM_DUMP[ i ] = 0;
1429 1465 }
1430 1466 }
Show file before | Show file after | Hide comments
@@ -1,514 +1,514
1 1 /** Functions to send TM packets related to TC parsing and execution.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to send appropriate TM packets after parsing and execution:
7 7 * - TM_LFR_TC_EXE_SUCCESS
8 8 * - TM_LFR_TC_EXE_INCONSISTENT
9 9 * - TM_LFR_TC_EXE_NOT_EXECUTABLE
10 10 * - TM_LFR_TC_EXE_NOT_IMPLEMENTED
11 11 * - TM_LFR_TC_EXE_ERROR
12 12 * - TM_LFR_TC_EXE_CORRUPTED
13 13 *
14 14 */
15 15
16 16 #include "tm_lfr_tc_exe.h"
17 17
18 18 int send_tm_lfr_tc_exe_success( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
19 19 {
20 20 /** This function sends a TM_LFR_TC_EXE_SUCCESS packet in the dedicated RTEMS message queue.
21 21 *
22 22 * @param TC points to the TeleCommand packet that is being processed
23 23 * @param queue_id is the id of the queue which handles TM
24 24 *
25 25 * @return RTEMS directive status code:
26 26 * - RTEMS_SUCCESSFUL - message sent successfully
27 27 * - RTEMS_INVALID_ID - invalid queue id
28 28 * - RTEMS_INVALID_SIZE - invalid message size
29 29 * - RTEMS_INVALID_ADDRESS - buffer is NULL
30 30 * - RTEMS_UNSATISFIED - out of message buffers
31 31 * - RTEMS_TOO_MANY - queue s limit has been reached
32 32 *
33 33 */
34 34
35 35 rtems_status_code status;
36 36 Packet_TM_LFR_TC_EXE_SUCCESS_t TM;
37 37 unsigned char messageSize;
38 38
39 39 TM.targetLogicalAddress = CCSDS_DESTINATION_ID;
40 40 TM.protocolIdentifier = CCSDS_PROTOCOLE_ID;
41 41 TM.reserved = DEFAULT_RESERVED;
42 42 TM.userApplication = CCSDS_USER_APP;
43 43 // PACKET HEADER
44 44 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> 8);
45 45 TM.packetID[1] = (unsigned char) (APID_TM_TC_EXE );
46 46 increment_seq_counter_destination_id( TM.packetSequenceControl, TC->sourceID );
47 47 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_SUCCESS >> 8);
48 48 TM.packetLength[1] = (unsigned char) (PACKET_LENGTH_TC_EXE_SUCCESS );
49 49 // DATA FIELD HEADER
50 50 TM.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
51 51 TM.serviceType = TM_TYPE_TC_EXE;
52 52 TM.serviceSubType = TM_SUBTYPE_EXE_OK;
53 53 TM.destinationID = TC->sourceID;
54 54 TM.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
55 55 TM.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
56 56 TM.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
57 57 TM.time[3] = (unsigned char) (time_management_regs->coarse_time);
58 58 TM.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
59 59 TM.time[5] = (unsigned char) (time_management_regs->fine_time);
60 60 //
61 61 TM.telecommand_pkt_id[0] = TC->packetID[0];
62 62 TM.telecommand_pkt_id[1] = TC->packetID[1];
63 63 TM.pkt_seq_control[0] = TC->packetSequenceControl[0];
64 64 TM.pkt_seq_control[1] = TC->packetSequenceControl[1];
65 65
66 66 messageSize = PACKET_LENGTH_TC_EXE_SUCCESS + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
67 67
68 68 // SEND DATA
69 69 status = rtems_message_queue_send( queue_id, &TM, messageSize);
70 70 if (status != RTEMS_SUCCESSFUL) {
71 71 PRINTF("in send_tm_lfr_tc_exe_success *** ERR\n")
72 72 }
73 73
74 74 // UPDATE HK FIELDS
75 75 update_last_TC_exe( TC, TM.time );
76 76
77 77 return status;
78 78 }
79 79
80 80 int send_tm_lfr_tc_exe_inconsistent( ccsdsTelecommandPacket_t *TC, rtems_id queue_id,
81 81 unsigned char byte_position, unsigned char rcv_value )
82 82 {
83 83 /** This function sends a TM_LFR_TC_EXE_INCONSISTENT packet in the dedicated RTEMS message queue.
84 84 *
85 85 * @param TC points to the TeleCommand packet that is being processed
86 86 * @param queue_id is the id of the queue which handles TM
87 87 * @param byte_position is the byte position of the MSB of the parameter that has been seen as inconsistent
88 * @param rcv_value is the value of the LSB of the parameter that has been deteced as inconsistent
88 * @param rcv_value is the value of the LSB of the parameter that has been detected as inconsistent
89 89 *
90 90 * @return RTEMS directive status code:
91 91 * - RTEMS_SUCCESSFUL - message sent successfully
92 92 * - RTEMS_INVALID_ID - invalid queue id
93 93 * - RTEMS_INVALID_SIZE - invalid message size
94 94 * - RTEMS_INVALID_ADDRESS - buffer is NULL
95 95 * - RTEMS_UNSATISFIED - out of message buffers
96 96 * - RTEMS_TOO_MANY - queue s limit has been reached
97 97 *
98 98 */
99 99
100 100 rtems_status_code status;
101 101 Packet_TM_LFR_TC_EXE_INCONSISTENT_t TM;
102 102 unsigned char messageSize;
103 103
104 104 TM.targetLogicalAddress = CCSDS_DESTINATION_ID;
105 105 TM.protocolIdentifier = CCSDS_PROTOCOLE_ID;
106 106 TM.reserved = DEFAULT_RESERVED;
107 107 TM.userApplication = CCSDS_USER_APP;
108 108 // PACKET HEADER
109 109 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> 8);
110 110 TM.packetID[1] = (unsigned char) (APID_TM_TC_EXE );
111 111 increment_seq_counter_destination_id( TM.packetSequenceControl, TC->sourceID );
112 112 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_INCONSISTENT >> 8);
113 113 TM.packetLength[1] = (unsigned char) (PACKET_LENGTH_TC_EXE_INCONSISTENT );
114 114 // DATA FIELD HEADER
115 115 TM.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
116 116 TM.serviceType = TM_TYPE_TC_EXE;
117 117 TM.serviceSubType = TM_SUBTYPE_EXE_NOK;
118 118 TM.destinationID = TC->sourceID;
119 119 TM.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
120 120 TM.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
121 121 TM.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
122 122 TM.time[3] = (unsigned char) (time_management_regs->coarse_time);
123 123 TM.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
124 124 TM.time[5] = (unsigned char) (time_management_regs->fine_time);
125 125 //
126 126 TM.tc_failure_code[0] = (char) (WRONG_APP_DATA >> 8);
127 127 TM.tc_failure_code[1] = (char) (WRONG_APP_DATA );
128 128 TM.telecommand_pkt_id[0] = TC->packetID[0];
129 129 TM.telecommand_pkt_id[1] = TC->packetID[1];
130 130 TM.pkt_seq_control[0] = TC->packetSequenceControl[0];
131 131 TM.pkt_seq_control[1] = TC->packetSequenceControl[1];
132 132 TM.tc_service = TC->serviceType; // type of the rejected TC
133 133 TM.tc_subtype = TC->serviceSubType; // subtype of the rejected TC
134 134 TM.byte_position = byte_position;
135 135 TM.rcv_value = (unsigned char) rcv_value;
136 136
137 137 messageSize = PACKET_LENGTH_TC_EXE_INCONSISTENT + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
138 138
139 139 // SEND DATA
140 140 status = rtems_message_queue_send( queue_id, &TM, messageSize);
141 141 if (status != RTEMS_SUCCESSFUL) {
142 142 PRINTF("in send_tm_lfr_tc_exe_inconsistent *** ERR\n")
143 143 }
144 144
145 145 // UPDATE HK FIELDS
146 146 update_last_TC_rej( TC, TM.time );
147 147
148 148 return status;
149 149 }
150 150
151 151 int send_tm_lfr_tc_exe_not_executable( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
152 152 {
153 153 /** This function sends a TM_LFR_TC_EXE_NOT_EXECUTABLE packet in the dedicated RTEMS message queue.
154 154 *
155 155 * @param TC points to the TeleCommand packet that is being processed
156 156 * @param queue_id is the id of the queue which handles TM
157 157 *
158 158 * @return RTEMS directive status code:
159 159 * - RTEMS_SUCCESSFUL - message sent successfully
160 160 * - RTEMS_INVALID_ID - invalid queue id
161 161 * - RTEMS_INVALID_SIZE - invalid message size
162 162 * - RTEMS_INVALID_ADDRESS - buffer is NULL
163 163 * - RTEMS_UNSATISFIED - out of message buffers
164 164 * - RTEMS_TOO_MANY - queue s limit has been reached
165 165 *
166 166 */
167 167
168 168 rtems_status_code status;
169 169 Packet_TM_LFR_TC_EXE_NOT_EXECUTABLE_t TM;
170 170 unsigned char messageSize;
171 171
172 172 TM.targetLogicalAddress = CCSDS_DESTINATION_ID;
173 173 TM.protocolIdentifier = CCSDS_PROTOCOLE_ID;
174 174 TM.reserved = DEFAULT_RESERVED;
175 175 TM.userApplication = CCSDS_USER_APP;
176 176 // PACKET HEADER
177 177 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> 8);
178 178 TM.packetID[1] = (unsigned char) (APID_TM_TC_EXE );
179 179 increment_seq_counter_destination_id( TM.packetSequenceControl, TC->sourceID );
180 180 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_NOT_EXECUTABLE >> 8);
181 181 TM.packetLength[1] = (unsigned char) (PACKET_LENGTH_TC_EXE_NOT_EXECUTABLE );
182 182 // DATA FIELD HEADER
183 183 TM.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
184 184 TM.serviceType = TM_TYPE_TC_EXE;
185 185 TM.serviceSubType = TM_SUBTYPE_EXE_NOK;
186 186 TM.destinationID = TC->sourceID; // default destination id
187 187 TM.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
188 188 TM.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
189 189 TM.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
190 190 TM.time[3] = (unsigned char) (time_management_regs->coarse_time);
191 191 TM.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
192 192 TM.time[5] = (unsigned char) (time_management_regs->fine_time);
193 193 //
194 194 TM.tc_failure_code[0] = (char) (TC_NOT_EXE >> 8);
195 195 TM.tc_failure_code[1] = (char) (TC_NOT_EXE );
196 196 TM.telecommand_pkt_id[0] = TC->packetID[0];
197 197 TM.telecommand_pkt_id[1] = TC->packetID[1];
198 198 TM.pkt_seq_control[0] = TC->packetSequenceControl[0];
199 199 TM.pkt_seq_control[1] = TC->packetSequenceControl[1];
200 200 TM.tc_service = TC->serviceType; // type of the rejected TC
201 201 TM.tc_subtype = TC->serviceSubType; // subtype of the rejected TC
202 202 TM.lfr_status_word[0] = housekeeping_packet.lfr_status_word[0];
203 203 TM.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1];
204 204
205 205 messageSize = PACKET_LENGTH_TC_EXE_NOT_EXECUTABLE + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
206 206
207 207 // SEND DATA
208 208 status = rtems_message_queue_send( queue_id, &TM, messageSize);
209 209 if (status != RTEMS_SUCCESSFUL) {
210 210 PRINTF("in send_tm_lfr_tc_exe_not_executable *** ERR\n")
211 211 }
212 212
213 213 // UPDATE HK FIELDS
214 214 update_last_TC_rej( TC, TM.time );
215 215
216 216 return status;
217 217 }
218 218
219 219 int send_tm_lfr_tc_exe_not_implemented( ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time )
220 220 {
221 221 /** This function sends a TM_LFR_TC_EXE_NOT_IMPLEMENTED packet in the dedicated RTEMS message queue.
222 222 *
223 223 * @param TC points to the TeleCommand packet that is being processed
224 224 * @param queue_id is the id of the queue which handles TM
225 225 *
226 226 * @return RTEMS directive status code:
227 227 * - RTEMS_SUCCESSFUL - message sent successfully
228 228 * - RTEMS_INVALID_ID - invalid queue id
229 229 * - RTEMS_INVALID_SIZE - invalid message size
230 230 * - RTEMS_INVALID_ADDRESS - buffer is NULL
231 231 * - RTEMS_UNSATISFIED - out of message buffers
232 232 * - RTEMS_TOO_MANY - queue s limit has been reached
233 233 *
234 234 */
235 235
236 236 rtems_status_code status;
237 237 Packet_TM_LFR_TC_EXE_NOT_IMPLEMENTED_t TM;
238 238 unsigned char messageSize;
239 239
240 240 TM.targetLogicalAddress = CCSDS_DESTINATION_ID;
241 241 TM.protocolIdentifier = CCSDS_PROTOCOLE_ID;
242 242 TM.reserved = DEFAULT_RESERVED;
243 243 TM.userApplication = CCSDS_USER_APP;
244 244 // PACKET HEADER
245 245 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> 8);
246 246 TM.packetID[1] = (unsigned char) (APID_TM_TC_EXE );
247 247 increment_seq_counter_destination_id( TM.packetSequenceControl, TC->sourceID );
248 248 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_NOT_IMPLEMENTED >> 8);
249 249 TM.packetLength[1] = (unsigned char) (PACKET_LENGTH_TC_EXE_NOT_IMPLEMENTED );
250 250 // DATA FIELD HEADER
251 251 TM.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
252 252 TM.serviceType = TM_TYPE_TC_EXE;
253 253 TM.serviceSubType = TM_SUBTYPE_EXE_NOK;
254 254 TM.destinationID = TC->sourceID; // default destination id
255 255 TM.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
256 256 TM.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
257 257 TM.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
258 258 TM.time[3] = (unsigned char) (time_management_regs->coarse_time);
259 259 TM.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
260 260 TM.time[5] = (unsigned char) (time_management_regs->fine_time);
261 261 //
262 262 TM.tc_failure_code[0] = (char) (FUNCT_NOT_IMPL >> 8);
263 263 TM.tc_failure_code[1] = (char) (FUNCT_NOT_IMPL );
264 264 TM.telecommand_pkt_id[0] = TC->packetID[0];
265 265 TM.telecommand_pkt_id[1] = TC->packetID[1];
266 266 TM.pkt_seq_control[0] = TC->packetSequenceControl[0];
267 267 TM.pkt_seq_control[1] = TC->packetSequenceControl[1];
268 268 TM.tc_service = TC->serviceType; // type of the rejected TC
269 269 TM.tc_subtype = TC->serviceSubType; // subtype of the rejected TC
270 270
271 271 messageSize = PACKET_LENGTH_TC_EXE_NOT_IMPLEMENTED + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
272 272
273 273 // SEND DATA
274 274 status = rtems_message_queue_send( queue_id, &TM, messageSize);
275 275 if (status != RTEMS_SUCCESSFUL) {
276 276 PRINTF("in send_tm_lfr_tc_exe_not_implemented *** ERR\n")
277 277 }
278 278
279 279 // UPDATE HK FIELDS
280 280 update_last_TC_rej( TC, TM.time );
281 281
282 282 return status;
283 283 }
284 284
285 285 int send_tm_lfr_tc_exe_error( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
286 286 {
287 287 /** This function sends a TM_LFR_TC_EXE_ERROR packet in the dedicated RTEMS message queue.
288 288 *
289 289 * @param TC points to the TeleCommand packet that is being processed
290 290 * @param queue_id is the id of the queue which handles TM
291 291 *
292 292 * @return RTEMS directive status code:
293 293 * - RTEMS_SUCCESSFUL - message sent successfully
294 294 * - RTEMS_INVALID_ID - invalid queue id
295 295 * - RTEMS_INVALID_SIZE - invalid message size
296 296 * - RTEMS_INVALID_ADDRESS - buffer is NULL
297 297 * - RTEMS_UNSATISFIED - out of message buffers
298 298 * - RTEMS_TOO_MANY - queue s limit has been reached
299 299 *
300 300 */
301 301
302 302 rtems_status_code status;
303 303 Packet_TM_LFR_TC_EXE_ERROR_t TM;
304 304 unsigned char messageSize;
305 305
306 306 TM.targetLogicalAddress = CCSDS_DESTINATION_ID;
307 307 TM.protocolIdentifier = CCSDS_PROTOCOLE_ID;
308 308 TM.reserved = DEFAULT_RESERVED;
309 309 TM.userApplication = CCSDS_USER_APP;
310 310 // PACKET HEADER
311 311 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> 8);
312 312 TM.packetID[1] = (unsigned char) (APID_TM_TC_EXE );
313 313 increment_seq_counter_destination_id( TM.packetSequenceControl, TC->sourceID );
314 314 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_ERROR >> 8);
315 315 TM.packetLength[1] = (unsigned char) (PACKET_LENGTH_TC_EXE_ERROR );
316 316 // DATA FIELD HEADER
317 317 TM.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
318 318 TM.serviceType = TM_TYPE_TC_EXE;
319 319 TM.serviceSubType = TM_SUBTYPE_EXE_NOK;
320 320 TM.destinationID = TC->sourceID; // default destination id
321 321 TM.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
322 322 TM.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
323 323 TM.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
324 324 TM.time[3] = (unsigned char) (time_management_regs->coarse_time);
325 325 TM.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
326 326 TM.time[5] = (unsigned char) (time_management_regs->fine_time);
327 327 //
328 328 TM.tc_failure_code[0] = (char) (FAIL_DETECTED >> 8);
329 329 TM.tc_failure_code[1] = (char) (FAIL_DETECTED );
330 330 TM.telecommand_pkt_id[0] = TC->packetID[0];
331 331 TM.telecommand_pkt_id[1] = TC->packetID[1];
332 332 TM.pkt_seq_control[0] = TC->packetSequenceControl[0];
333 333 TM.pkt_seq_control[1] = TC->packetSequenceControl[1];
334 334 TM.tc_service = TC->serviceType; // type of the rejected TC
335 335 TM.tc_subtype = TC->serviceSubType; // subtype of the rejected TC
336 336
337 337 messageSize = PACKET_LENGTH_TC_EXE_ERROR + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
338 338
339 339 // SEND DATA
340 340 status = rtems_message_queue_send( queue_id, &TM, messageSize);
341 341 if (status != RTEMS_SUCCESSFUL) {
342 342 PRINTF("in send_tm_lfr_tc_exe_error *** ERR\n")
343 343 }
344 344
345 345 // UPDATE HK FIELDS
346 346 update_last_TC_rej( TC, TM.time );
347 347
348 348 return status;
349 349 }
350 350
351 351 int send_tm_lfr_tc_exe_corrupted(ccsdsTelecommandPacket_t *TC, rtems_id queue_id,
352 352 unsigned char *computed_CRC, unsigned char *currentTC_LEN_RCV,
353 353 unsigned char destinationID )
354 354 {
355 355 /** This function sends a TM_LFR_TC_EXE_CORRUPTED packet in the dedicated RTEMS message queue.
356 356 *
357 357 * @param TC points to the TeleCommand packet that is being processed
358 358 * @param queue_id is the id of the queue which handles TM
359 359 * @param computed_CRC points to a buffer of two bytes containing the CRC computed during the parsing of the TeleCommand
360 360 * @param currentTC_LEN_RCV points to a buffer of two bytes containing a packet size field computed on the received data
361 361 *
362 362 * @return RTEMS directive status code:
363 363 * - RTEMS_SUCCESSFUL - message sent successfully
364 364 * - RTEMS_INVALID_ID - invalid queue id
365 365 * - RTEMS_INVALID_SIZE - invalid message size
366 366 * - RTEMS_INVALID_ADDRESS - buffer is NULL
367 367 * - RTEMS_UNSATISFIED - out of message buffers
368 368 * - RTEMS_TOO_MANY - queue s limit has been reached
369 369 *
370 370 */
371 371
372 372 rtems_status_code status;
373 373 Packet_TM_LFR_TC_EXE_CORRUPTED_t TM;
374 374 unsigned char messageSize;
375 375 unsigned int packetLength;
376 376 unsigned int estimatedPacketLength;
377 377 unsigned char *packetDataField;
378 378
379 379 packetLength = (TC->packetLength[0] * 256) + TC->packetLength[1]; // compute the packet length parameter written in the TC
380 380 estimatedPacketLength = (unsigned int) (currentTC_LEN_RCV[0] * 256 + currentTC_LEN_RCV[1]);
381 381 packetDataField = (unsigned char *) &TC->headerFlag_pusVersion_Ack; // get the beginning of the data field
382 382
383 383 TM.targetLogicalAddress = CCSDS_DESTINATION_ID;
384 384 TM.protocolIdentifier = CCSDS_PROTOCOLE_ID;
385 385 TM.reserved = DEFAULT_RESERVED;
386 386 TM.userApplication = CCSDS_USER_APP;
387 387 // PACKET HEADER
388 388 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> 8);
389 389 TM.packetID[1] = (unsigned char) (APID_TM_TC_EXE );
390 390 increment_seq_counter_destination_id( TM.packetSequenceControl, TC->sourceID );
391 391 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_CORRUPTED >> 8);
392 392 TM.packetLength[1] = (unsigned char) (PACKET_LENGTH_TC_EXE_CORRUPTED );
393 393 // DATA FIELD HEADER
394 394 TM.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
395 395 TM.serviceType = TM_TYPE_TC_EXE;
396 396 TM.serviceSubType = TM_SUBTYPE_EXE_NOK;
397 397 TM.destinationID = destinationID;
398 398 TM.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
399 399 TM.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
400 400 TM.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
401 401 TM.time[3] = (unsigned char) (time_management_regs->coarse_time);
402 402 TM.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
403 403 TM.time[5] = (unsigned char) (time_management_regs->fine_time);
404 404 //
405 405 TM.tc_failure_code[0] = (unsigned char) (CORRUPTED >> 8);
406 406 TM.tc_failure_code[1] = (unsigned char) (CORRUPTED );
407 407 TM.telecommand_pkt_id[0] = TC->packetID[0];
408 408 TM.telecommand_pkt_id[1] = TC->packetID[1];
409 409 TM.pkt_seq_control[0] = TC->packetSequenceControl[0];
410 410 TM.pkt_seq_control[1] = TC->packetSequenceControl[1];
411 411 TM.tc_service = TC->serviceType; // type of the rejected TC
412 412 TM.tc_subtype = TC->serviceSubType; // subtype of the rejected TC
413 413 TM.pkt_len_rcv_value[0] = TC->packetLength[0];
414 414 TM.pkt_len_rcv_value[1] = TC->packetLength[1];
415 415 TM.pkt_datafieldsize_cnt[0] = currentTC_LEN_RCV[0];
416 416 TM.pkt_datafieldsize_cnt[1] = currentTC_LEN_RCV[1];
417 417 // TM.rcv_crc[0] = packetDataField[ packetLength - 1 ];
418 418 // TM.rcv_crc[1] = packetDataField[ packetLength ];
419 419 TM.rcv_crc[0] = packetDataField[ estimatedPacketLength - 1 ];
420 420 TM.rcv_crc[1] = packetDataField[ estimatedPacketLength ];
421 421 TM.computed_crc[0] = computed_CRC[0];
422 422 TM.computed_crc[1] = computed_CRC[1];
423 423
424 424 messageSize = PACKET_LENGTH_TC_EXE_CORRUPTED + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
425 425
426 426 // SEND DATA
427 427 status = rtems_message_queue_send( queue_id, &TM, messageSize);
428 428 if (status != RTEMS_SUCCESSFUL) {
429 429 PRINTF("in send_tm_lfr_tc_exe_error *** ERR\n")
430 430 }
431 431
432 432 // UPDATE HK FIELDS
433 433 update_last_TC_rej( TC, TM.time );
434 434
435 435 return status;
436 436 }
437 437
438 438 void increment_seq_counter_destination_id( unsigned char *packet_sequence_control, unsigned char destination_id )
439 439 {
440 440 /** This function increment the packet sequence control parameter of a TC, depending on its destination ID.
441 441 *
442 442 * @param packet_sequence_control points to the packet sequence control which will be incremented
443 443 * @param destination_id is the destination ID of the TM, there is one counter by destination ID
444 444 *
445 445 * If the destination ID is not known, a dedicated counter is incremented.
446 446 *
447 447 */
448 448
449 449 unsigned short sequence_cnt;
450 450 unsigned short segmentation_grouping_flag;
451 451 unsigned short new_packet_sequence_control;
452 452 unsigned char i;
453 453
454 454 switch (destination_id)
455 455 {
456 456 case SID_TC_GROUND:
457 457 i = GROUND;
458 458 break;
459 459 case SID_TC_MISSION_TIMELINE:
460 460 i = MISSION_TIMELINE;
461 461 break;
462 462 case SID_TC_TC_SEQUENCES:
463 463 i = TC_SEQUENCES;
464 464 break;
465 465 case SID_TC_RECOVERY_ACTION_CMD:
466 466 i = RECOVERY_ACTION_CMD;
467 467 break;
468 468 case SID_TC_BACKUP_MISSION_TIMELINE:
469 469 i = BACKUP_MISSION_TIMELINE;
470 470 break;
471 471 case SID_TC_DIRECT_CMD:
472 472 i = DIRECT_CMD;
473 473 break;
474 474 case SID_TC_SPARE_GRD_SRC1:
475 475 i = SPARE_GRD_SRC1;
476 476 break;
477 477 case SID_TC_SPARE_GRD_SRC2:
478 478 i = SPARE_GRD_SRC2;
479 479 break;
480 480 case SID_TC_OBCP:
481 481 i = OBCP;
482 482 break;
483 483 case SID_TC_SYSTEM_CONTROL:
484 484 i = SYSTEM_CONTROL;
485 485 break;
486 486 case SID_TC_AOCS:
487 487 i = AOCS;
488 488 break;
489 489 case SID_TC_RPW_INTERNAL:
490 490 i = RPW_INTERNAL;
491 491 break;
492 492 default:
493 493 i = GROUND;
494 494 break;
495 495 }
496 496
497 497 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << 8;
498 498 sequence_cnt = sequenceCounters_TC_EXE[ i ] & 0x3fff;
499 499
500 500 new_packet_sequence_control = segmentation_grouping_flag | sequence_cnt ;
501 501
502 502 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> 8);
503 503 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
504 504
505 505 // increment the sequence counter
506 506 if ( sequenceCounters_TC_EXE[ i ] < SEQ_CNT_MAX )
507 507 {
508 508 sequenceCounters_TC_EXE[ i ] = sequenceCounters_TC_EXE[ i ] + 1;
509 509 }
510 510 else
511 511 {
512 512 sequenceCounters_TC_EXE[ i ] = 0;
513 513 }
514 514 }
General Comments 0
You need to be logged in to leave comments. Login now