##// 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 #include "avf0_prc0.h"
12 #include "avf0_prc0.h"
13
13
14 #define FLOAT_EQUAL_ZERO 0.001
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 extern unsigned short sequenceCounterParameterDump;
20 extern unsigned short sequenceCounterParameterDump;
17 extern unsigned short sequenceCounters_TM_DUMP[];
21 extern unsigned short sequenceCounters_TM_DUMP[];
@@ -61,7 +65,7 unsigned int check_update_info_hk_thr_mo
61 void set_hk_lfr_sc_rw_f_flag( unsigned char wheel, unsigned char freq, float value );
65 void set_hk_lfr_sc_rw_f_flag( unsigned char wheel, unsigned char freq, float value );
62 void set_hk_lfr_sc_rw_f_flags( void );
66 void set_hk_lfr_sc_rw_f_flags( void );
63 void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC );
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 void build_sy_lfr_rw_mask( unsigned int channel );
69 void build_sy_lfr_rw_mask( unsigned int channel );
66 void build_sy_lfr_rw_masks();
70 void build_sy_lfr_rw_masks();
67 void merge_fbins_masks( void );
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 k++;
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 PRINTF("in AVGV *** deleting task\n");
431 PRINTF("in AVGV *** deleting task\n");
432
432
433 status = rtems_task_delete( RTEMS_SELF ); // should not return
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 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
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 float f_RW_min;
1078 float f_RW_min;
1079 float f_RW_MAX;
1079 float f_RW_MAX;
1080 float fi_min;
1080 float bandWidth;
1081 float fi_MAX;
1082 float fi;
1083 float deltaBelow;
1084 float deltaAbove;
1085 int binBelow;
1081 int binBelow;
1086 int binAbove;
1082 int binAbove;
1087 int closestBin;
1088 unsigned int whichByte;
1083 unsigned int whichByte;
1089 int selectedByte;
1084 int selectedByte;
1090 int bin;
1085 int bin;
1091 int binToRemove[3];
1086 int binToRemove;
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.;
1104
1087
1105 // compute the index of the frequency bin immediately below rw_f
1088 f_RW_min = 0.0;
1106 binBelow = (int) ( floor( ((double) rw_f) / ((double) deltaFreq)) );
1089 f_RW_MAX = 0.0;
1107 deltaBelow = rw_f - binBelow * deltaFreq;
1090 bandWidth = 0.0;
1108
1091 binBelow = -1;
1109 // compute the index of the frequency bin immediately above rw_f
1092 binAbove = -1;
1110 binAbove = (int) ( ceil( ((double) rw_f) / ((double) deltaFreq)) );
1093 whichByte = 0;
1111 deltaAbove = binAbove * deltaFreq - rw_f;
1094 selectedByte = -1;
1112
1095 bin = -1;
1113 // search the closest bin
1096 binToRemove = -1;
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;
1125
1097
1126 fi_min = fi - (deltaFreq * 0.285);
1098 if (!isnan(rw_f))
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 ) )
1145 {
1099 {
1146 binToRemove[0] = closestBin - 1;
1100 // compute the frequency range to filter [ rw_f - delta_f/2; rw_f + delta_f/2 ]
1147 binToRemove[1] = closestBin;
1101 bandWidth = filterPar.sy_lfr_sc_rw_delta_f * k;
1148 binToRemove[2] = closestBin + 1;
1102 f_RW_min = rw_f - bandWidth / 2.;
1149 }
1103 f_RW_MAX = rw_f + bandWidth / 2.;
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 }
1158
1104
1159 for (k = 0; k <= 3; k++)
1105 // compute the index of the frequency bin immediately below f_RW_min
1160 {
1106 binBelow = (int) ( floor( ((double) f_RW_min) / ((double) deltaFreq)) );
1161 bin = binToRemove[k];
1107 if (binBelow < 0)
1162 if ( (bin >= 0) && (bin <= 127) )
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 whichByte = (bin >> 3); // division by 8
1123 whichByte = (bin >> 3); // division by 8
1167 selectedByte = ( 1 << (bin - (whichByte * 8)) );
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 void build_sy_lfr_rw_mask( unsigned int channel )
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 // RW1
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]
1168 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f1, deltaF, filterPar.sy_lfr_rw1_k1 );
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]
1169 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f2, deltaF, filterPar.sy_lfr_rw1_k2 );
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]
1170 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f3, deltaF, filterPar.sy_lfr_rw1_k3 );
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]
1171 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f4, deltaF, filterPar.sy_lfr_rw1_k4 );
1214
1172
1215 // RW2
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]
1174 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f1, deltaF, filterPar.sy_lfr_rw2_k1 );
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]
1175 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f2, deltaF, filterPar.sy_lfr_rw2_k2 );
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]
1176 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f3, deltaF, filterPar.sy_lfr_rw2_k3 );
1219 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f2, deltaF, (cp_rpw_sc_rw1_rw2_f_flags & 0x01) ); // [0000 0001]
1177 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f4, deltaF, filterPar.sy_lfr_rw2_k4 );
1220
1178
1221 // RW3
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]
1180 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f1, deltaF, filterPar.sy_lfr_rw3_k1 );
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]
1181 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f2, deltaF, filterPar.sy_lfr_rw3_k2 );
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]
1182 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f3, deltaF, filterPar.sy_lfr_rw3_k3 );
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]
1183 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f4, deltaF, filterPar.sy_lfr_rw3_k4 );
1226
1184
1227 // RW4
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]
1186 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f1, deltaF, filterPar.sy_lfr_rw4_k1 );
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]
1187 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f2, deltaF, filterPar.sy_lfr_rw4_k2 );
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]
1188 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f3, deltaF, filterPar.sy_lfr_rw4_k3 );
1231 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f2, deltaF, (cp_rpw_sc_rw3_rw4_f_flags & 0x03) ); // [0000 0001]
1189 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f4, deltaF, filterPar.sy_lfr_rw4_k4 );
1232
1190
1233 // update the value of the fbins related to reaction wheels frequency filtering
1191 // update the value of the fbins related to reaction wheels frequency filtering
1234 if (maskPtr != NULL)
1192 if (maskPtr != NULL)
General Comments 0
You need to be logged in to leave comments. Login now