Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1 ingob 1
#include "main.h"
1320 hbuss 2
#include "spectrum.h"
1 ingob 3
 
4
volatile unsigned int CountMilliseconds = 0;
5
volatile static unsigned int tim_main;
6
volatile unsigned char UpdateMotor = 0;
7
volatile unsigned int cntKompass = 0;
8
volatile unsigned int beeptime = 0;
1171 hbuss 9
volatile unsigned char SendSPI = 0, ServoActive = 0;
723 hbuss 10
 
173 holgerb 11
unsigned int BeepMuster = 0xffff;
1 ingob 12
 
1156 hbuss 13
volatile int16_t        ServoNickValue = 0;
14
volatile int16_t        ServoRollValue = 0;
15
 
1504 kneet 16
// Arthur P: Added two variables for control of the shutter servo cycle.
17
// 091114 Inserted same changes into v.0.76g code.
18
// 091114 Inactivated the following two lines as the shutter interval funtion is not
19
// used at the moment.
20
// volatile static unsigned int CameraShutterCycleCounter = 0;
21
// volatile static unsigned int CameraShutterCycle = 0;
1156 hbuss 22
 
1504 kneet 23
 
24
 
1 ingob 25
enum {
26
  STOP             = 0,
27
  CK               = 1,
28
  CK8              = 2,
29
  CK64             = 3,
30
  CK256            = 4,
31
  CK1024           = 5,
32
  T0_FALLING_EDGE  = 6,
33
  T0_RISING_EDGE   = 7
34
};
35
 
36
 
1322 hbuss 37
SIGNAL (SIG_OVERFLOW0)    // 9,7kHz
1 ingob 38
{
1504 kneet 39
        static unsigned char cnt_1ms = 1,cnt = 0;
173 holgerb 40
    unsigned char pieper_ein = 0;
1504 kneet 41
        if(SendSPI) SendSPI--;
42
        if(SpektrumTimer) SpektrumTimer--;
1320 hbuss 43
 
1504 kneet 44
        if(!cnt--)
1 ingob 45
    {
1504 kneet 46
                cnt = 9;
47
                cnt_1ms++;
48
                cnt_1ms %= 2;
49
                if(!cnt_1ms) UpdateMotor = 1;
50
                CountMilliseconds++;
1105 killagreg 51
    }
1 ingob 52
 
1504 kneet 53
    if(beeptime >= 1)
54
    {
55
                beeptime--;
1105 killagreg 56
        if(beeptime & BeepMuster)
1504 kneet 57
                {
58
                        pieper_ein = 1;
1 ingob 59
        }
1504 kneet 60
        else
61
                {
62
                        pieper_ein = 0;
63
        }
64
    }
65
        else
66
    {
67
                pieper_ein = 0;
68
                BeepMuster = 0xffff;
69
    }
173 holgerb 70
 
1504 kneet 71
    if(pieper_ein)
72
    {
73
                if(PlatinenVersion == 10)
74
                {
75
                        PORTD |= (1<<2); // Speaker an PORTD.2
173 holgerb 76
        }
1504 kneet 77
                else                      
78
                {
79
                        PORTC |= (1<<7); // Speaker an PORTC.7
1105 killagreg 80
        }
1504 kneet 81
        }
82
    else
1105 killagreg 83
    {
1504 kneet 84
                if(PlatinenVersion == 10)
85
                {
86
                        PORTD &= ~(1<<2);
87
                }
88
        else                      
89
                {
90
                        PORTC &= ~(1<<7);
91
                }
1105 killagreg 92
    }
1320 hbuss 93
 
1504 kneet 94
        if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)
95
        {
96
        if(PINC & 0x10)
97
        {
98
                cntKompass++;
99
        }
100
        else
101
        {
102
                if((cntKompass) && (cntKompass < 362))
103
                {
104
                        cntKompass += cntKompass / 41;
105
                        if(cntKompass > 10)
106
                        {
107
                                KompassValue = cntKompass - 10;
108
                        }
109
                        else
110
                        {
111
                                KompassValue = 0;
112
                        }
113
                }
114
 
115
// if(cntKompass < 10) cntKompass =r 10;
116
// KompassValue = (unsigned long)((unsigned long)(cntKompass-10)*720L + 1L) / 703L;
117
 
118
                KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
119
                cntKompass = 0;
120
                }
121
        }
