Subversion Repositories FlightCtrl

Rev

Rev 1156 | Rev 1171 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
1 ingob 1
#include "main.h"
2
 
3
volatile unsigned int CountMilliseconds = 0;
4
volatile static unsigned int tim_main;
5
volatile unsigned char UpdateMotor = 0;
6
volatile unsigned int cntKompass = 0;
7
volatile unsigned int beeptime = 0;
723 hbuss 8
volatile unsigned char SendSPI = 0;
910 hbuss 9
volatile unsigned int ServoState = 40;
723 hbuss 10
 
173 holgerb 11
unsigned int BeepMuster = 0xffff;
1105 killagreg 12
int ServoValue = 0;
1 ingob 13
 
1156 hbuss 14
volatile int16_t        ServoNickValue = 0;
15
volatile int16_t        ServoRollValue = 0;
16
 
17
#define HEF4017R_ON     PORTC |=  (1<<PORTC6)
18
#define HEF4017R_OFF    PORTC &= ~(1<<PORTC6)
19
 
20
 
1 ingob 21
enum {
22
  STOP             = 0,
23
  CK               = 1,
24
  CK8              = 2,
25
  CK64             = 3,
26
  CK256            = 4,
27
  CK1024           = 5,
28
  T0_FALLING_EDGE  = 6,
29
  T0_RISING_EDGE   = 7
30
};
31
 
32
 
33
SIGNAL (SIG_OVERFLOW0)    // 8kHz
34
{
35
    static unsigned char cnt_1ms = 1,cnt = 0;
173 holgerb 36
    unsigned char pieper_ein = 0;
1 ingob 37
//    TCNT0 -= 250;//TIMER_RELOAD_VALUE;
723 hbuss 38
   if(SendSPI) SendSPI--;
1 ingob 39
   if(!cnt--)
40
    {
1105 killagreg 41
     cnt = 9;
1 ingob 42
     cnt_1ms++;
43
     cnt_1ms %= 2;
44
     if(!cnt_1ms) UpdateMotor = 1;
45
     CountMilliseconds++;
1105 killagreg 46
    }
1 ingob 47
 
48
     if(beeptime > 1)
49
        {
1105 killagreg 50
        beeptime--;
51
        if(beeptime & BeepMuster)
173 holgerb 52
         {
53
          pieper_ein = 1;
54
         }
55
         else pieper_ein = 0;
1 ingob 56
        }
1105 killagreg 57
     else
173 holgerb 58
      {
59
       pieper_ein = 0;
60
       BeepMuster = 0xffff;
1105 killagreg 61
      }
173 holgerb 62
 
63
 
64
     if(pieper_ein)
65
        {
66
          if(PlatinenVersion == 10) PORTD |= (1<<2); // Speaker an PORTD.2
67
          else                      PORTC |= (1<<7); // Speaker an PORTC.7
68
        }
1105 killagreg 69
     else
173 holgerb 70
        {
71
         if(PlatinenVersion == 10) PORTD &= ~(1<<2);
72
         else                      PORTC &= ~(1<<7);
1105 killagreg 73
        }
74
 
1 ingob 75
 if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)
76
 {
77
  if(PINC & 0x10)
78
   {
1105 killagreg 79
    cntKompass++;
1 ingob 80
   }
81
  else
82
   {
1105 killagreg 83
    if((cntKompass) && (cntKompass < 362))
84
    {
693 hbuss 85
     cntKompass += cntKompass / 41;
86
     if(cntKompass > 10) KompassValue = cntKompass - 10; else KompassValue = 0;
1105 killagreg 87
    }
1 ingob 88
//     if(cntKompass < 10) cntKompass = 10;
89
//     KompassValue = (unsigned long)((unsigned long)(cntKompass-10)*720L + 1L) / 703L;
90
     KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
91
    cntKompass = 0;
1105 killagreg 92
   }
1 ingob 93
 }
94
}
95
 
96
 
97
// -----------------------------------------------------------------------
98
 
