Dmitry Kovalev / Mbed 2 deprecated LG

n

Dependencies:   mbed

Fork of LG by igor Apu

mathDSP.c@1:f2adcae3d304, 2016-01-30 (annotated)

Committer:
igor_v
Date:
Sat Jan 30 13:53:19 2016 +0000
Revision:
1:f2adcae3d304
Parent:
0:8ad47e2b6f00
Child:
21:bc8c1cec3da6
123

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