122
 
1 ingob 123
}
124
 
125
 
126
// -----------------------------------------------------------------------
127
 
128
unsigned int SetDelay (unsigned int t)
129
{
130
//  TIMSK0 &= ~_BV(TOIE0);
1105 killagreg 131
  return(CountMilliseconds + t + 1);
1 ingob 132
//  TIMSK0 |= _BV(TOIE0);
133
}
134
 
135
// -----------------------------------------------------------------------
136
char CheckDelay(unsigned int t)
137
{
138
//  TIMSK0 &= ~_BV(TOIE0);
139
  return(((t - CountMilliseconds) & 0x8000) >> 9);
140
//  TIMSK0 |= _BV(TOIE0);
141
}
142
 
143
// -----------------------------------------------------------------------
144
void Delay_ms(unsigned int w)
145
{
146
 unsigned int akt;
147
 akt = SetDelay(w);
148
 while (!CheckDelay(akt));
149
}
150
 
395 hbuss 151
void Delay_ms_Mess(unsigned int w)
152
{
153
 unsigned int akt;
154
 akt = SetDelay(w);
1166 hbuss 155
 while (!CheckDelay(akt)) if(AdReady) {AdReady = 0; ANALOG_ON;}
395 hbuss 156
}
157
 
1156 hbuss 158
/*****************************************************/
159
/*              Initialize Timer 2                   */
160
/*****************************************************/
161
// The timer 2 is used to generate the PWM at PD7 (J7)
162
// to control a camera servo for nick compensation.
163
void TIMER2_Init(void)
910 hbuss 164
{
1156 hbuss 165
        uint8_t sreg = SREG;
166
 
167
        // disable all interrupts before reconfiguration
168
        cli();
1232 hbuss 169
 
1156 hbuss 170
        PORTD &= ~(1<<PORTD7);  // set PD7 to low
171
 
172
        DDRC  |= (1<<DDC6);     // set PC6 as output (Reset for HEF4017)
1171 hbuss 173
    HEF4017R_ON;
1156 hbuss 174
        // Timer/Counter 2 Control Register A
175
 
176
        // Timer Mode is FastPWM with timer reload at OCR2A (Bits: WGM22 = 1, WGM21 = 1, WGM20 = 1)
177
    // PD7: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, COM2A0 = 0)
178
    // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0)
179
        TCCR2A &= ~((1<<COM2A1)|(1<<COM2A0)|(1<<COM2B1)|(1<<COM2B0));
180
    TCCR2A |= (1<<WGM21)|(1<<WGM20);
181
 
182
    // Timer/Counter 2 Control Register B
183
 
184
        // Set clock divider for timer 2 to SYSKLOCK/32 = 20MHz / 32 = 625 kHz
185
        // The timer increments from 0x00 to 0xFF with an update rate of 625 kHz or 1.6 us
186
        // hence the timer overflow interrupt frequency is 625 kHz / 256 = 2.44 kHz or 0.4096 ms
187
 
188
    // divider 32 (Bits: CS022 = 0, CS21 = 1, CS20 = 1)
189
        TCCR2B &= ~((1<<FOC2A)|(1<<FOC2B)|(1<<CS22));
190
    TCCR2B |= (1<<CS21)|(1<<CS20)|(1<<WGM22);
191
 
192
        // Initialize the Timer/Counter 2 Register
193
    TCNT2 = 0;
194
 
195
        // Initialize the Output Compare Register A used for PWM generation on port PD7.
196
        OCR2A = 255;
197
        TCCR2A |= (1<<COM2A1); // set or clear at compare match depends on value of COM2A0
198
 
199
        // Timer/Counter 2 Interrupt Mask Register
200
        // Enable timer output compare match A Interrupt only
201
        TIMSK2 &= ~((1<<OCIE2B)|(1<<TOIE2));
202
        TIMSK2 |= (1<<OCIE2A);
203
 
204
    SREG = sreg;
910 hbuss 205
}
206
 
