174,10 → 174,21 |
else if( ServoValue > ( (int) EE_Parameter.ServoNickMax * 3 ) ) |
ServoValue = (int) EE_Parameter.ServoNickMax * 3; |
|
long integral; |
|
/* Über Parameter läßt sich zwischen "+" und "X" - Formations |
* umschalten (sh. parameter.h) |
*/ |
if( PARAM_X_FORMATION ) { |
integral = IntegralNick - IntegralRoll; |
} else { |
integral = IntegralNick; |
} |
|
if( EE_Parameter.ServoNickCompInvert & 0x01 ) |
ServoValue += ( (long) ( (long) EE_Parameter.ServoNickComp * ( IntegralNick - IntegralRoll ) ) >> DIV_128 ) / ( 512L >> DIV_4 ); |
ServoValue += ( (long) ( (long) EE_Parameter.ServoNickComp * integral ) >> DIV_128 ) / ( 512L >> DIV_4 ); |
else |
ServoValue -= ( (long) ( (long) EE_Parameter.ServoNickComp * ( IntegralNick - IntegralRoll ) ) >> DIV_128 ) / ( 512L >> DIV_4 ); |
ServoValue -= ( (long) ( (long) EE_Parameter.ServoNickComp * integral ) >> DIV_128 ) / ( 512L >> DIV_4 ); |
|
DebugOut.Analog[20] = ServoValue; |
|