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 @@ -93,7 +93,7 @@ unsigned int check_update_info_hk_thr_mo 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 setFBinMask(unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, float k ); void build_sy_lfr_rw_mask( unsigned int channel ); void build_sy_lfr_rw_masks(); void merge_fbins_masks( void ); diff --git a/src/fsw_init.c b/src/fsw_init.c --- a/src/fsw_init.c +++ b/src/fsw_init.c @@ -167,7 +167,6 @@ rtems_task Init( rtems_task_argument ign init_k_coefficients_prc1(); init_k_coefficients_prc2(); pa_bia_status_info = INIT_CHAR; - cp_rpw_sc_rw_f_flags = INIT_CHAR; // initialize all reaction wheels frequencies to NaN rw_f.cp_rpw_sc_rw1_f1 = NAN; 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 @@ -1080,7 +1080,7 @@ void getReactionWheelsFrequencies( ccsds } -void setFBinMask( unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, unsigned char flag ) +void setFBinMask(unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, float k ) { /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received. * @@ -1107,73 +1107,75 @@ void setFBinMask( unsigned char *fbins_m int selectedByte; int bin; int binToRemove[NB_BINS_TO_REMOVE]; - int k; + int i; closestBin = 0; whichByte = 0; bin = 0; - for (k = 0; k < NB_BINS_TO_REMOVE; k++) + for (i = 0; i < NB_BINS_TO_REMOVE; i++) { - binToRemove[k] = -1; - } - - // compute the frequency range to filter [ rw_f - delta_f/2; rw_f + delta_f/2 ] - f_RW_min = rw_f - (filterPar.sy_lfr_sc_rw_delta_f / 2.); - f_RW_MAX = rw_f + (filterPar.sy_lfr_sc_rw_delta_f / 2.); - - // compute the index of the frequency bin immediately below rw_f - binBelow = (int) ( floor( ((double) rw_f) / ((double) deltaFreq)) ); - deltaBelow = rw_f - binBelow * deltaFreq; - - // compute the index of the frequency bin immediately above rw_f - binAbove = (int) ( ceil( ((double) rw_f) / ((double) deltaFreq)) ); - deltaAbove = binAbove * deltaFreq - rw_f; - - // search the closest bin - if (deltaAbove > deltaBelow) - { - closestBin = binBelow; - } - else - { - closestBin = binAbove; + binToRemove[i] = -1; } - // compute the fi interval [fi - deltaFreq * 0.285, fi + deltaFreq * 0.285] - fi = closestBin * deltaFreq; - fi_min = fi - (deltaFreq * FI_INTERVAL_COEFF); - fi_MAX = fi + (deltaFreq * FI_INTERVAL_COEFF); - - //************************************************************************************** - // be careful here, one shall take into account that the bin 0 IS DROPPED in the spectra - // thus, the index 0 in a mask corresponds to the bin 1 of the spectrum - //************************************************************************************** - - // 1. IF [ f_RW_min, f_RW_MAX] is included in [ fi_min; fi_MAX ] - // => remove f_(i), f_(i-1) and f_(i+1) - if ( ( f_RW_min > fi_min ) && ( f_RW_MAX < fi_MAX ) ) + if (!isnan(rw_f)) { - binToRemove[0] = (closestBin - 1) - 1; - binToRemove[1] = (closestBin) - 1; - binToRemove[2] = (closestBin + 1) - 1; - } - // 2. ELSE - // => remove the two f_(i) which are around f_RW - else - { - binToRemove[0] = (binBelow) - 1; - binToRemove[1] = (binAbove) - 1; - binToRemove[2] = (-1); - } + + // compute the frequency range to filter [ rw_f - delta_f/2; rw_f + delta_f/2 ] + f_RW_min = rw_f - (filterPar.sy_lfr_sc_rw_delta_f / 2.); + f_RW_MAX = rw_f + (filterPar.sy_lfr_sc_rw_delta_f / 2.); + + // compute the index of the frequency bin immediately below rw_f + binBelow = (int) ( floor( ((double) rw_f) / ((double) deltaFreq)) ); + deltaBelow = rw_f - binBelow * deltaFreq; + + // compute the index of the frequency bin immediately above rw_f + binAbove = (int) ( ceil( ((double) rw_f) / ((double) deltaFreq)) ); + deltaAbove = binAbove * deltaFreq - rw_f; + + // search the closest bin + if (deltaAbove > deltaBelow) + { + closestBin = binBelow; + } + else + { + closestBin = binAbove; + } + + // compute the fi interval [fi - deltaFreq * 0.285, fi + deltaFreq * 0.285] + fi = closestBin * deltaFreq; + fi_min = fi - (deltaFreq * FI_INTERVAL_COEFF); + fi_MAX = fi + (deltaFreq * FI_INTERVAL_COEFF); - for (k = 0; k < NB_BINS_TO_REMOVE; k++) - { - bin = binToRemove[k]; - if ( (bin >= BIN_MIN) && (bin <= BIN_MAX) ) + //************************************************************************************** + // be careful here, one shall take into account that the bin 0 IS DROPPED in the spectra + // thus, the index 0 in a mask corresponds to the bin 1 of the spectrum + //************************************************************************************** + + // 1. IF [ f_RW_min, f_RW_MAX] is included in [ fi_min; fi_MAX ] + // => remove f_(i), f_(i-1) and f_(i+1) + if ( ( f_RW_min > fi_min ) && ( f_RW_MAX < fi_MAX ) ) { - if (flag == 1) + binToRemove[0] = (closestBin - 1) - 1; + binToRemove[1] = (closestBin) - 1; + binToRemove[2] = (closestBin + 1) - 1; + } + // 2. ELSE + // => remove the two f_(i) which are around f_RW + else + { + binToRemove[0] = (binBelow) - 1; + binToRemove[1] = (binAbove) - 1; + binToRemove[2] = (-1); + } + + for (i = 0; i < NB_BINS_TO_REMOVE; i++) + { + bin = binToRemove[i]; + if ( (bin >= BIN_MIN) && (bin <= BIN_MAX) ) { + whichByte = (bin >> SHIFT_3_BITS); // division by 8 selectedByte = ( 1 << (bin - (whichByte * BITS_PER_BYTE)) ); fbins_mask[BYTES_PER_MASK - 1 - whichByte] = @@ -1219,28 +1221,28 @@ void build_sy_lfr_rw_mask( unsigned int } // 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] + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f1, deltaF, filterPar.sy_lfr_rw1_k1 ); + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f2, deltaF, filterPar.sy_lfr_rw1_k2 ); + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f3, deltaF, filterPar.sy_lfr_rw1_k3 ); + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f4, deltaF, filterPar.sy_lfr_rw1_k4 ); - // 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] + // RW2 + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f1, deltaF, filterPar.sy_lfr_rw2_k1 ); + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f2, deltaF, filterPar.sy_lfr_rw2_k2 ); + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f3, deltaF, filterPar.sy_lfr_rw2_k3 ); + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f4, deltaF, filterPar.sy_lfr_rw2_k4 ); - // 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] + // RW3 + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f1, deltaF, filterPar.sy_lfr_rw3_k1 ); + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f2, deltaF, filterPar.sy_lfr_rw3_k2 ); + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f3, deltaF, filterPar.sy_lfr_rw3_k3 ); + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f4, deltaF, filterPar.sy_lfr_rw3_k4 ); - // 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] + // RW4 + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f1, deltaF, filterPar.sy_lfr_rw4_k1 ); + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f2, deltaF, filterPar.sy_lfr_rw4_k2 ); + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f3, deltaF, filterPar.sy_lfr_rw4_k3 ); + setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f4, deltaF, filterPar.sy_lfr_rw4_k4 ); // update the value of the fbins related to reaction wheels frequency filtering if (maskPtr != NULL)