216,6 → 216,7 |
|
#define MULTIPLYER 4 |
static int16_t ServoNickOffset = (255 / 2) * MULTIPLYER; // initial value near center positon |
static int16_t ServoRollOffset = (255 / 2) * MULTIPLYER; // initial value near center positon |
|
if(PlatinenVersion < 20) |
{ |
317,7 → 318,56 |
ServoNickValue /= MULTIPLYER; |
DebugOut.Analog[20] = ServoNickValue; |
break; |
case 2: // Roll Compensation Servo |
ServoRollOffset = (ServoRollOffset * 3 + (int16_t) 80 * MULTIPLYER) / 4; // lowpass offset |
ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765) |
//if(EE_Parameter.ServoRollCompInvert & 0x01) |
{ // inverting movement of servo |
ServoRollValue += (int16_t)( ( (int32_t) 50 * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) ); |
} |
/* else |
{ // non inverting movement of servo |
ServoRollValue -= (int16_t)( ( (int32_t) 40 * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) ); |
} |
*/ // limit servo value to its parameter range definition |
if(ServoRollValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) ) |
{ |
ServoRollValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER; |
} |
else |
if(ServoRollValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) ) |
{ |
ServoRollValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER; |
} |
RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
ServoRollValue /= MULTIPLYER; |
//DebugOut.Analog[20] = ServoRollValue; |
|
/* ServoRollOffset = (ServoRollOffset * 3 + (int16_t)Parameter_ServoRollControl * MULTIPLYER) / 4; // lowpass offset |
ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765) |
if(EE_Parameter.ServoRollCompInvert & 0x01) |
{ // inverting movement of servo |
ServoRollValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) ); |
} |
else |
{ // non inverting movement of servo |
ServoRollValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) ); |
} |
// limit servo value to its parameter range definition |
if(ServoRollValue < ((int16_t)EE_Parameter.ServoRollMin * MULTIPLYER) ) |
{ |
ServoRollValue = (int16_t)EE_Parameter.ServoRollMin * MULTIPLYER; |
} |
else |
if(ServoRollValue > ((int16_t)EE_Parameter.ServoRollMax * MULTIPLYER) ) |
{ |
ServoRollValue = (int16_t)EE_Parameter.ServoRollMax * MULTIPLYER; |
} |
RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
ServoRollValue /= MULTIPLYER; |
//DebugOut.Analog[20] = ServoRollValue; |
*/ break; |
|
default: // other servo channels |
RemainingPulse += 2 * PPM_in[ServoIndex]; // add channel value, factor of 2 because timer 1 increments 3.2µs |
break; |