Dmitry Kovalev
/
LGfiltr
forkd
Fork of LGstaandart by
SPI.c
- Committer:
- Kovalev_D
- Date:
- 2018-01-25
- Revision:
- 231:079835d508ef
- Parent:
- 230:8f7eeb47ca51
- Child:
- 232:130a2b5003e6
File content as of revision 231:079835d508ef:
#include "Global.h" #define NCoef 2 #define DCgain 8192 struct SPI Spi; 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; int HFOAmpSUM; int HFOAmp; int HFOAmpfilt; int HFOAmpfiltC; 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}; 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) { LPC_DAC->DACR = (ModArraySin [(CountV64+Gyro.PLC_Phase)&0x3f])*Gyro.ModAmp + Gyro.ShiftMod-32000; } void ADS_Acum(void) { Spi.ADC_NewData = 0; /*Gyro.IN1_Accum += Spi.ADC2; Gyro.IN2_Accum += Spi.ADC3;*/ Gyro.Termo = (unsigned int)((((Spi.ADC1>>1)*100)*0.0122)-27300); 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; } void HFOFilt(void) { HFOAmp = BUTTER(HFOAmp); HFOdelta=(int)((Gyro.HFO_ref)-(HFOAmp)); HFOSumDelta+=HFOdelta; HFOregul=HFOSumDelta/(int)(Gyro.HFO_Gain); HFOSumDelta-=Gyro.HFO_Gain*HFOregul; tempDeltaRegul += HFOregul; HFO_Flag=1; } void PLCFilt(void) { static unsigned int countAVR=0; if (count10v>=64) { countAVR++; count10v=0; for(int q=0; q<31; q++) { WormFilt[q]= WormFilt[q]*(-1); } for(int q=0; q<64; q++) { WormFilt2[q] = WormFilt[q]; PlcWormDelta+=WormFilt[q]; WormFilt[q]=0; } 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; PLC_Flag=1; } } void TempClac(void) { static unsigned int AVR_T_Count=0; 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; int FilAds; ADC5New = LPC_SSP0->DR;// Чтение АЦП HFOAmpSUM+=0xffff-ADC5New; 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; Spi.ADC4 = Spi.ADC4_Accum >> 5; Spi.ADC5 = Spi.ADC5_Accum >> 5; Spi.ADC1_Accum = 0; // сбрасывкем аккамулятор Spi.ADC2_Accum = 0; Spi.ADC3_Accum = 0; Spi.ADC4_Accum = 0; Spi.ADC5_Accum = 0; Spi.ADC_NewData = 1; TempClac(); } if(CountV31==31) { HFOAmp=HFOAmpSUM>>5; HFOAmpSUM=0; HFOFilt(); } count10v++; if(Time1Hz>3) PLCFilt(); } 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; } } } #define NCoefB 4 float iir(float NewSampleB) { /* float ACoefB[NCoefB+1] = { 0.00013312359920503476, 0.00000000000000000000, -0.00026624719841006952, 0.00000000000000000000, 0.00013312359920503476 }; 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 }; 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=NCoefB; n>0; n--) { xB[n] = xB[n-1]; yB[n] = yB[n-1]; } //Calculate the new output 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]; return yB[0]; } 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; //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]; } #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 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 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 DAC_OutPut(void)//выдача в цапы { /*if(Gyro.RgConA&0x10)*/ Modulator(); LPC_SSP0->DR=0x5555; LPC_SSP0->DR=0x5555; LPC_SSP0->DR=0x5555; if (CountV31 & 1) { //если нечетный такт то LPC_SSP0->DR = WRITE_DAC0; //e.команда для ЦАП_0 передавать. LPC_SSP0->DR = (Spi.DAC_A); //e. передача 12 бит } else { //если такт четный. LPC_SSP0->DR = WRITE_DAC1 ; //e.команда для ЦАП_1 передавать. LPC_SSP0->DR = (Spi.DAC_B) ; } }