82,10 → 82,8 |
case 4: |
// recalculate new ServoValue |
ServoValue = 0x0030; // Offset (part 1) |
DebugOut.Analog[16] = ServoValue; |
FilterServo = (3 * FilterServo + (uint16_t)FCParam.ServoNickControl * 2) / 4; // lowpass static offset |
ServoValue += FilterServo; // add filtered static offset |
|
if(ParamSet.ServoNickCompInvert & 0x01) |
{ // inverting movement of servo |
ServoValue += (int16_t)( ( (int32_t)ParamSet.ServoNickComp * (IntegralNick / 128L ) ) / (512L/MULTIPLIER) ); |
94,7 → 92,6 |
{ // non inverting movement of servo |
ServoValue -= (int16_t)( ( (int32_t)ParamSet.ServoNickComp * (IntegralNick / 128L ) ) / (512L/MULTIPLIER) ); |
} |
|
// limit servo value to its parameter range definition |
if(ServoValue < ((int16_t)ParamSet.ServoNickMin * 3) ) |
{ |
105,8 → 102,8 |
{ |
ServoValue = (int16_t)ParamSet.ServoNickMax * 3; |
} |
DebugOut.Analog[20] = ServoValue; |
|
DebugOut.Analog[20] = ServoValue; |
// determine prepulse width (remaining part of ServoValue/Timer Cycle) |
if ((ServoValue % 255) < 45) |
{ // if prepulse width is to short the execution time of this ISR is longer than the next compare match |