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
00036
00037
00038 #if !defined(_BIQUAD_H_)
00039 #define _BIQUAD_H_
00040
00041 typedef struct
00042 {
00043 int32_t gain;
00044 int32_t a1;
00045 int32_t a2;
00046 int32_t b1;
00047 int32_t b2;
00048
00049 int32_t z1;
00050 int32_t z2;
00051
00052 #if FIRST_ORDER_NOISE_SHAPING
00053 int32_t residue;
00054 #elif SECOND_ORDER_NOISE_SHAPING
00055 int32_t residue1;
00056 int32_t residue2;
00057 #endif
00058 } biquad2_state_t;
00059
00060 #ifdef __cplusplus
00061 extern "C" {
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
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 y;
00109 }
00110
00111
00112 #ifdef __cplusplus
00113 }
00114 #endif
00115
00116 #endif
00117