Subversion Repositories FlightCtrl

Rev

Rev 1022 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1022 Rev 1023
Line 161... Line 161...
161
  if(ServoState == 4)  
161
  if(ServoState == 4)  
162
    {
162
    {
163
     ServoValue = 0x0030;  // Offset Part1
163
     ServoValue = 0x0030;  // Offset Part1
164
     filterServo = (filterServo * 3 + (int) Parameter_ServoNickControl * 2)/4;
164
     filterServo = (filterServo * 3 + (int) Parameter_ServoNickControl * 2)/4;
165
         ServoValue += filterServo;
165
         ServoValue += filterServo;
-
 
166
         
166
     if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += ((long) ((long)EE_Parameter.ServoNickComp * IntegralNick) / 128L )/ (512L/MULTIPLIER);
167
                if( EE_Parameter.ServoNickCompInvert & 0x01 )
167
     else ServoValue -= ((long) ((long)EE_Parameter.ServoNickComp * IntegralNick) / 128L) / (512L/MULTIPLIER);
168
                        ServoValue += ( (long) ( (long) EE_Parameter.ServoNickComp * ( IntegralNick - IntegralRoll ) ) / 128L ) / ( 512L / MULTIPLIER );
-
 
169
                else
168
     if((ServoValue) < ((int)EE_Parameter.ServoNickMin*3)) ServoValue = (int)EE_Parameter.ServoNickMin*3;
170
                        ServoValue -= ( (long) ( (long) EE_Parameter.ServoNickComp * ( IntegralNick - IntegralRoll ) ) / 128L ) / ( 512L / MULTIPLIER );
169
     else if((ServoValue) > ((int)EE_Parameter.ServoNickMax*3)) ServoValue = (int)EE_Parameter.ServoNickMax*3;
-
 
170
   
171
         
171
         DebugOut.Analog[20] = ServoValue;
172
         DebugOut.Analog[20] = ServoValue;
172
         if ((ServoValue % 255) < 45) { ServoValue+= 77; postPulse = 0x60 - 77; } else postPulse = 0x60;
173
         if ((ServoValue % 255) < 45) { ServoValue+= 77; postPulse = 0x60 - 77; } else postPulse = 0x60;
173
         OCR2A = 255-(ServoValue % 256);
174
         OCR2A = 255-(ServoValue % 256);
174
         TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;  
175
         TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;  
175
        }
176
        }