git.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/Soft/Lib/Maths/filter.h

   1 /**
   2  *  \file     filter.h
   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 #ifndef _FILTER_H_
   9 #define _FILTER_H_
  10
  11 #include <VP_Os/vp_os_types.h>
  12
  13
  14 #define NB_FIRST_ORDER                1
  15 #define NB_SECOND_ORDER               2
  16 #define NB_THIRD_ORDER                3
  17 #define NB_FOURTH_ORDER               4
  18 #define NB_SIXTH_ORDER                6
  19 #define DEFAULT_DELAY_STEP_TIME_DELAY TWENTY_STEP_TIME_DELAY
  20 #define FORTY_STEP_TIME_DELAY         40
  21 #define THIRTY_TWO_STEP_TIME_DELAY    32
  22 #define TWENTY_STEP_TIME_DELAY        20
  23
  24 ///////////////////////////////////////////////
  25 // STRUCTURES
  26
  27
  28 /**
  29  * \struct _first_order_filter_
  30  * \brief  First order filter states using Matlab notation.
  31  */
  32 typedef struct {
  33   float32_t old_outputs[NB_FIRST_ORDER];  //< filter output history
  34   float32_t old_inputs [NB_FIRST_ORDER];  //< filter input history
  35 } first_order_filter_t;
  36
  37
  38 /**
  39  * \struct _second_order_filter_
  40  * \brief  Second order filter states using Matlab notation.
  41  */
  42 typedef struct {
  43   float32_t old_outputs[NB_SECOND_ORDER]; //< filter output history
  44   float32_t old_inputs [NB_SECOND_ORDER]; //< filter input history
  45 } second_order_filter_t;
  46
  47
  48 /**
  49  * \struct _third_order_filter_
  50  * \brief  Third order filter states using Matlab notation.
  51  */
  52 typedef struct {
  53   float32_t old_outputs[NB_THIRD_ORDER];  //< filter output history
  54   float32_t old_inputs [NB_THIRD_ORDER];  //< filter input history
  55 } third_order_filter_t;
  56
  57
  58 /**
  59  * \struct _fourth_order_filter_
  60  * \brief  Fourth order filter states using Matlab notation.
  61  */
  62 typedef struct {
  63   float32_t old_outputs[NB_FOURTH_ORDER]; //< filter output history
  64   float32_t old_inputs [NB_FOURTH_ORDER]; //< filter input history
  65 } fourth_order_filter_t;
  66
  67 typedef struct {
  68   float32_t old_outputs[NB_SIXTH_ORDER];  //< filter output history
  69   float32_t old_inputs [NB_SIXTH_ORDER];  //< filter input history
  70 } sixth_order_filter_t;
  71
  72
  73 typedef struct _deriv_param_t {
  74   float32_t kd;
  75   float32_t td;
  76   float32_t te;
  77
  78   float32_t internal_state;
  79   float32_t old_input;
  80 } deriv_param_t;
  81
  82 /**
  83  * \struct _delay_
  84  * \brief  m sampling step time delay.
  85  */
  86 typedef struct {
  87   float32_t old_inputs[FORTY_STEP_TIME_DELAY];  //< input history
  88 } delay_t;
  89
  90 ///////////////////////////////////////////////
  91 // FUNCTIONS
  92
  93
  94 /**
  95  * \fn      Digital filter initialization.
  96  * \brief   This function is used to initialize previous values in digital filter.
  97  * \param   Filter order.
  98  * \param   address of previous inputs list.
  99  * \param   initial input value.
 100  * \param   address of previous outputs list.
 101  * \param   initial output value.
 102  * \return  void.
 103  *
 104  * \section History
 105  *
 106  * \par date: 2007-06-25  author: <florian.pantaleao.ext\@parrot.com> <jean-baptiste.lanfrey\@parrot.com>
 107  *  - first version
 108  */
 109 void filter_init(uint32_t n, float32_t *old_input, float32_t initial_input, float32_t *old_output, float32_t initial_output);
 110
 111
 112
 113 /**
 114  * \fn      Filter a value.
 115  * \brief   This function uses the same notation as Matlab except that array indices start at 0
 116  * \brief   a(1)*y(n) = b(1)*x(n) + b(2)*x(n-1) + ... + b(nb+1)*x(n-nb) - a(2)*y(n-1) - ... - a(na+1)*y(n-na)
 117  * \brief   This function automatically shifts old inputs and outputs.
 118  * \param   Filter order
 119  * \param   address of B coefficients list.
 120  * \param   address of A coefficients list.
 121  * \param   input value.
 122  * \param   address of previous inputs list.
 123  * \param   address of previous outputs list.
 124  * \return  filtered value.
 125  *
 126  * \section History
 127  *
 128  * \par date: 2007-06-25  author: <florian.pantaleao.ext\@parrot.com> <jean-baptiste.lanfrey\@parrot.com>
 129  *  - first version
 130  */
 131 float32_t filter(uint32_t n, const float32_t *b, const float32_t *a, float32_t input, float32_t *old_input, float32_t *old_output);
 132
 133 /**
 134  * \fn      Derivative filter
 135  * \brief   Y/U = kd;p/(1+Td.p)
 136  * \param   Kd, Td, Te, previous values (state and input)
 137  * \return  filtered value.
 138  *
 139  * \section History
 140  *
 141  * \par date: 2007-10-15  author: <jean-baptiste.lanfrey\@parrot.com>
 142  *  - first version
 143  */
 144 float32_t deriv(deriv_param_t *param, float32_t input);
 145
 146 /**
 147  * \fn      Digital delay initialization
 148  * \brief   This function is used to initialize previous inputs of the digital delay.
 149  * \param   m number of sampling step time delay.
 150  * \return  void.
 151  *
 152  * \section History
 153  *
 154  * \par date: 2008-5-13  author: <yannick.foloppe.ext\@parrot.com>
 155  *  - first version
 156  */
 157 void delay_init(uint32_t m, float32_t *old_input, float32_t initial_input);
 158
 159 /**
 160  * \fn      Delay a value
 161  * \brief   y(k)=u(k-m).
 162  * \param   m number of sampling step time delay, previous values (state and input).
 163  * \return  delayed input.
 164  *
 165  * \section History
 166  *
 167  * \par date: 2008-5-13  author: <yannick.foloppe.ext\@parrot.com>
 168  *  - first version
 169  */
 170 float delay(uint32_t m, float32_t input, float32_t *old_input);
 171
 172 // rate limiter
 173 // rate_max is the highest rate allowed in one sample time
 174 // warning : rate_max must be positive
 175 /**
 176  * \fn      rate limiter
 177  * \brief  limit the rate of an input.
 178  * \param   rate_max is the highest rate allowed in one sample time. Warning : rate_max must be positive
 179  * \return  rate limiter output.
 180  *
 181  * \section History
 182  *
 183  * \par date: 2008-7-24 author: <yannick.foloppe.ext\@parrot.com>
 184  *  - first version
 185  */
 186 float32_t rate_limiter(float32_t input, float32_t old_output, float32_t rate_max);
 187
 188 #endif // FILTER