2 * Copyright Droids Corporation, Microb Technology, Eirbot (2006)
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.
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.
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
18 * Revision : $Id: biquad.c,v 1.2.2.2 2007-05-12 16:42:39 zer0 Exp $
26 /** init sets an unity filter, as usual */
27 void biquad_init (struct biquad_filter * p)
32 /* set all structure to 0 */
33 memset(p, 0, sizeof(struct biquad_filter));
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.
46 void biquad_flush_memories(struct biquad_filter *p)
60 /** accessors to coefficients */
61 void biquad_set_numerator_coeffs(struct biquad_filter *p, int16_t b0, int16_t b1, int16_t b2)
73 void biquad_set_deniminator_coeffs(struct biquad_filter *p, int16_t a1, int16_t a2)
84 void biquad_set_divisor_shifts(struct biquad_filter *p, uint8_t recursive_shift, uint8_t out_shift)
89 p-> out_shift = out_shift;
90 p-> recursive_shift = recursive_shift;
95 void biquad_set_series_son(struct biquad_filter *p, struct biquad_filter *son)
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)
112 struct biquad_filter * p = data;
114 /* filter computation */
117 output += p->b1 * p->mem_in_1;
118 output += p->b2 * p->mem_in_2;
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 */
125 /* update of the memories for next iteration */
126 p->mem_in_2 = p->mem_in_1;
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.
133 /* output division with shifting */
134 output >>= p-> out_shift;
137 /* execute series filter */
139 output = biquad_do_filter(p->son, output);