Diff: SPI.c

Revision:

228:a8195e1b1123

Parent:

227:2774b56bfab0

Child:

229:f359509f0c70

Child:

230:8f7eeb47ca51

--- 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;