Dmitry Kovalev
/
LGstaandart
forkd
Fork of LG2 by
mathDSP.c@129:406995a91322, 2016-04-12 (annotated)
- Committer:
- Kovalev_D
- Date:
- Tue Apr 12 11:10:49 2016 +0000
- Revision:
- 129:406995a91322
- Parent:
- 21:bc8c1cec3da6
?????? ? ?????? ??????????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
igor_v | 0:8ad47e2b6f00 | 1 | |
igor_v | 1:f2adcae3d304 | 2 | #include "Global.h" |
igor_v | 0:8ad47e2b6f00 | 3 | |
igor_v | 0:8ad47e2b6f00 | 4 | BAND_PASS_TYPE BandPassType; |
igor_v | 0:8ad47e2b6f00 | 5 | |
igor_v | 0:8ad47e2b6f00 | 6 | |
igor_v | 0:8ad47e2b6f00 | 7 | #define L_PLC 3 |
igor_v | 0:8ad47e2b6f00 | 8 | #define L_DUP 3 |
igor_v | 0:8ad47e2b6f00 | 9 | #define DIV_CONST 768 |
igor_v | 0:8ad47e2b6f00 | 10 | #define DIV_CONST2 384 |
igor_v | 0:8ad47e2b6f00 | 11 | #define BUF_SIZE 64 |
igor_v | 0:8ad47e2b6f00 | 12 | #define HALFINT 16384 |
igor_v | 0:8ad47e2b6f00 | 13 | |
igor_v | 0:8ad47e2b6f00 | 14 | int highPls = 0; |
igor_v | 0:8ad47e2b6f00 | 15 | int lowPls = 0; |
igor_v | 0:8ad47e2b6f00 | 16 | int BufInMovAverPls [67]; |
igor_v | 0:8ad47e2b6f00 | 17 | int BufInMovAverMns [67]; |
igor_v | 0:8ad47e2b6f00 | 18 | int BufInMovAverPls_2 [67]; |
igor_v | 0:8ad47e2b6f00 | 19 | int BufInMovAverMns_2 [67]; |
igor_v | 0:8ad47e2b6f00 | 20 | int hMovAver [67]; |
igor_v | 0:8ad47e2b6f00 | 21 | |
igor_v | 0:8ad47e2b6f00 | 22 | int aPLC[L_PLC], bPLC[L_PLC], aDUP[L_DUP], bDUP[L_DUP]; |
igor_v | 0:8ad47e2b6f00 | 23 | int aDUP_2[L_DUP] = {A0_HP, A1_HP, 0}, bDUP_2[L_DUP] = {0, B1_HP, 0}; |
igor_v | 0:8ad47e2b6f00 | 24 | |
igor_v | 0:8ad47e2b6f00 | 25 | unsigned int Vibro_Filter_Aperture; |
igor_v | 0:8ad47e2b6f00 | 26 | unsigned int Vibro_2_CountIn; |
igor_v | 0:8ad47e2b6f00 | 27 | /****************************************************************************** |
igor_v | 0:8ad47e2b6f00 | 28 | ** Function name: init_VibroReduce |
igor_v | 0:8ad47e2b6f00 | 29 | ** |
igor_v | 0:8ad47e2b6f00 | 30 | ** Descriptions: Prepare coefficiennts and delay |
igor_v | 0:8ad47e2b6f00 | 31 | ** line for vibro reduce filter |
igor_v | 0:8ad47e2b6f00 | 32 | ** |
igor_v | 0:8ad47e2b6f00 | 33 | ** parameters: None |
igor_v | 0:8ad47e2b6f00 | 34 | ** Returned value: None |
igor_v | 0:8ad47e2b6f00 | 35 | ** |
igor_v | 0:8ad47e2b6f00 | 36 | ******************************************************************************/ |
igor_v | 0:8ad47e2b6f00 | 37 | void init_VibroReduce() |
igor_v | 0:8ad47e2b6f00 | 38 | { |
Kovalev_D | 129:406995a91322 | 39 | /*unsigned int i; |
igor_v | 0:8ad47e2b6f00 | 40 | __int64 coeff; |
igor_v | 0:8ad47e2b6f00 | 41 | // ~22.9 ~17600 768 | 10000Hz = 10 KHz |*| ????? |/|768000| |
igor_v | 0:8ad47e2b6f00 | 42 | Vibro_Filter_Aperture = Device_blk.Str.VB_N/DIV_CONST; //e. real expression is DEVICE_SAMPLE_RATE_HZ*Device_blk.Str.VB_N/7680000 |
igor_v | 21:bc8c1cec3da6 | 43 | //8832 определяется выше 384 |
igor_v | 21:bc8c1cec3da6 | 44 | i = L_mult(Vibro_Filter_Aperture,DIV_CONST2); //e. i до ближайшего целого.//умножение 2 16-ти разрядных знаковых чисел и получение 32-х разрядного числа |
igor_v | 0:8ad47e2b6f00 | 45 | // ? |
igor_v | 21:bc8c1cec3da6 | 46 | if ((Device_blk.Str.VB_N - i)>DIV_CONST2) Vibro_Filter_Aperture++; //проверка на переполнение L_mult() |
igor_v | 0:8ad47e2b6f00 | 47 | |
igor_v | 21:bc8c1cec3da6 | 48 | coeff = 0x7FFFFFFF/Vibro_Filter_Aperture;//насыщение при переполнении |
igor_v | 0:8ad47e2b6f00 | 49 | |
igor_v | 21:bc8c1cec3da6 | 50 | for ( i=0; i < Vibro_Filter_Aperture; i++) /// не понятный цикл заполняющий три буфера заведенных выше... |
igor_v | 0:8ad47e2b6f00 | 51 | { |
igor_v | 0:8ad47e2b6f00 | 52 | BufInMovAverPls[i] = 0; |
igor_v | 0:8ad47e2b6f00 | 53 | BufInMovAverMns[i] = 0; |
igor_v | 0:8ad47e2b6f00 | 54 | hMovAver[i]= coeff; |
igor_v | 0:8ad47e2b6f00 | 55 | } |
igor_v | 0:8ad47e2b6f00 | 56 | Vibro_2_CountIn = MULT_7680_12500/Vibro_Filter_Aperture; |
Kovalev_D | 129:406995a91322 | 57 | Vibro_2_CountIn++; //какойто счетчик*/ |
igor_v | 0:8ad47e2b6f00 | 58 | } |
igor_v | 0:8ad47e2b6f00 | 59 | |
igor_v | 0:8ad47e2b6f00 | 60 | /****************************************************************************** |
igor_v | 0:8ad47e2b6f00 | 61 | ** Function name: VibroReduce |
igor_v | 0:8ad47e2b6f00 | 62 | ** |
igor_v | 0:8ad47e2b6f00 | 63 | ** Descriptions: Routine for reduce of vibro |
igor_v | 0:8ad47e2b6f00 | 64 | ** |
igor_v | 0:8ad47e2b6f00 | 65 | ** parameters: |
igor_v | 0:8ad47e2b6f00 | 66 | ** Returned value: Filtered magnitude |
igor_v | 0:8ad47e2b6f00 | 67 | ** |
igor_v | 0:8ad47e2b6f00 | 68 | ******************************************************************************/ |
igor_v | 0:8ad47e2b6f00 | 69 | int VibroReduce (int input) |
igor_v | 0:8ad47e2b6f00 | 70 | { |
igor_v | 0:8ad47e2b6f00 | 71 | static unsigned kIn = 0; |
igor_v | 0:8ad47e2b6f00 | 72 | unsigned s; |
igor_v | 0:8ad47e2b6f00 | 73 | __int64 outMns = 0; |
igor_v | 0:8ad47e2b6f00 | 74 | __int64 outPls = 0; |
igor_v | 0:8ad47e2b6f00 | 75 | BufInMovAverPls[kIn] = input; |
igor_v | 0:8ad47e2b6f00 | 76 | BufInMovAverMns[kIn] = -input; |
igor_v | 0:8ad47e2b6f00 | 77 | for (s=0; s<Vibro_Filter_Aperture; s++) |
igor_v | 0:8ad47e2b6f00 | 78 | { |
igor_v | 0:8ad47e2b6f00 | 79 | outPls += (__int64)hMovAver[s]*(__int64)BufInMovAverPls[s]; |
igor_v | 0:8ad47e2b6f00 | 80 | outMns += (__int64)hMovAver[s]*(__int64)BufInMovAverMns[s]; |
igor_v | 0:8ad47e2b6f00 | 81 | } |
igor_v | 0:8ad47e2b6f00 | 82 | highPls = (int)(outPls>>32); |
igor_v | 0:8ad47e2b6f00 | 83 | lowPls = (int)outPls; |
igor_v | 0:8ad47e2b6f00 | 84 | |
igor_v | 0:8ad47e2b6f00 | 85 | BufInMovAverPls_2[kIn] = (int)(outPls-(outMns>>32)); |
igor_v | 0:8ad47e2b6f00 | 86 | BufInMovAverMns_2[kIn] = -BufInMovAverPls_2[kIn]; |
igor_v | 0:8ad47e2b6f00 | 87 | outPls = 0; |
igor_v | 0:8ad47e2b6f00 | 88 | outMns = 0; |
igor_v | 0:8ad47e2b6f00 | 89 | for (s=0; s<Vibro_Filter_Aperture; s++) |
igor_v | 0:8ad47e2b6f00 | 90 | { |
igor_v | 0:8ad47e2b6f00 | 91 | outPls += (__int64)hMovAver[s]*(__int64)BufInMovAverPls_2[s]; |
igor_v | 0:8ad47e2b6f00 | 92 | outMns += (__int64)hMovAver[s]*(__int64)BufInMovAverMns_2[s]; |
igor_v | 0:8ad47e2b6f00 | 93 | } |
igor_v | 0:8ad47e2b6f00 | 94 | kIn++; |
igor_v | 0:8ad47e2b6f00 | 95 | if (kIn>(Vibro_Filter_Aperture-1)) kIn = 0; |
igor_v | 0:8ad47e2b6f00 | 96 | |
igor_v | 0:8ad47e2b6f00 | 97 | |
igor_v | 0:8ad47e2b6f00 | 98 | return (int)(outPls-(outMns>>32)); |
igor_v | 0:8ad47e2b6f00 | 99 | } |
igor_v | 0:8ad47e2b6f00 | 100 | /*#endif |
igor_v | 0:8ad47e2b6f00 | 101 | int VibroReduce (int input) |
igor_v | 0:8ad47e2b6f00 | 102 | { |
igor_v | 0:8ad47e2b6f00 | 103 | static unsigned k = 0; |
igor_v | 0:8ad47e2b6f00 | 104 | static __int64 out = 0, buf[67]; |
igor_v | 0:8ad47e2b6f00 | 105 | |
igor_v | 0:8ad47e2b6f00 | 106 | out -= buf[k]; |
igor_v | 0:8ad47e2b6f00 | 107 | buf[k] = (__int64)hMovAver[k] * (__int64)input; |
igor_v | 0:8ad47e2b6f00 | 108 | out += buf[k]; |
igor_v | 0:8ad47e2b6f00 | 109 | |
igor_v | 0:8ad47e2b6f00 | 110 | if (k++ > (Vibro_Filter_Aperture-1)) k = 0; |
igor_v | 0:8ad47e2b6f00 | 111 | |
igor_v | 0:8ad47e2b6f00 | 112 | // if ((int)out >> 16) |
igor_v | 0:8ad47e2b6f00 | 113 | // return (int)(out>>31)+1; |
igor_v | 0:8ad47e2b6f00 | 114 | // else |
igor_v | 0:8ad47e2b6f00 | 115 | return (int)(out>>31); |
igor_v | 0:8ad47e2b6f00 | 116 | }*/ |
igor_v | 0:8ad47e2b6f00 | 117 | /****************************************************************************** |
igor_v | 0:8ad47e2b6f00 | 118 | ** Function name: DUP_Filt |
igor_v | 0:8ad47e2b6f00 | 119 | ** |
igor_v | 0:8ad47e2b6f00 | 120 | ** Descriptions: Filter for dither frequency regulator |
igor_v | 0:8ad47e2b6f00 | 121 | ** |
igor_v | 0:8ad47e2b6f00 | 122 | ** parameters: |
igor_v | 0:8ad47e2b6f00 | 123 | ** Returned value: Filtered magnitude |
igor_v | 0:8ad47e2b6f00 | 124 | ** |
igor_v | 0:8ad47e2b6f00 | 125 | ******************************************************************************/ |
igor_v | 0:8ad47e2b6f00 | 126 | int DUP_Filt (int input) |
igor_v | 0:8ad47e2b6f00 | 127 | { |
igor_v | 0:8ad47e2b6f00 | 128 | static unsigned int kIn = 0; |
igor_v | 0:8ad47e2b6f00 | 129 | int ind; |
igor_v | 0:8ad47e2b6f00 | 130 | __int64 temp = 0; |
igor_v | 0:8ad47e2b6f00 | 131 | unsigned int i; |
igor_v | 0:8ad47e2b6f00 | 132 | static int BufInDUP_1 [L_DUP] = {0,0,0}; |
igor_v | 0:8ad47e2b6f00 | 133 | static int BufInDUP_2 [L_DUP] = {0,0,0}; |
igor_v | 0:8ad47e2b6f00 | 134 | //static __int64 BufOutDUP[L_DUP] = {0,0,0}; |
igor_v | 0:8ad47e2b6f00 | 135 | |
igor_v | 0:8ad47e2b6f00 | 136 | if (kIn>(L_DUP-1)) kIn = 0; |
igor_v | 0:8ad47e2b6f00 | 137 | |
igor_v | 0:8ad47e2b6f00 | 138 | BufInDUP_1[kIn] = input; |
igor_v | 0:8ad47e2b6f00 | 139 | ind = kIn; |
igor_v | 0:8ad47e2b6f00 | 140 | BufInDUP_2[kIn] = 0; |
igor_v | 0:8ad47e2b6f00 | 141 | for (i=0; i<L_DUP; i++) |
igor_v | 0:8ad47e2b6f00 | 142 | { |
igor_v | 0:8ad47e2b6f00 | 143 | temp += aDUP[i]*BufInDUP_1[ind]; |
igor_v | 0:8ad47e2b6f00 | 144 | temp += bDUP[i]*BufInDUP_2[ind]; |
igor_v | 0:8ad47e2b6f00 | 145 | if ((--ind) < 0) ind = L_DUP-1; |
igor_v | 0:8ad47e2b6f00 | 146 | } |
igor_v | 0:8ad47e2b6f00 | 147 | BufInDUP_2[kIn] =(int)(temp>>14); //e.-----16----------- take into account that filter coefficients are divided on 2 |
igor_v | 0:8ad47e2b6f00 | 148 | |
igor_v | 0:8ad47e2b6f00 | 149 | //-----------------------------2 section (HF-filtration)---------------------------------- |
igor_v | 0:8ad47e2b6f00 | 150 | /* BufOutDUP[kIn] = 0; |
igor_v | 0:8ad47e2b6f00 | 151 | for (i=0; i<L_DUP; i++) |
igor_v | 0:8ad47e2b6f00 | 152 | { |
igor_v | 0:8ad47e2b6f00 | 153 | BufOutDUP[kIn] += (__int64)aDUP_2[i]*BufInDUP_2[ind] + (__int64)bDUP_2[i]*BufOutDUP[ind]; |
igor_v | 0:8ad47e2b6f00 | 154 | if ((--ind) < 0) ind = L_DUP-1; |
igor_v | 0:8ad47e2b6f00 | 155 | } |
igor_v | 0:8ad47e2b6f00 | 156 | BufOutDUP[kIn] >>= 30; */ |
igor_v | 0:8ad47e2b6f00 | 157 | |
igor_v | 0:8ad47e2b6f00 | 158 | return (BufInDUP_2[kIn++]); |
igor_v | 0:8ad47e2b6f00 | 159 | } |
igor_v | 0:8ad47e2b6f00 | 160 | |
igor_v | 0:8ad47e2b6f00 | 161 | |
igor_v | 0:8ad47e2b6f00 | 162 | |
igor_v | 0:8ad47e2b6f00 | 163 | |
igor_v | 0:8ad47e2b6f00 | 164 | //-------------------------PLC phase detector---------------------------------- |
igor_v | 0:8ad47e2b6f00 | 165 | int PLC_PhaseDetFilt (int input) |
igor_v | 0:8ad47e2b6f00 | 166 | { |
igor_v | 0:8ad47e2b6f00 | 167 | static unsigned kIn = 0; |
igor_v | 0:8ad47e2b6f00 | 168 | int ind; |
igor_v | 0:8ad47e2b6f00 | 169 | __int64 temp = 0; |
igor_v | 0:8ad47e2b6f00 | 170 | unsigned i; |
igor_v | 0:8ad47e2b6f00 | 171 | static int BufInPLC_1 [L_PLC] = {0,0,0}; |
igor_v | 0:8ad47e2b6f00 | 172 | static int BufInPLC_2 [L_PLC] = {0,0,0}; |
igor_v | 0:8ad47e2b6f00 | 173 | static int BufOutPLC [L_PLC] = {0,0,0}; |
igor_v | 0:8ad47e2b6f00 | 174 | |
igor_v | 0:8ad47e2b6f00 | 175 | if (kIn>(L_PLC-1)) kIn = 0; |
igor_v | 0:8ad47e2b6f00 | 176 | |
igor_v | 0:8ad47e2b6f00 | 177 | BufInPLC_1[kIn] = input; |
igor_v | 0:8ad47e2b6f00 | 178 | ind = kIn; |
igor_v | 0:8ad47e2b6f00 | 179 | // BufInPLC_2[kIn] = 0; |
igor_v | 0:8ad47e2b6f00 | 180 | |
igor_v | 0:8ad47e2b6f00 | 181 | for (i=0; i<L_PLC; i++) |
igor_v | 0:8ad47e2b6f00 | 182 | { |
igor_v | 0:8ad47e2b6f00 | 183 | temp += aPLC[i]*BufInPLC_1[ind]; |
igor_v | 0:8ad47e2b6f00 | 184 | temp += bPLC[i]*BufInPLC_2[ind]; |
igor_v | 0:8ad47e2b6f00 | 185 | if ((--ind) < 0) ind = L_PLC-1; |
igor_v | 0:8ad47e2b6f00 | 186 | } |
igor_v | 0:8ad47e2b6f00 | 187 | BufInPLC_2[kIn] =(int)(temp>>14); |
igor_v | 0:8ad47e2b6f00 | 188 | //-----------------------------2 section---------------------------------------- |
igor_v | 0:8ad47e2b6f00 | 189 | // BufOutPLC[kIn] = 0; |
igor_v | 0:8ad47e2b6f00 | 190 | temp = 0; |
igor_v | 0:8ad47e2b6f00 | 191 | for (i=0; i<L_PLC; i++) |
igor_v | 0:8ad47e2b6f00 | 192 | { |
igor_v | 0:8ad47e2b6f00 | 193 | temp += aPLC[i]*BufInPLC_2[ind]; |
igor_v | 0:8ad47e2b6f00 | 194 | temp += bPLC[i]*BufOutPLC[ind]; |
igor_v | 0:8ad47e2b6f00 | 195 | if ((--ind) < 0) ind = L_PLC-1; |
igor_v | 0:8ad47e2b6f00 | 196 | } |
igor_v | 0:8ad47e2b6f00 | 197 | BufOutPLC[kIn] =(int)(temp>>14); |
igor_v | 0:8ad47e2b6f00 | 198 | |
igor_v | 0:8ad47e2b6f00 | 199 | return (BufOutPLC[kIn++]); |
igor_v | 0:8ad47e2b6f00 | 200 | } |
igor_v | 0:8ad47e2b6f00 | 201 | /****************************************************************************** |
igor_v | 0:8ad47e2b6f00 | 202 | ** Function name: init_BandPass |
igor_v | 0:8ad47e2b6f00 | 203 | ** |
igor_v | 0:8ad47e2b6f00 | 204 | ** Descriptions: Initialization of IIR filters for PLC and DUP signals |
igor_v | 0:8ad47e2b6f00 | 205 | ** |
igor_v | 0:8ad47e2b6f00 | 206 | ** |
igor_v | 0:8ad47e2b6f00 | 207 | ** parameters: None |
igor_v | 0:8ad47e2b6f00 | 208 | ** Returned value: None |
igor_v | 0:8ad47e2b6f00 | 209 | ** |
igor_v | 0:8ad47e2b6f00 | 210 | ******************************************************************************/ |
igor_v | 0:8ad47e2b6f00 | 211 | void init_BandPass(double CenterFreq, double BandWidth, BAND_PASS_TYPE FiltType) |
igor_v | 0:8ad47e2b6f00 | 212 | { |
igor_v | 0:8ad47e2b6f00 | 213 | double K, R, Cos_x_2, R_x_R; |
igor_v | 0:8ad47e2b6f00 | 214 | |
igor_v | 0:8ad47e2b6f00 | 215 | R = 1.0 - 3.0 * BandWidth; |
igor_v | 0:8ad47e2b6f00 | 216 | R_x_R = R * R; |
igor_v | 0:8ad47e2b6f00 | 217 | Cos_x_2 = cos(2.0 * PI * CenterFreq) * 2.0; |
igor_v | 0:8ad47e2b6f00 | 218 | K = (1.0 - R * Cos_x_2 + R_x_R)/(2.0 - Cos_x_2); |
igor_v | 0:8ad47e2b6f00 | 219 | switch (FiltType) |
igor_v | 0:8ad47e2b6f00 | 220 | { |
igor_v | 0:8ad47e2b6f00 | 221 | case PLC: |
igor_v | 0:8ad47e2b6f00 | 222 | aPLC[0] = (int)((1.0 - K)*HALFINT); |
igor_v | 0:8ad47e2b6f00 | 223 | aPLC[1] = (int)(((K - R) * Cos_x_2)*HALFINT); |
igor_v | 0:8ad47e2b6f00 | 224 | aPLC[2] = (int)((R_x_R - K)*HALFINT); |
igor_v | 0:8ad47e2b6f00 | 225 | bPLC[0] = 0; |
igor_v | 0:8ad47e2b6f00 | 226 | bPLC[1] = (int)((R * Cos_x_2)*HALFINT); |
igor_v | 0:8ad47e2b6f00 | 227 | bPLC[2] = (int)((- R_x_R)*HALFINT); |
igor_v | 0:8ad47e2b6f00 | 228 | break; |
igor_v | 0:8ad47e2b6f00 | 229 | case DUP: |
igor_v | 0:8ad47e2b6f00 | 230 | aDUP[0] = (int)((1.0 - K)*HALFINT); |
igor_v | 0:8ad47e2b6f00 | 231 | aDUP[1] = (int)(((K - R) * Cos_x_2)*HALFINT); |
igor_v | 0:8ad47e2b6f00 | 232 | aDUP[2] = (int)((R_x_R - K)*HALFINT); |
igor_v | 0:8ad47e2b6f00 | 233 | bDUP[0] = 0; |
igor_v | 0:8ad47e2b6f00 | 234 | bDUP[1] = (int)((R * Cos_x_2)*HALFINT); |
igor_v | 0:8ad47e2b6f00 | 235 | bDUP[2] = (int)((- R_x_R)*HALFINT); |
igor_v | 0:8ad47e2b6f00 | 236 | break; |
igor_v | 0:8ad47e2b6f00 | 237 | } |
igor_v | 0:8ad47e2b6f00 | 238 | } |
igor_v | 0:8ad47e2b6f00 | 239 | /****************************************************************************** |
igor_v | 0:8ad47e2b6f00 | 240 | ** Function name: HFO_MovAverFilt |
igor_v | 0:8ad47e2b6f00 | 241 | ** |
igor_v | 0:8ad47e2b6f00 | 242 | ** Descriptions: Moving average filter for ammplitude signal filtration |
igor_v | 0:8ad47e2b6f00 | 243 | ** |
igor_v | 0:8ad47e2b6f00 | 244 | ** |
igor_v | 0:8ad47e2b6f00 | 245 | ** parameters: None |
igor_v | 0:8ad47e2b6f00 | 246 | ** Returned value: None |
igor_v | 0:8ad47e2b6f00 | 247 | ** |
igor_v | 0:8ad47e2b6f00 | 248 | ******************************************************************************/ |
igor_v | 0:8ad47e2b6f00 | 249 | int HFO_MovAverFilt (int Input) |
igor_v | 0:8ad47e2b6f00 | 250 | { |
igor_v | 0:8ad47e2b6f00 | 251 | static __int64 smooth_HF = 0; |
igor_v | 0:8ad47e2b6f00 | 252 | static int buffer_HF[BUF_SIZE]; |
igor_v | 0:8ad47e2b6f00 | 253 | static unsigned i_HF = 0; |
igor_v | 0:8ad47e2b6f00 | 254 | |
igor_v | 0:8ad47e2b6f00 | 255 | smooth_HF -= buffer_HF[i_HF]; |
igor_v | 0:8ad47e2b6f00 | 256 | buffer_HF[i_HF] = Input; |
igor_v | 0:8ad47e2b6f00 | 257 | smooth_HF += Input; |
igor_v | 0:8ad47e2b6f00 | 258 | |
igor_v | 0:8ad47e2b6f00 | 259 | i_HF++; |
igor_v | 0:8ad47e2b6f00 | 260 | i_HF &= (BUF_SIZE-1); |
igor_v | 0:8ad47e2b6f00 | 261 | |
igor_v | 0:8ad47e2b6f00 | 262 | return (smooth_HF>>22); //shift on additional 6 bits for smoothing 2^6 = 64 |
igor_v | 0:8ad47e2b6f00 | 263 | } |