libspandsp 0.0.4
|
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 * $Id: biquad.h,v 1.14 2008/04/17 14:26:59 steveu Exp $ 00027 */ 00028 00029 /*! \page biquad_page Bi-quadratic filter sections 00030 \section biquad_page_sec_1 What does it do? 00031 ???. 00032 00033 \section biquad_page_sec_2 How does it work? 00034 ???. 00035 */ 00036 00037 #if !defined(_SPANDSP_BIQUAD_H_) 00038 #define _SPANDSP_BIQUAD_H_ 00039 00040 typedef struct 00041 { 00042 int32_t gain; 00043 int32_t a1; 00044 int32_t a2; 00045 int32_t b1; 00046 int32_t b2; 00047 00048 int32_t z1; 00049 int32_t z2; 00050 00051 #if FIRST_ORDER_NOISE_SHAPING 00052 int32_t residue; 00053 #elif SECOND_ORDER_NOISE_SHAPING 00054 int32_t residue1; 00055 int32_t residue2; 00056 #endif 00057 } biquad2_state_t; 00058 00059 #if defined(__cplusplus) 00060 extern "C" 00061 { 00062 #endif 00063 00064 static __inline__ void biquad2_init(biquad2_state_t *bq, 00065 int32_t gain, 00066 int32_t a1, 00067 int32_t a2, 00068 int32_t b1, 00069 int32_t b2) 00070 { 00071 bq->gain = gain; 00072 bq->a1 = a1; 00073 bq->a2 = a2; 00074 bq->b1 = b1; 00075 bq->b2 = b2; 00076 00077 bq->z1 = 0; 00078 bq->z2 = 0; 00079 00080 #if FIRST_ORDER_NOISE_SHAPING 00081 bq->residue = 0; 00082 #elif SECOND_ORDER_NOISE_SHAPING 00083 bq->residue1 = 0; 00084 bq->residue2 = 0; 00085 #endif 00086 } 00087 /*- End of function --------------------------------------------------------*/ 00088 00089 static __inline__ int16_t biquad2(biquad2_state_t *bq, int16_t sample) 00090 { 00091 int32_t y; 00092 int32_t z0; 00093 00094 z0 = sample*bq->gain + bq->z1*bq->a1 + bq->z2*bq->a2; 00095 y = z0 + bq->z1*bq->b1 + bq->z2*bq->b2; 00096 00097 bq->z2 = bq->z1; 00098 bq->z1 = z0 >> 15; 00099 #if FIRST_ORDER_NOISE_SHAPING 00100 y += bq->residue; 00101 bq->residue = y & 0x7FFF; 00102 #elif SECOND_ORDER_NOISE_SHAPING 00103 y += (2*bq->residue1 - bq->residue2); 00104 bq->residue2 = bq->residue1; 00105 bq->residue1 = y & 0x7FFF; 00106 #endif 00107 y >>= 15; 00108 return (int16_t) y; 00109 } 00110 /*- End of function --------------------------------------------------------*/ 00111 00112 #if defined(__cplusplus) 00113 } 00114 #endif 00115 00116 #endif 00117 /*- End of file ------------------------------------------------------------*/