##// 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 93 void set_hk_lfr_sc_rw_f_flag( unsigned char wheel, unsigned char freq, float value );
94 94 void set_hk_lfr_sc_rw_f_flags( void );
95 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 97 void build_sy_lfr_rw_mask( unsigned int channel );
98 98 void build_sy_lfr_rw_masks();
99 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 167 init_k_coefficients_prc1();
168 168 init_k_coefficients_prc2();
169 169 pa_bia_status_info = INIT_CHAR;
170 cp_rpw_sc_rw_f_flags = INIT_CHAR;
171 170
172 171 // initialize all reaction wheels frequencies to NaN
173 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 1085 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
1086 1086 *
@@ -1107,17 +1107,20 void setFBinMask( unsigned char *fbins_m
1107 1107 int selectedByte;
1108 1108 int bin;
1109 1109 int binToRemove[NB_BINS_TO_REMOVE];
1110 int k;
1110 int i;
1111 1111
1112 1112 closestBin = 0;
1113 1113 whichByte = 0;
1114 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 1119 }
1120 1120
1121 if (!isnan(rw_f))
1122 {
1123
1121 1124 // compute the frequency range to filter [ rw_f - delta_f/2; rw_f + delta_f/2 ]
1122 1125 f_RW_min = rw_f - (filterPar.sy_lfr_sc_rw_delta_f / 2.);
1123 1126 f_RW_MAX = rw_f + (filterPar.sy_lfr_sc_rw_delta_f / 2.);
@@ -1167,13 +1170,12 void setFBinMask( unsigned char *fbins_m
1167 1170 binToRemove[2] = (-1);
1168 1171 }
1169 1172
1170 for (k = 0; k < NB_BINS_TO_REMOVE; k++)
1173 for (i = 0; i < NB_BINS_TO_REMOVE; i++)
1171 1174 {
1172 bin = binToRemove[k];
1175 bin = binToRemove[i];
1173 1176 if ( (bin >= BIN_MIN) && (bin <= BIN_MAX) )
1174 1177 {
1175 if (flag == 1)
1176 {
1178
1177 1179 whichByte = (bin >> SHIFT_3_BITS); // division by 8
1178 1180 selectedByte = ( 1 << (bin - (whichByte * BITS_PER_BYTE)) );
1179 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 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]
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]
1224 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f1, deltaF, filterPar.sy_lfr_rw1_k1 );
1225 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f2, deltaF, filterPar.sy_lfr_rw1_k2 );
1226 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f3, deltaF, filterPar.sy_lfr_rw1_k3 );
1227 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f4, deltaF, filterPar.sy_lfr_rw1_k4 );
1226 1228
1227 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]
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]
1230 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f1, deltaF, filterPar.sy_lfr_rw2_k1 );
1231 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f2, deltaF, filterPar.sy_lfr_rw2_k2 );
1232 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f3, deltaF, filterPar.sy_lfr_rw2_k3 );
1233 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f4, deltaF, filterPar.sy_lfr_rw2_k4 );
1232 1234
1233 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]
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]
1236 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f1, deltaF, filterPar.sy_lfr_rw3_k1 );
1237 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f2, deltaF, filterPar.sy_lfr_rw3_k2 );
1238 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f3, deltaF, filterPar.sy_lfr_rw3_k3 );
1239 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f4, deltaF, filterPar.sy_lfr_rw3_k4 );
1238 1240
1239 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]
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]
1242 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f1, deltaF, filterPar.sy_lfr_rw4_k1 );
1243 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f2, deltaF, filterPar.sy_lfr_rw4_k2 );
1244 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f3, deltaF, filterPar.sy_lfr_rw4_k3 );
1245 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f4, deltaF, filterPar.sy_lfr_rw4_k4 );
1244 1246
1245 1247 // update the value of the fbins related to reaction wheels frequency filtering
1246 1248 if (maskPtr != NULL)
General Comments 0
You need to be logged in to leave comments. Login now