1156 hbuss 207
//----------------------------
208
void Timer_Init(void)
1 ingob 209
{
1156 hbuss 210
    tim_main = SetDelay(10);
211
    TCCR0B = CK8;
212
    TCCR0A = (1<<COM0A1)|(1<<COM0B1)|3;//fast PWM
213
    OCR0A =  0;
214
    OCR0B = 120;
215
    TCNT0 = (unsigned char)-TIMER_RELOAD_VALUE;  // reload
216
    //OCR1  = 0x00;
1105 killagreg 217
 
1156 hbuss 218
    TIMSK0 |= _BV(TOIE0);
219
}
220
 
221
 
222
/*****************************************************/
223
/*              Control Servo Position               */
224
/*****************************************************/
225
 
226
ISR(TIMER2_COMPA_vect)
227
{
228
        // frame len 22.5 ms = 14063 * 1.6 us
229
        // stop pulse: 0.3 ms = 188 * 1.6 us
230
        // min servo pulse: 0.6 ms =  375 * 1.6 us
231
        // max servo pulse: 2.4 ms = 1500 * 1.6 us
232
        // resolution: 1500 - 375 = 1125 steps
233
 
234
        #define IRS_RUNTIME 127
235
        #define PPM_STOPPULSE 188
236
//      #define PPM_FRAMELEN (14063
1171 hbuss 237
    #define PPM_FRAMELEN (1757 * EE_Parameter.ServoNickRefresh)
1156 hbuss 238
        #define MINSERVOPULSE 375
239
        #define MAXSERVOPULSE 1500
240
        #define SERVORANGE (MAXSERVOPULSE - MINSERVOPULSE)
241
 
242
        static uint8_t  PulseOutput = 0;
243
        static uint16_t RemainingPulse = 0;
244
        static uint16_t ServoFrameTime = 0;
245
        static uint8_t  ServoIndex = 0;
246
 
247
        #define MULTIPLYER 4
248
        static int16_t ServoNickOffset = (255 / 2) * MULTIPLYER; // initial value near center positon
1224 hbuss 249
        static int16_t ServoRollOffset = (255 / 2) * MULTIPLYER; // initial value near center positon
1156 hbuss 250
 
1504 kneet 251
    // Arthur P: Added initialization of the CameraShutterCycle value here as this routine is only 
252
        // called once. This retains all code changes in timer0.c. If parameter 6 > 0 then the user
253
        // has set a value for the cycle. CameraShuytterCycle == 5x Para6 to get approx 0.1sec increments.
254
        // 090807: Arthur P.: Removed the shutter cycle parts as they may be impacting timing loops.
255
        // CameraShutterCycle = 5 *  Parameter_UserParam6;
256
    // CameraShutterCycle = Parameter_UserParam6;
257
 
258
        // Arthur P: Modified the code to scheck the value of parameter 8. If 128 or higher then a HEF4017 is
259
        // expected and will be used. Else J7 and J9 are seen as separate normal outputs.
260
        // if((PlatinenVersion < 20)
261
    // 091114. Inserted same changes into v.0.76g code.
262
 
1503 kneet 263
        if((PlatinenVersion < 20) && (Parameter_UserParam8 < 128 ))
1156 hbuss 264
        {
265
                //---------------------------
266
                // Nick servo state machine
267
                //---------------------------
268
                if(!PulseOutput) // pulse output complete
269
                {
270
                        if(TCCR2A & (1<<COM2A0)) // we had a low pulse
271
                        {
272
                                TCCR2A &= ~(1<<COM2A0);// make a high pulse
273
                                RemainingPulse  = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms
274
 
275
                                ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset
276
                                ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765)
1232 hbuss 277
                                if(EE_Parameter.ServoCompInvert & 0x01)
1156 hbuss 278
                                {       // inverting movement of servo
279
                                        ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
280
                                }
281
                                else
282
                                {       // non inverting movement of servo
283
                                        ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
284
                                }
285
                                // limit servo value to its parameter range definition
286
                                if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )
287
                                {
288
                                        ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;
289
                                }
290
                                else
291
                                if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) )
