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