Subversion Repositories FlightCtrl

Rev

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

Rev 1503 Rev 1504
Line 11... Line 11...
11
unsigned int BeepMuster = 0xffff;
11
unsigned int BeepMuster = 0xffff;
Line 12... Line 12...
12
 
12
 
13
volatile int16_t        ServoNickValue = 0;
13
volatile int16_t        ServoNickValue = 0;
Line -... Line 14...
-
 
14
volatile int16_t        ServoRollValue = 0;
-
 
15
 
-
 
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;
Line 14... Line 22...
14
volatile int16_t        ServoRollValue = 0;
22
 
15
 
23
 
16
 
24
 
17
enum {
25
enum {
Line 26... Line 34...
26
};
34
};
Line 27... Line 35...
27
 
35
 
28
 
36
 
29
SIGNAL (SIG_OVERFLOW0)    // 9,7kHz
37
SIGNAL (SIG_OVERFLOW0)    // 9,7kHz
30
{
38
{
31
    static unsigned char cnt_1ms = 1,cnt = 0;
39
        static unsigned char cnt_1ms = 1,cnt = 0;
32
    unsigned char pieper_ein = 0;
40
    unsigned char pieper_ein = 0;
Line 33... Line 41...
33
   if(SendSPI) SendSPI--;
41
        if(SendSPI) SendSPI--;
34
   if(SpektrumTimer) SpektrumTimer--;
42
        if(SpektrumTimer) SpektrumTimer--;
35
 
43
 
36
   if(!cnt--)
44
        if(!cnt--)
37
    {
45
    {
38
     cnt = 9;
46
                cnt = 9;
39
     cnt_1ms++;
47
                cnt_1ms++;
40
     cnt_1ms %= 2;
48
                cnt_1ms %= 2;
Line 41... Line 49...
41
     if(!cnt_1ms) UpdateMotor = 1;
49
                if(!cnt_1ms) UpdateMotor = 1;
42
     CountMilliseconds++;
50
                CountMilliseconds++;
43
    }
51
    }
44
 
52
 
45
     if(beeptime >= 1)
-
 
46
        {
-
 
47
        beeptime--;
-
 
48
        if(beeptime & BeepMuster)
-
 
49
         {
-
 
50
          pieper_ein = 1;
-
 
51
         }
53
    if(beeptime >= 1)
52
         else pieper_ein = 0;
54
    {
53
        }
-
 
54
     else
-
 
55
      {
-
 
56
       pieper_ein = 0;
-
 
57
       BeepMuster = 0xffff;
-
 
58
      }
-
 
59
 
-
 
60
     if(pieper_ein)
55
                beeptime--;
61
        {
56
        if(beeptime & BeepMuster)
62
          if(PlatinenVersion == 10) PORTD |= (1<<2); // Speaker an PORTD.2
57
                {
63
          else                      PORTC |= (1<<7); // Speaker an PORTC.7
58
                        pieper_ein = 1;
64
        }
-
 
65
     else
59
        }
-
 
60
        else
-
 
61
                {
-
 
62
                        pieper_ein = 0;
-
 
63
        }
-
 
64
    }
-
 
65
        else
Line 66... Line -...
66
        {
-
 
67
         if(PlatinenVersion == 10) PORTD &= ~(1<<2);
-
 
68
         else                      PORTC &= ~(1<<7);
-
 
69
        }
-
 
70
 
66
    {
71
 if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)
-
 
72
 {
-
 
73
  if(PINC & 0x10)
-
 
74
   {
-
 
75
    cntKompass++;
67
                pieper_ein = 0;
-
 
68
                BeepMuster = 0xffff;
-
 
69
    }
76
   }
70
 
-
 
71
    if(pieper_ein)
-
 
72
    {
-
 
73
                if(PlatinenVersion == 10)
77
  else
74
                {
-
 
75
                        PORTD |= (1<<2); // Speaker an PORTD.2
-
 
76
        }
-
 
77
                else                      
-
 
78
                {
-
 
79
                        PORTC |= (1<<7); // Speaker an PORTC.7
-
 
80
        }
-
 
81
        }
-
 
82
    else
-
 
83
    {
-
 
84
                if(PlatinenVersion == 10)
-
 
85
                {
-
 
86
                        PORTD &= ~(1<<2);
78
   {
87
                }
-
 
88
        else                      
-
 
89
                {
-
 
90
                        PORTC &= ~(1<<7);
-
 
91
                }
-
 
92
    }
-
 
93
 
-
 
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
79
    if((cntKompass) && (cntKompass < 362))
110
                        {
80
    {
111
                                KompassValue = 0;
-
 
112
                        }
81
     cntKompass += cntKompass / 41;
113
                }
82
     if(cntKompass > 10) KompassValue = cntKompass - 10; else KompassValue = 0;
114
 
83
    }
115
// if(cntKompass < 10) cntKompass =r 10;
84
//     if(cntKompass < 10) cntKompass =r 10;
116
// KompassValue = (unsigned long)((unsigned long)(cntKompass-10)*720L + 1L) / 703L;
Line 85... Line 117...
85
//     KompassValue = (unsigned long)((unsigned long)(cntKompass-10)*720L + 1L) / 703L;
117
               
Line 86... Line 118...
86
     KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
118
                KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
Line 214... Line 246...
214
 
246
 
215
        #define MULTIPLYER 4
247
        #define MULTIPLYER 4
216
        static int16_t ServoNickOffset = (255 / 2) * MULTIPLYER; // initial value near center positon
248
        static int16_t ServoNickOffset = (255 / 2) * MULTIPLYER; // initial value near center positon
Line 217... Line 249...
217
        static int16_t ServoRollOffset = (255 / 2) * MULTIPLYER; // initial value near center positon
249
        static int16_t ServoRollOffset = (255 / 2) * MULTIPLYER; // initial value near center positon
-
 
250
 
-
 
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)
218
 
261
    // 091114. Inserted same changes into v.0.76g code.
219
    // replace below line with if(PlatinenVersion < 20) to revert to original (WKN-change)
262
       
220
        if((PlatinenVersion < 20) && (Parameter_UserParam8 < 128 ))
263
        if((PlatinenVersion < 20) && (Parameter_UserParam8 < 128 ))
221
        {
264
        {
222
                //---------------------------
265
                //---------------------------
Line 313... Line 356...
313
                                                                ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER;
356
                                                                ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER;
314
                                                        }
357
                                                        }
315
                                                        RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
358
                                                        RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
316
                                                        ServoNickValue /= MULTIPLYER;
359
                                                        ServoNickValue /= MULTIPLYER;
317
                                                        break;
360
                                                        break;
318
                                         case 2: // Roll Compensation Servo
361
                                                case 2: // Roll Compensation Servo
319
                                                        ServoRollOffset = (ServoRollOffset * 3 + (int16_t) Parameter_ServoRollControl  * MULTIPLYER) / 4; // lowpass offset
362
                                                        ServoRollOffset = (ServoRollOffset * 3 + (int16_t) Parameter_ServoRollControl  * MULTIPLYER) / 4; // lowpass offset
320
                                                        ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765)
