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

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

partial recoding of reaction wheel filtering
paul -
r329:95a6df42a7d2 R3++ draft
parent child
Show More
Show file before | Show file after | Hide comments
@@ -93,7 +93,7 unsigned int check_update_info_hk_thr_mo
93 void set_hk_lfr_sc_rw_f_flag( unsigned char wheel, unsigned char freq, float value );
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 );
94 void set_hk_lfr_sc_rw_f_flags( void );
95 void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC );
95 void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC );
96 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, float k );
97 void build_sy_lfr_rw_mask( unsigned int channel );
97 void build_sy_lfr_rw_mask( unsigned int channel );
98 void build_sy_lfr_rw_masks();
98 void build_sy_lfr_rw_masks();
99 void merge_fbins_masks( void );
99 void merge_fbins_masks( void );
Show file before | Show file after | Hide comments
@@ -167,7 +167,6 rtems_task Init( rtems_task_argument ign
167 init_k_coefficients_prc1();
167 init_k_coefficients_prc1();
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;
171
170
172 // initialize all reaction wheels frequencies to NaN
171 // initialize all reaction wheels frequencies to NaN
173 rw_f.cp_rpw_sc_rw1_f1 = NAN;
172 rw_f.cp_rpw_sc_rw1_f1 = NAN;
Show file before | Show file after | Hide comments
@@ -1080,7 +1080,7 void getReactionWheelsFrequencies( ccsds
1080
1080
1081 }
1081 }
1082
1082
1083 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, float k )
1084 {
1084 {
1085 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
1085 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
1086 *
1086 *
@@ -1107,73 +1107,75 void setFBinMask( unsigned char *fbins_m
1107 int selectedByte;
1107 int selectedByte;
1108 int bin;
1108 int bin;
1109 int binToRemove[NB_BINS_TO_REMOVE];
1109 int binToRemove[NB_BINS_TO_REMOVE];
1110 int k;
1110 int i;
1111
1111
1112 closestBin = 0;
1112 closestBin = 0;
1113 whichByte = 0;
1113 whichByte = 0;
1114 bin = 0;
1114 bin = 0;
1115
1115
1116 for (k = 0; k < NB_BINS_TO_REMOVE; k++)
1116 for (i = 0; i < NB_BINS_TO_REMOVE; i++)
1117 {
1117 {
1118 binToRemove[k] = -1;
1118 binToRemove[i] = -1;
1119 }
1120
1121 // compute the frequency range to filter [ rw_f - delta_f/2; rw_f + delta_f/2 ]
1122 f_RW_min = rw_f - (filterPar.sy_lfr_sc_rw_delta_f / 2.);
1123 f_RW_MAX = rw_f + (filterPar.sy_lfr_sc_rw_delta_f / 2.);
1124
1125 // compute the index of the frequency bin immediately below rw_f
1126 binBelow = (int) ( floor( ((double) rw_f) / ((double) deltaFreq)) );
1127 deltaBelow = rw_f - binBelow * deltaFreq;
1128
1129 // compute the index of the frequency bin immediately above rw_f
1130 binAbove = (int) ( ceil( ((double) rw_f) / ((double) deltaFreq)) );
1131 deltaAbove = binAbove * deltaFreq - rw_f;
1132
1133 // search the closest bin
1134 if (deltaAbove > deltaBelow)
1135 {
1136 closestBin = binBelow;
1137 }
1138 else
1139 {
1140 closestBin = binAbove;
1141 }
1119 }
1142
1120
1143 // compute the fi interval [fi - deltaFreq * 0.285, fi + deltaFreq * 0.285]
1121 if (!isnan(rw_f))
1144 fi = closestBin * deltaFreq;
1145 fi_min = fi - (deltaFreq * FI_INTERVAL_COEFF);
1146 fi_MAX = fi + (deltaFreq * FI_INTERVAL_COEFF);
1147
1148 //**************************************************************************************
1149 // be careful here, one shall take into account that the bin 0 IS DROPPED in the spectra
1150 // thus, the index 0 in a mask corresponds to the bin 1 of the spectrum
1151 //**************************************************************************************
1152
1153 // 1. IF [ f_RW_min, f_RW_MAX] is included in [ fi_min; fi_MAX ]
1154 // => remove f_(i), f_(i-1) and f_(i+1)
1155 if ( ( f_RW_min > fi_min ) && ( f_RW_MAX < fi_MAX ) )
1156 {
1122 {
1157 binToRemove[0] = (closestBin - 1) - 1;
1123
1158 binToRemove[1] = (closestBin) - 1;
1124 // compute the frequency range to filter [ rw_f - delta_f/2; rw_f + delta_f/2 ]
1159 binToRemove[2] = (closestBin + 1) - 1;
1125 f_RW_min = rw_f - (filterPar.sy_lfr_sc_rw_delta_f / 2.);
1160 }
1126 f_RW_MAX = rw_f + (filterPar.sy_lfr_sc_rw_delta_f / 2.);
1161 // 2. ELSE
1127
1162 // => remove the two f_(i) which are around f_RW
1128 // compute the index of the frequency bin immediately below rw_f
1163 else
1129 binBelow = (int) ( floor( ((double) rw_f) / ((double) deltaFreq)) );
1164 {
1130 deltaBelow = rw_f - binBelow * deltaFreq;
1165 binToRemove[0] = (binBelow) - 1;
1131
1166 binToRemove[1] = (binAbove) - 1;
1132 // compute the index of the frequency bin immediately above rw_f
1167 binToRemove[2] = (-1);
1133 binAbove = (int) ( ceil( ((double) rw_f) / ((double) deltaFreq)) );
1168 }
1134 deltaAbove = binAbove * deltaFreq - rw_f;
1135
1136 // search the closest bin
1137 if (deltaAbove > deltaBelow)
1138 {
1139 closestBin = binBelow;
1140 }
1141 else
1142 {
1143 closestBin = binAbove;
1144 }
1145
1146 // compute the fi interval [fi - deltaFreq * 0.285, fi + deltaFreq * 0.285]
1147 fi = closestBin * deltaFreq;
1148 fi_min = fi - (deltaFreq * FI_INTERVAL_COEFF);
1149 fi_MAX = fi + (deltaFreq * FI_INTERVAL_COEFF);
1169
1150
1170 for (k = 0; k < NB_BINS_TO_REMOVE; k++)
1151 //**************************************************************************************
1171 {
1152 // be careful here, one shall take into account that the bin 0 IS DROPPED in the spectra
1172 bin = binToRemove[k];
1153 // thus, the index 0 in a mask corresponds to the bin 1 of the spectrum
1173 if ( (bin >= BIN_MIN) && (bin <= BIN_MAX) )
1154 //**************************************************************************************
1155
1156 // 1. IF [ f_RW_min, f_RW_MAX] is included in [ fi_min; fi_MAX ]
1157 // => remove f_(i), f_(i-1) and f_(i+1)
1158 if ( ( f_RW_min > fi_min ) && ( f_RW_MAX < fi_MAX ) )
1174 {
1159 {
1175 if (flag == 1)
1160 binToRemove[0] = (closestBin - 1) - 1;
1161 binToRemove[1] = (closestBin) - 1;
1162 binToRemove[2] = (closestBin + 1) - 1;
1163 }
1164 // 2. ELSE
1165 // => remove the two f_(i) which are around f_RW
1166 else
1167 {
1168 binToRemove[0] = (binBelow) - 1;
1169 binToRemove[1] = (binAbove) - 1;
1170 binToRemove[2] = (-1);
1171 }
1172
1173 for (i = 0; i < NB_BINS_TO_REMOVE; i++)
1174 {
1175 bin = binToRemove[i];
1176 if ( (bin >= BIN_MIN) && (bin <= BIN_MAX) )
1176 {
1177 {
1178
1177 whichByte = (bin >> SHIFT_3_BITS); // division by 8
1179 whichByte = (bin >> SHIFT_3_BITS); // division by 8
1178 selectedByte = ( 1 << (bin - (whichByte * BITS_PER_BYTE)) );
1180 selectedByte = ( 1 << (bin - (whichByte * BITS_PER_BYTE)) );
1179 fbins_mask[BYTES_PER_MASK - 1 - whichByte] =
1181 fbins_mask[BYTES_PER_MASK - 1 - whichByte] =
@@ -1219,28 +1221,28 void build_sy_lfr_rw_mask( unsigned int
1219 }
1221 }
1220
1222
1221 // RW1
1223 // 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]
1224 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f1, deltaF, filterPar.sy_lfr_rw1_k1 );
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]
1225 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f2, deltaF, filterPar.sy_lfr_rw1_k2 );
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]
1226 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f3, deltaF, filterPar.sy_lfr_rw1_k3 );
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]
1227 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f4, deltaF, filterPar.sy_lfr_rw1_k4 );
1226
1228
1227 // RW2
1229 // 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]
1230 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f1, deltaF, filterPar.sy_lfr_rw2_k1 );
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]
1231 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f2, deltaF, filterPar.sy_lfr_rw2_k2 );
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]
1232 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f3, deltaF, filterPar.sy_lfr_rw2_k3 );
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]
1233 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f4, deltaF, filterPar.sy_lfr_rw2_k4 );
1232
1234
1233 // RW3
1235 // 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]
1236 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f1, deltaF, filterPar.sy_lfr_rw3_k1 );
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]
1237 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f2, deltaF, filterPar.sy_lfr_rw3_k2 );
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]
1238 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f3, deltaF, filterPar.sy_lfr_rw3_k3 );
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]
1239 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f4, deltaF, filterPar.sy_lfr_rw3_k4 );
1238
1240
1239 // RW4
1241 // 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]
1242 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f1, deltaF, filterPar.sy_lfr_rw4_k1 );
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]
1243 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f2, deltaF, filterPar.sy_lfr_rw4_k2 );
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]
1244 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f3, deltaF, filterPar.sy_lfr_rw4_k3 );
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]
1245 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f4, deltaF, filterPar.sy_lfr_rw4_k4 );
1244
1246
1245 // update the value of the fbins related to reaction wheels frequency filtering
1247 // update the value of the fbins related to reaction wheels frequency filtering
1246 if (maskPtr != NULL)
1248 if (maskPtr != NULL)
General Comments 0
You need to be logged in to leave comments. Login now