99
unsigned int SetDelay (unsigned int t)
100
{
101
//  TIMSK0 &= ~_BV(TOIE0);
1105 killagreg 102
  return(CountMilliseconds + t + 1);
1 ingob 103
//  TIMSK0 |= _BV(TOIE0);
104
}
105
 
106
// -----------------------------------------------------------------------
107
char CheckDelay(unsigned int t)
108
{
109
//  TIMSK0 &= ~_BV(TOIE0);
110
  return(((t - CountMilliseconds) & 0x8000) >> 9);
111
//  TIMSK0 |= _BV(TOIE0);
112
}
113
 
114
// -----------------------------------------------------------------------
115
void Delay_ms(unsigned int w)
116
{
117
 unsigned int akt;
118
 akt = SetDelay(w);
119
 while (!CheckDelay(akt));
120
}
121
 
395 hbuss 122
void Delay_ms_Mess(unsigned int w)
123
{
124
 unsigned int akt;
125
 akt = SetDelay(w);
1166 hbuss 126
 while (!CheckDelay(akt)) if(AdReady) {AdReady = 0; ANALOG_ON;}
395 hbuss 127
}
128
 
1156 hbuss 129
/*****************************************************/
130
/*              Initialize Timer 2                   */
131
/*****************************************************/
132
// The timer 2 is used to generate the PWM at PD7 (J7)
133
// to control a camera servo for nick compensation.
134
void TIMER2_Init(void)
910 hbuss 135
{
1156 hbuss 136
        uint8_t sreg = SREG;
137
 
138
        // disable all interrupts before reconfiguration
139
        cli();
140
 
141
        // set PD7 as output of the PWM for nick servo
142
        DDRD  |= (1<<DDD7);
143
        PORTD &= ~(1<<PORTD7);  // set PD7 to low
144
 
145
        DDRC  |= (1<<DDC6);     // set PC6 as output (Reset for HEF4017)
146
        PORTC &= ~(1<<PORTC6);  // set PC6 to low
147
 
148
        // Timer/Counter 2 Control Register A
149
 
150
        // Timer Mode is FastPWM with timer reload at OCR2A (Bits: WGM22 = 1, WGM21 = 1, WGM20 = 1)
151
    // PD7: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, COM2A0 = 0)
152
    // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0)
153
        TCCR2A &= ~((1<<COM2A1)|(1<<COM2A0)|(1<<COM2B1)|(1<<COM2B0));
154
    TCCR2A |= (1<<WGM21)|(1<<WGM20);
155
 
156
    // Timer/Counter 2 Control Register B
157
 
158
        // Set clock divider for timer 2 to SYSKLOCK/32 = 20MHz / 32 = 625 kHz
159
        // The timer increments from 0x00 to 0xFF with an update rate of 625 kHz or 1.6 us
160
        // hence the timer overflow interrupt frequency is 625 kHz / 256 = 2.44 kHz or 0.4096 ms
161
 
162
    // divider 32 (Bits: CS022 = 0, CS21 = 1, CS20 = 1)
163
        TCCR2B &= ~((1<<FOC2A)|(1<<FOC2B)|(1<<CS22));
164
    TCCR2B |= (1<<CS21)|(1<<CS20)|(1<<WGM22);
165
 
166
        // Initialize the Timer/Counter 2 Register
167
    TCNT2 = 0;
168
 
169
        // Initialize the Output Compare Register A used for PWM generation on port PD7.
170
        OCR2A = 255;
171
        TCCR2A |= (1<<COM2A1); // set or clear at compare match depends on value of COM2A0
172
 
173
        // Timer/Counter 2 Interrupt Mask Register
174
        // Enable timer output compare match A Interrupt only
175
        TIMSK2 &= ~((1<<OCIE2B)|(1<<TOIE2));
176
        TIMSK2 |= (1<<OCIE2A);
177
 
178
    SREG = sreg;
910 hbuss 179
}
180
 
