| 163,11 → 163,12 |
|---|
| ServoValue = 0x0030; // Offset Part1 |
| filterServo = (filterServo * 3 + (int) Parameter_ServoNickControl * 2)/4; |
| ServoValue += filterServo; |
| if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += ((long) ((long)EE_Parameter.ServoNickComp * IntegralNick) / 128L )/ (512L/MULTIPLIER); |
| else ServoValue -= ((long) ((long)EE_Parameter.ServoNickComp * IntegralNick) / 128L) / (512L/MULTIPLIER); |
| if((ServoValue) < ((int)EE_Parameter.ServoNickMin*3)) ServoValue = (int)EE_Parameter.ServoNickMin*3; |
| else if((ServoValue) > ((int)EE_Parameter.ServoNickMax*3)) ServoValue = (int)EE_Parameter.ServoNickMax*3; |
| |
| |
| if( EE_Parameter.ServoNickCompInvert & 0x01 ) |
| ServoValue += ( (long) ( (long) EE_Parameter.ServoNickComp * ( IntegralNick - IntegralRoll ) ) / 128L ) / ( 512L / MULTIPLIER ); |
| else |
| ServoValue -= ( (long) ( (long) EE_Parameter.ServoNickComp * ( IntegralNick - IntegralRoll ) ) / 128L ) / ( 512L / MULTIPLIER ); |
| |
| DebugOut.Analog[20] = ServoValue; |
| if ((ServoValue % 255) < 45) { ServoValue+= 77; postPulse = 0x60 - 77; } else postPulse = 0x60; |
| OCR2A = 255-(ServoValue % 256); |