Dmitry Kovalev / Mbed 2 deprecated LGfiltr

forkd

Dependencies:   mbed

Fork of LGstaandart by Dmitry Kovalev

SPI.c@232:130a2b5003e6, 2018-01-31 (annotated)

Committer:
Kovalev_D
Date:
Wed Jan 31 13:41:23 2018 +0000
Revision:
232:130a2b5003e6
Parent:
231:079835d508ef 
static termocorr

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