spandsp 0.0.6
|
00001 /* 00002 * SpanDSP - a series of DSP components for telephony 00003 * 00004 * biquad.h - General telephony bi-quad section routines (currently this just 00005 * handles canonic/type 2 form) 00006 * 00007 * Written by Steve Underwood <steveu@coppice.org> 00008 * 00009 * Copyright (C) 2001 Steve Underwood 00010 * 00011 * All rights reserved. 00012 * 00013 * This program is free software; you can redistribute it and/or modify 00014 * it under the terms of the GNU Lesser General Public License version 2.1, 00015 * as published by the Free Software Foundation. 00016 * 00017 * This program is distributed in the hope that it will be useful, 00018 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00019 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00020 * GNU Lesser General Public License for more details. 00021 * 00022 * You should have received a copy of the GNU Lesser General Public 00023 * License along with this program; if not, write to the Free Software 00024 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. 00025 */ 00026 00027 /*! \page biquad_page Bi-quadratic filter sections 00028 \section biquad_page_sec_1 What does it do? 00029 ???. 00030 00031 \section biquad_page_sec_2 How does it work? 00032 ???. 00033 */ 00034 00035 #if !defined(_SPANDSP_BIQUAD_H_) 00036 #define _SPANDSP_BIQUAD_H_ 00037 00038 typedef struct 00039 { 00040 int32_t gain; 00041 int32_t a1; 00042 int32_t a2; 00043 int32_t b1; 00044 int32_t b2; 00045 00046 int32_t z1; 00047 int32_t z2; 00048 00049 #if FIRST_ORDER_NOISE_SHAPING 00050 int32_t residue; 00051 #elif SECOND_ORDER_NOISE_SHAPING 00052 int32_t residue1; 00053 int32_t residue2; 00054 #endif 00055 } biquad2_state_t; 00056 00057 #if defined(__cplusplus) 00058 extern "C" 00059 { 00060 #endif 00061 00062 static __inline__ void biquad2_init(biquad2_state_t *bq, 00063 int32_t gain, 00064 int32_t a1, 00065 int32_t a2, 00066 int32_t b1, 00067 int32_t b2) 00068 { 00069 bq->gain = gain; 00070 bq->a1 = a1; 00071 bq->a2 = a2; 00072 bq->b1 = b1; 00073 bq->b2 = b2; 00074 00075 bq->z1 = 0; 00076 bq->z2 = 0; 00077 00078 #if FIRST_ORDER_NOISE_SHAPING 00079 bq->residue = 0; 00080 #elif SECOND_ORDER_NOISE_SHAPING 00081 bq->residue1 = 0; 00082 bq->residue2 = 0; 00083 #endif 00084 } 00085 /*- End of function --------------------------------------------------------*/ 00086 00087 static __inline__ int16_t biquad2(biquad2_state_t *bq, int16_t sample) 00088 { 00089 int32_t y; 00090 int32_t z0; 00091 00092 z0 = sample*bq->gain + bq->z1*bq->a1 + bq->z2*bq->a2; 00093 y = z0 + bq->z1*bq->b1 + bq->z2*bq->b2; 00094 00095 bq->z2 = bq->z1; 00096 bq->z1 = z0 >> 15; 00097 #if FIRST_ORDER_NOISE_SHAPING 00098 y += bq->residue; 00099 bq->residue = y & 0x7FFF; 00100 #elif SECOND_ORDER_NOISE_SHAPING 00101 y += (2*bq->residue1 - bq->residue2); 00102 bq->residue2 = bq->residue1; 00103 bq->residue1 = y & 0x7FFF; 00104 #endif 00105 y >>= 15; 00106 return (int16_t) y; 00107 } 00108 /*- End of function --------------------------------------------------------*/ 00109 00110 #if defined(__cplusplus) 00111 } 00112 #endif 00113 00114 #endif 00115 /*- End of file ------------------------------------------------------------*/