@@ -1,2 +1,2 | |||
|
1 | 1 | 3081d1f9bb20b2b64a192585337a292a9804e0c5 LFR_basic-parameters |
|
2 |
|
|
|
2 | 058c1234c2defe215d3dd655a7ef65abb33c922d header/lfr_common_headers |
@@ -21,16 +21,11 extern rtems_id Task_id[]; /* | |||
|
21 | 21 | extern rtems_name timecode_timer_name; |
|
22 | 22 | extern rtems_id timecode_timer_id; |
|
23 | 23 | extern unsigned char pa_bia_status_info; |
|
24 | extern unsigned char cp_rpw_sc_rw_f_flags; | |
|
25 |
extern |
|
|
26 | extern float cp_rpw_sc_rw1_f2; | |
|
27 | extern float cp_rpw_sc_rw2_f1; | |
|
28 | extern float cp_rpw_sc_rw2_f2; | |
|
29 | extern float cp_rpw_sc_rw3_f1; | |
|
30 | extern float cp_rpw_sc_rw3_f2; | |
|
31 | extern float cp_rpw_sc_rw4_f1; | |
|
32 | extern float cp_rpw_sc_rw4_f2; | |
|
24 | extern unsigned char cp_rpw_sc_rw1_rw2_f_flags; | |
|
25 | extern unsigned char cp_rpw_sc_rw3_rw4_f_flags; | |
|
26 | ||
|
33 | 27 | extern filterPar_t filterPar; |
|
28 | extern rw_f_t rw_f; | |
|
34 | 29 | |
|
35 | 30 | // RTEMS TASKS |
|
36 | 31 | rtems_task Init( rtems_task_argument argument); |
@@ -29,6 +29,20 | |||
|
29 | 29 | #define BIT_RW4_F1 0x02 |
|
30 | 30 | #define BIT_RW4_F2 0x01 |
|
31 | 31 | |
|
32 | #define WHEEL_1 1 | |
|
33 | #define WHEEL_2 2 | |
|
34 | #define WHEEL_3 3 | |
|
35 | #define WHEEL_4 4 | |
|
36 | #define FREQ_1 1 | |
|
37 | #define FREQ_2 2 | |
|
38 | #define FREQ_3 3 | |
|
39 | #define FREQ_4 4 | |
|
40 | #define FLAG_OFFSET_WHEELS_1_3 8 | |
|
41 | #define FLAG_OFFSET_WHEELS_2_4 4 | |
|
42 | ||
|
43 | #define FLAG_NAN 0 // Not A NUMBER | |
|
44 | #define FLAG_IAN 1 // Is A Number | |
|
45 | ||
|
32 | 46 | #define SBM_KCOEFF_PER_NORM_KCOEFF 2 |
|
33 | 47 | |
|
34 | 48 | extern unsigned short sequenceCounterParameterDump; |
@@ -76,6 +90,8 int set_sy_lfr_s2_bp_p1( ccsdsTelecomman | |||
|
76 | 90 | unsigned int check_update_info_hk_lfr_mode( unsigned char mode ); |
|
77 | 91 | unsigned int check_update_info_hk_tds_mode( unsigned char mode ); |
|
78 | 92 | unsigned int check_update_info_hk_thr_mode( unsigned char mode ); |
|
93 | void set_hk_lfr_sc_rw_f_flag( unsigned char wheel, unsigned char freq, float value ); | |
|
94 | void set_hk_lfr_sc_rw_f_flags( void ); | |
|
79 | 95 | void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC ); |
|
80 | 96 | void setFBinMask(unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, unsigned char flag ); |
|
81 | 97 | void build_sy_lfr_rw_mask( unsigned int channel ); |
@@ -73,7 +73,8 unsigned int lastValidEnterModeTime | |||
|
73 | 73 | |
|
74 | 74 | // HK PACKETS |
|
75 | 75 | Packet_TM_LFR_HK_t housekeeping_packet = {0}; |
|
76 |
unsigned char cp_rpw_sc_rw_f_flags |
|
|
76 | unsigned char cp_rpw_sc_rw1_rw2_f_flags = 0; | |
|
77 | unsigned char cp_rpw_sc_rw3_rw4_f_flags = 0; | |
|
77 | 78 | // message queues occupancy |
|
78 | 79 | unsigned char hk_lfr_q_sd_fifo_size_max = 0; |
|
79 | 80 | unsigned char hk_lfr_q_rv_fifo_size_max = 0; |
@@ -89,14 +90,7 unsigned short sequenceCounterHK | |||
|
89 | 90 | spw_stats grspw_stats = {0}; |
|
90 | 91 | |
|
91 | 92 | // TC_LFR_UPDATE_INFO |
|
92 | float cp_rpw_sc_rw1_f1 = INIT_FLOAT; | |
|
93 | float cp_rpw_sc_rw1_f2 = INIT_FLOAT; | |
|
94 | float cp_rpw_sc_rw2_f1 = INIT_FLOAT; | |
|
95 | float cp_rpw_sc_rw2_f2 = INIT_FLOAT; | |
|
96 | float cp_rpw_sc_rw3_f1 = INIT_FLOAT; | |
|
97 | float cp_rpw_sc_rw3_f2 = INIT_FLOAT; | |
|
98 | float cp_rpw_sc_rw4_f1 = INIT_FLOAT; | |
|
99 | float cp_rpw_sc_rw4_f2 = INIT_FLOAT; | |
|
93 | rw_f_t rw_f; | |
|
100 | 94 | |
|
101 | 95 | // TC_LFR_LOAD_FILTER_PAR |
|
102 | 96 | filterPar_t filterPar = {0}; |
@@ -168,14 +168,28 rtems_task Init( rtems_task_argument ign | |||
|
168 | 168 | init_k_coefficients_prc2(); |
|
169 | 169 | pa_bia_status_info = INIT_CHAR; |
|
170 | 170 | cp_rpw_sc_rw_f_flags = INIT_CHAR; |
|
171 | cp_rpw_sc_rw1_f1 = INIT_FLOAT; | |
|
172 | cp_rpw_sc_rw1_f2 = INIT_FLOAT; | |
|
173 |
cp_rpw_sc_rw |
|
|
174 |
cp_rpw_sc_rw |
|
|
175 |
cp_rpw_sc_rw |
|
|
176 |
cp_rpw_sc_rw |
|
|
177 |
cp_rpw_sc_rw |
|
|
178 |
cp_rpw_sc_rw |
|
|
171 | ||
|
172 | // initialize all reaction wheels frequencies to NaN | |
|
173 | rw_f.cp_rpw_sc_rw1_f1 = NAN; | |
|
174 | rw_f.cp_rpw_sc_rw1_f2 = NAN; | |
|
175 | rw_f.cp_rpw_sc_rw1_f3 = NAN; | |
|
176 | rw_f.cp_rpw_sc_rw1_f4 = NAN; | |
|
177 | rw_f.cp_rpw_sc_rw2_f1 = NAN; | |
|
178 | rw_f.cp_rpw_sc_rw2_f2 = NAN; | |
|
179 | rw_f.cp_rpw_sc_rw2_f3 = NAN; | |
|
180 | rw_f.cp_rpw_sc_rw2_f4 = NAN; | |
|
181 | rw_f.cp_rpw_sc_rw3_f1 = NAN; | |
|
182 | rw_f.cp_rpw_sc_rw3_f2 = NAN; | |
|
183 | rw_f.cp_rpw_sc_rw3_f3 = NAN; | |
|
184 | rw_f.cp_rpw_sc_rw3_f4 = NAN; | |
|
185 | rw_f.cp_rpw_sc_rw4_f1 = NAN; | |
|
186 | rw_f.cp_rpw_sc_rw4_f2 = NAN; | |
|
187 | rw_f.cp_rpw_sc_rw4_f3 = NAN; | |
|
188 | rw_f.cp_rpw_sc_rw4_f4 = NAN; | |
|
189 | ||
|
190 | cp_rpw_sc_rw1_rw2_f_flags = INIT_CHAR; | |
|
191 | cp_rpw_sc_rw3_rw4_f_flags = INIT_CHAR; | |
|
192 | ||
|
179 | 193 | // initialize filtering parameters |
|
180 | 194 | filterPar.spare_sy_lfr_pas_filter_enabled = DEFAULT_SY_LFR_PAS_FILTER_ENABLED; |
|
181 | 195 | filterPar.sy_lfr_pas_filter_modulus = DEFAULT_SY_LFR_PAS_FILTER_MODULUS; |
@@ -7,6 +7,10 | |||
|
7 | 7 | |
|
8 | 8 | #include "fsw_misc.h" |
|
9 | 9 | |
|
10 | int16_t hk_lfr_sc_v_f3_as_int16 = 0; | |
|
11 | int16_t hk_lfr_sc_e1_f3_as_int16 = 0; | |
|
12 | int16_t hk_lfr_sc_e2_f3_as_int16 = 0; | |
|
13 | ||
|
10 | 14 | void timer_configure(unsigned char timer, unsigned int clock_divider, |
|
11 | 15 | unsigned char interrupt_level, rtems_isr (*timer_isr)() ) |
|
12 | 16 | { |
@@ -327,7 +331,8 rtems_task hous_task(rtems_task_argument | |||
|
327 | 331 | |
|
328 | 332 | hk_lfr_le_me_he_update(); |
|
329 | 333 | |
|
330 | housekeeping_packet.hk_lfr_sc_rw_f_flags = cp_rpw_sc_rw_f_flags; | |
|
334 | housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags = cp_rpw_sc_rw1_rw2_f_flags; | |
|
335 | housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags = cp_rpw_sc_rw3_rw4_f_flags; | |
|
331 | 336 | |
|
332 | 337 | // SEND PACKET |
|
333 | 338 | status = rtems_message_queue_send( queue_id, &housekeeping_packet, |
@@ -355,6 +360,9 rtems_task avgv_task(rtems_task_argument | |||
|
355 | 360 | float average_v; |
|
356 | 361 | float average_e1; |
|
357 | 362 | float average_e2; |
|
363 | float newValue_v; | |
|
364 | float newValue_e1; | |
|
365 | float newValue_e2; | |
|
358 | 366 | unsigned char k; |
|
359 | 367 | unsigned char indexOfOldValue; |
|
360 | 368 | |
@@ -377,33 +385,38 rtems_task avgv_task(rtems_task_argument | |||
|
377 | 385 | |
|
378 | 386 | // initialize values |
|
379 | 387 | indexOfOldValue = MOVING_AVERAGE - 1; |
|
380 |
average_v = |
|
|
381 |
average_e1 = |
|
|
382 |
average_e2 = |
|
|
388 | average_v = INIT_FLOAT; | |
|
389 | average_e1 = INIT_FLOAT; | |
|
390 | average_e2 = INIT_FLOAT; | |
|
391 | newValue_v = INIT_FLOAT; | |
|
392 | newValue_e1 = INIT_FLOAT; | |
|
393 | newValue_e2 = INIT_FLOAT; | |
|
383 | 394 | |
|
384 |
k = |
|
|
395 | k = INIT_CHAR; | |
|
385 | 396 | |
|
386 | while(1){ // launch the rate monotonic task | |
|
397 | while(1) | |
|
398 | { // launch the rate monotonic task | |
|
387 | 399 | status = rtems_rate_monotonic_period( AVGV_id, AVGV_PERIOD ); |
|
388 |
if ( status != RTEMS_SUCCESSFUL ) |
|
|
400 | if ( status != RTEMS_SUCCESSFUL ) | |
|
401 | { | |
|
389 | 402 | PRINTF1( "in AVGV *** ERR period: %d\n", status); |
|
390 | 403 | } |
|
391 |
else |
|
|
392 | // store new value in buffer | |
|
393 | v[k] = waveform_picker_regs->v; | |
|
394 |
|
|
|
395 |
|
|
|
396 | if (k == (MOVING_AVERAGE - 1)) | |
|
397 | { | |
|
398 | indexOfOldValue = 0; | |
|
399 | } | |
|
400 | else | |
|
401 | { | |
|
402 | indexOfOldValue = k + 1; | |
|
403 | } | |
|
404 | average_v = average_v + v[k] - v[indexOfOldValue]; | |
|
405 | average_e1 = average_e1 + e1[k] - e1[indexOfOldValue]; | |
|
406 | average_e2 = average_e2 + e2[k] - e2[indexOfOldValue]; | |
|
404 | else | |
|
405 | { | |
|
406 | // get new values | |
|
407 | newValue_v = waveform_picker_regs->v; | |
|
408 | newValue_e1 = waveform_picker_regs->e1; | |
|
409 | newValue_e2 = waveform_picker_regs->e2; | |
|
410 | ||
|
411 | // compute the moving average | |
|
412 | average_v = average_v + newValue_v - v[k]; | |
|
413 | average_e1 = average_e1 + newValue_e1 - e1[k]; | |
|
414 | average_e2 = average_e2 + newValue_e2 - e2[k]; | |
|
415 | ||
|
416 | // store new values in buffers | |
|
417 | v[k] = newValue_v; | |
|
418 | e1[k] = newValue_e1; | |
|
419 | e2[k] = newValue_e2; | |
|
407 | 420 | } |
|
408 | 421 | if (k == (MOVING_AVERAGE-1)) |
|
409 | 422 | { |
@@ -414,11 +427,15 rtems_task avgv_task(rtems_task_argument | |||
|
414 | 427 | { |
|
415 | 428 | k++; |
|
416 | 429 | } |
|
430 | //update int16 values | |
|
431 | hk_lfr_sc_v_f3_as_int16 = (int16_t) (average_v / ((float) MOVING_AVERAGE) ); | |
|
432 | hk_lfr_sc_e1_f3_as_int16 = (int16_t) (average_e1 / ((float) MOVING_AVERAGE) ); | |
|
433 | hk_lfr_sc_e2_f3_as_int16 = (int16_t) (average_e2 / ((float) MOVING_AVERAGE) ); | |
|
417 | 434 | } |
|
418 | 435 | |
|
419 | PRINTF("in AVGV *** deleting task\n") | |
|
436 | PRINTF("in AVGV *** deleting task\n"); | |
|
420 | 437 | |
|
421 |
|
|
|
438 | status = rtems_task_delete( RTEMS_SELF ); // should not return | |
|
422 | 439 | |
|
423 | 440 | return; |
|
424 | 441 | } |
@@ -672,16 +689,16 void get_v_e1_e2_f3( unsigned char *spac | |||
|
672 | 689 | unsigned char* e1_ptr; |
|
673 | 690 | unsigned char* e2_ptr; |
|
674 | 691 | |
|
675 |
v_ptr = (unsigned char *) & |
|
|
676 |
e1_ptr = (unsigned char *) & |
|
|
677 |
e2_ptr = (unsigned char *) & |
|
|
692 | v_ptr = (unsigned char *) &hk_lfr_sc_v_f3_as_int16; | |
|
693 | e1_ptr = (unsigned char *) &hk_lfr_sc_e1_f3_as_int16; | |
|
694 | e2_ptr = (unsigned char *) &hk_lfr_sc_e2_f3_as_int16; | |
|
678 | 695 | |
|
679 |
spacecraft_potential[ |
|
|
680 |
spacecraft_potential[ |
|
|
681 |
spacecraft_potential[ |
|
|
682 |
spacecraft_potential[ |
|
|
683 |
spacecraft_potential[ |
|
|
684 |
spacecraft_potential[ |
|
|
696 | spacecraft_potential[BYTE_0] = v_ptr[0]; | |
|
697 | spacecraft_potential[BYTE_1] = v_ptr[1]; | |
|
698 | spacecraft_potential[BYTE_2] = e1_ptr[0]; | |
|
699 | spacecraft_potential[BYTE_3] = e1_ptr[1]; | |
|
700 | spacecraft_potential[BYTE_4] = e2_ptr[0]; | |
|
701 | spacecraft_potential[BYTE_5] = e2_ptr[1]; | |
|
685 | 702 | } |
|
686 | 703 | |
|
687 | 704 | void get_cpu_load( unsigned char *resource_statistics ) |
@@ -301,8 +301,9 int action_update_info(ccsdsTelecommandP | |||
|
301 | 301 | |
|
302 | 302 | // REACTION_WHEELS_FREQUENCY, copy the incoming parameters in the local variable (to be copied in HK packets) |
|
303 | 303 | |
|
304 | cp_rpw_sc_rw_f_flags = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW_F_FLAGS ]; | |
|
304 | //cp_rpw_sc_rw_f_flags = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW_F_FLAGS ]; | |
|
305 | 305 | getReactionWheelsFrequencies( TC ); |
|
306 | set_hk_lfr_sc_rw_f_flags(); | |
|
306 | 307 | build_sy_lfr_rw_masks(); |
|
307 | 308 | |
|
308 | 309 | // once the masks are built, they have to be merged with the fbins_mask |
@@ -324,8 +324,10 int action_load_filter_par(ccsdsTelecomm | |||
|
324 | 324 | */ |
|
325 | 325 | |
|
326 | 326 | int flag; |
|
327 | unsigned char k; | |
|
327 | 328 | |
|
328 | 329 | flag = LFR_DEFAULT; |
|
330 | k = INIT_CHAR; | |
|
329 | 331 | |
|
330 | 332 | flag = check_sy_lfr_filter_parameters( TC, queue_id ); |
|
331 | 333 | |
@@ -367,6 +369,36 int action_load_filter_par(ccsdsTelecomm | |||
|
367 | 369 | // store the parameter sy_lfr_sc_rw_delta_f as a float |
|
368 | 370 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_sc_rw_delta_f, |
|
369 | 371 | parameter_dump_packet.sy_lfr_sc_rw_delta_f ); |
|
372 | ||
|
373 | // copy rw.._k.. from the incoming TC to the local parameter_dump_packet | |
|
374 | for (k = 0; k < NB_RW_K_COEFFS * NB_BYTES_PER_RW_K_COEFF; k++) | |
|
375 | { | |
|
376 | parameter_dump_packet.sy_lfr_rw1_k1[k] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_RW1_K1 + k ]; | |
|
377 | } | |
|
378 | ||
|
379 | //*********************************************** | |
|
380 | // store the parameter sy_lfr_rw.._k.. as a float | |
|
381 | // rw1_k | |
|
382 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k1, parameter_dump_packet.sy_lfr_rw1_k1 ); | |
|
383 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k2, parameter_dump_packet.sy_lfr_rw1_k2 ); | |
|
384 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k3, parameter_dump_packet.sy_lfr_rw1_k3 ); | |
|
385 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k4, parameter_dump_packet.sy_lfr_rw1_k4 ); | |
|
386 | // rw2_k | |
|
387 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k1, parameter_dump_packet.sy_lfr_rw2_k1 ); | |
|
388 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k2, parameter_dump_packet.sy_lfr_rw2_k2 ); | |
|
389 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k3, parameter_dump_packet.sy_lfr_rw2_k3 ); | |
|
390 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k4, parameter_dump_packet.sy_lfr_rw2_k4 ); | |
|
391 | // rw3_k | |
|
392 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k1, parameter_dump_packet.sy_lfr_rw3_k1 ); | |
|
393 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k2, parameter_dump_packet.sy_lfr_rw3_k2 ); | |
|
394 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k3, parameter_dump_packet.sy_lfr_rw3_k3 ); | |
|
395 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k4, parameter_dump_packet.sy_lfr_rw3_k4 ); | |
|
396 | // rw4_k | |
|
397 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k1, parameter_dump_packet.sy_lfr_rw4_k1 ); | |
|
398 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k2, parameter_dump_packet.sy_lfr_rw4_k2 ); | |
|
399 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k3, parameter_dump_packet.sy_lfr_rw4_k3 ); | |
|
400 | copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k4, parameter_dump_packet.sy_lfr_rw4_k4 ); | |
|
401 | ||
|
370 | 402 | } |
|
371 | 403 | |
|
372 | 404 | return flag; |
@@ -933,6 +965,81 unsigned int check_update_info_hk_thr_mo | |||
|
933 | 965 | return status; |
|
934 | 966 | } |
|
935 | 967 | |
|
968 | void set_hk_lfr_sc_rw_f_flag( unsigned char wheel, unsigned char freq, float value ) | |
|
969 | { | |
|
970 | unsigned char flag; | |
|
971 | unsigned char flagPosInByte; | |
|
972 | unsigned char newFlag; | |
|
973 | unsigned char flagMask; | |
|
974 | ||
|
975 | // if the frequency value is not a number, the flag is set to 0 and the frequency RWx_Fy is not filtered | |
|
976 | if (isnan(value)) | |
|
977 | { | |
|
978 | flag = FLAG_NAN; | |
|
979 | } | |
|
980 | else | |
|
981 | { | |
|
982 | flag = FLAG_IAN; | |
|
983 | } | |
|
984 | ||
|
985 | switch(wheel) | |
|
986 | { | |
|
987 | case WHEEL_1: | |
|
988 | flagPosInByte = FLAG_OFFSET_WHEELS_1_3 - freq; | |
|
989 | flagMask = ~(1 << flagPosInByte); | |
|
990 | newFlag = flag << flagPosInByte; | |
|
991 | housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags = (housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags & flagMask) | newFlag; | |
|
992 | break; | |
|
993 | case WHEEL_2: | |
|
994 | flagPosInByte = FLAG_OFFSET_WHEELS_2_4 - freq; | |
|
995 | flagMask = ~(1 << flagPosInByte); | |
|
996 | newFlag = flag << flagPosInByte; | |
|
997 | housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags = (housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags & flagMask) | newFlag; | |
|
998 | break; | |
|
999 | case WHEEL_3: | |
|
1000 | flagPosInByte = FLAG_OFFSET_WHEELS_1_3 - freq; | |
|
1001 | flagMask = ~(1 << flagPosInByte); | |
|
1002 | newFlag = flag << flagPosInByte; | |
|
1003 | housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags = (housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags & flagMask) | newFlag; | |
|
1004 | break; | |
|
1005 | case WHEEL_4: | |
|
1006 | flagPosInByte = FLAG_OFFSET_WHEELS_2_4 - freq; | |
|
1007 | flagMask = ~(1 << flagPosInByte); | |
|
1008 | newFlag = flag << flagPosInByte; | |
|
1009 | housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags = (housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags & flagMask) | newFlag; | |
|
1010 | break; | |
|
1011 | default: | |
|
1012 | break; | |
|
1013 | } | |
|
1014 | } | |
|
1015 | ||
|
1016 | void set_hk_lfr_sc_rw_f_flags( void ) | |
|
1017 | { | |
|
1018 | // RW1 | |
|
1019 | set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_1, rw_f.cp_rpw_sc_rw1_f1 ); | |
|
1020 | set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_2, rw_f.cp_rpw_sc_rw1_f2 ); | |
|
1021 | set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_3, rw_f.cp_rpw_sc_rw1_f3 ); | |
|
1022 | set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_4, rw_f.cp_rpw_sc_rw1_f4 ); | |
|
1023 | ||
|
1024 | // RW2 | |
|
1025 | set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_1, rw_f.cp_rpw_sc_rw2_f1 ); | |
|
1026 | set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_2, rw_f.cp_rpw_sc_rw2_f2 ); | |
|
1027 | set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_3, rw_f.cp_rpw_sc_rw2_f3 ); | |
|
1028 | set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_4, rw_f.cp_rpw_sc_rw2_f4 ); | |
|
1029 | ||
|
1030 | // RW3 | |
|
1031 | set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_1, rw_f.cp_rpw_sc_rw3_f1 ); | |
|
1032 | set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_2, rw_f.cp_rpw_sc_rw3_f2 ); | |
|
1033 | set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_3, rw_f.cp_rpw_sc_rw3_f3 ); | |
|
1034 | set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_4, rw_f.cp_rpw_sc_rw3_f4 ); | |
|
1035 | ||
|
1036 | // RW4 | |
|
1037 | set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_1, rw_f.cp_rpw_sc_rw4_f1 ); | |
|
1038 | set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_2, rw_f.cp_rpw_sc_rw4_f2 ); | |
|
1039 | set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_3, rw_f.cp_rpw_sc_rw4_f3 ); | |
|
1040 | set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_4, rw_f.cp_rpw_sc_rw4_f4 ); | |
|
1041 | } | |
|
1042 | ||
|
936 | 1043 | void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC ) |
|
937 | 1044 | { |
|
938 | 1045 | /** This function get the reaction wheels frequencies in the incoming TC_LFR_UPDATE_INFO and copy the values locally. |
@@ -945,37 +1052,32 void getReactionWheelsFrequencies( ccsds | |||
|
945 | 1052 | |
|
946 | 1053 | bytePosPtr = (unsigned char *) &TC->packetID; |
|
947 | 1054 | |
|
948 |
// |
|
|
949 | copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw1_f1, | |
|
950 |
|
|
|
951 | ||
|
952 | // cp_rpw_sc_rw1_f2 | |
|
953 | copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw1_f2, | |
|
954 | (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2 ] ); | |
|
1055 | // rw1_f | |
|
1056 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1 ] ); | |
|
1057 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2 ] ); | |
|
1058 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F3 ] ); | |
|
1059 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F4 ] ); | |
|
955 | 1060 | |
|
956 |
// |
|
|
957 | copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw2_f1, | |
|
958 |
|
|
|
959 | ||
|
960 | // cp_rpw_sc_rw2_f2 | |
|
961 | copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw2_f2, | |
|
962 | (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2 ] ); | |
|
1061 | // rw2_f | |
|
1062 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1 ] ); | |
|
1063 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2 ] ); | |
|
1064 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F3 ] ); | |
|
1065 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F4 ] ); | |
|
963 | 1066 | |
|
964 |
// |
|
|
965 | copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw3_f1, | |
|
966 |
|
|
|
967 | ||
|
968 | // cp_rpw_sc_rw3_f2 | |
|
969 | copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw3_f2, | |
|
970 | (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2 ] ); | |
|
1067 | // rw3_f | |
|
1068 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1 ] ); | |
|
1069 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2 ] ); | |
|
1070 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F3 ] ); | |
|
1071 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F4 ] ); | |
|
971 | 1072 | |
|
972 |
// |
|
|
973 | copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw4_f1, | |
|
974 |
|
|
|
1073 | // rw4_f | |
|
1074 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1 ] ); | |
|
1075 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 ] ); | |
|
1076 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F3 ] ); | |
|
1077 | copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F4 ] ); | |
|
975 | 1078 | |
|
976 | // cp_rpw_sc_rw4_f2 | |
|
977 | copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw4_f2, | |
|
978 | (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 ] ); | |
|
1079 | // test each reaction wheel frequency value. NaN means that the frequency is not filtered | |
|
1080 | ||
|
979 | 1081 | } |
|
980 | 1082 | |
|
981 | 1083 | void setFBinMask( unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, unsigned char flag ) |
@@ -1116,29 +1218,29 void build_sy_lfr_rw_mask( unsigned int | |||
|
1116 | 1218 | local_rw_fbins_mask[k] = INT8_ALL_F; |
|
1117 | 1219 | } |
|
1118 | 1220 | |
|
1119 |
// RW1 |
|
|
1120 |
setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw1_f1, deltaF, (cp_rpw_sc_rw_f_flags & BIT_ |
|
|
1121 | ||
|
1122 | // RW1 F2 | |
|
1123 |
setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw1_f2, deltaF, (cp_rpw_sc_rw_f_flags & BIT_ |
|
|
1221 | // RW1 | |
|
1222 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f1, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & BIT_7) >> SHIFT_7_BITS ); // [1000 0000] | |
|
1223 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f2, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & BIT_6) >> SHIFT_6_BITS ); // [0100 0000] | |
|
1224 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f1, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & BIT_5) >> SHIFT_5_BITS ); // [0010 0000] | |
|
1225 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f2, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & BIT_4) >> SHIFT_4_BITS ); // [0001 0000] | |
|
1124 | 1226 | |
|
1125 |
// RW2 |
|
|
1126 |
setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw2_f1, deltaF, (cp_rpw_sc_rw_f_flags & BIT_ |
|
|
1127 | ||
|
1128 | // RW2 F2 | |
|
1129 |
setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw2_f2, deltaF, (cp_rpw_sc_rw_f_flags & BIT_ |
|
|
1227 | // RW2 | |
|
1228 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f1, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & BIT_3) >> SHIFT_3_BITS ); // [0000 1000] | |
|
1229 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f2, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & BIT_2) >> SHIFT_2_BITS ); // [0000 0100] | |
|
1230 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f1, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & BIT_1) >> SHIFT_1_BITS ); // [0000 0010] | |
|
1231 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f2, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & BIT_0) ); // [0000 0001] | |
|
1130 | 1232 | |
|
1131 |
// RW3 |
|
|
1132 |
setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw3_f1, deltaF, (cp_rpw_sc_rw_f_flags & BIT_ |
|
|
1133 | ||
|
1134 | // RW3 F2 | |
|
1135 |
setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw3_f2, deltaF, (cp_rpw_sc_rw_f_flags & BIT_ |
|
|
1233 | // RW3 | |
|
1234 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f1, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & BIT_7) >> SHIFT_7_BITS ); // [1000 0000] | |
|
1235 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f2, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & BIT_6) >> SHIFT_6_BITS ); // [0100 0000] | |
|
1236 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f1, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & BIT_5) >> SHIFT_5_BITS ); // [0010 0000] | |
|
1237 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f2, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & BIT_4) >> SHIFT_4_BITS ); // [0001 0000] | |
|
1136 | 1238 | |
|
1137 |
// RW4 |
|
|
1138 |
setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw4_f1, deltaF, (cp_rpw_sc_rw_f_flags & BIT_ |
|
|
1139 | ||
|
1140 | // RW4 F2 | |
|
1141 |
setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw4_f2, deltaF, (cp_rpw_sc_rw_f_flags & BIT_ |
|
|
1239 | // RW4 | |
|
1240 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f1, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & BIT_3) >> SHIFT_3_BITS ); // [0000 1000] | |
|
1241 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f2, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & BIT_2) >> SHIFT_2_BITS ); // [0000 0100] | |
|
1242 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f1, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & BIT_1) >> SHIFT_1_BITS ); // [0000 0010] | |
|
1243 | setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f2, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & BIT_0) ); // [0000 0001] | |
|
1142 | 1244 | |
|
1143 | 1245 | // update the value of the fbins related to reaction wheels frequency filtering |
|
1144 | 1246 | if (maskPtr != NULL) |
@@ -1502,6 +1604,27 void init_parameter_dump( void ) | |||
|
1502 | 1604 | floatToChar( DEFAULT_SY_LFR_PAS_FILTER_SHIFT, parameter_dump_packet.sy_lfr_pas_filter_shift ); |
|
1503 | 1605 | floatToChar( DEFAULT_SY_LFR_SC_RW_DELTA_F, parameter_dump_packet.sy_lfr_sc_rw_delta_f ); |
|
1504 | 1606 | |
|
1607 | // RW1_K | |
|
1608 | floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw1_k1); | |
|
1609 | floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw1_k2); | |
|
1610 | floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw1_k3); | |
|
1611 | floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw1_k4); | |
|
1612 | // RW2_K | |
|
1613 | floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw2_k1); | |
|
1614 | floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw2_k2); | |
|
1615 | floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw2_k3); | |
|
1616 | floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw2_k4); | |
|
1617 | // RW3_K | |
|
1618 | floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw3_k1); | |
|
1619 | floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw3_k2); | |
|
1620 | floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw3_k3); | |
|
1621 | floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw3_k4); | |
|
1622 | // RW4_K | |
|
1623 | floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw4_k1); | |
|
1624 | floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw4_k2); | |
|
1625 | floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw4_k3); | |
|
1626 | floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw4_k4); | |
|
1627 | ||
|
1505 | 1628 | // LFR_RW_MASK |
|
1506 | 1629 | for (k=0; k < BYTES_PER_MASKS_SET; k++) |
|
1507 | 1630 | { |
General Comments 0
You need to be logged in to leave comments.
Login now