Dmitry Kovalev / Mbed 2 deprecated LG

n

Dependencies:   mbed

Fork of LG by igor Apu

mathDSP.c@28:1c9acd3b224d, 2016-02-04 (annotated)

Committer:
Kovalev_D
Date:
Thu Feb 04 10:21:57 2016 +0000
Revision:
28:1c9acd3b224d
Parent:
21:bc8c1cec3da6 
13:21

Who changed what in which revision?

UserRevisionLine numberNew 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 {
igor_v 0:8ad47e2b6f00 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;
igor_v 21:bc8c1cec3da6 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 }