00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
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
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
00109
00110 #if defined(__cplusplus)
00111 }
00112 #endif
00113
00114 #endif
00115