|
5
|
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 General Public License version 2, as
|
|
|
15 * 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 General Public License for more details.
|
|
|
21 *
|
|
|
22 * You should have received a copy of the GNU General Public License
|
|
|
23 * 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.9 2006/10/24 13:45:28 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(_BIQUAD_H_)
|
|
|
38 #define _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 #ifdef __cplusplus
|
|
|
60 extern "C" {
|
|
|
61 #endif
|
|
|
62
|
|
|
63 static __inline__ void biquad2_init(biquad2_state_t *bq,
|
|
|
64 int32_t gain,
|
|
|
65 int32_t a1,
|
|
|
66 int32_t a2,
|
|
|
67 int32_t b1,
|
|
|
68 int32_t b2)
|
|
|
69 {
|
|
|
70 bq->gain = gain;
|
|
|
71 bq->a1 = a1;
|
|
|
72 bq->a2 = a2;
|
|
|
73 bq->b1 = b1;
|
|
|
74 bq->b2 = b2;
|
|
|
75
|
|
|
76 bq->z1 = 0;
|
|
|
77 bq->z2 = 0;
|
|
|
78
|
|
|
79 #if FIRST_ORDER_NOISE_SHAPING
|
|
|
80 bq->residue = 0;
|
|
|
81 #elif SECOND_ORDER_NOISE_SHAPING
|
|
|
82 bq->residue1 = 0;
|
|
|
83 bq->residue2 = 0;
|
|
|
84 #endif
|
|
|
85 }
|
|
|
86 /*- End of function --------------------------------------------------------*/
|
|
|
87
|
|
|
88 static __inline__ int16_t biquad2(biquad2_state_t *bq, int16_t sample)
|
|
|
89 {
|
|
|
90 int32_t y;
|
|
|
91 int32_t z0;
|
|
|
92
|
|
|
93 z0 = sample*bq->gain + bq->z1*bq->a1 + bq->z2*bq->a2;
|
|
|
94 y = z0 + bq->z1*bq->b1 + bq->z2*bq->b2;
|
|
|
95
|
|
|
96 bq->z2 = bq->z1;
|
|
|
97 bq->z1 = z0 >> 15;
|
|
|
98 #if FIRST_ORDER_NOISE_SHAPING
|
|
|
99 y += bq->residue;
|
|
|
100 bq->residue = y & 0x7FFF;
|
|
|
101 #elif SECOND_ORDER_NOISE_SHAPING
|
|
|
102 y += (2*bq->residue1 - bq->residue2);
|
|
|
103 bq->residue2 = bq->residue1;
|
|
|
104 bq->residue1 = y & 0x7FFF;
|
|
|
105 #endif
|
|
|
106 y >>= 15;
|
|
|
107 return (int16_t) y;
|
|
|
108 }
|
|
|
109 /*- End of function --------------------------------------------------------*/
|
|
|
110
|
|
|
111 #ifdef __cplusplus
|
|
|
112 }
|
|
|
113 #endif
|
|
|
114
|
|
|
115 #endif
|
|
|
116 /*- End of file ------------------------------------------------------------*/
|
