##// END OF EJS Templates
compliance with ICD 4.3...
paul -
r328:5023f9ef69f2 R3++ draft
parent child
Show More
@@ -1,2 +1,2
1 3081d1f9bb20b2b64a192585337a292a9804e0c5 LFR_basic-parameters
1 3081d1f9bb20b2b64a192585337a292a9804e0c5 LFR_basic-parameters
2 7c46de6059673d3239fcc7103e16510727f35923 header/lfr_common_headers
2 058c1234c2defe215d3dd655a7ef65abb33c922d header/lfr_common_headers
@@ -21,16 +21,11 extern rtems_id Task_id[]; /*
21 extern rtems_name timecode_timer_name;
21 extern rtems_name timecode_timer_name;
22 extern rtems_id timecode_timer_id;
22 extern rtems_id timecode_timer_id;
23 extern unsigned char pa_bia_status_info;
23 extern unsigned char pa_bia_status_info;
24 extern unsigned char cp_rpw_sc_rw_f_flags;
24 extern unsigned char cp_rpw_sc_rw1_rw2_f_flags;
25 extern float cp_rpw_sc_rw1_f1;
25 extern unsigned char cp_rpw_sc_rw3_rw4_f_flags;
26 extern float cp_rpw_sc_rw1_f2;
26
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;
33 extern filterPar_t filterPar;
27 extern filterPar_t filterPar;
28 extern rw_f_t rw_f;
34
29
35 // RTEMS TASKS
30 // RTEMS TASKS
36 rtems_task Init( rtems_task_argument argument);
31 rtems_task Init( rtems_task_argument argument);
@@ -29,6 +29,20
29 #define BIT_RW4_F1 0x02
29 #define BIT_RW4_F1 0x02
30 #define BIT_RW4_F2 0x01
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 #define SBM_KCOEFF_PER_NORM_KCOEFF 2
46 #define SBM_KCOEFF_PER_NORM_KCOEFF 2
33
47
34 extern unsigned short sequenceCounterParameterDump;
48 extern unsigned short sequenceCounterParameterDump;
@@ -76,6 +90,8 int set_sy_lfr_s2_bp_p1( ccsdsTelecomman
76 unsigned int check_update_info_hk_lfr_mode( unsigned char mode );
90 unsigned int check_update_info_hk_lfr_mode( unsigned char mode );
77 unsigned int check_update_info_hk_tds_mode( unsigned char mode );
91 unsigned int check_update_info_hk_tds_mode( unsigned char mode );
78 unsigned int check_update_info_hk_thr_mode( unsigned char mode );
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 void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC );
95 void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC );
80 void setFBinMask(unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, unsigned char flag );
96 void setFBinMask(unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, unsigned char flag );
81 void build_sy_lfr_rw_mask( unsigned int channel );
97 void build_sy_lfr_rw_mask( unsigned int channel );
@@ -73,7 +73,8 unsigned int lastValidEnterModeTime
73
73
74 // HK PACKETS
74 // HK PACKETS
75 Packet_TM_LFR_HK_t housekeeping_packet = {0};
75 Packet_TM_LFR_HK_t housekeeping_packet = {0};
76 unsigned char cp_rpw_sc_rw_f_flags = 0;
76 unsigned char cp_rpw_sc_rw1_rw2_f_flags = 0;
77 unsigned char cp_rpw_sc_rw3_rw4_f_flags = 0;
77 // message queues occupancy
78 // message queues occupancy
78 unsigned char hk_lfr_q_sd_fifo_size_max = 0;
79 unsigned char hk_lfr_q_sd_fifo_size_max = 0;
79 unsigned char hk_lfr_q_rv_fifo_size_max = 0;
80 unsigned char hk_lfr_q_rv_fifo_size_max = 0;
@@ -89,14 +90,7 unsigned short sequenceCounterHK
89 spw_stats grspw_stats = {0};
90 spw_stats grspw_stats = {0};
90
91
91 // TC_LFR_UPDATE_INFO
92 // TC_LFR_UPDATE_INFO
92 float cp_rpw_sc_rw1_f1 = INIT_FLOAT;
93 rw_f_t rw_f;
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;
100
94
101 // TC_LFR_LOAD_FILTER_PAR
95 // TC_LFR_LOAD_FILTER_PAR
102 filterPar_t filterPar = {0};
96 filterPar_t filterPar = {0};
@@ -168,14 +168,28 rtems_task Init( rtems_task_argument ign
168 init_k_coefficients_prc2();
168 init_k_coefficients_prc2();
169 pa_bia_status_info = INIT_CHAR;
169 pa_bia_status_info = INIT_CHAR;
170 cp_rpw_sc_rw_f_flags = INIT_CHAR;
170 cp_rpw_sc_rw_f_flags = INIT_CHAR;
171 cp_rpw_sc_rw1_f1 = INIT_FLOAT;
171
172 cp_rpw_sc_rw1_f2 = INIT_FLOAT;
172 // initialize all reaction wheels frequencies to NaN
173 cp_rpw_sc_rw2_f1 = INIT_FLOAT;
173 rw_f.cp_rpw_sc_rw1_f1 = NAN;
174 cp_rpw_sc_rw2_f2 = INIT_FLOAT;
174 rw_f.cp_rpw_sc_rw1_f2 = NAN;
175 cp_rpw_sc_rw3_f1 = INIT_FLOAT;
175 rw_f.cp_rpw_sc_rw1_f3 = NAN;
176 cp_rpw_sc_rw3_f2 = INIT_FLOAT;
176 rw_f.cp_rpw_sc_rw1_f4 = NAN;
177 cp_rpw_sc_rw4_f1 = INIT_FLOAT;
177 rw_f.cp_rpw_sc_rw2_f1 = NAN;
178 cp_rpw_sc_rw4_f2 = INIT_FLOAT;
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 // initialize filtering parameters
193 // initialize filtering parameters
180 filterPar.spare_sy_lfr_pas_filter_enabled = DEFAULT_SY_LFR_PAS_FILTER_ENABLED;
194 filterPar.spare_sy_lfr_pas_filter_enabled = DEFAULT_SY_LFR_PAS_FILTER_ENABLED;
181 filterPar.sy_lfr_pas_filter_modulus = DEFAULT_SY_LFR_PAS_FILTER_MODULUS;
195 filterPar.sy_lfr_pas_filter_modulus = DEFAULT_SY_LFR_PAS_FILTER_MODULUS;
@@ -7,6 +7,10
7
7
8 #include "fsw_misc.h"
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 void timer_configure(unsigned char timer, unsigned int clock_divider,
14 void timer_configure(unsigned char timer, unsigned int clock_divider,
11 unsigned char interrupt_level, rtems_isr (*timer_isr)() )
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 hk_lfr_le_me_he_update();
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 // SEND PACKET
337 // SEND PACKET
333 status = rtems_message_queue_send( queue_id, &housekeeping_packet,
338 status = rtems_message_queue_send( queue_id, &housekeeping_packet,
@@ -355,6 +360,9 rtems_task avgv_task(rtems_task_argument
355 float average_v;
360 float average_v;
356 float average_e1;
361 float average_e1;
357 float average_e2;
362 float average_e2;
363 float newValue_v;
364 float newValue_e1;
365 float newValue_e2;
358 unsigned char k;
366 unsigned char k;
359 unsigned char indexOfOldValue;
367 unsigned char indexOfOldValue;
360
368
@@ -377,33 +385,38 rtems_task avgv_task(rtems_task_argument
377
385
378 // initialize values
386 // initialize values
379 indexOfOldValue = MOVING_AVERAGE - 1;
387 indexOfOldValue = MOVING_AVERAGE - 1;
380 average_v = 0.;
388 average_v = INIT_FLOAT;
381 average_e1 = 0.;
389 average_e1 = INIT_FLOAT;
382 average_e2 = 0.;
390 average_e2 = INIT_FLOAT;
391 newValue_v = INIT_FLOAT;
392 newValue_e1 = INIT_FLOAT;
393 newValue_e2 = INIT_FLOAT;
383
394
384 k = 0;
395 k = INIT_CHAR;
385
396
386 while(1){ // launch the rate monotonic task
397 while(1)
398 { // launch the rate monotonic task
387 status = rtems_rate_monotonic_period( AVGV_id, AVGV_PERIOD );
399 status = rtems_rate_monotonic_period( AVGV_id, AVGV_PERIOD );
388 if ( status != RTEMS_SUCCESSFUL ) {
400 if ( status != RTEMS_SUCCESSFUL )
401 {
389 PRINTF1( "in AVGV *** ERR period: %d\n", status);
402 PRINTF1( "in AVGV *** ERR period: %d\n", status);
390 }
403 }
391 else {
404 else
392 // store new value in buffer
405 {
393 v[k] = waveform_picker_regs->v;
406 // get new values
394 e1[k] = waveform_picker_regs->e1;
407 newValue_v = waveform_picker_regs->v;
395 e2[k] = waveform_picker_regs->e2;
408 newValue_e1 = waveform_picker_regs->e1;
396 if (k == (MOVING_AVERAGE - 1))
409 newValue_e2 = waveform_picker_regs->e2;
397 {
410
398 indexOfOldValue = 0;
411 // compute the moving average
399 }
412 average_v = average_v + newValue_v - v[k];
400 else
413 average_e1 = average_e1 + newValue_e1 - e1[k];
401 {
414 average_e2 = average_e2 + newValue_e2 - e2[k];
402 indexOfOldValue = k + 1;
415
403 }
416 // store new values in buffers
404 average_v = average_v + v[k] - v[indexOfOldValue];
417 v[k] = newValue_v;
405 average_e1 = average_e1 + e1[k] - e1[indexOfOldValue];
418 e1[k] = newValue_e1;
406 average_e2 = average_e2 + e2[k] - e2[indexOfOldValue];
419 e2[k] = newValue_e2;
407 }
420 }
408 if (k == (MOVING_AVERAGE-1))
421 if (k == (MOVING_AVERAGE-1))
409 {
422 {
@@ -414,11 +427,15 rtems_task avgv_task(rtems_task_argument
414 {
427 {
415 k++;
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 status = rtems_task_delete( RTEMS_SELF ); // should not return
438 status = rtems_task_delete( RTEMS_SELF ); // should not return
422
439
423 return;
440 return;
424 }
441 }
@@ -672,16 +689,16 void get_v_e1_e2_f3( unsigned char *spac
672 unsigned char* e1_ptr;
689 unsigned char* e1_ptr;
673 unsigned char* e2_ptr;
690 unsigned char* e2_ptr;
674
691
675 v_ptr = (unsigned char *) &waveform_picker_regs->v;
692 v_ptr = (unsigned char *) &hk_lfr_sc_v_f3_as_int16;
676 e1_ptr = (unsigned char *) &waveform_picker_regs->e1;
693 e1_ptr = (unsigned char *) &hk_lfr_sc_e1_f3_as_int16;
677 e2_ptr = (unsigned char *) &waveform_picker_regs->e2;
694 e2_ptr = (unsigned char *) &hk_lfr_sc_e2_f3_as_int16;
678
695
679 spacecraft_potential[ BYTE_0 ] = v_ptr[ BYTE_2 ];
696 spacecraft_potential[BYTE_0] = v_ptr[0];
680 spacecraft_potential[ BYTE_1 ] = v_ptr[ BYTE_3 ];
697 spacecraft_potential[BYTE_1] = v_ptr[1];
681 spacecraft_potential[ BYTE_2 ] = e1_ptr[ BYTE_2 ];
698 spacecraft_potential[BYTE_2] = e1_ptr[0];
682 spacecraft_potential[ BYTE_3 ] = e1_ptr[ BYTE_3 ];
699 spacecraft_potential[BYTE_3] = e1_ptr[1];
683 spacecraft_potential[ BYTE_4 ] = e2_ptr[ BYTE_2 ];
700 spacecraft_potential[BYTE_4] = e2_ptr[0];
684 spacecraft_potential[ BYTE_5 ] = e2_ptr[ BYTE_3 ];
701 spacecraft_potential[BYTE_5] = e2_ptr[1];
685 }
702 }
686
703
687 void get_cpu_load( unsigned char *resource_statistics )
704 void get_cpu_load( unsigned char *resource_statistics )
@@ -301,8 +301,9 int action_update_info(ccsdsTelecommandP
301
301
302 // REACTION_WHEELS_FREQUENCY, copy the incoming parameters in the local variable (to be copied in HK packets)
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 getReactionWheelsFrequencies( TC );
305 getReactionWheelsFrequencies( TC );
306 set_hk_lfr_sc_rw_f_flags();
306 build_sy_lfr_rw_masks();
307 build_sy_lfr_rw_masks();
307
308
308 // once the masks are built, they have to be merged with the fbins_mask
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 int flag;
326 int flag;
327 unsigned char k;
327
328
328 flag = LFR_DEFAULT;
329 flag = LFR_DEFAULT;
330 k = INIT_CHAR;
329
331
330 flag = check_sy_lfr_filter_parameters( TC, queue_id );
332 flag = check_sy_lfr_filter_parameters( TC, queue_id );
331
333
@@ -367,6 +369,36 int action_load_filter_par(ccsdsTelecomm
367 // store the parameter sy_lfr_sc_rw_delta_f as a float
369 // store the parameter sy_lfr_sc_rw_delta_f as a float
368 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_sc_rw_delta_f,
370 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_sc_rw_delta_f,
369 parameter_dump_packet.sy_lfr_sc_rw_delta_f );
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 return flag;
404 return flag;
@@ -933,6 +965,81 unsigned int check_update_info_hk_thr_mo
933 return status;
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 void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC )
1043 void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC )
937 {
1044 {
938 /** This function get the reaction wheels frequencies in the incoming TC_LFR_UPDATE_INFO and copy the values locally.
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 bytePosPtr = (unsigned char *) &TC->packetID;
1053 bytePosPtr = (unsigned char *) &TC->packetID;
947
1054
948 // cp_rpw_sc_rw1_f1
1055 // rw1_f
949 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw1_f1,
1056 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1 ] );
950 (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 ] );
951
1058 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F3 ] );
952 // cp_rpw_sc_rw1_f2
1059 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F4 ] );
953 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw1_f2,
954 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2 ] );
955
1060
956 // cp_rpw_sc_rw2_f1
1061 // rw2_f
957 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw2_f1,
1062 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1 ] );
958 (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 ] );
959
1064 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F3 ] );
960 // cp_rpw_sc_rw2_f2
1065 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F4 ] );
961 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw2_f2,
962 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2 ] );
963
1066
964 // cp_rpw_sc_rw3_f1
1067 // rw3_f
965 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw3_f1,
1068 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1 ] );
966 (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 ] );
967
1070 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F3 ] );
968 // cp_rpw_sc_rw3_f2
1071 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F4 ] );
969 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw3_f2,
970 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2 ] );
971
1072
972 // cp_rpw_sc_rw4_f1
1073 // rw4_f
973 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw4_f1,
1074 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1 ] );
974 (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
1079 // test each reaction wheel frequency value. NaN means that the frequency is not filtered
977 copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw4_f2,
1080
978 (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 ] );
979 }
1081 }
980
1082
981 void setFBinMask( unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, unsigned char flag )
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 local_rw_fbins_mask[k] = INT8_ALL_F;
1218 local_rw_fbins_mask[k] = INT8_ALL_F;
1117 }
1219 }
1118
1220
1119 // RW1 F1
1221 // RW1
1120 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw1_f1, deltaF, (cp_rpw_sc_rw_f_flags & BIT_RW1_F1) >> SHIFT_7_BITS ); // [1000 0000]
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]
1121
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]
1122 // RW1 F2
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]
1123 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw1_f2, deltaF, (cp_rpw_sc_rw_f_flags & BIT_RW1_F2) >> SHIFT_6_BITS ); // [0100 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 F1
1227 // RW2
1126 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw2_f1, deltaF, (cp_rpw_sc_rw_f_flags & BIT_RW2_F1) >> SHIFT_5_BITS ); // [0010 0000]
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]
1127
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]
1128 // RW2 F2
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]
1129 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw2_f2, deltaF, (cp_rpw_sc_rw_f_flags & BIT_RW2_F2) >> SHIFT_4_BITS ); // [0001 0000]
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 F1
1233 // RW3
1132 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw3_f1, deltaF, (cp_rpw_sc_rw_f_flags & BIT_RW3_F1) >> SHIFT_3_BITS ); // [0000 1000]
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]
1133
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]
1134 // RW3 F2
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]
1135 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw3_f2, deltaF, (cp_rpw_sc_rw_f_flags & BIT_RW3_F2) >> SHIFT_2_BITS ); // [0000 0100]
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 F1
1239 // RW4
1138 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw4_f1, deltaF, (cp_rpw_sc_rw_f_flags & BIT_RW4_F1) >> 1 ); // [0000 0010]
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]
1139
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]
1140 // RW4 F2
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]
1141 setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw4_f2, deltaF, (cp_rpw_sc_rw_f_flags & BIT_RW4_F2) ); // [0000 0001]
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 // update the value of the fbins related to reaction wheels frequency filtering
1245 // update the value of the fbins related to reaction wheels frequency filtering
1144 if (maskPtr != NULL)
1246 if (maskPtr != NULL)
@@ -1502,6 +1604,27 void init_parameter_dump( void )
1502 floatToChar( DEFAULT_SY_LFR_PAS_FILTER_SHIFT, parameter_dump_packet.sy_lfr_pas_filter_shift );
1604 floatToChar( DEFAULT_SY_LFR_PAS_FILTER_SHIFT, parameter_dump_packet.sy_lfr_pas_filter_shift );
1503 floatToChar( DEFAULT_SY_LFR_SC_RW_DELTA_F, parameter_dump_packet.sy_lfr_sc_rw_delta_f );
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 // LFR_RW_MASK
1628 // LFR_RW_MASK
1506 for (k=0; k < BYTES_PER_MASKS_SET; k++)
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