1156 hbuss 181
//----------------------------
182
void Timer_Init(void)
1 ingob 183
{
1156 hbuss 184
    tim_main = SetDelay(10);
185
    TCCR0B = CK8;
186
    TCCR0A = (1<<COM0A1)|(1<<COM0B1)|3;//fast PWM
187
    OCR0A =  0;
188
    OCR0B = 120;
189
    TCNT0 = (unsigned char)-TIMER_RELOAD_VALUE;  // reload
190
    //OCR1  = 0x00;
1105 killagreg 191
 
1156 hbuss 192
    TIMSK0 |= _BV(TOIE0);
193
}
194
 
195
 
196
/*****************************************************/
197
/*              Control Servo Position               */
198
/*****************************************************/
199
 
200
ISR(TIMER2_COMPA_vect)
201
{
202
 
203
        // frame len 22.5 ms = 14063 * 1.6 us
204
        // stop pulse: 0.3 ms = 188 * 1.6 us
205
        // min servo pulse: 0.6 ms =  375 * 1.6 us
206
        // max servo pulse: 2.4 ms = 1500 * 1.6 us
207
        // resolution: 1500 - 375 = 1125 steps
208
 
209
        #define IRS_RUNTIME 127
210
        #define PPM_STOPPULSE 188
211
//      #define PPM_FRAMELEN (14063
212
#define PPM_FRAMELEN (1757 * EE_Parameter.ServoNickRefresh)
213
        #define MINSERVOPULSE 375
214
        #define MAXSERVOPULSE 1500
215
        #define SERVORANGE (MAXSERVOPULSE - MINSERVOPULSE)
216
 
217
        static uint8_t  PulseOutput = 0;
218
        static uint16_t RemainingPulse = 0;
219
        static uint16_t ServoFrameTime = 0;
220
        static uint8_t  ServoIndex = 0;
221
 
222
        #define MULTIPLYER 4
223
        static int16_t ServoNickOffset = (255 / 2) * MULTIPLYER; // initial value near center positon
224
 
225
 
226
        if(PlatinenVersion < 20)
227
        {
228
                //---------------------------
229
                // Nick servo state machine
230
                //---------------------------
231
                if(!PulseOutput) // pulse output complete
232
                {
233
                        if(TCCR2A & (1<<COM2A0)) // we had a low pulse
234
                        {
235
                                TCCR2A &= ~(1<<COM2A0);// make a high pulse
236
                                RemainingPulse  = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms
237
 
238
                                ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset
239
                                ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765)
240
                                if(EE_Parameter.ServoNickCompInvert & 0x01)
241
                                {       // inverting movement of servo
242
                                        ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
243
                                }
244
                                else
245
                                {       // non inverting movement of servo
246
                                        ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
247
                                }
248
                                // limit servo value to its parameter range definition
249
                                if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )
250
                                {
251
                                        ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;
252
                                }
253
                                else
254
                                if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) )
255
                                {
256
                                        ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER;
257
                                }
258
 
259
                                RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
260
 
261
                                ServoNickValue /= MULTIPLYER;
262
                                DebugOut.Analog[20] = ServoNickValue;
263
 
264
                                // range servo pulse width
265
                                if(RemainingPulse > MAXSERVOPULSE )                     RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit
266
                                else if(RemainingPulse < MINSERVOPULSE )        RemainingPulse = MINSERVOPULSE; // lower servo pulse limit
267
                                // accumulate time for correct update rate
268
                                ServoFrameTime = RemainingPulse;
269
                        }
270
                        else // we had a high pulse
271
                        {
272
                                TCCR2A |= (1<<COM2A0); // make a low pulse
273
                                RemainingPulse = PPM_FRAMELEN - ServoFrameTime;
274
                        }
275
                        // set pulse output active
276
                        PulseOutput = 1;
277
                }
278
        } // EOF Nick servo state machine
279
        else
