1 /*
2 * SpanDSP - a series of DSP components for telephony
3 *
4 * biquad.h - General telephony bi-quad section routines (currently this just
5 * handles canonic/type 2 form)
6 *
7 * Written by Steve Underwood <steveu@coppice.org>
8 *
9 * Copyright (C) 2001 Steve Underwood
10 *
11 * All rights reserved.
12 *
13 * This program is free software; you can redistribute it and/or modify
14 * it under the terms of the GNU Lesser General Public License version 2.1,
15 * as published by the Free Software Foundation.
16 *
17 * This program is distributed in the hope that it will be useful,
18 * but WITHOUT ANY WARRANTY; without even the implied warranty of
19 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
20 * GNU Lesser General Public License for more details.
21 *
22 * You should have received a copy of the GNU Lesser General Public
23 * License along with this program; if not, write to the Free Software
24 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
25 *
26 * $Id: biquad.h,v 1.14 2008/04/17 14:26:59 steveu Exp $
27 */
28
29 /*! \page biquad_page Bi-quadratic filter sections
30 \section biquad_page_sec_1 What does it do?
31 ???.
32
33 \section biquad_page_sec_2 How does it work?
34 ???.
35 */
36
37 #if !defined(_SPANDSP_BIQUAD_H_)
38 #define _SPANDSP_BIQUAD_H_
39
40 typedef struct
41 {
42 int32_t gain;
43 int32_t a1;
44 int32_t a2;
45 int32_t b1;
46 int32_t b2;
47
48 int32_t z1;
49 int32_t z2;
50
51 #if FIRST_ORDER_NOISE_SHAPING
52 int32_t residue;
53 #elif SECOND_ORDER_NOISE_SHAPING
54 int32_t residue1;
55 int32_t residue2;
56 #endif
57 } biquad2_state_t;
58
59 #if defined(__cplusplus)
60 extern "C"
61 {
62 #endif
63
biquad2_init(biquad2_state_t * bq,int32_t gain,int32_t a1,int32_t a2,int32_t b1,int32_t b2)64 static __inline__ void biquad2_init(biquad2_state_t *bq,
65 int32_t gain,
66 int32_t a1,
67 int32_t a2,
68 int32_t b1,
69 int32_t b2)
70 {
71 bq->gain = gain;
72 bq->a1 = a1;
73 bq->a2 = a2;
74 bq->b1 = b1;
75 bq->b2 = b2;
76
77 bq->z1 = 0;
78 bq->z2 = 0;
79
80 #if FIRST_ORDER_NOISE_SHAPING
81 bq->residue = 0;
82 #elif SECOND_ORDER_NOISE_SHAPING
83 bq->residue1 = 0;
84 bq->residue2 = 0;
85 #endif
86 }
87 /*- End of function --------------------------------------------------------*/
88
biquad2(biquad2_state_t * bq,int16_t sample)89 static __inline__ int16_t biquad2(biquad2_state_t *bq, int16_t sample)
90 {
91 int32_t y;
92 int32_t z0;
93
94 z0 = sample*bq->gain + bq->z1*bq->a1 + bq->z2*bq->a2;
95 y = z0 + bq->z1*bq->b1 + bq->z2*bq->b2;
96
97 bq->z2 = bq->z1;
98 bq->z1 = z0 >> 15;
99 #if FIRST_ORDER_NOISE_SHAPING
100 y += bq->residue;
101 bq->residue = y & 0x7FFF;
102 #elif SECOND_ORDER_NOISE_SHAPING
103 y += (2*bq->residue1 - bq->residue2);
104 bq->residue2 = bq->residue1;
105 bq->residue1 = y & 0x7FFF;
106 #endif
107 y >>= 15;
108 return (int16_t) y;
109 }
110 /*- End of function --------------------------------------------------------*/
111
112 #if defined(__cplusplus)
113 }
114 #endif
115
116 #endif
117 /*- End of file ------------------------------------------------------------*/
118