Subversion Repositories FlightCtrl

Rev

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

Rev 872 Rev 910
Line 4... Line 4...
4 
volatile static unsigned int tim_main;
 4 
volatile static unsigned int tim_main;
5 
volatile unsigned char UpdateMotor = 0;
 5 
volatile unsigned char UpdateMotor = 0;
6 
volatile unsigned int cntKompass = 0;
 6 
volatile unsigned int cntKompass = 0;
7 
volatile unsigned int beeptime = 0;
 7 
volatile unsigned int beeptime = 0;
8 
volatile unsigned char SendSPI = 0;
 8 
volatile unsigned char SendSPI = 0;
- 
 
 9 
volatile unsigned int ServoState = 40;
Line 9... Line 10...
9 
 
 10 
 
10 
unsigned int BeepMuster = 0xffff;
 11 
unsigned int BeepMuster = 0xffff;
Line 11... Line 12...
11 
int ServoValue = 0;
 12 
unsigned int ServoValue = 0;
12 
 
 13 
 
13 
enum {
 14 
enum {
14 
  STOP             = 0,
 15 
  STOP             = 0,
Line 83... Line 84...
83 
    cntKompass = 0;
 84 
    cntKompass = 0;
84 
   }
 85 
   }
85 
 }
 86 
 }
86 
}
 87 
}
Line 87... Line -...
87 
 
 - 
 
- 
 
 88 
 
88 
 
 89 
//----------------------------
89 
void Timer_Init(void)
 90 
void Timer_Init(void)
90 
{
 91 
{
91 
    tim_main = SetDelay(10);
 92 
    tim_main = SetDelay(10);
92 
    TCCR0B = CK8;
 93 
    TCCR0B = CK8;
Line 95... Line 96...
95 
    OCR0B = 120;
 96 
    OCR0B = 120;
96 
    TCNT0 = (unsigned char)-TIMER_RELOAD_VALUE;  // reload
 97 
    TCNT0 = (unsigned char)-TIMER_RELOAD_VALUE;  // reload
97 
    //OCR1  = 0x00;
 98 
    //OCR1  = 0x00;
Line 98... Line 99...
98 
 
 99 
 
99 
    TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;
 100 
    TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;
- 
 
 101 
//    TCCR2B=(0<<CS20)|(1<<CS21)|(1<<CS22);    // clk/256
Line 100... Line -...
100 
    TCCR2B=(0<<CS20)|(1<<CS21)|(1<<CS22);
 - 
 
- 
 
 102 
    TCCR2B=(0<<CS20)|(0<<CS21)|(1<<CS22);      // clk/64
101 
   
 103 
   
Line 102... Line 104...
102 
//    TIMSK2 |= _BV(TOIE2);
 104 
 
103 
TIMSK2 |= _BV(OCIE2A);
 105 
  TIMSK2 |= _BV(OCIE2A);
104 
 
 106 
 
Line 141... Line 143...
141 
}
 143 
}
Line 142... Line 144...
142 
 
 144 
 
143 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
145 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
144 
//  Servo ansteuern
 146 
//  Servo ansteuern
- 
 
 147 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
- 
 
 148 
SIGNAL(SIG_OVERFLOW2)
- 
 
 149 
{
- 
 
 150 
  if (ServoState > 0) PORTD |= 0x80;
- 
 
 151 
  else PORTD &= ~0x80;
- 
 
 152 
  TCCR2A =3;
- 
 
 153 
  TIMSK2 &= ~_BV(TOIE2);
- 
 
 154 
}
145 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
155 
 
146 
SIGNAL(SIG_OUTPUT_COMPARE2A)
 156 
SIGNAL(SIG_OUTPUT_COMPARE2A)
147 
{
 157 
{
148 
  static unsigned char timer = 10;
 - 
 
- 
 
 158 
  static unsigned char postPulse = 0x80;
- 
 
 159 
  static int filterServo = 100;
149 
 
 160 
  #define MULTIPLIER 4
150 
  if(!timer--)  
 161 
  if(ServoState == 4)  
151 
    {
 162 
    {
152 
     TCCR2A=(1<<COM2A1)|(0<<COM2A0)|3;  
 163 
     ServoValue = 0x0030;  // Offset Part1
- 
 
 164 
     filterServo = (filterServo * 3 + (int) Parameter_ServoNickControl * 2)/4;
153 
     ServoValue =  Parameter_ServoNickControl;
 165 
         ServoValue += filterServo;
154 
     if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += ((long) EE_Parameter.ServoNickComp * (IntegralNick / 128)) / 512;
 166 
     if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += ((long) ((long)EE_Parameter.ServoNickComp * IntegralNick) / 128L )/ (512L/MULTIPLIER);
155 
     else ServoValue -= ((long) EE_Parameter.ServoNickComp * (IntegralNick / 128)) / 512;
 - 
 
156 
     
 167 
     else ServoValue -= ((long) ((long)EE_Parameter.ServoNickComp * IntegralNick) / 128L) / (512L/MULTIPLIER);
157 
     if(ServoValue < EE_Parameter.ServoNickMin) ServoValue = EE_Parameter.ServoNickMin;
 168 
     if((ServoValue) < ((int)EE_Parameter.ServoNickMin*3)) ServoValue = (int)EE_Parameter.ServoNickMin*3;
158 
     else if(ServoValue > EE_Parameter.ServoNickMax) ServoValue = EE_Parameter.ServoNickMax;
 169 
     else if((ServoValue) > ((int)EE_Parameter.ServoNickMax*3)) ServoValue = (int)EE_Parameter.ServoNickMax*3;
- 
 
 170 
   
- 
 
 171 
         DebugOut.Analog[20] = ServoValue;
159 
 
 172 
         if ((ServoValue % 255) < 45) { ServoValue+= 77; postPulse = 0x60 - 77; } else postPulse = 0x60;
160 
     OCR2A = ServoValue;// + 75;
 173 
         OCR2A = 255-(ServoValue % 256);
161 
     timer = EE_Parameter.ServoNickRefresh;
 174 
         TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;  
162 
    }
 175 
        }
163 
    else
 176 
  else if ((ServoState > 0) && (ServoState < 4))
- 
 
 177 
    {
- 
 
 178 
          if(ServoValue > 255)
164 
    {
 179 
          { PORTD |= 0x80;
- 
 
 180 
                TCCR2A =3;
- 
 
 181 
                ServoValue -= 255;
- 
 
 182 
          }
- 
 
 183 
          else
- 
 
 184 
      {  
- 
 
 185 
             TCCR2A=(1<<COM2A1)|(0<<COM2A0)|3;  
- 
 
 186 
                 OCR2A = postPulse; // Offset Part2
- 
 
 187 
                 ServoState = 1;
- 
 
 188 
      }
- 
 
 189 
  }
- 
 
 190 
  else if (ServoState == 0)
- 
 
 191 
          {
165 
     TCCR2A =3;
 192 
             ServoState  = (int) EE_Parameter.ServoNickRefresh * MULTIPLIER;
- 
 
 193 
         PORTD&=~0x80;
166 
     PORTD&=~0x80;
 194 
                 TCCR2A = 3;
- 
 
 195 
          }
- 
 
 196 
  ServoState--;
167 
    }
 197