diff --git a/.hgsubstate b/.hgsubstate --- a/.hgsubstate +++ b/.hgsubstate @@ -1,2 +1,2 @@ 3081d1f9bb20b2b64a192585337a292a9804e0c5 LFR_basic-parameters -7c46de6059673d3239fcc7103e16510727f35923 header/lfr_common_headers +058c1234c2defe215d3dd655a7ef65abb33c922d header/lfr_common_headers diff --git a/header/fsw_init.h b/header/fsw_init.h --- a/header/fsw_init.h +++ b/header/fsw_init.h @@ -21,16 +21,11 @@ extern rtems_id Task_id[]; /* extern rtems_name timecode_timer_name; extern rtems_id timecode_timer_id; extern unsigned char pa_bia_status_info; -extern unsigned char cp_rpw_sc_rw_f_flags; -extern float cp_rpw_sc_rw1_f1; -extern float cp_rpw_sc_rw1_f2; -extern float cp_rpw_sc_rw2_f1; -extern float cp_rpw_sc_rw2_f2; -extern float cp_rpw_sc_rw3_f1; -extern float cp_rpw_sc_rw3_f2; -extern float cp_rpw_sc_rw4_f1; -extern float cp_rpw_sc_rw4_f2; +extern unsigned char cp_rpw_sc_rw1_rw2_f_flags; +extern unsigned char cp_rpw_sc_rw3_rw4_f_flags; + extern filterPar_t filterPar; +extern rw_f_t rw_f; // RTEMS TASKS rtems_task Init( rtems_task_argument argument); diff --git a/header/tc_load_dump_parameters.h b/header/tc_load_dump_parameters.h --- a/header/tc_load_dump_parameters.h +++ b/header/tc_load_dump_parameters.h @@ -29,6 +29,20 @@ #define BIT_RW4_F1 0x02 #define BIT_RW4_F2 0x01 +#define WHEEL_1 1 +#define WHEEL_2 2 +#define WHEEL_3 3 +#define WHEEL_4 4 +#define FREQ_1 1 +#define FREQ_2 2 +#define FREQ_3 3 +#define FREQ_4 4 +#define FLAG_OFFSET_WHEELS_1_3 8 +#define FLAG_OFFSET_WHEELS_2_4 4 + +#define FLAG_NAN 0 // Not A NUMBER +#define FLAG_IAN 1 // Is A Number + #define SBM_KCOEFF_PER_NORM_KCOEFF 2 extern unsigned short sequenceCounterParameterDump; @@ -76,6 +90,8 @@ int set_sy_lfr_s2_bp_p1( ccsdsTelecomman unsigned int check_update_info_hk_lfr_mode( unsigned char mode ); unsigned int check_update_info_hk_tds_mode( unsigned char mode ); unsigned int check_update_info_hk_thr_mode( unsigned char mode ); +void set_hk_lfr_sc_rw_f_flag( unsigned char wheel, unsigned char freq, float value ); +void set_hk_lfr_sc_rw_f_flags( void ); void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC ); void setFBinMask(unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, unsigned char flag ); void build_sy_lfr_rw_mask( unsigned int channel ); diff --git a/src/fsw_globals.c b/src/fsw_globals.c --- a/src/fsw_globals.c +++ b/src/fsw_globals.c @@ -73,7 +73,8 @@ unsigned int lastValidEnterModeTime // HK PACKETS Packet_TM_LFR_HK_t housekeeping_packet = {0}; -unsigned char cp_rpw_sc_rw_f_flags = 0; +unsigned char cp_rpw_sc_rw1_rw2_f_flags = 0; +unsigned char cp_rpw_sc_rw3_rw4_f_flags = 0; // message queues occupancy unsigned char hk_lfr_q_sd_fifo_size_max = 0; unsigned char hk_lfr_q_rv_fifo_size_max = 0; @@ -89,14 +90,7 @@ unsigned short sequenceCounterHK spw_stats grspw_stats = {0}; // TC_LFR_UPDATE_INFO -float cp_rpw_sc_rw1_f1 = INIT_FLOAT; -float cp_rpw_sc_rw1_f2 = INIT_FLOAT; -float cp_rpw_sc_rw2_f1 = INIT_FLOAT; -float cp_rpw_sc_rw2_f2 = INIT_FLOAT; -float cp_rpw_sc_rw3_f1 = INIT_FLOAT; -float cp_rpw_sc_rw3_f2 = INIT_FLOAT; -float cp_rpw_sc_rw4_f1 = INIT_FLOAT; -float cp_rpw_sc_rw4_f2 = INIT_FLOAT; +rw_f_t rw_f; // TC_LFR_LOAD_FILTER_PAR filterPar_t filterPar = {0}; diff --git a/src/fsw_init.c b/src/fsw_init.c --- a/src/fsw_init.c +++ b/src/fsw_init.c @@ -168,14 +168,28 @@ rtems_task Init( rtems_task_argument ign init_k_coefficients_prc2(); pa_bia_status_info = INIT_CHAR; cp_rpw_sc_rw_f_flags = INIT_CHAR; - cp_rpw_sc_rw1_f1 = INIT_FLOAT; - cp_rpw_sc_rw1_f2 = INIT_FLOAT; - cp_rpw_sc_rw2_f1 = INIT_FLOAT; - cp_rpw_sc_rw2_f2 = INIT_FLOAT; - cp_rpw_sc_rw3_f1 = INIT_FLOAT; - cp_rpw_sc_rw3_f2 = INIT_FLOAT; - cp_rpw_sc_rw4_f1 = INIT_FLOAT; - cp_rpw_sc_rw4_f2 = INIT_FLOAT; + + // initialize all reaction wheels frequencies to NaN + rw_f.cp_rpw_sc_rw1_f1 = NAN; + rw_f.cp_rpw_sc_rw1_f2 = NAN; + rw_f.cp_rpw_sc_rw1_f3 = NAN; + rw_f.cp_rpw_sc_rw1_f4 = NAN; + rw_f.cp_rpw_sc_rw2_f1 = NAN; + rw_f.cp_rpw_sc_rw2_f2 = NAN; + rw_f.cp_rpw_sc_rw2_f3 = NAN; + rw_f.cp_rpw_sc_rw2_f4 = NAN; + rw_f.cp_rpw_sc_rw3_f1 = NAN; + rw_f.cp_rpw_sc_rw3_f2 = NAN; + rw_f.cp_rpw_sc_rw3_f3 = NAN; + rw_f.cp_rpw_sc_rw3_f4 = NAN; + rw_f.cp_rpw_sc_rw4_f1 = NAN; + rw_f.cp_rpw_sc_rw4_f2 = NAN; + rw_f.cp_rpw_sc_rw4_f3 = NAN; + rw_f.cp_rpw_sc_rw4_f4 = NAN; + + cp_rpw_sc_rw1_rw2_f_flags = INIT_CHAR; + cp_rpw_sc_rw3_rw4_f_flags = INIT_CHAR; + // initialize filtering parameters filterPar.spare_sy_lfr_pas_filter_enabled = DEFAULT_SY_LFR_PAS_FILTER_ENABLED; filterPar.sy_lfr_pas_filter_modulus = DEFAULT_SY_LFR_PAS_FILTER_MODULUS; diff --git a/src/fsw_misc.c b/src/fsw_misc.c --- a/src/fsw_misc.c +++ b/src/fsw_misc.c @@ -7,6 +7,10 @@ #include "fsw_misc.h" +int16_t hk_lfr_sc_v_f3_as_int16 = 0; +int16_t hk_lfr_sc_e1_f3_as_int16 = 0; +int16_t hk_lfr_sc_e2_f3_as_int16 = 0; + void timer_configure(unsigned char timer, unsigned int clock_divider, unsigned char interrupt_level, rtems_isr (*timer_isr)() ) { @@ -327,7 +331,8 @@ rtems_task hous_task(rtems_task_argument hk_lfr_le_me_he_update(); - housekeeping_packet.hk_lfr_sc_rw_f_flags = cp_rpw_sc_rw_f_flags; + housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags = cp_rpw_sc_rw1_rw2_f_flags; + housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags = cp_rpw_sc_rw3_rw4_f_flags; // SEND PACKET status = rtems_message_queue_send( queue_id, &housekeeping_packet, @@ -355,6 +360,9 @@ rtems_task avgv_task(rtems_task_argument float average_v; float average_e1; float average_e2; + float newValue_v; + float newValue_e1; + float newValue_e2; unsigned char k; unsigned char indexOfOldValue; @@ -377,33 +385,38 @@ rtems_task avgv_task(rtems_task_argument // initialize values indexOfOldValue = MOVING_AVERAGE - 1; - average_v = 0.; - average_e1 = 0.; - average_e2 = 0.; + average_v = INIT_FLOAT; + average_e1 = INIT_FLOAT; + average_e2 = INIT_FLOAT; + newValue_v = INIT_FLOAT; + newValue_e1 = INIT_FLOAT; + newValue_e2 = INIT_FLOAT; - k = 0; + k = INIT_CHAR; - while(1){ // launch the rate monotonic task + while(1) + { // launch the rate monotonic task status = rtems_rate_monotonic_period( AVGV_id, AVGV_PERIOD ); - if ( status != RTEMS_SUCCESSFUL ) { + if ( status != RTEMS_SUCCESSFUL ) + { PRINTF1( "in AVGV *** ERR period: %d\n", status); } - else { - // store new value in buffer - v[k] = waveform_picker_regs->v; - e1[k] = waveform_picker_regs->e1; - e2[k] = waveform_picker_regs->e2; - if (k == (MOVING_AVERAGE - 1)) - { - indexOfOldValue = 0; - } - else - { - indexOfOldValue = k + 1; - } - average_v = average_v + v[k] - v[indexOfOldValue]; - average_e1 = average_e1 + e1[k] - e1[indexOfOldValue]; - average_e2 = average_e2 + e2[k] - e2[indexOfOldValue]; + else + { + // get new values + newValue_v = waveform_picker_regs->v; + newValue_e1 = waveform_picker_regs->e1; + newValue_e2 = waveform_picker_regs->e2; + + // compute the moving average + average_v = average_v + newValue_v - v[k]; + average_e1 = average_e1 + newValue_e1 - e1[k]; + average_e2 = average_e2 + newValue_e2 - e2[k]; + + // store new values in buffers + v[k] = newValue_v; + e1[k] = newValue_e1; + e2[k] = newValue_e2; } if (k == (MOVING_AVERAGE-1)) { @@ -414,11 +427,15 @@ rtems_task avgv_task(rtems_task_argument { k++; } + //update int16 values + hk_lfr_sc_v_f3_as_int16 = (int16_t) (average_v / ((float) MOVING_AVERAGE) ); + hk_lfr_sc_e1_f3_as_int16 = (int16_t) (average_e1 / ((float) MOVING_AVERAGE) ); + hk_lfr_sc_e2_f3_as_int16 = (int16_t) (average_e2 / ((float) MOVING_AVERAGE) ); } - PRINTF("in AVGV *** deleting task\n") + PRINTF("in AVGV *** deleting task\n"); - status = rtems_task_delete( RTEMS_SELF ); // should not return + status = rtems_task_delete( RTEMS_SELF ); // should not return return; } @@ -672,16 +689,16 @@ void get_v_e1_e2_f3( unsigned char *spac unsigned char* e1_ptr; unsigned char* e2_ptr; - v_ptr = (unsigned char *) &waveform_picker_regs->v; - e1_ptr = (unsigned char *) &waveform_picker_regs->e1; - e2_ptr = (unsigned char *) &waveform_picker_regs->e2; + v_ptr = (unsigned char *) &hk_lfr_sc_v_f3_as_int16; + e1_ptr = (unsigned char *) &hk_lfr_sc_e1_f3_as_int16; + e2_ptr = (unsigned char *) &hk_lfr_sc_e2_f3_as_int16; - spacecraft_potential[ BYTE_0 ] = v_ptr[ BYTE_2 ]; - spacecraft_potential[ BYTE_1 ] = v_ptr[ BYTE_3 ]; - spacecraft_potential[ BYTE_2 ] = e1_ptr[ BYTE_2 ]; - spacecraft_potential[ BYTE_3 ] = e1_ptr[ BYTE_3 ]; - spacecraft_potential[ BYTE_4 ] = e2_ptr[ BYTE_2 ]; - spacecraft_potential[ BYTE_5 ] = e2_ptr[ BYTE_3 ]; + spacecraft_potential[BYTE_0] = v_ptr[0]; + spacecraft_potential[BYTE_1] = v_ptr[1]; + spacecraft_potential[BYTE_2] = e1_ptr[0]; + spacecraft_potential[BYTE_3] = e1_ptr[1]; + spacecraft_potential[BYTE_4] = e2_ptr[0]; + spacecraft_potential[BYTE_5] = e2_ptr[1]; } void get_cpu_load( unsigned char *resource_statistics ) diff --git a/src/tc_handler.c b/src/tc_handler.c --- a/src/tc_handler.c +++ b/src/tc_handler.c @@ -301,8 +301,9 @@ int action_update_info(ccsdsTelecommandP // REACTION_WHEELS_FREQUENCY, copy the incoming parameters in the local variable (to be copied in HK packets) - cp_rpw_sc_rw_f_flags = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW_F_FLAGS ]; + //cp_rpw_sc_rw_f_flags = bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW_F_FLAGS ]; getReactionWheelsFrequencies( TC ); + set_hk_lfr_sc_rw_f_flags(); build_sy_lfr_rw_masks(); // once the masks are built, they have to be merged with the fbins_mask diff --git a/src/tc_load_dump_parameters.c b/src/tc_load_dump_parameters.c --- a/src/tc_load_dump_parameters.c +++ b/src/tc_load_dump_parameters.c @@ -324,8 +324,10 @@ int action_load_filter_par(ccsdsTelecomm */ int flag; + unsigned char k; flag = LFR_DEFAULT; + k = INIT_CHAR; flag = check_sy_lfr_filter_parameters( TC, queue_id ); @@ -367,6 +369,36 @@ int action_load_filter_par(ccsdsTelecomm // store the parameter sy_lfr_sc_rw_delta_f as a float copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_sc_rw_delta_f, parameter_dump_packet.sy_lfr_sc_rw_delta_f ); + + // copy rw.._k.. from the incoming TC to the local parameter_dump_packet + for (k = 0; k < NB_RW_K_COEFFS * NB_BYTES_PER_RW_K_COEFF; k++) + { + parameter_dump_packet.sy_lfr_rw1_k1[k] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_RW1_K1 + k ]; + } + + //*********************************************** + // store the parameter sy_lfr_rw.._k.. as a float + // rw1_k + copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k1, parameter_dump_packet.sy_lfr_rw1_k1 ); + copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k2, parameter_dump_packet.sy_lfr_rw1_k2 ); + copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k3, parameter_dump_packet.sy_lfr_rw1_k3 ); + copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k4, parameter_dump_packet.sy_lfr_rw1_k4 ); + // rw2_k + copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k1, parameter_dump_packet.sy_lfr_rw2_k1 ); + copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k2, parameter_dump_packet.sy_lfr_rw2_k2 ); + copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k3, parameter_dump_packet.sy_lfr_rw2_k3 ); + copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k4, parameter_dump_packet.sy_lfr_rw2_k4 ); + // rw3_k + copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k1, parameter_dump_packet.sy_lfr_rw3_k1 ); + copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k2, parameter_dump_packet.sy_lfr_rw3_k2 ); + copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k3, parameter_dump_packet.sy_lfr_rw3_k3 ); + copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k4, parameter_dump_packet.sy_lfr_rw3_k4 ); + // rw4_k + copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k1, parameter_dump_packet.sy_lfr_rw4_k1 ); + copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k2, parameter_dump_packet.sy_lfr_rw4_k2 ); + copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k3, parameter_dump_packet.sy_lfr_rw4_k3 ); + copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k4, parameter_dump_packet.sy_lfr_rw4_k4 ); + } return flag; @@ -933,6 +965,81 @@ unsigned int check_update_info_hk_thr_mo return status; } +void set_hk_lfr_sc_rw_f_flag( unsigned char wheel, unsigned char freq, float value ) +{ + unsigned char flag; + unsigned char flagPosInByte; + unsigned char newFlag; + unsigned char flagMask; + + // if the frequency value is not a number, the flag is set to 0 and the frequency RWx_Fy is not filtered + if (isnan(value)) + { + flag = FLAG_NAN; + } + else + { + flag = FLAG_IAN; + } + + switch(wheel) + { + case WHEEL_1: + flagPosInByte = FLAG_OFFSET_WHEELS_1_3 - freq; + flagMask = ~(1 << flagPosInByte); + newFlag = flag << flagPosInByte; + housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags = (housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags & flagMask) | newFlag; + break; + case WHEEL_2: + flagPosInByte = FLAG_OFFSET_WHEELS_2_4 - freq; + flagMask = ~(1 << flagPosInByte); + newFlag = flag << flagPosInByte; + housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags = (housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags & flagMask) | newFlag; + break; + case WHEEL_3: + flagPosInByte = FLAG_OFFSET_WHEELS_1_3 - freq; + flagMask = ~(1 << flagPosInByte); + newFlag = flag << flagPosInByte; + housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags = (housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags & flagMask) | newFlag; + break; + case WHEEL_4: + flagPosInByte = FLAG_OFFSET_WHEELS_2_4 - freq; + flagMask = ~(1 << flagPosInByte); + newFlag = flag << flagPosInByte; + housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags = (housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags & flagMask) | newFlag; + break; + default: + break; + } +} + +void set_hk_lfr_sc_rw_f_flags( void ) +{ + // RW1 + set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_1, rw_f.cp_rpw_sc_rw1_f1 ); + set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_2, rw_f.cp_rpw_sc_rw1_f2 ); + set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_3, rw_f.cp_rpw_sc_rw1_f3 ); + set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_4, rw_f.cp_rpw_sc_rw1_f4 ); + + // RW2 + set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_1, rw_f.cp_rpw_sc_rw2_f1 ); + set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_2, rw_f.cp_rpw_sc_rw2_f2 ); + set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_3, rw_f.cp_rpw_sc_rw2_f3 ); + set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_4, rw_f.cp_rpw_sc_rw2_f4 ); + + // RW3 + set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_1, rw_f.cp_rpw_sc_rw3_f1 ); + set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_2, rw_f.cp_rpw_sc_rw3_f2 ); + set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_3, rw_f.cp_rpw_sc_rw3_f3 ); + set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_4, rw_f.cp_rpw_sc_rw3_f4 ); + + // RW4 + set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_1, rw_f.cp_rpw_sc_rw4_f1 ); + set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_2, rw_f.cp_rpw_sc_rw4_f2 ); + set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_3, rw_f.cp_rpw_sc_rw4_f3 ); + set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_4, rw_f.cp_rpw_sc_rw4_f4 ); +} + void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC ) { /** 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 bytePosPtr = (unsigned char *) &TC->packetID; - // cp_rpw_sc_rw1_f1 - copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw1_f1, - (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1 ] ); - - // cp_rpw_sc_rw1_f2 - copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw1_f2, - (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2 ] ); + // rw1_f + copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1 ] ); + copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2 ] ); + copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F3 ] ); + copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F4 ] ); - // cp_rpw_sc_rw2_f1 - copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw2_f1, - (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1 ] ); - - // cp_rpw_sc_rw2_f2 - copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw2_f2, - (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2 ] ); + // rw2_f + copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1 ] ); + copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2 ] ); + copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F3 ] ); + copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F4 ] ); - // cp_rpw_sc_rw3_f1 - copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw3_f1, - (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1 ] ); - - // cp_rpw_sc_rw3_f2 - copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw3_f2, - (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2 ] ); + // rw3_f + copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1 ] ); + copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2 ] ); + copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F3 ] ); + copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F4 ] ); - // cp_rpw_sc_rw4_f1 - copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw4_f1, - (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1 ] ); + // rw4_f + copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1 ] ); + copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 ] ); + copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F3 ] ); + copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F4 ] ); - // cp_rpw_sc_rw4_f2 - copyFloatByChar( (unsigned char*) &cp_rpw_sc_rw4_f2, - (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 ] ); + // test each reaction wheel frequency value. NaN means that the frequency is not filtered + } 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 local_rw_fbins_mask[k] = INT8_ALL_F; } - // RW1 F1 - 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] - - // RW1 F2 - 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] + // RW1 + 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] + 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] + 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] + 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] - // RW2 F1 - 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] - - // RW2 F2 - 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] + // RW2 + 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] + 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] + 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] + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f2, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & BIT_0) ); // [0000 0001] - // RW3 F1 - 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] - - // RW3 F2 - 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] + // RW3 + 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] + 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] + 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] + 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] - // RW4 F1 - setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw4_f1, deltaF, (cp_rpw_sc_rw_f_flags & BIT_RW4_F1) >> 1 ); // [0000 0010] - - // RW4 F2 - setFBinMask( local_rw_fbins_mask, cp_rpw_sc_rw4_f2, deltaF, (cp_rpw_sc_rw_f_flags & BIT_RW4_F2) ); // [0000 0001] + // RW4 + 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] + 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] + 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] + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f2, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & BIT_0) ); // [0000 0001] // update the value of the fbins related to reaction wheels frequency filtering if (maskPtr != NULL) @@ -1502,6 +1604,27 @@ void init_parameter_dump( void ) floatToChar( DEFAULT_SY_LFR_PAS_FILTER_SHIFT, parameter_dump_packet.sy_lfr_pas_filter_shift ); floatToChar( DEFAULT_SY_LFR_SC_RW_DELTA_F, parameter_dump_packet.sy_lfr_sc_rw_delta_f ); + // RW1_K + floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw1_k1); + floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw1_k2); + floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw1_k3); + floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw1_k4); + // RW2_K + floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw2_k1); + floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw2_k2); + floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw2_k3); + floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw2_k4); + // RW3_K + floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw3_k1); + floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw3_k2); + floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw3_k3); + floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw3_k4); + // RW4_K + floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw4_k1); + floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw4_k2); + floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw4_k3); + floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw4_k4); + // LFR_RW_MASK for (k=0; k < BYTES_PER_MASKS_SET; k++) {