Dmitry Kovalev
forkd
Fork of
LGstaandart
by 
Dmitry Kovalev
Diff: SPI.c
- Revision:
- 231:079835d508ef
- Parent:
- 230:8f7eeb47ca51
- Child:
- 232:130a2b5003e6
--- a/SPI.c Tue Jan 09 13:25:45 2018 +0000
+++ b/SPI.c Thu Jan 25 13:59:43 2018 +0000
@@ -2,319 +2,190 @@
#define NCoef 2
#define DCgain 8192
struct SPI Spi;
-//unsigned int Temp_AMP;
-unsigned int Temp_AMP64P;
-int ttt=1;
-unsigned int Count_AMP, ADD_AMP, Cur_Amp;
-int Znak_Amp;
-int AD_Regul = 0;
-int temp9,tempADC5;
-int AD_MAX=0;
-unsigned int SPIlog;
-int k=0,l=0,r=0,n=0;//счетчики для регулировки периметра
-int flagmod=0,Bdelta;
-int start=10;
-int dispersion=0,side=1,tempstrafe=15000;
-int ADC_5_T;
-int PLC_ERR_DAC,PLC_ERR_DAC_F;
-unsigned int TempA;
-unsigned int TempTermLM;
-unsigned int conuntPLS;
-int ADC5Old,ADCDIF=0;
-/*int DACModReg;*/
-int SinPls=0,SinMns=0;
-int TSinPls=0,TSinMns=0;
- int timer=750;
-int sum=0;
- unsigned int testcount=0,faza, fazaFlag=0;
-unsigned int ADC5New;
-unsigned int Buff_ADC_1 [32];
-unsigned int Buff_ADC_2 [32];
-unsigned int Buff_ADC_3 [32];
-unsigned int Buff_ADC_4 [32];
-int Buff_OUT1 [64];
-int Buff_OUT [64];
-int RegulADC,DeltaRegul,tempDeltaRegul;
-int count10HFO=0, count10v=0, AmpWorms,MinWorms,MaxWorms,AMPSUM,AMPSUMP,AMPSUMM,AMPSUMPout,AMPSUMMout,AMPSUMout,SumDelta,PLC_EROR,PLC_Flag,HFO_Flag,HFOdelta,HFOregul,HFOSumDelta;
-
-
-//int BuffADC_32Point [64];
-
-//unsigned int Buff_ADC_5 [255];
-
-//unsigned int PulseADC_16Point;
-//unsigned int PulseADC_32Point;
-//unsigned int PulseADC_64Point;
-//unsigned int PulseADC_32PointD;
+unsigned int TempTermLM;
+unsigned int ADC5New;
+int AMPSUM,SumDelta,HFOSumDelta,DeltaRegul,tempDeltaRegul;
+int count10v=0,PLC_EROR, PLC_Flag,HFO_Flag,HFOdelta,HFOregul;
+int PlcWormMN, PlcWormPL, PlcWormDelta, PlcWormMNout, PlcWormPLout, PlcWormDeltaout;
-//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};
-unsigned int ModArrayTriangle [64];
-unsigned int ModArraySaw [64];
-unsigned int Mod=0;
-int znak;
-unsigned int HFO_AVR;
-void InitMOD(void)
-{
- for (int i = 0; i < 64; i++ )
- {
- if(i<32) { ModArrayTriangle[i]=Mod; Mod=100;}
- else { ModArrayTriangle[i]=Mod; Mod=0;}
- }
-
- for (int i = 0; i < 16; i++ )
- {
- ModArraySaw[i]=Mod;
- Mod+=1;
- }
-}
+int WormFilt [64];
+int WormFilt2 [64];
+int Delt_T4 [40];
+int Delt_T5 [40];
+//int AVR_PLC_ERR [64];
+//int ERRPLC;
+int tempPlcErr;
+
+
+
+
+
+
+
+
+
+
+
void Modulator(void)
{
- switch(TypeMod)
- {
-
- // case 0: LPC_DAC->DACR = (ModArraySin [(CountV64+Gyro.PLC_Phase)&0x3f])*Gyro.ModAmp + Gyro.ShiftMod-32000; break;
- case 0: LPC_DAC->DACR = (ModArraySin [(CountV64+Gyro.PLC_Phase)&0x3f])*Gyro.ModAmp + Gyro.ShiftMod-32000; break;
-
- case 1: LPC_DAC->DACR = (ModArraySaw [CountV64]*Gyro.ModAmp); break;
- case 2: LPC_DAC->DACR = (ModArrayTriangle [(CountV64-2)&0x3f]*Gyro.ModAmp); break;
- case 3: LPC_DAC->DACR = (ModArrayTriangle [(CountV64-2)&0x3f]*Gyro.ModAmp); break;
-
- //case 4: LPC_DAC->DACR = ((ModArraySin32 [(CountV31)&0x1f]+150)*Gyro.ModAmp); break;ShiftMod
- case 4: LPC_DAC->DACR = ((ModArraySin32 [(CountV31+8)&0x1f])*Gyro.ModAmp + Gyro.ShiftMod-32000); break;
- case 5: break;
- }
-}
-
-void PLCRegul250(void)
-{
- unsigned int temp;
- static int CountFaza;
- temp = MODCount;
-
- /*for (CountFaza = 0; CountFaza <16; CountFaza++) SinPls+= (Buff_Restored_Mod[ (CountFaza - Gyro.PLC_Phase) & 0x3f]);
- for (CountFaza = 16; CountFaza <32; CountFaza++) SinMns-= (Buff_Restored_Mod[ (CountFaza - Gyro.PLC_Phase) & 0x3f]);
- for (CountFaza = 32; CountFaza <48; CountFaza++) SinPls+= (Buff_Restored_Mod[ (CountFaza - Gyro.PLC_Phase) & 0x3f]);
- for (CountFaza = 48; CountFaza <64; CountFaza++) SinMns-= (Buff_Restored_Mod[ (CountFaza - Gyro.PLC_Phase) & 0x3f]);*/
-
- /* for (CountFaza = 0; CountFaza <32; CountFaza++ ) SinPls+= BuffADC_64Point[ (CountFaza - Gyro.PLC_Phase) & 0x3f];
- for (CountFaza = 32; CountFaza <64; CountFaza++) SinMns+= BuffADC_64Point[ (CountFaza - Gyro.PLC_Phase) & 0x3f];*/
- Gyro.PLC_Eror = SinMns-SinPls;
- /* sprintf((Time)," %d %d %d %d %d\r\n",Spi.ADC5, Spi.DAC_B, SinPls, SinMns, Gyro.PLC_Eror);
- WriteCon(Time);*/
- if(Gyro.RgConA&0x8)
- {
- /* if(Gyro.PLC_Eror>0) {Spi.DAC_B+=1 * Gyro.PLC_Gain;}
- else {Spi.DAC_B-=1 * Gyro.PLC_Gain;}*/
- }
- /* if(Gyro.PLC_Eror>0) {Gyro.PLC_Eror_count++;}
- else {Gyro.PLC_Eror_count--;}*/
-
- if ( Spi.DAC_B < 10300 ) Spi.DAC_B = 32000; //проверка на переваливание за границу.
- else if ( Spi.DAC_B > 57000 ) Spi.DAC_B = 32000;
-
- SinPls=0;
- SinMns=0;
+ LPC_DAC->DACR = (ModArraySin [(CountV64+Gyro.PLC_Phase)&0x3f])*Gyro.ModAmp + Gyro.ShiftMod-32000;
}
void ADS_Acum(void)
{
Spi.ADC_NewData = 0;
- // Gyro.Termo = (unsigned int)(((Spi.ADC1>>1) + Gyro.Tmp_OffsetT4) * Gyro.Tmp_scaleT4);
+ /*Gyro.IN1_Accum += Spi.ADC2;
+ Gyro.IN2_Accum += Spi.ADC3;*/
Gyro.Termo = (unsigned int)((((Spi.ADC1>>1)*100)*0.0122)-27300);
- //Gyro.Termo =
- Gyro.IN1_Accum += Spi.ADC2;
- Gyro.IN2_Accum += Spi.ADC3;
- // Gyro.DeltaT = (unsigned int)(((Spi.ADC4>>1) + Gyro.Tmp_OffsetT5) * Gyro.Tmp_scaleT5);
- switch(Gyro.LG_Type)
- {
- case 1: Gyro.DeltaT = (unsigned int)((((Spi.ADC2>>1)*100)*0.0122)-27300); break;
-
- case 0: Gyro.DeltaT = (unsigned int)(Spi.ADC4>>1); break;
- }
-
-
- TempA = (0xffff - Spi.ADC5); // перевернем знак и умножим на два (было 32000...0 стало 0 ...32000 /*сдвиг(<<1) стало 0 ...64000*/)
+ Gyro.DeltaT = (unsigned int)((((Spi.ADC2>>1)*100)*0.0122)-27300);
+ Gyro.Termo = Spi.ADC1>>1;
+ Gyro.DeltaT = Spi.ADC2>>1;
Gyro.TermLM = Spi.ADC1;
- Gyro.ADF_Accum += TempA;
- Gyro.ADS_Accum += TempA;
- /// Gyro.ADS_AccumTermLM+=TempTermLM;
- Gyro.ADF_Count ++;
- Gyro.ADS_Count ++;
- Gyro.ADM_Count ++;
- /* sprintf((Time),"%d %d\r\n",((Gyro.In1>>1) - 0x4fff),(Spi.DAC_B-0x4fff));
- WriteCon(Time);*/
- if (Gyro.ADM_Count > 255) {
- Gyro.In1 = Gyro.IN1_Accum>>8;
- Gyro.In2 = Gyro.IN2_Accum>>8;
- Gyro.IN1_Accum=0;
- Gyro.IN2_Accum=0;
- Gyro.ADM_Count=0;
- }
- if (Gyro.ADF_Count > 15) { // если прошло 16 тактов виброподвеса
- Gyro.AD_Fast = Gyro.ADF_Accum << 11; //обновляем данные и приводим в один масштаб
- Gyro.ADF_Count = 0;//
- Gyro.ADF_Accum = 0;
- Gyro.ADF_NewData = 1;
- }
- if (Gyro.ADS_Count > 255) { // если прошло 256 тактов виброподвеса
- Gyro.AD_Slow = Gyro.ADS_Accum << 7; //обновляем данные и приводим в один масштаб
- // Gyro.TermLM = Gyro.ADS_AccumTermLM << 3;
- Gyro.ADS_Count = 0;
- Gyro.ADS_Accum = 0;
- Gyro.ADS_AccumTermLM=0;
- Gyro.ADS_NewData = 1;
- }
}
void HFOFilt(void)
{
-
-
- /* HFO_AVR+=(BuffADC_10v[CountV64]>>4);
- HFO_AVR-=(BuffADC_10v[CountV64 - 32]>>4);
- */
- HFOdelta=(int)((Gyro.HFO_ref)-(BuffADC_128Point[CountV64]>>5));
-
- HFOSumDelta+=HFOdelta;
-
-
-
+ HFOAmp = BUTTER(HFOAmp);
+
+ HFOdelta=(int)((Gyro.HFO_ref)-(HFOAmp));
+ HFOSumDelta+=HFOdelta;
HFOregul=HFOSumDelta/(int)(Gyro.HFO_Gain);
-
- HFOSumDelta-=Gyro.HFO_Gain*HFOregul;
+ HFOSumDelta-=Gyro.HFO_Gain*HFOregul;
+
tempDeltaRegul += HFOregul;
- AMPSUM=0;
- HFO_Flag=1;
-
+
+ HFO_Flag=1;
}
void PLCFilt(void)
-{
- static int SumBuff,Delta, OldDelta;
-
- if (count10v>=64*32)
- {
- C_PLC_F++;
- C_PLC_F = C_PLC_F & 0xf;
- count10v=0;
-
- for(int q=0; q<32; q++)
- {
- SumBuff += BuffADC_10v[q];
- SumBuff -= BuffADC_10v_OLD[q+32];
- BuffADC_10v_F [q] = SumBuff;
- }
-
- for(int q=32; q<64; q++)
+{ static unsigned int countAVR=0;
+
+ if (count10v>=64)
+ {
+ countAVR++;
+ count10v=0;
+ for(int q=0; q<31; q++)
{
- SumBuff += BuffADC_10v[q];
- SumBuff -= BuffADC_10v[q-32];
- BuffADC_10v_F [q] = SumBuff;
- }
-
- for(int q=0; q<32; q++) {AMPSUMP += BuffADC_10v_F [q];}
- for(int q=32; q<64; q++) {AMPSUMM += BuffADC_10v_F [q];}
-
- for(int q=0; q<64; q++)
+ WormFilt[q]= WormFilt[q]*(-1);
+ }
+ for(int q=0; q<64; q++)
{
- AMPSUM+=BuffADC_10v[q];
- AMPSUMout+=BuffADC_10v_F[q];
- BuffADC_10v_OLD[q]= BuffADC_10v[q];
- BuffADC_10v[q] =0;
- }
- for(int q=0; q<64; q++)
- {
- Buff_OUT[q]=(BuffADC_10v_F[q]-(AMPSUM>>1))>>3;
+ WormFilt2[q] = WormFilt[q];
+ PlcWormDelta+=WormFilt[q];
+ WormFilt[q]=0;
}
- /*sprintf((Time),"%d \r\n", Spi.DAC_A);
- WriteCon(Time);*/
-
- AMPSUM=AMPSUM>>6;
- AMPSUM=AMPSUM>>3;
- AMPSUM=AMPSUM>>3;
-
- AMPSUMPout=AMPSUMP>>1;
- AMPSUMMout=AMPSUMM>>1;
- AMPSUMout=AMPSUM;
-
- Delta= (AMPSUMPout - AMPSUMMout);
-
- // if((Delta<(-5000000))||(Delta>5000000))Delta=0;
-
- SumDelta+=Delta;
-
+ PlcWormDeltaout =PLCF1hz(PlcWormDelta>>6);
+ if(countAVR > 63) countAVR=0;
+ PlcWormDelta=0;
+
+ SumDelta-= PlcWormDeltaout;
Gyro.PLC_Eror_count=SumDelta/Gyro.PLC_Gain;
SumDelta-=Gyro.PLC_Gain*Gyro.PLC_Eror_count;
PLC_EROR+=Gyro.PLC_Eror_count;
- BuffPLC_FILT[C_PLC_F] = Gyro.PLC_Eror_count;
- if(Time1Hz>10)
- {
- PLC_ERR_DAC_F += BuffPLC_FILT[C_PLC_F];
- PLC_ERR_DAC_F -= BuffPLC_FILT[(C_PLC_F-8)&0xf];
- }
- PLC_ERR_DAC = PLC_ERR_DAC_F>>3;
PLC_Flag=1;
- AMPSUMM=0;
- AMPSUMP=0;
-
- // LoopOff
- }
+ }
+
}
-void ModFilt(void)
+
+void TempClac(void)
{
+ static unsigned int AVR_T_Count=0;
-
- PLCFilt();
- if(PLC_Flag)
- {
-
- HFOSumDelta=0;
- }
- else HFOFilt();
-
+
+
+ static int TempModT4=0;
+ static int TempModT5=0;
+
+ static int OldT4=0;
+ static int OldT5=0;
+ static int OldDeltaT4=0;
+ static int OldDeltaT5=0;
+ static int OldMod=0;
+
+
+ AVR_T_Count++;
+
+
+ Gyro.T4=Spi.ADC1;
+ Gyro.T5=Spi.ADC2;
+
+ TempModT4 = (int)(Gyro.T4-OldT4);
+ TempModT5 = (int)(Gyro.T5-OldT5);
+
+
+ OldT4=Gyro.T4;
+ OldT5=Gyro.T5;
+
+
+ Delt_T4 [AVR_T_Count & 0x1f] += TempModT4;
+ Delt_T5 [AVR_T_Count & 0x1f] += TempModT5;
+
+ if(AVR_T_Count==0x5ff)
+ {
+ for(int q=0; q<0x1f; q++)
+ {
+ Gyro.DeltaT4+=Delt_T4[q];
+ Delt_T4[q]=0;
+ Gyro.DeltaT5+=Delt_T5[q];
+ Delt_T5[q]=0;
+ }
+ if(Gyro.DeltaT4==0)Gyro.DeltaT4=OldDeltaT4;
+ if(Gyro.DeltaT5==0)Gyro.DeltaT5=OldDeltaT5;
+
+ OldDeltaT4=Gyro.DeltaT4;
+ OldDeltaT5=Gyro.DeltaT5;
+
+ Gyro.TermoMod = Gyro.DeltaT4 + Gyro.DeltaT5;
+
+ Gyro.DeltaT4=0;
+ Gyro.DeltaT5=0;
+
+ Gyro.TemperNewData=1;
+ AVR_T_Count=0;
+
+ }
+
}
void SPI_Exchange(void) // новая функция чтения, в нецй не должно быть ничего лишнего
{
-
unsigned int DummySPI;
-
- //unsigned int ADC5Dif;
+ int FilAds;
+
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;
Spi.ADC2_Accum += LPC_SSP0->DR;
Spi.ADC1_Accum += LPC_SSP0->DR;
Spi.ADC5_Accum += ADC5New;
+
+ FilAds = iir(0xffff-ADC5New);
+ WormFilt[CountV64] = FilAds;
+
while (LPC_SSP0->SR & RX_SSP_notEMPT)
{
DummySPI = LPC_SSP0->DR; //если буфер SPI не пуст.//очистить буфер.
}
DAC_OutPut();
if (CountV31 == 0) { // просто фильтруем по 32 точкам.
- // выставояем бит, что есть новы данные
+ // выставояем бит, что есть новы данные
Spi.ADC1 = Spi.ADC1_Accum >> 5; // подгоотавливаем данные (в той эе сетке) те ADC1 0..65535
Spi.ADC2 = Spi.ADC2_Accum >> 5;
Spi.ADC3 = Spi.ADC3_Accum >> 5;
@@ -326,86 +197,129 @@
Spi.ADC4_Accum = 0;
Spi.ADC5_Accum = 0;
Spi.ADC_NewData = 1;
-
+ TempClac();
}
- if(Time1Hz>6)
- {
- BuffADC_1Point[CountV255] = (0x7fff-ADC5New)&0x7fff;
- BuffADC_1Point_64[CountV31]=(0x7fff-ADC5New)&0x7fff;
-
- ADC_128Point += BuffADC_1Point[CountV255];
- ADC_128Point -= BuffADC_1Point[(CountV255 - 32) & 0xff]; // заполнение буфера накопленых приращений за 64 тактов
- BuffADC_128Point[CountV64] = ADC_128Point;
-
- if(Time1Hz>8)
- {
- if(CountV31==31)
- {
+ 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);*/
- }
-
+ HFOFilt();
+ }
+ count10v++;
+
+ if(Time1Hz>3) PLCFilt();
}
- // 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]);
-
-
- count10v++;
- count10HFO++;
- BuffADC_10v[CountV64] += BuffADC_1Point[CountV255];
-
- // BuffADC_10HFO[CountV64] += BuffADC_1Point[CountV255];
- ModFilt();
- }
-
+
+void HFORegul(void)
+{
+static unsigned int countHFO;
+ countHFO=0;
+ if(Gyro.RgConA&0x2)
+ {
+ if(HFO_Flag)
+ {
+ HFO_Flag=0;
+ Spi.DAC_A -= HFOregul;
+ DeltaRegul=0;
+ }
+ }
+ else DeltaRegul=0;
+ if(Spi.DAC_A>Gyro.HFO_Min-1) Spi.DAC_A=Gyro.HFO_Min-2;
+ else if(Spi.DAC_A<Gyro.HFO_Max+1) Spi.DAC_A=Gyro.HFO_Max+2;
+}
+
+
+void PLCRegul(void)
+{
+static unsigned int Flag_64=0, count;
+static int CountFaza,Sin;
+
+
+ if(Gyro.RgConA&0x8)
+ {
+ if(PLC_Flag)
+ {
+ PLC_Flag=0;
+ if(Gyro.PLCDelay) { }
+ else Spi.DAC_B+=Gyro.PLC_Eror_count;
+ }
+ switch(Gyro.TermoMod)
+ {
+ case -1:
+ if(Spi.DAC_B > Gyro.HighTreshold )
+ {
+ Spi.DAC_B = (Gyro.ResetLevelCool);
+ Gyro.PLCDelay = GyroP.Str.PLCDelay;
+ }
+ break;
+ case 1:
+ if(Spi.DAC_B < Gyro.DownTreshold )
+ {
+ Spi.DAC_B = (Gyro.ResetLevelHeat);
+ Gyro.PLCDelay = GyroP.Str.PLCDelay;
+ }
+ break;
+ }
+ }
+
}
-int BUTTER(int NewSample) {
- static unsigned int SHIFT=1;
- int ACoef[NCoef+1] = {
- 8195,
- 16390,
- 8195
+
+#define NCoefB 4
+float iir(float NewSampleB) {
+ /* float ACoefB[NCoefB+1] = {
+ 0.00013312359920503476,
+ 0.00000000000000000000,
+ -0.00026624719841006952,
+ 0.00000000000000000000,
+ 0.00013312359920503476
};
- int BCoef[NCoef+1] = {
- 16384,
- -32404,
- 16024
+ float BCoefB[NCoefB+1] = {
+ 1.00000000000000000000,
+ -3.95329685155055670000,
+ 5.87955844350996500000,
+ -3.89878590567108720000,
+ 0.97261396930668409000
+ };*/
+
+
+ float ACoefB[NCoefB+1] = {
+ 0.00002732713446345492,
+ 0.00000000000000000000,
+ -0.00005465426892690984,
+ 0.00000000000000000000,
+ 0.00002732713446345492
};
- 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
+ float BCoefB[NCoefB+1] = {
+ 1.00000000000000000000,
+ -3.97797739472029300000,
+ 5.95330114200914020000,
+ -3.97245819689350290000,
+ 0.99722704990382360000
+ };
+ static float yB[NCoefB+1]; //output samples
+ static float xB[NCoefB+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];
+ for(n=NCoefB; n>0; n--) {
+ xB[n] = xB[n-1];
+ yB[n] = yB[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];
+ xB[0] = NewSampleB;
+ yB[0] = ACoefB[0] * xB[0];
+ for(n=1; n<=NCoefB; n++)
+ yB[0] += ACoefB[n] * xB[n] - BCoefB[n] * yB[n];
- y1[0] =(int)(y1[0]/ BCoef[0]>>SHIFT);
-
- // return (int)(y1[0] / DCgain>>SHIFT);
- return (int)(y1[0]);
+ return yB[0];
}
@@ -415,125 +329,166 @@
-void HFORegul(void)
-{
-static unsigned int countHFO;
-
- countHFO=0;
-
- if(Gyro.RgConA&0x2)
- {
- if(HFO_Flag)
- {
- Spi.DAC_A -= HFOregul;
- HFO_Flag=0;
- DeltaRegul=0;
- }
- }
-
- else DeltaRegul=0;
-
- if(Spi.DAC_A>Gyro.HFO_Min-1) Spi.DAC_A=Gyro.HFO_Min-2;
- else if(Spi.DAC_A<Gyro.HFO_Max+1) Spi.DAC_A=Gyro.HFO_Max+2;
-
-}
-void PLCRegul(void)
+float BUTTER(float NewSample) {
+
+ float ACoef[NCoef+1] = {
+ 0.00140639079012754060,
+ 0.00281278158025508130,
+ 0.00140639079012754060
+ };
+
+ float BCoef[NCoef+1] = {
+ 1.00000000000000000000,
+ -1.88903307939452490000,
+ 0.89487434461663573000
+ };
+
+ static float y[NCoef+1]; //output samples
+ static float x[NCoef+1]; //input samples
+ int n;
-{
-static unsigned int Flag_64=0, count;
-static int CountFaza,Sin;
+ //shift the old samples
+ for(n=NCoef; n>0; n--)
+ {
+ x[n] = x[n-1];
+ y[n] = y[n-1];
+ }
+
+ //Calculate the new output
+ x[0] = NewSample;
+ y[0] = ACoef[0] * x[0];
+
+
+ for(n=1; n<=NCoef; n++)
+ {
+ y[0] += ACoef[n] * x[n] - BCoef[n] * y[n];
+
+ }
+
+ return y[0];
+}
+
+
+
- if(Gyro.RgConA&0x8)
- {
- if(PLC_Flag)
- {
- if (Gyro.PLCDelay)
- {
+#define NCoefP 2
+float PLCF1hz(float NewSampleP) {
+
+ /* float ACoefP[NCoefP+1] = {
+ 0.02010938982754319900,
+ 0.04021877965508639800,
+ 0.02010938982754319900
+ }; // 10HZ
+
+ float BCoefP[NCoefP+1] = {
+ 1.00000000000000000000,
+ -1.56101807580071790000,
+ 0.64135153805756284000
+ };*/
+ /*
+ float ACoefP[NCoefP+1] = {
+ 0.00554950417857849340,
+ 0.01109900835715698700,
+ 0.00554950417857849340
+ }; // 5HZ
- }
- else
- {
- /* if(Gyro.PLC_Eror_count>600) Gyro.PLC_Eror_count=0;
- else if(Gyro.PLC_Eror_count<(-600)) Gyro.PLC_Eror_count=0;*/
+ float BCoefP[NCoefP+1] = {
+ 1.00000000000000000000,
+ -1.77863177782458480000,
+ 0.80080264666570744000
+ };*/
+ /* float ACoefP[NCoefP+1] = {
+ 0.00024156733395523967,
+ 0.00048313466791047934,
+ 0.00024156733395523967
+ }; //2HZ
+
+ float BCoefP[NCoefP+1] = {
+ 1.00000000000000000000,
+ -1.95557824031503500000,
+ 0.95654367651120309000
+ };*//*
+ float ACoefP[NCoefP+1] = {
+ 0.00000530383940879362,
+ 0.00001060767881758724,
+ 0.00000530383940879362
+ }; //0.125HZ
+
+ float BCoefP[NCoefP+1] = {
+ 1.00000000000000000000,
+ -1.99444641054192680000,
+ 0.99446178907595384000
+ };*/
+ /*float ACoefP[NCoefP+1] = {
+ 0.00024156733395523967,
+ 0.00048313466791047934,
+ 0.00024156733395523967
+ }; //1HZ
- Spi.DAC_B+=PLC_ERR_DAC;
-
- }
- }
- if(Spi.DAC_B > Gyro.HighTreshold )
- {
- Spi.DAC_B = (Gyro.ResetLevelCool);
- Gyro.PLCDelay = GyroP.Str.PLCDelay;
- }
- else if(Spi.DAC_B < Gyro.DownTreshold )
- {
- Spi.DAC_B = (Gyro.ResetLevelHeat);
- Gyro.PLCDelay = GyroP.Str.PLCDelay;
- }
- }
-
-
-
-
- if(PLC_Flag)
- {
- /*sprintf((Time),"%d %d %d\r\n", PLC_ERR_DAC, Spi.DAC_B, Spi.DAC_A);
- WriteCon(Time);*/
- PLC_ERR_DAC=0;
- PLC_Flag=0;
- }
+ float BCoefP[NCoefP+1] = {
+ 1.00000000000000000000,
+ -1.95557824031503500000,
+ 0.95654367651120309000
+ };*/
+ /*
+ float ACoefP[NCoefP+1] = {
+ 0.00013397155178191382,
+ 0.00026794310356382763,
+ 0.00013397155178191382
+ }; //0.75HZ
+
+ float BCoefP[NCoefP+1] = {
+ 1.00000000000000000000,
+ -1.96668138526348500000,
+ 0.96722742815186025000
+ };*/
+ float ACoefP[NCoefP+1] = {
+ 0.00036133932576050832,
+ 0.00072267865152101664,
+ 0.00036133932576050832
+ }; //1.25HZ
+
+ float BCoefP[NCoefP+1] = {
+ 1.00000000000000000000,
+ -1.94447765776709370000,
+ 0.94597793623228155000
+ };
+
+
+ /*
+ float ACoefP[NCoefP+1] = {
+ 0.29289321881333569000,
+ 0.58578643762667137000,
+ 0.29289321881333569000
+ }; // 100HZ
+
+ float BCoefP[NCoefP+1] = {
+ 1.00000000000000000000,
+ 0.00000000000000004878,
+ 0.17157287525380990000
+ };*/
+ static float yP[NCoefP+1]; //output samples
+ static float xP[NCoefP+1]; //input samples
+ int n;
+
+ //shift the old samples
+ for(n=NCoef; n>0; n--) {
+ xP[n] = xP[n-1];
+ yP[n] = yP[n-1];
+ }
+
+ //Calculate the new output
+ xP[0] = NewSampleP;
+ yP[0] = ACoefP[0] * xP[0];
+ for(n=1; n<=NCoefP; n++)
+ yP[0] += ACoefP[n] * xP[n] - BCoefP[n] * yP[n];
+
+ return yP[0];
}
-
-
-
-void ShowMod(void)//технологическая функция для просмотра в ориджине мод на всем диапазпне цап
-{
-
- //////////////////////////////////////////////////////////////////////////////////////////////////
- //////////////////////////////////смотрим все моды/////////////////////////////////////////////////
- ///////////////////////////////////////////////////////////////////////////////////////////////////
- if(dispersion>5)
- {
- if( (Gyro.PLC_Lern<60000)&&(Gyro.PLC_Error2Mode >1))//пробигаем по нескольким значениям цап(60*0х3с=0хВВ8) для определения максимальной амплитуды.
- {
- Gyro.PLC_Error2Mode--;
- Gyro.PLC_Lern++;
- Spi.DAC_B += tempstrafe*side;
- if(side>0)side=(-1);
- else side = 1;
- tempstrafe-=40;
- dispersion=0;
- }
- else {Gyro.LogPLC=0;}
- }
- else dispersion++;
- /*sprintf((Time),"%d %d %d %d \r\n", Gyro.CuruAngle, Spi.DAC_B, Gyro.AD_Slow, Gyro.Termo);
- Gyro.CuruAngle=0;*/
- WriteCon(Time);
-}
-void ShowMod2(void)//технологическая функция для просмотра в ориджине мод на всем диапазпне цап
-{
- if(dispersion>3)
- {
- unsigned int step = 50, ENDMOD=65400;
- /*sprintf((Time),"%d %d %d %d %d %d %d\r\n", Gyro.CuruAngle, Spi.DAC_B, Gyro.AD_Slow, Spi.ADC5, 0xfFFf-Spi.ADC1, Spi.ADC1, Gyro.Termo);
- Gyro.CuruAngle=0;
- WriteCon(Time);*/
- Spi.DAC_B+=step;
- if(Spi.DAC_B>ENDMOD)
- {
- // Gyro.LogMod=0;
- PlcON
- Spi.DAC_B = 48000;
- }
- dispersion=0;
- }
- else dispersion++;
-}
void DAC_OutPut(void)//выдача в цапы
{
/*if(Gyro.RgConA&0x10)*/ Modulator();