292
                                {
293
                                        ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER;
294
                                }
295
 
296
                                RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
297
 
298
                                ServoNickValue /= MULTIPLYER;
299
 
300
                                // range servo pulse width
301
                                if(RemainingPulse > MAXSERVOPULSE )                     RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit
302
                                else if(RemainingPulse < MINSERVOPULSE )        RemainingPulse = MINSERVOPULSE; // lower servo pulse limit
303
                                // accumulate time for correct update rate
304
                                ServoFrameTime = RemainingPulse;
305
                        }
306
                        else // we had a high pulse
307
                        {
308
                                TCCR2A |= (1<<COM2A0); // make a low pulse
309
                                RemainingPulse = PPM_FRAMELEN - ServoFrameTime;
310
                        }
311
                        // set pulse output active
312
                        PulseOutput = 1;
313
                }
314
        } // EOF Nick servo state machine
315
        else
316
        {
317
                //-----------------------------------------------------
318
                // PPM state machine, onboard demultiplexed by HEF4017
319
                //-----------------------------------------------------
320
                if(!PulseOutput) // pulse output complete
321
                {
322
                        if(TCCR2A & (1<<COM2A0)) // we had a low pulse
323
                        {
324
                                TCCR2A &= ~(1<<COM2A0);// make a high pulse
325
 
326
                                if(ServoIndex == 0) // if we are at the sync gap
327
                                {
328
                                        RemainingPulse = PPM_FRAMELEN - ServoFrameTime; // generate sync gap by filling time to full frame time
329
                                        ServoFrameTime = 0; // reset servo frame time
330
                                        HEF4017R_ON; // enable HEF4017 reset
331
                                }
332
                                else // servo channels
333
                                {
334
                                        RemainingPulse  = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms
335
                                        switch(ServoIndex) // map servo channels
336
                                        {
337
                                                case 1: // Nick Compensation Servo
338
                                                        ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset
339
                                                        ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765)
1232 hbuss 340
                                                        if(EE_Parameter.ServoCompInvert & 0x01)
1156 hbuss 341
                                                        {       // inverting movement of servo
342
                                                                ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
343
                                                        }
344
                                                        else
345
                                                        {       // non inverting movement of servo
346
                                                                ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
347
                                                        }
348
                                                        // limit servo value to its parameter range definition
349
                                                        if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )
350
                                                        {
351
                                                                ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;
352
                                                        }
353
                                                        else
354
                                                        if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) )
355
                                                        {
356
                                                                ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER;
357
                                                        }
358
                                                        RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
359
                                                        ServoNickValue /= MULTIPLYER;
360
                                                        break;
1504 kneet 361
                                                case 2: // Roll Compensation Servo
1232 hbuss 362
                                                        ServoRollOffset = (ServoRollOffset * 3 + (int16_t) Parameter_ServoRollControl  * MULTIPLYER) / 4; // lowpass offset
1224 hbuss 363
                                                        ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765)
1232 hbuss 364
                                                        if(EE_Parameter.ServoCompInvert & 0x02)
1224 hbuss 365
                                                        {       // inverting movement of servo
1232 hbuss 366
                                                                ServoRollValue += (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) );
1224 hbuss 367
                                                        }
368
                                                        else
369
                                                        {       // non inverting movement of servo
1232 hbuss 370
                                                                ServoRollValue -= (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) );
1224 hbuss 371
                                                        }
1232 hbuss 372
                                                    // limit servo value to its parameter range definition
1224 hbuss 373
                                                        if(ServoRollValue < ((int16_t)EE_Parameter.ServoRollMin * MULTIPLYER) )
374
                                                        {
375
                                                                ServoRollValue = (int16_t)EE_Parameter.ServoRollMin * MULTIPLYER;
376
                                                        }
377
                                                        else
