Subversion Repositories FlightCtrl

Rev

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

Rev 838 Rev 966
Line 9... Line 9...
9 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 9 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Line 10... Line 10...
10 
 
 10 
 
11 
#include "rc.h"
 11 
#include "rc.h"
Line 12... Line 12...
12 
#include "main.h"
 12 
#include "main.h"
13 
 
 13 
 
14 
volatile int PPM_in[11];
 14 
int PPM_in[11] = {0,0,0,0,0,0,0,0,0,0,0};
Line 15... Line 15...
15 
int RCQuality = 0;
 15 
int RCQuality = 0;
16 
volatile unsigned char NewPpmData = 1;
 16 
unsigned char NewPpmData = 1;
17 
 
 17 
 
18 
//############################################################################
 18 
//############################################################################
19 
//zum decodieren des PPM-Signals wird Timer1 mit seiner Input
 19 
//zum decodieren des PPM-Signals wird Timer1 mit seiner Input
20 
//Capture Funktion benutzt:
 20 
//Capture Funktion benutzt:
21 
void rc_sum_init (void)
 21 
void rc_sum_init (void)
22 
//############################################################################
 22 
//############################################################################
23 
{
 23 
{
24 
        TCCR1B=(1<<CS11)|(1<<CS10)|(1<<ICES1)|(1<<ICNC1); //timer1 prescale 64
24 
  TCCR1B=(1<<CS11)|(1<<CS10)|(1<<ICES1)|(1<<ICNC1); //timer1 prescale 64
Line 25... Line 25...
25 
    TIMSK1 |= _BV(ICIE1);
 25 
  TIMSK1 |= _BV(ICIE1);
26 
    return;
 26 
  return;
27 
}
 27 
}
28 
 
 28 
 
29 
//############################################################################
 29 
//############################################################################
30 
//Diese Routine startet und inizialisiert den Timer für RC
 30 
//Diese Routine startet und inizialisiert den Timer für RC
31 
SIGNAL(SIG_INPUT_CAPTURE1)
 31 
SIGNAL(SIG_INPUT_CAPTURE1)
32 
//############################################################################
 32 
