Dmitry Kovalev
/
LGfiltr
forkd
Fork of LGstaandart by
Diff: vibro.c
- Revision:
- 218:b4067cac75c0
- Parent:
- 216:189b0ea1dc38
- Child:
- 226:4a4d5bd5fcd7
diff -r 15cd8752bb6c -r b4067cac75c0 vibro.c --- a/vibro.c Mon Sep 04 12:55:13 2017 +0000 +++ b/vibro.c Mon Sep 25 10:35:56 2017 +0000 @@ -128,44 +128,49 @@ { int temp=0; + static unsigned int FConunt=0; int LowDZ,HiDZ; + /* if(FConunt<4) + {*/ + //FConunt++; Gyro.CaunPlus = CaunAddPlus;//амплитуда по модулю из востановленного синиуса Buff_Restored_sin - // Gyro.CaunPlus = Gyro.CaunPlusReper; + Gyro.CaunMin = CaunAddMin; //амплитуда по модулю из востановленного синиуса Buff_Restored_sin CaunAddPlus = 0; - Gyro.CaunMin = CaunAddMin; //амплитуда по модулю из востановленного синиуса Buff_Restored_sin - //Gyro.CaunMin=Gyro.CaunMinReper; CaunAddMin = 0; Gyro.MaxAmp = Gyro.CaunPlus + Gyro.CaunMin; - - - if(countFras<512) - { - countFras++; - Gyro.F_rasAdd += Gyro.MaxAmp/32*Gyro.FrqHZ/40; - } - else - { - Gyro.F_rasAdd += Gyro.MaxAmp/32*Gyro.FrqHZ/40; - Gyro.F_ras=Gyro.F_rasAdd>>9; - Gyro.F_rasAdd=0; - countFras=0; - } - if(Gyro.RgConA&0x20) - { +// } + /*else + {*/ + // FConunt=0; + //Gyro.MaxAmp=Gyro.MaxAmp>>2; + if(countFras<512) + { + countFras++; + Gyro.F_rasAdd += Gyro.MaxAmp/32*Gyro.FrqHZ/40; + } + else + { + Gyro.F_rasAdd += Gyro.MaxAmp/32*Gyro.FrqHZ/40; + Gyro.F_ras=Gyro.F_rasAdd>>9; + Gyro.F_rasAdd=0; + countFras=0; + } + if(Gyro.RgConA&0x20) + { //расчет максимальной амплитуды из востановленного синуса р-р. - temp=(int)(((Gyro.MaxAmp - Gyro.AmpTarget*2/((Gyro.Frq)>>16)) * Gyro.AmpSpeed)); - temp=temp>>6; - LowDZ = ((Gyro.AmpSpeed<<3)*(-1)); - HiDZ = (Gyro.AmpSpeed<<3); - Gyro.Amp -= temp; // расчет амплитуды ВП с учетом разници - if((Gyro.Amp>>16) > (Gyro.AmpPerMax)) {Gyro.Amp = (Gyro.AmpPerMax << 16);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа////////// - if((Gyro.Amp>>16) < (Gyro.AmpPerMin)) {Gyro.Amp = (Gyro.AmpPerMin << 16);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа////////// + temp=(int)(((Gyro.MaxAmp - Gyro.AmpTarget*2/((Gyro.Frq)>>16)) * Gyro.AmpSpeed)); + temp=temp>>6; + LowDZ = ((Gyro.AmpSpeed<<3)*(-1)); + HiDZ = (Gyro.AmpSpeed<<3); + Gyro.Amp -= temp>>4; // расчет амплитуды ВП с учетом разници + if((Gyro.Amp>>16) > (Gyro.AmpPerMax)) {Gyro.Amp = (Gyro.AmpPerMax << 16);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа////////// + if((Gyro.Amp>>16) < (Gyro.AmpPerMin)) {Gyro.Amp = (Gyro.AmpPerMin << 16);} // временное ограничение роста амплитуды ВП в случае неподоженного гироскопа////////// // Gyro.AmpPer = (Gyro.Amp)>>16; //приведение амплитуды ВП к виду 0%-100% - } - - LPC_MCPWM->MAT1 = T_vib_1; - LPC_MCPWM->MAT2 = T_vib_2; - } + } + LPC_MCPWM->MAT1 = T_vib_1; + LPC_MCPWM->MAT2 = T_vib_2; + // } + }