378
                                                        if(ServoRollValue > ((int16_t)EE_Parameter.ServoRollMax * MULTIPLYER) )
379
                                                        {
380
                                                                ServoRollValue = (int16_t)EE_Parameter.ServoRollMax * MULTIPLYER;
381
                                                        }
382
                                                        RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
383
                                                        ServoRollValue /= MULTIPLYER;
384
                                                        //DebugOut.Analog[20] = ServoRollValue;
1232 hbuss 385
                                                        break;
1504 kneet 386
                        case 3: // Arthur P: Shutter Servo including interval control over parameter 5 and 6. 
387
                                                                // 091114 Inserted same modification into v.0.76g code, removing previously REM-ed out modified parts.
388
 
389
                                                        if(PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] < -32)
390
                                                        {
391
                                                        // Set servo to null position, turning camera off.
392
                                                                RemainingPulse  = MINSERVOPULSE;
393
                                                        }
394
                                                        else
395
                                                        {
396
                                                                RemainingPulse  = MINSERVOPULSE + SERVORANGE/2;
397
                                                        }
398
                                                        break;
1224 hbuss 399
 
1504 kneet 400
                                                // 090807 Arthur P: Removed the output of the remaining channels as this just eats time and probably
401
                                                // does not have much of a function. Better to add specific outputs as needed.
402
                                                // default: // other servo channels
403
                                                        // RemainingPulse += 2 * PPM_in[ServoIndex]; // add channel value, factor of 2 because timer 1 increments 3.2µs
404
                                                        // break;
1156 hbuss 405
                                        }
1504 kneet 406
 
1156 hbuss 407
                                        // range servo pulse width
408
                                        if(RemainingPulse > MAXSERVOPULSE )                     RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit
409
                                        else if(RemainingPulse < MINSERVOPULSE )        RemainingPulse = MINSERVOPULSE; // lower servo pulse limit
410
                                        // substract stop pulse width
411
                                        RemainingPulse -= PPM_STOPPULSE;
412
                                        // accumulate time for correct sync gap
413
                                        ServoFrameTime += RemainingPulse;
414
                                }
415
                        }
416
                        else // we had a high pulse
417
                        {
418
                                TCCR2A |= (1<<COM2A0); // make a low pulse
419
                                // set pulsewidth to stop pulse width
420
                                RemainingPulse = PPM_STOPPULSE;
421
                                // accumulate time for correct sync gap
422
                                ServoFrameTime += RemainingPulse;
1171 hbuss 423
                                if(ServoActive && SenderOkay > 180) HEF4017R_OFF; // disable HEF4017 reset
1232 hbuss 424
                                else HEF4017R_ON;
1156 hbuss 425
                                ServoIndex++; // change to next servo channel
426
                                if(ServoIndex > EE_Parameter.ServoNickRefresh) ServoIndex = 0; // reset to the sync gap
427
                        }
428
                        // set pulse output active
429
                        PulseOutput = 1;
430
                }
431
        } // EOF PPM state machine
432
 
433
        // General pulse output generator
434
        if(RemainingPulse > (255 + IRS_RUNTIME))
435
        {
436
                OCR2A = 255;
437
                RemainingPulse -= 255;
910 hbuss 438
        }
1156 hbuss 439
        else
440
        {
441
                if(RemainingPulse > 255) // this is the 2nd last part
442
                {
443
                        if((RemainingPulse - 255) < IRS_RUNTIME)
444
                        {
445
                                OCR2A = 255 - IRS_RUNTIME;
446
                                RemainingPulse -= 255 - IRS_RUNTIME;
447
 
448
                        }
449
                        else // last part > ISR_RUNTIME
450
                        {
451
                                OCR2A = 255;
452
                                RemainingPulse -= 255;
453
                        }
454
                }
455
                else // this is the last part
456
                {
457
                        OCR2A = RemainingPulse;
458
                        RemainingPulse = 0;
459
                        PulseOutput = 0; // trigger to stop pulse
460
                }
461
        } // EOF general pulse output generator
1111 hbuss 462
}