modules/devices/control_system/filters/biquad/biquad.c

   1 /*
   2  *  Copyright Droids Corporation, Microb Technology, Eirbot (2006)
   3  *
   4  *  This program is free software; you can redistribute it and/or modify
   5  *  it under the terms of the GNU General Public License as published by
   6  *  the Free Software Foundation; either version 2 of the License, or
   7  *  (at your option) any later version.
   8  *
   9  *  This program is distributed in the hope that it will be useful,
  10  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  *  GNU General Public License for more details.
  13  *
  14  *  You should have received a copy of the GNU General Public License
  15  *  along with this program; if not, write to the Free Software
  16  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  17  *
  18  *  Revision : $Id: biquad.c,v 1.2.2.2 2007-05-12 16:42:39 zer0 Exp $
  19  *
  20  */
  21
  22
  23 #include <biquad.h>
  24 #include <string.h>
  25
  26 /** init sets an unity filter, as usual */
  27 void biquad_init (struct biquad_filter * p)
  28 {
  29         uint8_t flags;
  30         IRQ_LOCK(flags);
  31
  32         /* set all structure to 0 */
  33         memset(p, 0, sizeof(struct biquad_filter));
  34
  35         /* unity filter */
  36         p-> b0=1;
  37
  38         IRQ_UNLOCK(flags);
  39 }
  40
  41 /** this is useful for cleaning the filter memories before a new data
  42  *  set.  With this you avoid having old data in the filter memories
  43  *  when beginning to filter a new stream.  Can also be used after
  44  *  changing the coefficients, to avoid jumps of the output value.
  45  */
  46 void biquad_flush_memories(struct biquad_filter *p)
  47 {
  48         uint8_t flags;
  49         IRQ_LOCK(flags);
  50
  51         /* empty mem cells */
  52         p->mem_in_1  = 0;
  53         p->mem_in_2  = 0;
  54         p->mem_out_1 = 0;
  55         p->mem_out_2 = 0;
  56
  57         IRQ_UNLOCK(flags);
  58 }
  59
  60 /** accessors to coefficients */
  61 void biquad_set_numerator_coeffs(struct biquad_filter *p, int16_t b0, int16_t b1, int16_t b2)
  62 {
  63         uint8_t flags;
  64         IRQ_LOCK(flags);
  65
  66         p->b0 = b0;
  67         p->b1 = b1;
  68         p->b2 = b2;
  69
  70         IRQ_UNLOCK(flags);
  71 }
  72
  73 void biquad_set_deniminator_coeffs(struct biquad_filter *p, int16_t a1, int16_t a2)
  74 {
  75         uint8_t flags;
  76         IRQ_LOCK(flags);
  77
  78         p->a1 = -a1;
  79         p->a2 = -a2;
  80
  81         IRQ_UNLOCK(flags);
  82 }
  83
  84 void biquad_set_divisor_shifts(struct biquad_filter *p, uint8_t recursive_shift, uint8_t out_shift)
  85 {
  86         uint8_t flags;
  87         IRQ_LOCK(flags);
  88
  89         p-> out_shift       =  out_shift;
  90   p-> recursive_shift =  recursive_shift;
  91
  92         IRQ_UNLOCK(flags);
  93 }
  94
  95 void biquad_set_series_son(struct biquad_filter *p,   struct biquad_filter *son)
  96 {
  97         uint8_t flags;
  98         IRQ_LOCK(flags);
  99
 100         p->son = son;
 101
 102         IRQ_UNLOCK(flags);
 103 }
 104
 105 /** filter processing, 1 iteration.
 106     This function is not protected against writing in the structure while execution is ongoing! */
 107 int32_t biquad_do_filter(void * data , int32_t in)
 108 {
 109
 110         int32_t output;
 111
 112         struct biquad_filter * p = data;
 113
 114         /* filter computation */
 115         output = p->b0 * in;
 116
 117         output += p->b1 *  p->mem_in_1;
 118         output += p->b2 *  p->mem_in_2;
 119
 120         output += p->a1 *  p->mem_out_1; /* minus, placed on the accessor */
 121         output += p->a2 *  p->mem_out_2; /* minus, placed on the accessor */
 122
 123
 124
 125         /* update of the memories for next iteration */
 126         p->mem_in_2 = p->mem_in_1;
 127         p->mem_in_1 = in;
 128
 129         p->mem_out_2 = p-> mem_out_1;
 130         p->mem_out_1 = output >> p-> recursive_shift; // recursive shift, this corresponds to a division on the a1 and a2 terms.
 131
 132
 133         /* output division with shifting */
 134         output >>= p-> out_shift;
 135
 136
 137         /* execute series filter */
 138         if(p->son != NULL)
 139                 output = biquad_do_filter(p->son, output);
 140
 141         /* finished */
 142         return output;
 143
 144 }