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

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

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