biquad.h
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 #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
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
00111
00112 #if defined(__cplusplus)
00113 }
00114 #endif
00115
00116 #endif
00117