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); |