![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
forkd
Fork of LGstaandart by
Diff: SPI.c
- Revision:
- 228:a8195e1b1123
- Parent:
- 227:2774b56bfab0
- Child:
- 229:f359509f0c70
- Child:
- 230:8f7eeb47ca51
diff -r 2774b56bfab0 -r a8195e1b1123 SPI.c --- a/SPI.c Fri Jan 05 08:14:38 2018 +0000 +++ b/SPI.c Tue Jan 09 13:19:26 2018 +0000 @@ -1,4 +1,6 @@ #include "Global.h" +#define NCoef 2 +#define DCgain 8192 struct SPI Spi; //unsigned int Temp_AMP; unsigned int Temp_AMP64P; @@ -53,6 +55,12 @@ //unsigned int Buff_AMP [256]; //unsigned int Buff_AMP64P [256]; +int HFO_Buff_in [10]; +int HFO_Buff_out [10]; +int HFOAmpSUM; +int HFOAmp; +int HFOAmpfilt; +int HFOAmpfiltC; unsigned int TypeMod=0; unsigned int ModArraySin [64] = {50,55,59,64,68,73,77,81,85,88,91,94,96,98,99,99,100,99,99,98,96,94,91,88,85,81,77,73,68,64,59,55,50,45,41,36,32,27,23,19,16,12,9,7,4,2,1,1,0,1,1,2,4,7,9,12,16,19,23,27,32,36,41,45}; unsigned int ModArraySin32 [32] = {50,59,68,77,85,91,96,99,100,99,96,91,85,77,68,59,50,41,32,23,16,9,4,1,0,1,4,9,16,23,32,41}; @@ -181,9 +189,12 @@ /* HFO_AVR+=(BuffADC_10v[CountV64]>>4); HFO_AVR-=(BuffADC_10v[CountV64 - 32]>>4); */ - HFOdelta=(int)((Gyro.HFO_ref)-(BuffADC_128Point[CountV64]>>7)); + HFOdelta=(int)((Gyro.HFO_ref)-(BuffADC_128Point[CountV64]>>5)); HFOSumDelta+=HFOdelta; + + + HFOregul=HFOSumDelta/(int)(Gyro.HFO_Gain); HFOSumDelta-=Gyro.HFO_Gain*HFOregul; @@ -289,6 +300,7 @@ //unsigned int ADC5Dif; ADC5New = LPC_SSP0->DR;// Чтение АЦП + HFOAmpSUM+=0xffff-ADC5New; //Spi.ADC5_Accum += LPC_SSP0->DR; Spi.ADC4_Accum += LPC_SSP0->DR; Spi.ADC3_Accum += LPC_SSP0->DR; @@ -322,23 +334,87 @@ BuffADC_1Point_64[CountV31]=(0x7fff-ADC5New)&0x7fff; ADC_128Point += BuffADC_1Point[CountV255]; - ADC_128Point -= BuffADC_1Point[(CountV255 - 128) & 0xff]; // заполнение буфера накопленых приращений за 64 тактов + ADC_128Point -= BuffADC_1Point[(CountV255 - 32) & 0xff]; // заполнение буфера накопленых приращений за 64 тактов BuffADC_128Point[CountV64] = ADC_128Point; + + if(Time1Hz>8) + { + if(CountV31==31) + { + HFOAmp=HFOAmpSUM>>5; + HFOAmpSUM=0; + + HFOAmpfilt=BUTTER(25); + //HFOAmpfiltC = iirr(HFOAmp); + + sprintf((Time),"%d %d %d \r\n",0xffff-ADC5New, HFOAmp, HFOAmpfilt); + WriteCon(Time); + } + +} - // Buff_Restored_Mod[CountV31] =(int)(BuffADC_1Point_64[CountV31] - ((BuffADC_128Point[CountV64])>>7)); - // Buff_Restored_Mod[CountV64] =(int)(BuffADC_32PointD[CountV64]*2 - BuffADC_64Point[CountV64]); + // Buff_Restored_Mod[CountV64] =(int)(BuffADC_32PointD[CountV64]*2 - BuffADC_64Point[CountV64]); count10v++; count10HFO++; BuffADC_10v[CountV64] += BuffADC_1Point[CountV255]; + // BuffADC_10HFO[CountV64] += BuffADC_1Point[CountV255]; ModFilt(); } } + + + +int BUTTER(int NewSample) { + static unsigned int SHIFT=1; + int ACoef[NCoef+1] = { + 8195, + 16390, + 8195 + }; + + int BCoef[NCoef+1] = { + 16384, + -32404, + 16024 + }; + + static int y1[NCoef+1]; //output samples + //Warning!!!!!! This variable should be signed (input sample width + Coefs width + 2 )-bit width to avoid saturation. + + static int x1[NCoef+1]; //input samples + int n; + + //shift the old samples + for(n=NCoef; n>0; n--) { + x1[n] = x1[n-1]; + y1[n] = y1[n-1]; + } + + //Calculate the new output + x1[0] = NewSample; + y1[0] = ACoef[0]>>SHIFT * x1[0]; + for(n=1; n<=NCoef; n++) + y1[0] += ACoef[n]>>SHIFT * x1[n] - BCoef[n]>>SHIFT * y1[n]; + + y1[0] =(int)(y1[0]/ BCoef[0]>>SHIFT); + + // return (int)(y1[0] / DCgain>>SHIFT); + return (int)(y1[0]); +} + + + + + + + + void HFORegul(void) { static unsigned int countHFO;