Dmitry Kovalev / Mbed 2 deprecated LGfiltr

forkd

Dependencies:   mbed

Fork of LGstaandart by Dmitry Kovalev

SPI.c@231:079835d508ef, 2018-01-25 (annotated)

Committer:
Kovalev_D
Date:
Thu Jan 25 13:59:43 2018 +0000
Revision:
231:079835d508ef
Parent:
230:8f7eeb47ca51
Child:
232:130a2b5003e6
termo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
igor_v 0:8ad47e2b6f00 1 #include "Global.h"
Kovalev_D 228:a8195e1b1123 2 #define NCoef 2
Kovalev_D 228:a8195e1b1123 3 #define DCgain 8192
igor_v 0:8ad47e2b6f00 4 struct SPI Spi;
Kovalev_D 227:2774b56bfab0 5
Kovalev_D 227:2774b56bfab0 6
Kovalev_D 215:b58b887fd367 7
Kovalev_D 197:7a05523bf588 8
Kovalev_D 197:7a05523bf588 9
Kovalev_D 231:079835d508ef 10 unsigned int TempTermLM;
Kovalev_D 231:079835d508ef 11 unsigned int ADC5New;
Kovalev_D 215:b58b887fd367 12
Kovalev_D 231:079835d508ef 13 int AMPSUM,SumDelta,HFOSumDelta,DeltaRegul,tempDeltaRegul;
Kovalev_D 231:079835d508ef 14 int count10v=0,PLC_EROR, PLC_Flag,HFO_Flag,HFOdelta,HFOregul;
Kovalev_D 231:079835d508ef 15 int PlcWormMN, PlcWormPL, PlcWormDelta, PlcWormMNout, PlcWormPLout, PlcWormDeltaout;
Kovalev_D 215:b58b887fd367 16
Kovalev_D 228:a8195e1b1123 17 int HFOAmpSUM;
Kovalev_D 228:a8195e1b1123 18 int HFOAmp;
Kovalev_D 228:a8195e1b1123 19 int HFOAmpfilt;
Kovalev_D 228:a8195e1b1123 20 int HFOAmpfiltC;
Kovalev_D 197:7a05523bf588 21 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};
Kovalev_D 231:079835d508ef 22 int WormFilt [64];
Kovalev_D 231:079835d508ef 23 int WormFilt2 [64];
Kovalev_D 231:079835d508ef 24 int Delt_T4 [40];
Kovalev_D 231:079835d508ef 25 int Delt_T5 [40];
Kovalev_D 231:079835d508ef 26 //int AVR_PLC_ERR [64];
Kovalev_D 231:079835d508ef 27 //int ERRPLC;
Kovalev_D 231:079835d508ef 28 int tempPlcErr;
Kovalev_D 231:079835d508ef 29
Kovalev_D 231:079835d508ef 30
Kovalev_D 231:079835d508ef 31
Kovalev_D 231:079835d508ef 32
Kovalev_D 231:079835d508ef 33
Kovalev_D 231:079835d508ef 34
Kovalev_D 231:079835d508ef 35
Kovalev_D 231:079835d508ef 36
Kovalev_D 231:079835d508ef 37
Kovalev_D 231:079835d508ef 38
Kovalev_D 231:079835d508ef 39
Kovalev_D 197:7a05523bf588 40 void Modulator(void)
Kovalev_D 197:7a05523bf588 41 {
Kovalev_D 231:079835d508ef 42 LPC_DAC->DACR = (ModArraySin [(CountV64+Gyro.PLC_Phase)&0x3f])*Gyro.ModAmp + Gyro.ShiftMod-32000;
Kovalev_D 209:224e7331a061 43 }
Kovalev_D 226:4a4d5bd5fcd7 44
Kovalev_D 147:1aed74f19a8f 45 void ADS_Acum(void)
Kovalev_D 147:1aed74f19a8f 46 {
Kovalev_D 157:1069c80f4944 47 Spi.ADC_NewData = 0;
Kovalev_D 231:079835d508ef 48 /*Gyro.IN1_Accum += Spi.ADC2;
Kovalev_D 231:079835d508ef 49 Gyro.IN2_Accum += Spi.ADC3;*/
Kovalev_D 227:2774b56bfab0 50 Gyro.Termo = (unsigned int)((((Spi.ADC1>>1)*100)*0.0122)-27300);
Kovalev_D 231:079835d508ef 51 Gyro.DeltaT = (unsigned int)((((Spi.ADC2>>1)*100)*0.0122)-27300);
Kovalev_D 231:079835d508ef 52 Gyro.Termo = Spi.ADC1>>1;
Kovalev_D 231:079835d508ef 53 Gyro.DeltaT = Spi.ADC2>>1;
Kovalev_D 211:ac8251b067d2 54 Gyro.TermLM = Spi.ADC1;
Kovalev_D 147:1aed74f19a8f 55 }
Kovalev_D 226:4a4d5bd5fcd7 56
Kovalev_D 226:4a4d5bd5fcd7 57 void HFOFilt(void)
Kovalev_D 220:04c54405b82d 58 {
Kovalev_D 231:079835d508ef 59 HFOAmp = BUTTER(HFOAmp);
Kovalev_D 231:079835d508ef 60
Kovalev_D 231:079835d508ef 61 HFOdelta=(int)((Gyro.HFO_ref)-(HFOAmp));
Kovalev_D 231:079835d508ef 62 HFOSumDelta+=HFOdelta;
Kovalev_D 226:4a4d5bd5fcd7 63 HFOregul=HFOSumDelta/(int)(Gyro.HFO_Gain);
Kovalev_D 231:079835d508ef 64 HFOSumDelta-=Gyro.HFO_Gain*HFOregul;
Kovalev_D 231:079835d508ef 65
Kovalev_D 226:4a4d5bd5fcd7 66 tempDeltaRegul += HFOregul;
Kovalev_D 231:079835d508ef 67
Kovalev_D 231:079835d508ef 68 HFO_Flag=1;
Kovalev_D 226:4a4d5bd5fcd7 69 }
Kovalev_D 226:4a4d5bd5fcd7 70
Kovalev_D 226:4a4d5bd5fcd7 71 void PLCFilt(void)
Kovalev_D 231:079835d508ef 72 { static unsigned int countAVR=0;
Kovalev_D 231:079835d508ef 73
Kovalev_D 231:079835d508ef 74 if (count10v>=64)
Kovalev_D 231:079835d508ef 75 {
Kovalev_D 231:079835d508ef 76 countAVR++;
Kovalev_D 231:079835d508ef 77 count10v=0;
Kovalev_D 231:079835d508ef 78 for(int q=0; q<31; q++)
Kovalev_D 220:04c54405b82d 79 {
Kovalev_D 231:079835d508ef 80 WormFilt[q]= WormFilt[q]*(-1);
Kovalev_D 231:079835d508ef 81 }
Kovalev_D 231:079835d508ef 82 for(int q=0; q<64; q++)
Kovalev_D 220:04c54405b82d 83 {
Kovalev_D 231:079835d508ef 84 WormFilt2[q] = WormFilt[q];
Kovalev_D 231:079835d508ef 85 PlcWormDelta+=WormFilt[q];
Kovalev_D 231:079835d508ef 86 WormFilt[q]=0;
Kovalev_D 222:7de7b3bf3a1d 87 }
Kovalev_D 222:7de7b3bf3a1d 88
Kovalev_D 231:079835d508ef 89 PlcWormDeltaout =PLCF1hz(PlcWormDelta>>6);
Kovalev_D 231:079835d508ef 90 if(countAVR > 63) countAVR=0;
Kovalev_D 231:079835d508ef 91 PlcWormDelta=0;
Kovalev_D 231:079835d508ef 92
Kovalev_D 231:079835d508ef 93 SumDelta-= PlcWormDeltaout;
Kovalev_D 220:04c54405b82d 94 Gyro.PLC_Eror_count=SumDelta/Gyro.PLC_Gain;
Kovalev_D 220:04c54405b82d 95 SumDelta-=Gyro.PLC_Gain*Gyro.PLC_Eror_count;
Kovalev_D 225:f8fee6c586cc 96
Kovalev_D 225:f8fee6c586cc 97 PLC_EROR+=Gyro.PLC_Eror_count;
Kovalev_D 226:4a4d5bd5fcd7 98
Kovalev_D 220:04c54405b82d 99 PLC_Flag=1;
Kovalev_D 231:079835d508ef 100 }
Kovalev_D 231:079835d508ef 101
Kovalev_D 220:04c54405b82d 102 }
Kovalev_D 231:079835d508ef 103
Kovalev_D 231:079835d508ef 104 void TempClac(void)
Kovalev_D 226:4a4d5bd5fcd7 105 {
Kovalev_D 231:079835d508ef 106 static unsigned int AVR_T_Count=0;
Kovalev_D 226:4a4d5bd5fcd7 107
Kovalev_D 231:079835d508ef 108
Kovalev_D 231:079835d508ef 109
Kovalev_D 231:079835d508ef 110 static int TempModT4=0;
Kovalev_D 231:079835d508ef 111 static int TempModT5=0;
Kovalev_D 231:079835d508ef 112
Kovalev_D 231:079835d508ef 113 static int OldT4=0;
Kovalev_D 231:079835d508ef 114 static int OldT5=0;
Kovalev_D 231:079835d508ef 115 static int OldDeltaT4=0;
Kovalev_D 231:079835d508ef 116 static int OldDeltaT5=0;
Kovalev_D 231:079835d508ef 117 static int OldMod=0;
Kovalev_D 231:079835d508ef 118
Kovalev_D 231:079835d508ef 119
Kovalev_D 231:079835d508ef 120 AVR_T_Count++;
Kovalev_D 231:079835d508ef 121
Kovalev_D 231:079835d508ef 122
Kovalev_D 231:079835d508ef 123 Gyro.T4=Spi.ADC1;
Kovalev_D 231:079835d508ef 124 Gyro.T5=Spi.ADC2;
Kovalev_D 231:079835d508ef 125
Kovalev_D 231:079835d508ef 126 TempModT4 = (int)(Gyro.T4-OldT4);
Kovalev_D 231:079835d508ef 127 TempModT5 = (int)(Gyro.T5-OldT5);
Kovalev_D 231:079835d508ef 128
Kovalev_D 231:079835d508ef 129
Kovalev_D 231:079835d508ef 130 OldT4=Gyro.T4;
Kovalev_D 231:079835d508ef 131 OldT5=Gyro.T5;
Kovalev_D 231:079835d508ef 132
Kovalev_D 231:079835d508ef 133
Kovalev_D 231:079835d508ef 134 Delt_T4 [AVR_T_Count & 0x1f] += TempModT4;
Kovalev_D 231:079835d508ef 135 Delt_T5 [AVR_T_Count & 0x1f] += TempModT5;
Kovalev_D 231:079835d508ef 136
Kovalev_D 231:079835d508ef 137 if(AVR_T_Count==0x5ff)
Kovalev_D 231:079835d508ef 138 {
Kovalev_D 231:079835d508ef 139 for(int q=0; q<0x1f; q++)
Kovalev_D 231:079835d508ef 140 {
Kovalev_D 231:079835d508ef 141 Gyro.DeltaT4+=Delt_T4[q];
Kovalev_D 231:079835d508ef 142 Delt_T4[q]=0;
Kovalev_D 231:079835d508ef 143 Gyro.DeltaT5+=Delt_T5[q];
Kovalev_D 231:079835d508ef 144 Delt_T5[q]=0;
Kovalev_D 231:079835d508ef 145 }
Kovalev_D 231:079835d508ef 146 if(Gyro.DeltaT4==0)Gyro.DeltaT4=OldDeltaT4;
Kovalev_D 231:079835d508ef 147 if(Gyro.DeltaT5==0)Gyro.DeltaT5=OldDeltaT5;
Kovalev_D 231:079835d508ef 148
Kovalev_D 231:079835d508ef 149 OldDeltaT4=Gyro.DeltaT4;
Kovalev_D 231:079835d508ef 150 OldDeltaT5=Gyro.DeltaT5;
Kovalev_D 231:079835d508ef 151
Kovalev_D 231:079835d508ef 152 Gyro.TermoMod = Gyro.DeltaT4 + Gyro.DeltaT5;
Kovalev_D 231:079835d508ef 153
Kovalev_D 231:079835d508ef 154 Gyro.DeltaT4=0;
Kovalev_D 231:079835d508ef 155 Gyro.DeltaT5=0;
Kovalev_D 231:079835d508ef 156
Kovalev_D 231:079835d508ef 157 Gyro.TemperNewData=1;
Kovalev_D 231:079835d508ef 158 AVR_T_Count=0;
Kovalev_D 231:079835d508ef 159
Kovalev_D 231:079835d508ef 160 }
Kovalev_D 231:079835d508ef 161
Kovalev_D 226:4a4d5bd5fcd7 162 }
Kovalev_D 220:04c54405b82d 163
igor_v 114:5cc38a53d8a7 164 void SPI_Exchange(void) // новая функция чтения, в нецй не должно быть ничего лишнего
Kovalev_D 157:1069c80f4944 165 {
Kovalev_D 157:1069c80f4944 166 unsigned int DummySPI;
Kovalev_D 231:079835d508ef 167 int FilAds;
Kovalev_D 231:079835d508ef 168
Kovalev_D 197:7a05523bf588 169 ADC5New = LPC_SSP0->DR;// Чтение АЦП
Kovalev_D 228:a8195e1b1123 170 HFOAmpSUM+=0xffff-ADC5New;
Kovalev_D 231:079835d508ef 171
igor_v 110:6406b7ac0442 172 Spi.ADC4_Accum += LPC_SSP0->DR;
igor_v 110:6406b7ac0442 173 Spi.ADC3_Accum += LPC_SSP0->DR;
igor_v 110:6406b7ac0442 174 Spi.ADC2_Accum += LPC_SSP0->DR;
igor_v 110:6406b7ac0442 175 Spi.ADC1_Accum += LPC_SSP0->DR;
Kovalev_D 207:d1ce992f5d17 176 Spi.ADC5_Accum += ADC5New;
Kovalev_D 231:079835d508ef 177
Kovalev_D 231:079835d508ef 178 FilAds = iir(0xffff-ADC5New);
Kovalev_D 231:079835d508ef 179 WormFilt[CountV64] = FilAds;
Kovalev_D 209:224e7331a061 180
Kovalev_D 231:079835d508ef 181
Kovalev_D 207:d1ce992f5d17 182 while (LPC_SSP0->SR & RX_SSP_notEMPT)
Kovalev_D 207:d1ce992f5d17 183 {
Kovalev_D 170:d099c3025f87 184 DummySPI = LPC_SSP0->DR; //если буфер SPI не пуст.//очистить буфер.
Kovalev_D 157:1069c80f4944 185 }
Kovalev_D 112:4a96133a1311 186 DAC_OutPut();
Kovalev_D 157:1069c80f4944 187 if (CountV31 == 0) { // просто фильтруем по 32 точкам.
Kovalev_D 231:079835d508ef 188 // выставояем бит, что есть новы данные
Kovalev_D 157:1069c80f4944 189 Spi.ADC1 = Spi.ADC1_Accum >> 5; // подгоотавливаем данные (в той эе сетке) те ADC1 0..65535
Kovalev_D 157:1069c80f4944 190 Spi.ADC2 = Spi.ADC2_Accum >> 5;
Kovalev_D 157:1069c80f4944 191 Spi.ADC3 = Spi.ADC3_Accum >> 5;
Kovalev_D 157:1069c80f4944 192 Spi.ADC4 = Spi.ADC4_Accum >> 5;
Kovalev_D 157:1069c80f4944 193 Spi.ADC5 = Spi.ADC5_Accum >> 5;
Kovalev_D 157:1069c80f4944 194 Spi.ADC1_Accum = 0; // сбрасывкем аккамулятор
Kovalev_D 157:1069c80f4944 195 Spi.ADC2_Accum = 0;
Kovalev_D 157:1069c80f4944 196 Spi.ADC3_Accum = 0;
Kovalev_D 157:1069c80f4944 197 Spi.ADC4_Accum = 0;
Kovalev_D 157:1069c80f4944 198 Spi.ADC5_Accum = 0;
Kovalev_D 157:1069c80f4944 199 Spi.ADC_NewData = 1;
Kovalev_D 231:079835d508ef 200 TempClac();
Kovalev_D 207:d1ce992f5d17 201 }
Kovalev_D 231:079835d508ef 202 if(CountV31==31)
Kovalev_D 231:079835d508ef 203 {
Kovalev_D 228:a8195e1b1123 204 HFOAmp=HFOAmpSUM>>5;
Kovalev_D 228:a8195e1b1123 205 HFOAmpSUM=0;
Kovalev_D 231:079835d508ef 206 HFOFilt();
Kovalev_D 231:079835d508ef 207 }
Kovalev_D 231:079835d508ef 208 count10v++;
Kovalev_D 231:079835d508ef 209
Kovalev_D 231:079835d508ef 210 if(Time1Hz>3) PLCFilt();
Kovalev_D 228:a8195e1b1123 211 }
Kovalev_D 220:04c54405b82d 212
Kovalev_D 231:079835d508ef 213
Kovalev_D 231:079835d508ef 214 void HFORegul(void)
Kovalev_D 231:079835d508ef 215 {
Kovalev_D 231:079835d508ef 216 static unsigned int countHFO;
Kovalev_D 231:079835d508ef 217 countHFO=0;
Kovalev_D 231:079835d508ef 218 if(Gyro.RgConA&0x2)
Kovalev_D 231:079835d508ef 219 {
Kovalev_D 231:079835d508ef 220 if(HFO_Flag)
Kovalev_D 231:079835d508ef 221 {
Kovalev_D 231:079835d508ef 222 HFO_Flag=0;
Kovalev_D 231:079835d508ef 223 Spi.DAC_A -= HFOregul;
Kovalev_D 231:079835d508ef 224 DeltaRegul=0;
Kovalev_D 231:079835d508ef 225 }
Kovalev_D 231:079835d508ef 226 }
Kovalev_D 231:079835d508ef 227 else DeltaRegul=0;
Kovalev_D 231:079835d508ef 228 if(Spi.DAC_A>Gyro.HFO_Min-1) Spi.DAC_A=Gyro.HFO_Min-2;
Kovalev_D 231:079835d508ef 229 else if(Spi.DAC_A<Gyro.HFO_Max+1) Spi.DAC_A=Gyro.HFO_Max+2;
Kovalev_D 231:079835d508ef 230 }
Kovalev_D 231:079835d508ef 231
Kovalev_D 231:079835d508ef 232
Kovalev_D 231:079835d508ef 233 void PLCRegul(void)
Kovalev_D 231:079835d508ef 234 {
Kovalev_D 231:079835d508ef 235 static unsigned int Flag_64=0, count;
Kovalev_D 231:079835d508ef 236 static int CountFaza,Sin;
Kovalev_D 231:079835d508ef 237
Kovalev_D 231:079835d508ef 238
Kovalev_D 231:079835d508ef 239 if(Gyro.RgConA&0x8)
Kovalev_D 231:079835d508ef 240 {
Kovalev_D 231:079835d508ef 241 if(PLC_Flag)
Kovalev_D 231:079835d508ef 242 {
Kovalev_D 231:079835d508ef 243 PLC_Flag=0;
Kovalev_D 231:079835d508ef 244 if(Gyro.PLCDelay) { }
Kovalev_D 231:079835d508ef 245 else Spi.DAC_B+=Gyro.PLC_Eror_count;
Kovalev_D 231:079835d508ef 246 }
Kovalev_D 231:079835d508ef 247 switch(Gyro.TermoMod)
Kovalev_D 231:079835d508ef 248 {
Kovalev_D 231:079835d508ef 249 case -1:
Kovalev_D 231:079835d508ef 250 if(Spi.DAC_B > Gyro.HighTreshold )
Kovalev_D 231:079835d508ef 251 {
Kovalev_D 231:079835d508ef 252 Spi.DAC_B = (Gyro.ResetLevelCool);
Kovalev_D 231:079835d508ef 253 Gyro.PLCDelay = GyroP.Str.PLCDelay;
Kovalev_D 231:079835d508ef 254 }
Kovalev_D 231:079835d508ef 255 break;
Kovalev_D 231:079835d508ef 256 case 1:
Kovalev_D 231:079835d508ef 257 if(Spi.DAC_B < Gyro.DownTreshold )
Kovalev_D 231:079835d508ef 258 {
Kovalev_D 231:079835d508ef 259 Spi.DAC_B = (Gyro.ResetLevelHeat);
Kovalev_D 231:079835d508ef 260 Gyro.PLCDelay = GyroP.Str.PLCDelay;
Kovalev_D 231:079835d508ef 261 }
Kovalev_D 231:079835d508ef 262 break;
Kovalev_D 231:079835d508ef 263 }
Kovalev_D 231:079835d508ef 264 }
Kovalev_D 231:079835d508ef 265
Kovalev_D 220:04c54405b82d 266 }
Kovalev_D 220:04c54405b82d 267
Kovalev_D 228:a8195e1b1123 268
Kovalev_D 228:a8195e1b1123 269
Kovalev_D 228:a8195e1b1123 270
Kovalev_D 231:079835d508ef 271
Kovalev_D 231:079835d508ef 272 #define NCoefB 4
Kovalev_D 231:079835d508ef 273 float iir(float NewSampleB) {
Kovalev_D 231:079835d508ef 274 /* float ACoefB[NCoefB+1] = {
Kovalev_D 231:079835d508ef 275 0.00013312359920503476,
Kovalev_D 231:079835d508ef 276 0.00000000000000000000,
Kovalev_D 231:079835d508ef 277 -0.00026624719841006952,
Kovalev_D 231:079835d508ef 278 0.00000000000000000000,
Kovalev_D 231:079835d508ef 279 0.00013312359920503476
Kovalev_D 228:a8195e1b1123 280 };
Kovalev_D 228:a8195e1b1123 281
Kovalev_D 231:079835d508ef 282 float BCoefB[NCoefB+1] = {
Kovalev_D 231:079835d508ef 283 1.00000000000000000000,
Kovalev_D 231:079835d508ef 284 -3.95329685155055670000,
Kovalev_D 231:079835d508ef 285 5.87955844350996500000,
Kovalev_D 231:079835d508ef 286 -3.89878590567108720000,
Kovalev_D 231:079835d508ef 287 0.97261396930668409000
Kovalev_D 231:079835d508ef 288 };*/
Kovalev_D 231:079835d508ef 289
Kovalev_D 231:079835d508ef 290
Kovalev_D 231:079835d508ef 291 float ACoefB[NCoefB+1] = {
Kovalev_D 231:079835d508ef 292 0.00002732713446345492,
Kovalev_D 231:079835d508ef 293 0.00000000000000000000,
Kovalev_D 231:079835d508ef 294 -0.00005465426892690984,
Kovalev_D 231:079835d508ef 295 0.00000000000000000000,
Kovalev_D 231:079835d508ef 296 0.00002732713446345492
Kovalev_D 228:a8195e1b1123 297 };
Kovalev_D 228:a8195e1b1123 298
Kovalev_D 231:079835d508ef 299 float BCoefB[NCoefB+1] = {
Kovalev_D 231:079835d508ef 300 1.00000000000000000000,
Kovalev_D 231:079835d508ef 301 -3.97797739472029300000,
Kovalev_D 231:079835d508ef 302 5.95330114200914020000,
Kovalev_D 231:079835d508ef 303 -3.97245819689350290000,
Kovalev_D 231:079835d508ef 304 0.99722704990382360000
Kovalev_D 231:079835d508ef 305 };
Kovalev_D 231:079835d508ef 306 static float yB[NCoefB+1]; //output samples
Kovalev_D 231:079835d508ef 307 static float xB[NCoefB+1]; //input samples
Kovalev_D 228:a8195e1b1123 308 int n;
Kovalev_D 228:a8195e1b1123 309
Kovalev_D 228:a8195e1b1123 310 //shift the old samples
Kovalev_D 231:079835d508ef 311 for(n=NCoefB; n>0; n--) {
Kovalev_D 231:079835d508ef 312 xB[n] = xB[n-1];
Kovalev_D 231:079835d508ef 313 yB[n] = yB[n-1];
Kovalev_D 228:a8195e1b1123 314 }
Kovalev_D 228:a8195e1b1123 315
Kovalev_D 228:a8195e1b1123 316 //Calculate the new output
Kovalev_D 231:079835d508ef 317 xB[0] = NewSampleB;
Kovalev_D 231:079835d508ef 318 yB[0] = ACoefB[0] * xB[0];
Kovalev_D 231:079835d508ef 319 for(n=1; n<=NCoefB; n++)
Kovalev_D 231:079835d508ef 320 yB[0] += ACoefB[n] * xB[n] - BCoefB[n] * yB[n];
Kovalev_D 228:a8195e1b1123 321
Kovalev_D 231:079835d508ef 322 return yB[0];
Kovalev_D 228:a8195e1b1123 323 }
Kovalev_D 228:a8195e1b1123 324
Kovalev_D 228:a8195e1b1123 325
Kovalev_D 228:a8195e1b1123 326
Kovalev_D 228:a8195e1b1123 327
Kovalev_D 228:a8195e1b1123 328
Kovalev_D 228:a8195e1b1123 329
Kovalev_D 228:a8195e1b1123 330
Kovalev_D 228:a8195e1b1123 331
Kovalev_D 220:04c54405b82d 332
Kovalev_D 214:4c70e452c491 333
Kovalev_D 231:079835d508ef 334 float BUTTER(float NewSample) {
Kovalev_D 231:079835d508ef 335
Kovalev_D 231:079835d508ef 336 float ACoef[NCoef+1] = {
Kovalev_D 231:079835d508ef 337 0.00140639079012754060,
Kovalev_D 231:079835d508ef 338 0.00281278158025508130,
Kovalev_D 231:079835d508ef 339 0.00140639079012754060
Kovalev_D 231:079835d508ef 340 };
Kovalev_D 231:079835d508ef 341
Kovalev_D 231:079835d508ef 342 float BCoef[NCoef+1] = {
Kovalev_D 231:079835d508ef 343 1.00000000000000000000,
Kovalev_D 231:079835d508ef 344 -1.88903307939452490000,
Kovalev_D 231:079835d508ef 345 0.89487434461663573000
Kovalev_D 231:079835d508ef 346 };
Kovalev_D 231:079835d508ef 347
Kovalev_D 231:079835d508ef 348 static float y[NCoef+1]; //output samples
Kovalev_D 231:079835d508ef 349 static float x[NCoef+1]; //input samples
Kovalev_D 231:079835d508ef 350 int n;
Kovalev_D 215:b58b887fd367 351
Kovalev_D 231:079835d508ef 352 //shift the old samples
Kovalev_D 231:079835d508ef 353 for(n=NCoef; n>0; n--)
Kovalev_D 231:079835d508ef 354 {
Kovalev_D 231:079835d508ef 355 x[n] = x[n-1];
Kovalev_D 231:079835d508ef 356 y[n] = y[n-1];
Kovalev_D 231:079835d508ef 357 }
Kovalev_D 231:079835d508ef 358
Kovalev_D 231:079835d508ef 359 //Calculate the new output
Kovalev_D 231:079835d508ef 360 x[0] = NewSample;
Kovalev_D 231:079835d508ef 361 y[0] = ACoef[0] * x[0];
Kovalev_D 231:079835d508ef 362
Kovalev_D 231:079835d508ef 363
Kovalev_D 231:079835d508ef 364 for(n=1; n<=NCoef; n++)
Kovalev_D 231:079835d508ef 365 {
Kovalev_D 231:079835d508ef 366 y[0] += ACoef[n] * x[n] - BCoef[n] * y[n];
Kovalev_D 231:079835d508ef 367
Kovalev_D 231:079835d508ef 368 }
Kovalev_D 231:079835d508ef 369
Kovalev_D 231:079835d508ef 370 return y[0];
Kovalev_D 231:079835d508ef 371 }
Kovalev_D 231:079835d508ef 372
Kovalev_D 231:079835d508ef 373
Kovalev_D 231:079835d508ef 374
Kovalev_D 216:189b0ea1dc38 375
Kovalev_D 216:189b0ea1dc38 376
Kovalev_D 231:079835d508ef 377 #define NCoefP 2
Kovalev_D 231:079835d508ef 378 float PLCF1hz(float NewSampleP) {
Kovalev_D 231:079835d508ef 379
Kovalev_D 231:079835d508ef 380 /* float ACoefP[NCoefP+1] = {
Kovalev_D 231:079835d508ef 381 0.02010938982754319900,
Kovalev_D 231:079835d508ef 382 0.04021877965508639800,
Kovalev_D 231:079835d508ef 383 0.02010938982754319900
Kovalev_D 231:079835d508ef 384 }; // 10HZ
Kovalev_D 231:079835d508ef 385
Kovalev_D 231:079835d508ef 386 float BCoefP[NCoefP+1] = {
Kovalev_D 231:079835d508ef 387 1.00000000000000000000,
Kovalev_D 231:079835d508ef 388 -1.56101807580071790000,
Kovalev_D 231:079835d508ef 389 0.64135153805756284000
Kovalev_D 231:079835d508ef 390 };*/
Kovalev_D 231:079835d508ef 391 /*
Kovalev_D 231:079835d508ef 392 float ACoefP[NCoefP+1] = {
Kovalev_D 231:079835d508ef 393 0.00554950417857849340,
Kovalev_D 231:079835d508ef 394 0.01109900835715698700,
Kovalev_D 231:079835d508ef 395 0.00554950417857849340
Kovalev_D 231:079835d508ef 396 }; // 5HZ
Kovalev_D 225:f8fee6c586cc 397
Kovalev_D 231:079835d508ef 398 float BCoefP[NCoefP+1] = {
Kovalev_D 231:079835d508ef 399 1.00000000000000000000,
Kovalev_D 231:079835d508ef 400 -1.77863177782458480000,
Kovalev_D 231:079835d508ef 401 0.80080264666570744000
Kovalev_D 231:079835d508ef 402 };*/
Kovalev_D 231:079835d508ef 403 /* float ACoefP[NCoefP+1] = {
Kovalev_D 231:079835d508ef 404 0.00024156733395523967,
Kovalev_D 231:079835d508ef 405 0.00048313466791047934,
Kovalev_D 231:079835d508ef 406 0.00024156733395523967
Kovalev_D 231:079835d508ef 407 }; //2HZ
Kovalev_D 231:079835d508ef 408
Kovalev_D 231:079835d508ef 409 float BCoefP[NCoefP+1] = {
Kovalev_D 231:079835d508ef 410 1.00000000000000000000,
Kovalev_D 231:079835d508ef 411 -1.95557824031503500000,
Kovalev_D 231:079835d508ef 412 0.95654367651120309000
Kovalev_D 231:079835d508ef 413 };*//*
Kovalev_D 231:079835d508ef 414 float ACoefP[NCoefP+1] = {
Kovalev_D 231:079835d508ef 415 0.00000530383940879362,
Kovalev_D 231:079835d508ef 416 0.00001060767881758724,
Kovalev_D 231:079835d508ef 417 0.00000530383940879362
Kovalev_D 231:079835d508ef 418 }; //0.125HZ
Kovalev_D 231:079835d508ef 419
Kovalev_D 231:079835d508ef 420 float BCoefP[NCoefP+1] = {
Kovalev_D 231:079835d508ef 421 1.00000000000000000000,
Kovalev_D 231:079835d508ef 422 -1.99444641054192680000,
Kovalev_D 231:079835d508ef 423 0.99446178907595384000
Kovalev_D 231:079835d508ef 424 };*/
Kovalev_D 231:079835d508ef 425 /*float ACoefP[NCoefP+1] = {
Kovalev_D 231:079835d508ef 426 0.00024156733395523967,
Kovalev_D 231:079835d508ef 427 0.00048313466791047934,
Kovalev_D 231:079835d508ef 428 0.00024156733395523967
Kovalev_D 231:079835d508ef 429 }; //1HZ
Kovalev_D 227:2774b56bfab0 430
Kovalev_D 231:079835d508ef 431 float BCoefP[NCoefP+1] = {
Kovalev_D 231:079835d508ef 432 1.00000000000000000000,
Kovalev_D 231:079835d508ef 433 -1.95557824031503500000,
Kovalev_D 231:079835d508ef 434 0.95654367651120309000
Kovalev_D 231:079835d508ef 435 };*/
Kovalev_D 231:079835d508ef 436 /*
Kovalev_D 231:079835d508ef 437 float ACoefP[NCoefP+1] = {
Kovalev_D 231:079835d508ef 438 0.00013397155178191382,
Kovalev_D 231:079835d508ef 439 0.00026794310356382763,
Kovalev_D 231:079835d508ef 440 0.00013397155178191382
Kovalev_D 231:079835d508ef 441 }; //0.75HZ
Kovalev_D 231:079835d508ef 442
Kovalev_D 231:079835d508ef 443 float BCoefP[NCoefP+1] = {
Kovalev_D 231:079835d508ef 444 1.00000000000000000000,
Kovalev_D 231:079835d508ef 445 -1.96668138526348500000,
Kovalev_D 231:079835d508ef 446 0.96722742815186025000
Kovalev_D 231:079835d508ef 447 };*/
Kovalev_D 231:079835d508ef 448 float ACoefP[NCoefP+1] = {
Kovalev_D 231:079835d508ef 449 0.00036133932576050832,
Kovalev_D 231:079835d508ef 450 0.00072267865152101664,
Kovalev_D 231:079835d508ef 451 0.00036133932576050832
Kovalev_D 231:079835d508ef 452 }; //1.25HZ
Kovalev_D 231:079835d508ef 453
Kovalev_D 231:079835d508ef 454 float BCoefP[NCoefP+1] = {
Kovalev_D 231:079835d508ef 455 1.00000000000000000000,
Kovalev_D 231:079835d508ef 456 -1.94447765776709370000,
Kovalev_D 231:079835d508ef 457 0.94597793623228155000
Kovalev_D 231:079835d508ef 458 };
Kovalev_D 231:079835d508ef 459
Kovalev_D 231:079835d508ef 460
Kovalev_D 231:079835d508ef 461 /*
Kovalev_D 231:079835d508ef 462 float ACoefP[NCoefP+1] = {
Kovalev_D 231:079835d508ef 463 0.29289321881333569000,
Kovalev_D 231:079835d508ef 464 0.58578643762667137000,
Kovalev_D 231:079835d508ef 465 0.29289321881333569000
Kovalev_D 231:079835d508ef 466 }; // 100HZ
Kovalev_D 231:079835d508ef 467
Kovalev_D 231:079835d508ef 468 float BCoefP[NCoefP+1] = {
Kovalev_D 231:079835d508ef 469 1.00000000000000000000,
Kovalev_D 231:079835d508ef 470 0.00000000000000004878,
Kovalev_D 231:079835d508ef 471 0.17157287525380990000
Kovalev_D 231:079835d508ef 472 };*/
Kovalev_D 231:079835d508ef 473 static float yP[NCoefP+1]; //output samples
Kovalev_D 231:079835d508ef 474 static float xP[NCoefP+1]; //input samples
Kovalev_D 231:079835d508ef 475 int n;
Kovalev_D 231:079835d508ef 476
Kovalev_D 231:079835d508ef 477 //shift the old samples
Kovalev_D 231:079835d508ef 478 for(n=NCoef; n>0; n--) {
Kovalev_D 231:079835d508ef 479 xP[n] = xP[n-1];
Kovalev_D 231:079835d508ef 480 yP[n] = yP[n-1];
Kovalev_D 231:079835d508ef 481 }
Kovalev_D 231:079835d508ef 482
Kovalev_D 231:079835d508ef 483 //Calculate the new output
Kovalev_D 231:079835d508ef 484 xP[0] = NewSampleP;
Kovalev_D 231:079835d508ef 485 yP[0] = ACoefP[0] * xP[0];
Kovalev_D 231:079835d508ef 486 for(n=1; n<=NCoefP; n++)
Kovalev_D 231:079835d508ef 487 yP[0] += ACoefP[n] * xP[n] - BCoefP[n] * yP[n];
Kovalev_D 231:079835d508ef 488
Kovalev_D 231:079835d508ef 489 return yP[0];
Kovalev_D 220:04c54405b82d 490 }
Kovalev_D 216:189b0ea1dc38 491
Kovalev_D 112:4a96133a1311 492 void DAC_OutPut(void)//выдача в цапы
igor_v 0:8ad47e2b6f00 493 {
Kovalev_D 220:04c54405b82d 494 /*if(Gyro.RgConA&0x10)*/ Modulator();
Kovalev_D 157:1069c80f4944 495 LPC_SSP0->DR=0x5555;
igor_v 31:c783288001b5 496 LPC_SSP0->DR=0x5555;
igor_v 31:c783288001b5 497 LPC_SSP0->DR=0x5555;
igor_v 0:8ad47e2b6f00 498
Kovalev_D 209:224e7331a061 499 if (CountV31 & 1)
Kovalev_D 209:224e7331a061 500 { //если нечетный такт то
Kovalev_D 218:b4067cac75c0 501 LPC_SSP0->DR = WRITE_DAC0; //e.команда для ЦАП_0 передавать.
Kovalev_D 220:04c54405b82d 502 LPC_SSP0->DR = (Spi.DAC_A); //e. передача 12 бит
Kovalev_D 209:224e7331a061 503 }
Kovalev_D 209:224e7331a061 504 else
Kovalev_D 209:224e7331a061 505 { //если такт четный.
Kovalev_D 157:1069c80f4944 506 LPC_SSP0->DR = WRITE_DAC1 ; //e.команда для ЦАП_1 передавать.
Kovalev_D 220:04c54405b82d 507 LPC_SSP0->DR = (Spi.DAC_B) ;
igor_v 31:c783288001b5 508 }
Kovalev_D 112:4a96133a1311 509 }
Kovalev_D 112:4a96133a1311 510
Kovalev_D 112:4a96133a1311 511
Kovalev_D 113:8be429494918 512
Kovalev_D 113:8be429494918 513
Kovalev_D 113:8be429494918 514
Kovalev_D 113:8be429494918 515