##// 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,17 +1107,20 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 }
1119 }
1120
1120
1121 if (!isnan(rw_f))
1122 {
1123
1121 // compute the frequency range to filter [ rw_f - delta_f/2; rw_f + delta_f/2 ]
1124 // 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.);
1125 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.);
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 binToRemove[2] = (-1);
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 if ( (bin >= BIN_MIN) && (bin <= BIN_MAX) )
1176 if ( (bin >= BIN_MIN) && (bin <= BIN_MAX) )
1174 {
1177 {
1175 if (flag == 1)
1178
1176 {
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