Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1762 → Rev 1763

/trunk/menu.c
154,7 → 154,7
case 12:
LCD_printfxy(0,0,"Servo " );
LCD_printfxy(0,1,"Setpoint %3i",Parameter_ServoNickControl);
LCD_printfxy(0,2,"Position: %3i",ServoNickValue);
LCD_printfxy(0,2,"Position: %3i",ServoNickValue/4);
LCD_printfxy(0,3,"Range:%3i-%3i",EE_Parameter.ServoNickMin,EE_Parameter.ServoNickMax);
break;
case 13:
/trunk/timer0.c
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
}
}