363
                                                        ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765)
321
                                                        if(EE_Parameter.ServoCompInvert & 0x02)
364
                                                        if(EE_Parameter.ServoCompInvert & 0x02)
322
                                                        {       // inverting movement of servo
365
                                                        {       // inverting movement of servo
323
                                                                ServoRollValue += (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) );
366
                                                                ServoRollValue += (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) );
Line 338... Line 381...
338
                                                        }
381
                                                        }
339
                                                        RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
382
                                                        RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
340
                                                        ServoRollValue /= MULTIPLYER;
383
                                                        ServoRollValue /= MULTIPLYER;
341
                                                        //DebugOut.Analog[20] = ServoRollValue;
384
                                                        //DebugOut.Analog[20] = ServoRollValue;
342
                                                        break;
385
                                                        break;
-
 
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.
343
 
388
                                                       
-
 
389
                                                        if(PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] < -32)
-
 
390
                                                        {
-
 
391
                                                        // Set servo to null position, turning camera off.
344
                                                default: // other servo channels
392
                                                                RemainingPulse  = MINSERVOPULSE;
-
 
393
                                                        }
-
 
394
                                                        else
-
 
395
                                                        {
345
                                                        RemainingPulse += 2 * PPM_in[ServoIndex]; // add channel value, factor of 2 because timer 1 increments 3.2µs
396
                                                                RemainingPulse  = MINSERVOPULSE + SERVORANGE/2;
-
 
397
                                                        }
346
                                                        break;
398
                                                        break;
-
 
399
 
-
 
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;
347
                                        }
405
                                        }
-
 
406
 
348
                                        // range servo pulse width
407
                                        // range servo pulse width
349
                                        if(RemainingPulse > MAXSERVOPULSE )                     RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit
408
                                        if(RemainingPulse > MAXSERVOPULSE )                     RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit
350
                                        else if(RemainingPulse < MINSERVOPULSE )        RemainingPulse = MINSERVOPULSE; // lower servo pulse limit
409
                                        else if(RemainingPulse < MINSERVOPULSE )        RemainingPulse = MINSERVOPULSE; // lower servo pulse limit
351
                                        // substract stop pulse width
410
                                        // substract stop pulse width
352
                                        RemainingPulse -= PPM_STOPPULSE;
411
                                        RemainingPulse -= PPM_STOPPULSE;