vcs.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/Soft/Lib/Maths/filter.c

   1 /**
   2  *  \file     filter.c
   3  *  \brief    1st and 2nd order filter implementation
   4  *  \author   Jean Baptiste Lanfrey <jean-baptiste.lanfrey@parrot.com>
   5  *  \version  1.0
   6  */
   7
   8 #include <Maths/maths.h>
   9 #include <Maths/filter.h>
  10 #include <math.h>
  11
  12 void filter_init(uint32_t n, float32_t *old_input, float32_t initial_input, float32_t *old_output, float32_t initial_output)
  13 {
  14   uint32_t ii;
  15
  16   for (ii=0; ii<n ; ii++)
  17     old_input[ii]  = initial_input;
  18
  19   for (ii=0; ii<n; ii++)
  20     old_output[ii] = initial_output;
  21 }
  22
  23 float filter(uint32_t n, const float32_t *b, const float32_t *a, float32_t input, float32_t *old_input, float32_t *old_output)
  24 {
  25   uint32_t ii;
  26   float32_t inno, past, output;
  27
  28   // innovation computation
  29   inno = b[0] * input;
  30   for (ii=1; ii<n+1; ii++)
  31     inno += b[ii] * old_input[ii-1];
  32
  33   // past computation
  34   past = 0.0;
  35   for (ii=1; ii<n+1; ii++)
  36     past -= a[ii] * old_output[ii-1];
  37
  38   // output = (inno + past) / a[0];
  39         // We assume a[0] is always equal to 1
  40   output = (inno + past);
  41
  42   // inputs and outputs shift
  43   for (ii=n-1; ii>0; ii--)
  44     old_input[ii] = old_input[ii-1];
  45
  46   old_input[0] = input;
  47
  48   for (ii=n-1; ii>0; ii--)
  49     old_output[ii] = old_output[ii-1];
  50
  51   old_output[0] = output;
  52
  53   return output;
  54 }
  55
  56
  57 // Filtre du type Kd.p / (1+Td.p)
  58 float32_t deriv(deriv_param_t *param, float32_t input)
  59 {
  60   static float32_t exp_1 = 0.3678794411714423412f; // expf(-1.0f);
  61
  62   float32_t exp;
  63   float32_t td;
  64   float32_t out;
  65
  66   if( f_is_zero( param->td ) )
  67   {
  68     // prevent from dividing by 0.
  69     td  = param->te;
  70     exp = exp_1;
  71   }
  72   else
  73   {
  74     td  = param->td;
  75     // exp = expf(-param->te/td);
  76     exp = exp_taylor(-param->te/td);
  77   }
  78
  79   param->internal_state = exp * param->internal_state + td * (1 - exp) * param->old_input;
  80
  81   out = ( param->kd / td ) * ( input - param->internal_state / td );
  82
  83   param->old_input = input;
  84
  85   return out;
  86 }
  87
  88 void delay_init(uint32_t m, float32_t *old_input, float32_t initial_input)
  89 {
  90   uint32_t ii;
  91
  92   for (ii=0; ii<m ; ii++)
  93     old_input[ii] = initial_input;
  94 }
  95
  96 float32_t delay(uint32_t m, float32_t input, float32_t *old_input)
  97 {
  98   uint32_t ii;
  99   float32_t output;
 100
 101   output = old_input[m-1];
 102
 103   // inputs and shift
 104   for (ii=m-1; ii>0; ii--)
 105     old_input[ii] = old_input[ii-1];
 106
 107   old_input[0] = input;
 108
 109   return output;
 110 }
 111
 112 float32_t rate_limiter(float32_t input, float32_t old_output, float32_t rate_max)
 113 {
 114   float32_t output;
 115   float_or_int_t FOI_rate,FOI_rate_max;
 116
 117   FOI_rate.f = input - old_output;
 118   FOI_rate_max.f = rate_max;
 119
 120   if( f_abs( FOI_rate ) > FOI_rate_max.f )
 121     output = old_output + f_set_clamp( FOI_rate, FOI_rate_max );
 122   else
 123     output = input;
 124
 125   return output;
 126 }