Dmitry Kovalev
/
LGfiltr
forkd
Fork of LGstaandart by
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();