my test
Embed:
(wiki syntax)
Show/hide line numbers
filter.cpp
00001 //****************************************************************************** 00002 // filter.c 00003 // 00004 // Description: filter module 00005 // 00006 // Author: Jason shen 00007 // Version 1.00 00008 // DDC BJ 00009 // October 2011 00010 // Built with IAR Embedded Workbench Version: 5.30A 00011 //****************************************************************************** 00012 //Change Log: 00013 //****************************************************************************** 00014 //Version: 1.00 00015 //Comments: Initial Release Version 00016 //****************************************************************************** 00017 #include "filter.h" 00018 00019 /************************************************************************************************** 00020 * @fn FirstOrderFilter 00021 * 00022 * @brief prototype -> result = OldData * ( 256 - K ) / 256 + NewData * K / 256. 00023 * optimized -> 00024 * result = OldData - ( OldData - NewData ) * K / 256 when ( NewData < OldData ) 00025 * result = OldData + ( NewData - OldData ) * K / 256 when ( NewData > OldData ) 00026 * 00027 * @param new_data = new sampled data 00028 * old_data = last time filter result 00029 * K = filter coefficient (0~255), the weighted of the NewData in the result 00030 * 00031 * @return The filter result for this time 00032 ************************************************************************************************** 00033 */ 00034 /* 00035 int32_t Filter::first_order_filter(int32_t old_data, int32_t new_data, int32_t K, int32_t threshold) 00036 { 00037 int32_t result; 00038 00039 if(((new_data - old_data) > threshold) || ((old_data - new_data) > threshold)) 00040 { 00041 result = new_data; 00042 } 00043 else 00044 { 00045 if(new_data < old_data) 00046 { 00047 result = old_data - (( old_data - new_data ) << K + 128) >> 8; 00048 } 00049 else if((new_data > old_data)) 00050 { 00051 result = old_data + (( new_data - old_data ) << K + 128) >> 8; 00052 } 00053 else 00054 { 00055 result = old_data; 00056 } 00057 } 00058 return result; 00059 } 00060 */ 00061 float Filter::first_order_filter(float old_data, float new_data, float K, float threshold) 00062 { 00063 float result; 00064 00065 if(((new_data - old_data) > threshold) || ((old_data - new_data) > threshold)) 00066 { 00067 result = new_data; 00068 } 00069 else 00070 { 00071 result = old_data + (new_data - old_data) * K; //it equals to result = old_data * (1.0 - K) + new_data* K; 00072 } 00073 return result; 00074 } 00075 00076 /************************************************************************************************** 00077 * @fn FirstOrderFilterAdaptiveK 00078 * 00079 * @brief prototype -> result = OldData * ( 256 - K ) / 256 + NewData * K / 256. 00080 * optimized -> 00081 * result = OldData - ( OldData - NewData ) * K / 256 when ( NewData < OldData ) 00082 * result = OldData + ( NewData - OldData ) * K / 256 when ( NewData > OldData ) 00083 * 00084 * @param new_data = new sampled data 00085 * old_data = last time filter result 00086 * K = filter coefficient (0~255), the weighted of the NewData in the result 00087 * threshold = 00088 * K_increment= increment for K every time 00089 * K_max = max of K 00090 * 00091 * @return The filter result for this time 00092 ************************************************************************************************** 00093 */ 00094 int32_t Filter::first_order_filter_adaptive_K(int32_t old_data, int32_t new_data, int32_t threshold, int32_t K_increment, int32_t K_max) 00095 { 00096 static char data_change_direction_flag = 0; // 0 is the positive direction(new<old), 1 is the negative direction(new>old) 00097 static char filter_cnt = 0; // filter times counter 00098 static int16_t K = 0; 00099 00100 static int32_t _v[3]; 00101 00102 _v[0] = _v[1]; 00103 _v[1] = _v[2]; 00104 _v[2] = new_data; 00105 00106 00107 if(((_v[1] >= _v[0]) && (_v[2] >= _v[1])) || ((_v[1] <= _v[0]) && (_v[2] <= _v[1]))) 00108 { 00109 data_change_direction_flag = 1; 00110 } 00111 else data_change_direction_flag = 0; 00112 00113 if(data_change_direction_flag == 0) 00114 { 00115 filter_cnt = 0; 00116 K = 1; 00117 } 00118 else 00119 { 00120 filter_cnt++; 00121 if(((_v[2] - _v[1]) > threshold) || ((_v[1] - _v[2]) > threshold)) 00122 { 00123 filter_cnt += 2; 00124 } 00125 00126 if(filter_cnt >= 4) 00127 { 00128 K += K_increment; 00129 if(K > K_max) 00130 { 00131 K = K_max; 00132 } 00133 //filter_cnt = 0; 00134 } 00135 } 00136 //FirstOrderFilter 00137 _v[2] = first_order_filter(_v[1], _v[2], K, threshold); 00138 00139 return _v[2]; 00140 } 00141 00142 #define FILTER_CNT_MAX 12 00143 float Filter::first_order_filter_adaptive_K(float new_data, filter_args_s * arg) 00144 { 00145 arg->v[0] = arg->v[1]; 00146 arg->v[1] = arg->v[2]; 00147 arg->v[2] = new_data; 00148 00149 00150 00151 if(((arg->v[2] - arg->v[1]) > arg->threshold) || ((arg->v[1] - arg->v[2]) > arg->threshold)) 00152 { 00153 arg->v[0] = new_data; 00154 arg->v[1] = new_data; 00155 arg->v[2] = new_data; 00156 return arg->v[2]; 00157 } 00158 00159 00160 if(((arg->v[2] >= arg->v[1]) && (arg->v[1] >= arg->v[0])) || ((arg->v[2] <= arg->v[1]) && (arg->v[1] <= arg->v[0]))) 00161 { 00162 arg->dir_flag = 1; 00163 } 00164 else arg->dir_flag = 0; 00165 00166 if(arg->dir_flag == 1) 00167 { 00168 arg->dir_cnt ++; 00169 if(((arg->v[2] - arg->v[1])*2.0 > arg->threshold) || ((arg->v[1] - arg->v[2])*2.0 > arg->threshold)) 00170 { 00171 arg->dir_cnt += 5; 00172 } 00173 else if(((arg->v[2] - arg->v[1])*4.0 > arg->threshold) || ((arg->v[1] - arg->v[2])*4.0 > arg->threshold)) 00174 { 00175 arg->dir_cnt += 4; 00176 } 00177 else if(((arg->v[2] - arg->v[1])*8.0 > arg->threshold) || ((arg->v[1] - arg->v[2])*8.0 > arg->threshold)) 00178 { 00179 arg->dir_cnt += 2; 00180 } 00181 if(arg->dir_cnt >= FILTER_CNT_MAX) 00182 { 00183 arg->dir_cnt = 0; 00184 arg->K_curr += arg->K_inc; 00185 } 00186 if(arg->K_curr >= arg->K_max) 00187 { 00188 arg->K_curr = arg->K_max; 00189 } 00190 } 00191 else 00192 { 00193 arg->dir_cnt = 0; 00194 arg->K_curr = arg->K_ini; 00195 } 00196 00197 arg->v[2] = (arg->v[1] + (arg->v[2] - arg->v[1]) * arg->K_curr); 00198 00199 return arg->v[2]; 00200 } 00201 00202 /************************************************************************************************** 00203 * @fn SignalFilter 00204 * Library to Filter Sensor Data using digital filters 00205 * Available filters: Chebyshev & Bessel low pass filter (1st & 2nd order) 00206 * 00207 * @brief 00208 * 00209 * 00210 * 00211 * 00212 * @param data = new sampled data 00213 * _filter = Select filter: 'c' -> Chebyshev, 'b' -> Bessel 00214 * _order = Select filter order (1 or 2) 00215 * 00216 * @return The filtered result for this time 00217 ************************************************************************************************** 00218 */ 00219 int16_t Filter::SignalFilter(int16_t data, int16_t _filter, int16_t _order) 00220 { 00221 static short _v[3]; 00222 // Uncomment for debugging 00223 // Serial.println(_filter); 00224 // Serial.println(_order); 00225 if(_filter=='c') // Chebyshev filters 00226 { 00227 if(_order==1) //ripple -3dB 00228 { 00229 _v[0] = _v[1]; 00230 int32_t tmp = ((((data * 3269048L) >> 2) //= (3.897009118e-1 * data) 00231 + ((_v[0] * 3701023L) >> 3) //+( 0.2205981765*v[0]) 00232 )+1048576) >> 21; // round and downshift fixed point /2097152 00233 _v[1]= (short)tmp; 00234 return (short)(_v[0] + _v[1]); // 2^ 00235 } 00236 if(_order==2) //ripple -1dB 00237 { 00238 _v[0] = _v[1]; 00239 _v[1] = _v[2]; 00240 int32_t tmp = ((((data * 662828L) >> 4) //= ( 7.901529699e-2 * x) 00241 + ((_v[0] * -540791L) >> 1) //+( -0.5157387562*v[0]) 00242 + (_v[1] * 628977L) //+( 1.1996775682*v[1]) 00243 )+262144) >> 19; // round and downshift fixed point /524288 00244 00245 _v[2]= (short)tmp; 00246 return (short)(( 00247 (_v[0] + _v[2]) 00248 +2 * _v[1])); // 2^ 00249 } 00250 } 00251 if(_filter=='b') // Bessel filters 00252 if(_order==1) //Alpha Low 0.1 00253 { 00254 _v[0] = _v[1]; 00255 int32_t tmp = ((((data * 2057199L) >> 3) //= ( 2.452372753e-1 * x) 00256 + ((_v[0] * 1068552L) >> 1) //+( 0.5095254495*v[0]) 00257 )+524288) >> 20; // round and downshift fixed point /1048576 00258 _v[1]= (short)tmp; 00259 return (short)(((_v[0] + _v[1]))); // 2^ 00260 } 00261 if(_order==2) //Alpha Low 0.1 00262 { 00263 _v[0] = _v[1]; 00264 _v[1] = _v[2]; 00265 int32_t tmp = ((((data * 759505L) >> 4) //= ( 9.053999670e-2 * x) 00266 + ((_v[0] * -1011418L) >> 3) //+( -0.2411407388*v[0]) 00267 + ((_v[1] * 921678L) >> 1) //+( 0.8789807520*v[1]) 00268 )+262144) >> 19; // round and downshift fixed point /524288 00269 00270 _v[2]= (short)tmp; 00271 return (short)(((_v[0] + _v[2])+2 * _v[1])); // 2^ 00272 } 00273 return 0; 00274 } 00275 00276 void Filter::max_min(uint16_t data[], uint16_t n, uint16_t *max, uint16_t *min) 00277 { 00278 *max=0x0000; 00279 *min=0xFFFF; 00280 00281 for(int16_t i=0;i<n;i++) 00282 { 00283 if(data[i] > *max) *max = data[i]; 00284 if(data[i] < *min) *min = data[i]; 00285 } 00286 } 00287 00288 void Filter::sum_avg(uint16_t data[], uint16_t n, uint32_t *sum, float *avg) 00289 { 00290 *sum = 0; 00291 for(int16_t i=0;i<n;i++) 00292 { 00293 *sum += (uint32_t)data[i]; 00294 } 00295 *avg = (float)*sum/(float)n; 00296 } 00297 00298 void Filter::stdev(uint16_t data[], uint16_t n, float *s) 00299 { 00300 int16_t i; 00301 uint32_t sum1; 00302 float avg, sum2; 00303 sum1 = 0; 00304 for(i=0;i<n;i++) 00305 { 00306 sum1 += (uint32_t)data[i]; 00307 } 00308 avg = (float)sum1/(float)n; 00309 sum2 = 0; 00310 for(i=0;i<n;i++) 00311 { 00312 sum2 += ((float)data[i] - avg)*((float)data[i] - avg); 00313 } 00314 sum2 = sum2/(float)(n-1); 00315 *s = sqrt(sum2); 00316 00317 }
Generated on Sat Aug 20 2022 09:59:31 by
1.7.2