igor Apu / Mbed 2 deprecated LG Featured

123

Dependencies:   mbed

Fork of LG by igor Apu

host/Source/App/mathDSP.c@22:12e6183f04d4, 2016-02-03 (annotated)

Committer:
Kovalev_D
Date:
Wed Feb 03 10:44:42 2016 +0300
Revision:
22:12e6183f04d4
[thyz

Who changed what in which revision?

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