Subversion Repositories FlightCtrl

Rev

Rev 687 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 687 Rev 689
Line 31... Line 31...
31 
        TCCR2A &= ~((1<<COM2B1)|(1<<COM2B0)|(1<<COM2A0));
 31 
        TCCR2A &= ~((1<<COM2B1)|(1<<COM2B0)|(1<<COM2A0));
32 
    TCCR2A |= (1<<COM2A1)|(1<<WGM21)|(1<<WGM20);
 32 
    TCCR2A |= (1<<COM2A1)|(1<<WGM21)|(1<<WGM20);
Line 33... Line 33...
33 
 
 33 
 
Line 34... Line 34...
34 
    // Timer/Counter 2 Control Register B
 34 
    // Timer/Counter 2 Control Register B
35 
 
 35 
 
36 
        // Set clock divider for timer 2 to SYSKLOCK/8 = 20MHz / 8 = 2.5MHz.
 36 
        // Set clock divider for timer 2 to SYSKLOCK/8 = 20MHz / 8 = 2.5MHz
Line 37... Line 37...
37 
        // The timer increments from 0x00 to 0xFF with an update rate of 2.5 MHz,
 37 
        // The timer increments from 0x00 to 0xFF with an update rate of 2.5 MHz,
38 
        // hence the timer overflow interrupt frequency is 2.5 MHz / 256 = 9.765 kHz
 38 
        // hence the timer overflow interrupt frequency is 2.5 MHz / 256 = 9.765 kHz or 0.1024 ms
39 
 
 39 
 
Line 63... Line 63...
63 
{
 63 
{
64 
  static uint8_t timer = 10;
 64 
  static uint8_t timer = 10;
Line 65... Line 65...
65 
 
 65 
 
66 
  if(!timer--)
 66 
  if(!timer--)
67 
    {
 67 
    {
68 
     // enable PWM on PD7 in non inverting mode
 68 
                 // enable PWM on PD7 in non inverting mode
Line 69... Line 69...
69 
     TCCR2A = (TCCR2A & 0x3F)|(1<<COM2A1)|(0<<COM2A0);
 69 
                 TCCR2A = (TCCR2A & 0x3F)|(1<<COM2A1)|(0<<COM2A0);
70 
 
 70 
 
71 
     ServoValue =  Parameter_ServoNickControl;
 71 
                 ServoValue =  Parameter_ServoNickControl;
72 
     // inverting movment of servo
 72 
                 // inverting movment of servo
73 
     if(ParamSet.ServoNickCompInvert & 0x01)
 73 
                 if(ParamSet.ServoNickCompInvert & 0x01)
74 
     {
 74 
                 {
75 
                 ServoValue += ((int32_t) ParamSet.ServoNickComp * (IntegralNick / 128)) / 512;
 75 
                         ServoValue += ((int32_t) ParamSet.ServoNickComp * (IntegralNick / 128)) / 512;
76 
         }
 76 
                 }
77 
     else // non inverting movement of servo
 77 
                 else // non inverting movement of servo
78 
     {
 78 
                 {
79 
                 ServoValue -= ((int32_t) ParamSet.ServoNickComp * (IntegralNick / 128)) / 512;
 79 
                         ServoValue -= ((int32_t) ParamSet.ServoNickComp * (IntegralNick / 128)) / 512;
80 
         }
 80 
                 }
81 
 
 81 
 
82 
         // limit servo value to its parameter range definition
 82 
                 // limit servo value to its parameter range definition
83 
     if(ServoValue < ParamSet.ServoNickMin)
 83 
                 if(ServoValue < ParamSet.ServoNickMin)
84 
     {
 84 
                 {
85 
                 ServoValue = ParamSet.ServoNickMin;
 85 
                         ServoValue = ParamSet.ServoNickMin;
86 
         }
 86 
                 }
87 
     else if(ServoValue > ParamSet.ServoNickMax)
 87 
                 else if(ServoValue > ParamSet.ServoNickMax)
88 
     {
 88 
                 {
89 
                 ServoValue = ParamSet.ServoNickMax;
 89 
                         ServoValue = ParamSet.ServoNickMax;
90 
     }
 90 
                 }
91 
 
 91 
 
92 
         // update PWM
 92 
                 // update PWM
93 
     OCR2A = ServoValue;
 93 
                 OCR2A = ServoValue;
94 
     timer = ParamSet.ServoNickRefresh;
 94 
                 timer = ParamSet.ServoNickRefresh;
95 
    }
 95 
    }
96 
    else
 96 
    else
97 
    {
 97 
    {