my test

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers filter.cpp Source File

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 }