280
        {
281
                //-----------------------------------------------------
282
                // PPM state machine, onboard demultiplexed by HEF4017
283
                //-----------------------------------------------------
284
                if(!PulseOutput) // pulse output complete
285
                {
286
                        if(TCCR2A & (1<<COM2A0)) // we had a low pulse
287
                        {
288
                                TCCR2A &= ~(1<<COM2A0);// make a high pulse
289
 
290
                                if(ServoIndex == 0) // if we are at the sync gap
291
                                {
292
                                        RemainingPulse = PPM_FRAMELEN - ServoFrameTime; // generate sync gap by filling time to full frame time
293
                                        ServoFrameTime = 0; // reset servo frame time
294
                                        HEF4017R_ON; // enable HEF4017 reset
295
                                }
296
                                else // servo channels
297
                                {
298
                                        RemainingPulse  = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms
299
                                        switch(ServoIndex) // map servo channels
300
                                        {
301
                                                case 1: // Nick Compensation Servo
302
                                                        ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset
303
                                                        ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765)
304
                                                        if(EE_Parameter.ServoNickCompInvert & 0x01)
305
                                                        {       // inverting movement of servo
306
                                                                ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
307
                                                        }
308
                                                        else
309
                                                        {       // non inverting movement of servo
310
                                                                ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
311
                                                        }
312
                                                        // limit servo value to its parameter range definition
313
                                                        if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )
314
                                                        {
315
                                                                ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;
316
                                                        }
317
                                                        else
318
                                                        if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) )
319
                                                        {
320
                                                                ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER;
321
                                                        }
322
                                                        RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
323
                                                        ServoNickValue /= MULTIPLYER;
324
                                                        DebugOut.Analog[20] = ServoNickValue;
325
                                                        break;
326
 
327
                                                default: // other servo channels
328
                                                        RemainingPulse += 2 * PPM_in[ServoIndex]; // add channel value, factor of 2 because timer 1 increments 3.2µs
329
                                                        break;
330
                                        }
331
                                        // range servo pulse width
332
                                        if(RemainingPulse > MAXSERVOPULSE )                     RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit
333
                                        else if(RemainingPulse < MINSERVOPULSE )        RemainingPulse = MINSERVOPULSE; // lower servo pulse limit
334
                                        // substract stop pulse width
335
                                        RemainingPulse -= PPM_STOPPULSE;
336
                                        // accumulate time for correct sync gap
337
                                        ServoFrameTime += RemainingPulse;
338
                                }
339
                        }
340
                        else // we had a high pulse
341
                        {
342
                                TCCR2A |= (1<<COM2A0); // make a low pulse
343
                                // set pulsewidth to stop pulse width
344
                                RemainingPulse = PPM_STOPPULSE;
345
                                // accumulate time for correct sync gap
346
                                ServoFrameTime += RemainingPulse;
347
                                HEF4017R_OFF; // disable HEF4017 reset
348
                                ServoIndex++; // change to next servo channel
349
                                if(ServoIndex > EE_Parameter.ServoNickRefresh) ServoIndex = 0; // reset to the sync gap
350
                        }
351
                        // set pulse output active
352
                        PulseOutput = 1;
353
                }
354
        } // EOF PPM state machine
355
 
356
        // General pulse output generator
357
        if(RemainingPulse > (255 + IRS_RUNTIME))
358
        {
359
                OCR2A = 255;
360
                RemainingPulse -= 255;
910 hbuss 361
        }
1156 hbuss 362
        else
363
        {
364
                if(RemainingPulse > 255) // this is the 2nd last part
365
                {
366
                        if((RemainingPulse - 255) < IRS_RUNTIME)
367
                        {
368
                                OCR2A = 255 - IRS_RUNTIME;
369
                                RemainingPulse -= 255 - IRS_RUNTIME;
370
 
371
                        }
372
                        else // last part > ISR_RUNTIME
373
                        {
374
                                OCR2A = 255;
375
                                RemainingPulse -= 255;
376
                        }
377
                }
378
                else // this is the last part
379
                {
380
                        OCR2A = RemainingPulse;
381
                        RemainingPulse = 0;
382
                        PulseOutput = 0; // trigger to stop pulse
383
                }
384
        } // EOF general pulse output generator
1111 hbuss 385
}