##// 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 -
r300:245f3ace2b2a R3++ draft
parent child
Show More
Show file before | Show file after | Hide comments
@@ -12,6 +12,10
12 12 #include "avf0_prc0.h"
13 13
14 14 #define FLOAT_EQUAL_ZERO 0.001
15 #define RW1 1
16 #define RW2 2
17 #define RW3 3
18 #define RW4 4
15 19
16 20 extern unsigned short sequenceCounterParameterDump;
17 21 extern unsigned short sequenceCounters_TM_DUMP[];
@@ -61,7 +65,7 unsigned int check_update_info_hk_thr_mo
61 65 void set_hk_lfr_sc_rw_f_flag( unsigned char wheel, unsigned char freq, float value );
62 66 void set_hk_lfr_sc_rw_f_flags( void );
63 67 void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC );
64 void setFBinMask(unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, unsigned char flag );
68 void setFBinMask(unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, float k);
65 69 void build_sy_lfr_rw_mask( unsigned int channel );
66 70 void build_sy_lfr_rw_masks();
67 71 void merge_fbins_masks( void );
Show file before | Show file after | Hide comments
@@ -421,13 +421,13 rtems_task avgv_task(rtems_task_argument
421 421 {
422 422 k++;
423 423 }
424
425 //update int16 values
426 hk_lfr_sc_v_f3_as_int16 = (int16_t) (average_v / ((float) MOVING_AVERAGE) );
427 hk_lfr_sc_e1_f3_as_int16 = (int16_t) (average_e1 / ((float) MOVING_AVERAGE) );
428 hk_lfr_sc_e2_f3_as_int16 = (int16_t) (average_e2 / ((float) MOVING_AVERAGE) );
424 429 }
425 430
426 //update int16 values
427 hk_lfr_sc_v_f3_as_int16 = (int16_t) (average_v / ((float) MOVING_AVERAGE) );
428 hk_lfr_sc_e1_f3_as_int16 = (int16_t) (average_e1 / ((float) MOVING_AVERAGE) );
429 hk_lfr_sc_e2_f3_as_int16 = (int16_t) (average_e2 / ((float) MOVING_AVERAGE) );
430
431 431 PRINTF("in AVGV *** deleting task\n");
432 432
433 433 status = rtems_task_delete( RTEMS_SELF ); // should not return
Show file before | Show file after | Hide comments
@@ -1062,7 +1062,7 void getReactionWheelsFrequencies( ccsds
1062 1062
1063 1063 }
1064 1064
1065 void setFBinMask( unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, unsigned char flag )
1065 void setFBinMask(unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, float k )
1066 1066 {
1067 1067 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
1068 1068 *
@@ -1077,91 +1077,48 void setFBinMask( unsigned char *fbins_m
1077 1077
1078 1078 float f_RW_min;
1079 1079 float f_RW_MAX;
1080 float fi_min;
1081 float fi_MAX;
1082 float fi;
1083 float deltaBelow;
1084 float deltaAbove;
1080 float bandWidth;
1085 1081 int binBelow;
1086 1082 int binAbove;
1087 int closestBin;
1088 1083 unsigned int whichByte;
1089 1084 int selectedByte;
1090 1085 int bin;
1091 int binToRemove[3];
1092 int k;
1093
1094 whichByte = 0;
1095 bin = 0;
1096
1097 binToRemove[0] = -1;
1098 binToRemove[1] = -1;
1099 binToRemove[2] = -1;
1100
1101 // compute the frequency range to filter [ rw_f - delta_f/2; rw_f + delta_f/2 ]
1102 f_RW_min = rw_f - filterPar.sy_lfr_sc_rw_delta_f / 2.;
1103 f_RW_MAX = rw_f + filterPar.sy_lfr_sc_rw_delta_f / 2.;
1086 int binToRemove;
1104 1087
1105 // compute the index of the frequency bin immediately below rw_f
1106 binBelow = (int) ( floor( ((double) rw_f) / ((double) deltaFreq)) );
1107 deltaBelow = rw_f - binBelow * deltaFreq;
1108
1109 // compute the index of the frequency bin immediately above rw_f
1110 binAbove = (int) ( ceil( ((double) rw_f) / ((double) deltaFreq)) );
1111 deltaAbove = binAbove * deltaFreq - rw_f;
1112
1113 // search the closest bin
1114 if (deltaAbove > deltaBelow)
1115 {
1116 closestBin = binBelow;
1117 }
1118 else
1119 {
1120 closestBin = binAbove;
1121 }
1122
1123 // compute the fi interval [fi - Delta_f * 0.285, fi + Delta_f * 0.285]
1124 fi = closestBin * deltaFreq;
1088 f_RW_min = 0.0;
1089 f_RW_MAX = 0.0;
1090 bandWidth = 0.0;
1091 binBelow = -1;
1092 binAbove = -1;
1093 whichByte = 0;
1094 selectedByte = -1;
1095 bin = -1;
1096 binToRemove = -1;
1125 1097
1126 fi_min = fi - (deltaFreq * 0.285);
1127 if ( fi_min < 0 )
1128 {
1129 fi_min = 0;
1130 }
1131 else if ( fi_min > (deltaFreq*127) )
1132 {
1133 fi_min = -1;
1134 }
1135
1136 fi_MAX = fi + (deltaFreq * 0.285);
1137 if ( fi_MAX > (deltaFreq*127) )
1138 {
1139 fi_MAX = -1;
1140 }
1141
1142 // 1. IF [ f_RW_min, f_RW_MAX] is included in [ fi_min; fi_MAX ]
1143 // => remove f_(i), f_(i-1) and f_(i+1)
1144 if ( ( f_RW_min > fi_min ) && ( f_RW_MAX < fi_MAX ) )
1098 if (!isnan(rw_f))
1145 1099 {
1146 binToRemove[0] = closestBin - 1;
1147 binToRemove[1] = closestBin;
1148 binToRemove[2] = closestBin + 1;
1149 }
1150 // 2. ELSE
1151 // => remove the two f_(i) which are around f_RW
1152 else
1153 {
1154 binToRemove[0] = binBelow;
1155 binToRemove[1] = binAbove;
1156 binToRemove[2] = -1;
1157 }
1100 // compute the frequency range to filter [ rw_f - delta_f/2; rw_f + delta_f/2 ]
1101 bandWidth = filterPar.sy_lfr_sc_rw_delta_f * k;
1102 f_RW_min = rw_f - bandWidth / 2.;
1103 f_RW_MAX = rw_f + bandWidth / 2.;
1158 1104
1159 for (k = 0; k <= 3; k++)
1160 {
1161 bin = binToRemove[k];
1162 if ( (bin >= 0) && (bin <= 127) )
1105 // compute the index of the frequency bin immediately below f_RW_min
1106 binBelow = (int) ( floor( ((double) f_RW_min) / ((double) deltaFreq)) );
1107 if (binBelow < 0)
1163 1108 {
1164 if (flag == 1)
1109 binBelow = -1;
1110 }
1111
1112 // compute the index of the frequency bin immediately above f_RW_MAX
1113 binAbove = (int) ( ceil( ((double) f_RW_MAX) / ((double) deltaFreq)) );
1114 if (binAbove > 127)
1115 {
1116 binAbove = 128;
1117 }
1118
1119 for (binToRemove = binBelow; binToRemove <= binAbove; binToRemove++)
1120 {
1121 if ( (binToRemove >= 0) && (binToRemove <= 127) )
1165 1122 {
1166 1123 whichByte = (bin >> 3); // division by 8
1167 1124 selectedByte = ( 1 << (bin - (whichByte * 8)) );
@@ -1169,6 +1126,7 void setFBinMask( unsigned char *fbins_m
1169 1126 }
1170 1127 }
1171 1128 }
1129
1172 1130 }
1173 1131
1174 1132 void build_sy_lfr_rw_mask( unsigned int channel )
@@ -1207,28 +1165,28 void build_sy_lfr_rw_mask( unsigned int
1207 1165 }
1208 1166
1209 1167 // RW1
1210 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f1, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & 0x80) >> 7 ); // [1000 0000]
1211 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f2, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & 0x40) >> 6 ); // [0100 0000]
1212 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f1, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & 0x20) >> 5 ); // [0010 0000]
1213 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f2, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & 0x10) >> 4 ); // [0001 0000]
1168 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f1, deltaF, filterPar.sy_lfr_rw1_k1 );
1169 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f2, deltaF, filterPar.sy_lfr_rw1_k2 );
1170 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f3, deltaF, filterPar.sy_lfr_rw1_k3 );
1171 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f4, deltaF, filterPar.sy_lfr_rw1_k4 );
1214 1172
1215 1173 // RW2
1216 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f1, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & 0x08) >> 3 ); // [0000 1000]
1217 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f2, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & 0x04) >> 2 ); // [0000 0100]
1218 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f1, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & 0x02) >> 1 ); // [0000 0010]
1219 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f2, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & 0x01) ); // [0000 0001]
1174 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f1, deltaF, filterPar.sy_lfr_rw2_k1 );
1175 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f2, deltaF, filterPar.sy_lfr_rw2_k2 );
1176 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f3, deltaF, filterPar.sy_lfr_rw2_k3 );
1177 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f4, deltaF, filterPar.sy_lfr_rw2_k4 );
1220 1178
1221 1179 // RW3
1222 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f1, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & 0x80) >> 7 ); // [1000 0000]
1223 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f2, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & 0x40) >> 6 ); // [0100 0000]
1224 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f1, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & 0x20) >> 5 ); // [0010 0000]
1225 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f2, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & 0x10) >> 4 ); // [0001 0000]
1180 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f1, deltaF, filterPar.sy_lfr_rw3_k1 );
1181 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f2, deltaF, filterPar.sy_lfr_rw3_k2 );
1182 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f3, deltaF, filterPar.sy_lfr_rw3_k3 );
1183 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f4, deltaF, filterPar.sy_lfr_rw3_k4 );
1226 1184
1227 1185 // RW4
1228 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f1, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & 0x08) >> 3 ); // [0000 1000]
1229 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f2, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & 0x04) >> 2 ); // [0000 0100]
1230 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f1, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & 0x02) >> 1 ); // [0000 0010]
1231 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f2, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & 0x03) ); // [0000 0001]
1186 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f1, deltaF, filterPar.sy_lfr_rw4_k1 );
1187 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f2, deltaF, filterPar.sy_lfr_rw4_k2 );
1188 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f3, deltaF, filterPar.sy_lfr_rw4_k3 );
1189 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f4, deltaF, filterPar.sy_lfr_rw4_k4 );
1232 1190
1233 1191 // update the value of the fbins related to reaction wheels frequency filtering
1234 1192 if (maskPtr != NULL)
General Comments 0
You need to be logged in to leave comments. Login now