//############################################################################
33 
{
 33 
{
34 
        static unsigned int AltICR=0;
 34 
  static unsigned int AltICR=0;
35 
    signed int signal = 0,tmp;
 35 
  signed int signal = 0,tmp;
36 
        static int index;              
 36 
  static int index;
37 
        static float RC_Quality = 0.0F;        
 37 
  static float RC_Quality = 0.0F;
38 
    static int PPM_org[11];
 38 
  static int PPM_org[11];
39 
 
 39 
 
40 
        signal = (unsigned int) ICR1 - AltICR;         
 40 
  signal = (unsigned int) ICR1 - AltICR;
- 
 
 41 
  AltICR = ICR1;
- 
 
 42 
 
- 
 
 43 
  //Syncronisationspause?
- 
 
 44 
  if ((signal > 1500) && (signal < 8000))        
- 
 
 45 
  {
- 
 
 46 
    index = 1;
- 
 
 47 
    NewPpmData = 0;  // Null bedeutet: Neue Daten
41 
        AltICR = ICR1; 
 48 
  }
- 
 
 49 
  else
- 
 
 50 
  {
42 
       
 51 
    if(index < 10)
- 
 
 52 
    {
43 
    //Syncronisationspause?
 53 
      if((signal > 250) && (signal < 687))
- 
 
 54 
      {
- 
 
 55 
        signal -= 466;
- 
 
 56 
        // Stabiles Signal
- 
 
 57 
        if(abs(signal - PPM_in[index]) < 6)
- 
 
 58 
        {
44 
        if ((signal > 1500) && (signal < 8000))  
 59 
          if(SenderOkay < 200)
- 
 
 60 
          {
- 
 
 61 
            SenderOkay += 10;
- 
 
 62 
          }
45 
    {
 63 
        }
- 
 
 64 
        /* Give an estimate for the Signal Level based on the RC-Jitter see
46 
        index = 1;             
 65 
        http://forum.mikrokopter.de/topic-post44807.html#post44807*/
47 
        NewPpmData = 0;  // Null bedeutet: Neue Daten
 66 
        if (abs(2 * (signal - PPM_org[index]) > (int) RC_Quality))
48 
    }
 67 
        {
49 
        else
 - 
 
50 
    {
 - 
 
51 
        if(index < 10)
 - 
 
52 
        {
 - 
 
53 
            if((signal > 250) && (signal < 687))
 - 
 
54 
            {
 - 
 
55 
                signal -= 466;
 - 
 
56 
                // Stabiles Signal     
- 
 
57 
                                if(abs(signal - PPM_in[index]) < 6)
 - 
 
58 
                                {
 - 
 
59 
                                        if(SenderOkay < 200)
 - 
 
60 
                                        {
 - 
 
61 
                                                SenderOkay += 10;
 - 
 
62 
                                        }
 - 
 
63 
                                }
 - 
 
64 
                                /* Give an estimate for the Signal Level based on the RC-Jitter */
 - 
 
65 
                                if (abs(2 * (signal - PPM_org[index]) > (int) RC_Quality))
 - 
 
66 
                                {
 - 
 
67 
                                        RC_Quality = 0.99F * RC_Quality + 0.01F * (float) abs(2 * (signal - PPM_org[index])) ;                         
 68 
          RC_Quality = 0.99F * RC_Quality + 0.01F * (float) abs(2 * (signal - PPM_org[index])) ;
68 
                                }
 - 
 
69 
                                else
 - 
 
70 
                                {
 - 
 
71 
                                        RC_Quality = 0.998F * RC_Quality + 0.002F * (float) abs(2 * (signal - PPM_org[index])) ;
 - 
 
72 
                                }              
 - 
 
73 
                                tmp = (3 * (PPM_in[index]) + signal) / 4;  
 - 
 
74 
                                PPM_in[index] = tmp;
 - 
 
75 
                                PPM_org[index] = signal;
 - 
 
76 
                        }        
 - 
 
77 
                        else
 - 
 
78 
                        {
 - 
 
79 
                                        RC_Quality = 0.95F * RC_Quality + 0.05F * 100;         
 - 
 
80 
                        }
 - 
 
81 
                        RC_Quality = MIN(100.F, RC_Quality);
 - 
 
82 
                        RCQuality = 100 - (int) RC_Quality;        
 - 
 
83 
                        index++;  
 69 
        }
- 
 
 70 
        else
- 
 
 71 
        {
- 
 
 72 
          RC_Quality = 0.998F * RC_Quality + 0.002F * (float) abs(2 * (signal - PPM_org[index]));
- 
 
 73 
        }
- 
 
 74 
        tmp = (3 * (PPM_in[index]) + signal) / 4;  
- 
 
 75 
        PPM_in[index] = tmp;
- 
 
 76 
        PPM_org[index] = signal;
- 
 
 77 
      }
- 
 
 78 
      else
- 
 
 79 
      {
- 
 
 80 
        RC_Quality = 0.95F * RC_Quality + 0.05F * 100;         
- 
 
 81 
      }
- 
 
 82 
      RC_Quality = MIN(100.F, RC_Quality);
- 
 
 83 
      RCQuality = 100 - (int) RC_Quality;
- 
 
 84 
      DebugOut.Analog[12] = RCQuality;
- 
 
 85 
      index++;  
84 
                        if(index == 5) PORTD |= 0x20; else PORTD &= ~0x20;  // Servosignal an J3 anlegen
 86 
      if(index == 5) PORTD |= 0x20; else PORTD &= ~0x20;  // Servosignal an J3 anlegen
85 
                        if(index == 6) PORTD |= 0x10; else PORTD &= ~0x10;  // Servosignal an J4 anlegen
 87 
      if(index == 6) PORTD |= 0x10; else PORTD &= ~0x10;  // Servosignal an J4 anlegen