199,55 → 199,55 |
|
if(CalculateServoSignals == 1) |
{ |
nick = (cosinus * IntegralNick) / 128L - (sinus * IntegralRoll) / 128L; |
nick = (EE_Parameter.ServoNickComp * nick) / 512L; |
ServoNickOffset += ((int16_t)Parameter_ServoNickControl * (MULTIPLYER*16) - ServoNickOffset) / EE_Parameter.ServoManualControlSpeed; |
ServoNickValue = ServoNickOffset / 16; // offset (Range from 0 to 255 * 3 = 765) |
if(EE_Parameter.ServoCompInvert & 0x01) |
{ // inverting movement of servo |
ServoNickValue += nick;//(int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * nick) / (256L) ); |
} |
else |
{ // non inverting movement of servo |
ServoNickValue -= nick; |
} |
// limit servo value to its parameter range definition |
if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) ) |
{ |
ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER; |
} |
else |
if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) ) |
{ |
ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER; |
} |
if(PlatinenVersion < 20) CalculateServoSignals = 0; else CalculateServoSignals++; |
nick = (cosinus * IntegralNick) / 128L - (sinus * IntegralRoll) / 128L; |
nick = ((long)EE_Parameter.ServoNickComp * nick) / 512L; |
ServoNickOffset += ((int16_t)Parameter_ServoNickControl * (MULTIPLYER*16) - ServoNickOffset) / EE_Parameter.ServoManualControlSpeed; |
ServoNickValue = ServoNickOffset / 16; // offset (Range from 0 to 255 * 3 = 765) |
if(EE_Parameter.ServoCompInvert & 0x01) |
{ // inverting movement of servo |
ServoNickValue += nick;//(int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * nick) / (256L) ); |
} |
else |
{ // non inverting movement of servo |
ServoNickValue -= nick; |
} |
// limit servo value to its parameter range definition |
if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) ) |
{ |
ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER; |
} |
else |
if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) ) |
{ |
ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER; |
} |
if(PlatinenVersion < 20) CalculateServoSignals = 0; else CalculateServoSignals++; |
} |
else |
{ |
roll = (cosinus * IntegralRoll) / 128L + (sinus * IntegralNick) / 128L; |
roll = (EE_Parameter.ServoRollComp * roll) / 512L; |
ServoRollOffset += ((int16_t)Parameter_ServoRollControl * (MULTIPLYER*16) - ServoRollOffset) / EE_Parameter.ServoManualControlSpeed; |
ServoRollValue = ServoRollOffset/16; // offset (Range from 0 to 255 * 3 = 765) |
if(EE_Parameter.ServoCompInvert & 0x02) |
{ // inverting movement of servo |
ServoRollValue += roll; |
} |
else |
{ // non inverting movement of servo |
ServoRollValue -= roll; |
} |
// 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; |
} |
CalculateServoSignals = 0; |
roll = (cosinus * IntegralRoll) / 128L + (sinus * IntegralNick) / 128L; |
roll = ((long)EE_Parameter.ServoRollComp * roll) / 512L; |
ServoRollOffset += ((int16_t)Parameter_ServoRollControl * (MULTIPLYER*16) - ServoRollOffset) / EE_Parameter.ServoManualControlSpeed; |
ServoRollValue = ServoRollOffset/16; // offset (Range from 0 to 255 * 3 = 765) |
if(EE_Parameter.ServoCompInvert & 0x02) |
{ // inverting movement of servo |
ServoRollValue += roll; |
} |
else |
{ // non inverting movement of servo |
ServoRollValue -= roll; |
} |
// 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; |
} |
CalculateServoSignals = 0; |
} |
} |
|
293,6 → 293,7 |
{ |
TCCR2A |= (1<<COM2A0); // make a low pulse |
RemainingPulse = PPM_FRAMELEN - ServoFrameTime; |
CalculateServoSignals = 1; |
} |
// set pulse output active |
PulseOutput = 1; |
358,9 → 359,9 |
if((ServoActive && SenderOkay > 180) || ServoActive == 2) HEF4017R_OFF; // disable HEF4017 reset |
else HEF4017R_ON; |
ServoIndex++; // change to next servo channel |
if(ServoIndex > EE_Parameter.ServoNickRefresh) |
{ |
CalculateServoSignals = 1; |
if(ServoIndex > EE_Parameter.ServoNickRefresh) |
{ |
CalculateServoSignals = 1; |
ServoIndex = 0; // reset to the sync gap |
} |
} |