Subversion Repositories FlightCtrl

Rev

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

Rev 1105 Rev 1111
Line 155... Line 155...
155 
 
 155 
 
156 
SIGNAL(SIG_OUTPUT_COMPARE2A)
 156 
SIGNAL(SIG_OUTPUT_COMPARE2A)
157 
{
 157 
{
158 
  static unsigned char postPulse = 0x80;
 158 
  static unsigned char postPulse = 0x80;
- 
 
 159 
  static int filterServo = 100;
159 
  static int filterServo = 100;
 160 
  static unsigned char restPulse = 50;
- 
 
 161 
  #define MULTIPLIER 4
- 
 
 162 
  if(PlatinenVersion < 20)
160 
  #define MULTIPLIER 4
 163 
  {
161 
  if(ServoState == 4)
 164 
   if(ServoState == 4)
162 
    {
 165 
    {
163 
     ServoValue = 0x0030;  // Offset Part1
 166 
     ServoValue = 0x0030;  // Offset Part1
164 
     filterServo = (filterServo * 3 + (int) Parameter_ServoNickControl * 2)/4;
 167 
     filterServo = (filterServo * 3 + (int) Parameter_ServoNickControl * 2)/4;
165 
         ServoValue += filterServo;
 168 
         ServoValue += filterServo;
Line 171... Line 174...
171 
         DebugOut.Analog[20] = ServoValue;
 174 
         DebugOut.Analog[20] = ServoValue;
172 
         if ((ServoValue % 255) < 45) { ServoValue+= 77; postPulse = 0x60 - 77; } else postPulse = 0x60;
 175 
         if ((ServoValue % 255) < 45) { ServoValue+= 77; postPulse = 0x60 - 77; } else postPulse = 0x60;
173 
         OCR2A = 255-(ServoValue % 256);
 176 
         OCR2A = 255-(ServoValue % 256);
174 
         TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;
 177 
         TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;
175 
        }
 178 
        }
176 
  else if ((ServoState > 0) && (ServoState < 4))
 179 
    else if ((ServoState > 0) && (ServoState < 4))
177 
    {
 180 
    {
178 
          if(ServoValue > 255)
 181 
          if(ServoValue > 255)
179 
          { PORTD |= 0x80;
 182 
          { PORTD |= 0x80;
180 
                TCCR2A =3;
 183 
                TCCR2A =3;
181 
                ServoValue -= 255;
 184 
                ServoValue -= 255;
Line 184... Line 187...
184 
      {
 187 
      {
185 
             TCCR2A=(1<<COM2A1)|(0<<COM2A0)|3;
 188 
             TCCR2A=(1<<COM2A1)|(0<<COM2A0)|3;
186 
                 OCR2A = postPulse; // Offset Part2
 189 
                 OCR2A = postPulse; // Offset Part2
187 
                 ServoState = 1;
 190 
                 ServoState = 1;
188 
      }
 191 
      }
189 
  }
 192 
    }
190 
  else if (ServoState == 0)
 193 
    else if (ServoState == 0)
191 
          {
 194 
          {
192 
             ServoState  = (int) EE_Parameter.ServoNickRefresh * MULTIPLIER;
 195 
             ServoState  = (int) EE_Parameter.ServoNickRefresh * MULTIPLIER;
193 
         PORTD&=~0x80;
 196 
         PORTD&=~0x80;
194 
                 TCCR2A = 3;
 197 
                 TCCR2A = 3;
195 
          }
 198 
          }
- 
 
 199 
  }
- 
 
 200 
 else
- 
 
 201 
  {
196 
  ServoState--;
 202 
  if(ServoState == 4)
- 
 
 203 
    {
- 
 
 204 
     PORTD &= ~0x80;
- 
 
 205 
     PORTC |= _BV(PC6);
- 
 
 206 
     ServoValue = 0x00030;  // Offset Part1
- 
 
 207 
     filterServo = (filterServo * 3 + (int) Parameter_ServoNickControl * 2)/4;
- 
 
 208 
     ServoValue += filterServo;
- 
 
 209 
     if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += (int)( ( (long)EE_Parameter.ServoNickComp * (IntegralNick / 128L) ) / (512L/MULTIPLIER) );
- 
 
 210 
     else ServoValue -= (int)( ( (long)EE_Parameter.ServoNickComp * (IntegralNick / 128L) ) / (512L/MULTIPLIER) );
- 
 
 211 
     if((ServoValue) < ((int)EE_Parameter.ServoNickMin*3)) ServoValue = (int)EE_Parameter.ServoNickMin*3;
- 
 
 212 
     else if((ServoValue) > ((int)EE_Parameter.ServoNickMax*3)) ServoValue = (int)EE_Parameter.ServoNickMax*3;
- 
 
 213 
    PORTD |= 0x80;     // dummy clock to skip output Q0
- 
 
 214 
  if ((ServoValue % 256) < 1)  ServoValue -=2;
- 
 
 215 
  if ((ServoValue % 256) > 253)  ServoValue +=2;
- 
 
 216 
  DebugOut.Analog[20] = ServoValue;
- 
 
 217 
 
- 
 
 218 
  OCR2A = 254-(ServoValue % 255);
- 
 
 219 
  PORTD &= ~0x80;
197 
 
 220 
 
- 
 
 221 
  TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;    // set on match
- 
 
 222 
  //ServoValue -= OCR2A;
- 
 
 223 
  PORTC &= ~_BV(PC6);
- 
 
 224 
    }
- 
 
 225 
 else if ((ServoState > 0) && (ServoState < 4))
- 
 
 226 
    {
- 
 
 227 
   if(ServoValue > 255)
- 
 
 228 
   {
- 
 
 229 
    PORTD &= ~0x80;
- 
 
 230 
    TCCR2A =3;
- 
 
 231 
    ServoValue -= 255;
- 
 
 232 
    OCR2A = postPulse; // Offset Part2
- 
 
 233 
    //OCR2A = (ServoValue % 256);
- 
 
 234 
     }
- 
 
 235 
   else
- 
 
 236 
      {
- 
 
 237 
      OCR2A = postPulse; // Offset Part2
- 
 
 238 
      //OCR2A = (ServoValue % 256);
- 
 
 239 
      //TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;
- 
 
 240 
      ServoState = 1;
- 
 
 241 
      }
- 
 
 242 
    }
- 
 
 243 
  else if (ServoState == 0)
- 
 
 244 
    {
- 
 
 245 
     PORTD &= ~0x80;
- 
 
 246 
     TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;
- 
 
 247 
     ServoState  = (int) EE_Parameter.ServoNickRefresh * MULTIPLIER;
- 
 
 248 
     TCCR2A =3;
- 
 
 249 
    }
- 
 
 250 
   ServoState--;
- 
 
 251 
 }
198 
}
 252 
}
- 
 
 253