Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1446 rain-er 1
/*#######################################################################################
2
Decodieren eines RC Summen Signals
3
#######################################################################################*/
4
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5
// + Copyright (c) Holger Buss, Ingo Busker
6
// + only for non-profit use
7
// + www.MikroKopter.com
8
// + porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
9
// + see the File "License.txt" for further Informations
10
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
11
 
12
#include "rc.h"
13
#include "main.h"
14
//#define ACT_S3D_SUMMENSIGNAL
15
 
16
volatile int PPM_in[11];
17
volatile int PPM_diff[11];  // das diffenzierte Stick-Signal
18
volatile char Channels,tmpChannels = 0;
19
volatile unsigned char NewPpmData = 1;
20
 
21
//############################################################################
22
//zum decodieren des PPM-Signals wird Timer1 mit seiner Input
23
//Capture Funktion benutzt:
24
void rc_sum_init (void)
25
//############################################################################
26
{
27
#ifndef ACT_S3D_SUMMENSIGNAL
28
        TCCR1B=(1<<CS11)|(1<<CS10)|(1<<ICES1)|(1<<ICNC1);//|(1 << WGM12); //timer1 prescale 64
29
#else 
30
        TCCR1B=(1<<CS11)|(0<<CS10)|(1<<ICES1)|(1<<ICNC1); //timer1 prescale 64
31
#endif
32
    TIMSK1 |= _BV(ICIE1);
33
    AdNeutralGier = 0;
34
    AdNeutralRoll = 0;
35
    AdNeutralNick = 0;
36
    return;
37
}
38
 
39
#ifndef ACT_S3D_SUMMENSIGNAL
40
//############################################################################
41
//Diese Routine startet und inizialisiert den Timer für RC
42
SIGNAL(SIG_INPUT_CAPTURE1)
43
//############################################################################
44
{
45
if(!(EE_Parameter.ExtraConfig & CFG_SENSITIVE_RC))
46
 {
47
        static unsigned int AltICR=0;
48
    signed int signal = 0,tmp;
49
        static int index;              
50
 
51
        signal = (unsigned int) ICR1 - AltICR;         
52
        AltICR = ICR1; 
53
    //Syncronisationspause? (3.52 ms < signal < 25.6 ms)
54
        if((signal > 1100) && (signal < 8000))   
55
        {
56
        Channels = index;
57
        if(index >= 4)  NewPpmData = 0;  // Null bedeutet: Neue Daten
58
        index = 1;             
59
        }
60
        else
61
        {
62
        if(index < 10)
63
            {
64
            if((signal > 250) && (signal < 687))
65
                {
66
                signal -= 466;
67
                // Stabiles Signal
68
                if(abs(signal - PPM_in[index]) < 6) { if(SenderOkay < 200) SenderOkay += 10; else SenderOkay = 200;}
69
                tmp = (3 * (PPM_in[index]) + signal) / 4;  
70
                if(tmp > signal+1) tmp--; else
71
                if(tmp < signal-1) tmp++;
72
                if(SenderOkay >= 195)  PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3;
73
                else PPM_diff[index] = 0;
74
                PPM_in[index] = tmp;
75
                }
76
            index++;  
77
         if(index == 5) J3High; else J3Low;  // Servosignal an J3 anlegen
78
         if(index == 6) J4High; else J4Low;  // Servosignal an J4 anlegen
79
         if(index == 7) J5High; else J5Low;  // Servosignal an J5 anlegen 
80
        }
81
        }
82
 }
83
 else
84
 {
85
        static unsigned int AltICR=0;
86
    static int ppm_in[11];
87
    static int ppm_diff[11];  
88
    static int old_ppm_in[11];
89
    static int old_ppm_diff[11];  
90
    signed int signal = 0,tmp;
91
        static unsigned char index, okay_cnt = 0;              
92
        signal = (unsigned int) ICR1 - AltICR;         
93
        AltICR = ICR1; 
94
    //Syncronisationspause? (3.52 ms < signal < 25.6 ms)
95
        if((signal > 1100) && (signal < 8000))   
96
        {
97
        tmpChannels = index;
98
        if(tmpChannels >= 4 && Channels == tmpChannels)  
99
                 {
100
          if(okay_cnt > 10)
101
                   {
102
                   NewPpmData = 0;  // Null bedeutet: Neue Daten
103
                   for(index = 0; index < 11; index++)
104
                    {
105
                         if(okay_cnt > 30)
106
                          {
107
                           old_ppm_in[index] = PPM_in[index];
108
                           old_ppm_diff[index] = PPM_diff[index];
109
                          }
110
                     PPM_in[index] = ppm_in[index];
111
                     PPM_diff[index] = ppm_diff[index];
112
                    }
113
                   }
114
          if(okay_cnt < 255) okay_cnt++;
115
                 }
116
         else
117
                  {
118
                   if(okay_cnt > 100) okay_cnt = 10; else okay_cnt = 0;
119
                   ROT_ON;
120
                  }
121
        index = 1;             
122
        if(!MotorenEin) Channels = tmpChannels;
123
        }
124
        else
125
        {
126
        if(index < 10)
127
            {
128
            if((signal > 250) && (signal < 687))
129
                {
130
                signal -= 466;
131
                // Stabiles Signal
132
                if((abs(signal - ppm_in[index]) < 6))
133
                                 {
134
                                  if(okay_cnt > 25)  SenderOkay += 10;
135
                                  else
136
                                  if(okay_cnt > 10)  SenderOkay += 2;
137
                                  if(SenderOkay > 200) SenderOkay = 200;
138
                                 }
139
                tmp = (3 * (ppm_in[index]) + signal) / 4;  
140
                if(tmp > signal+1) tmp--; else
141
                if(tmp < signal-1) tmp++;
142
                if(SenderOkay >= 190)  ppm_diff[index] = ((tmp - ppm_in[index]) / 3) * 3;
143
                else ppm_diff[index] = 0;
144
                ppm_in[index] = tmp;
145
                }
146
                        else ROT_ON;
147
            if(index == 5) J3High; else J3Low;  // Servosignal an J3 anlegen
148
            if(index == 6) J4High; else J4Low;  // Servosignal an J4 anlegen
149
            if(index == 7) J5High; else J5Low;  // Servosignal an J5 anlegen 
150
          }
151
                  if(index < 20) index++;
152
          else
153
                  if(index == 20)
154
                  {
155
            unsigned char i;
156
            ROT_ON;
157
                    index = 30;
158
            for(i=0;i<11;i++) // restore from older data
159
                         {
160
                      PPM_in[i] = old_ppm_in[i];
161
                      PPM_diff[i] = 0;
162
//                        okay_cnt /= 2;
163
                 }
164
                  }
165
            }
166
 }
167
}
168
 
169
#else 
170
 
171
//############################################################################
172
//Diese Routine startet und inizialisiert den Timer für RC
173
SIGNAL(SIG_INPUT_CAPTURE1)
174
//############################################################################
175
 
176
{
177
        static unsigned int AltICR=0;
178
    signed int signal = 0,tmp;
179
        static int index;              
180
 
181
        signal = (unsigned int) ICR1 - AltICR;         
182
DebugOut.Analog[16] = signal;
183
        signal /= 2;
184
        AltICR = ICR1; 
185
    //Syncronisationspause?
186
        if((signal > 1100*2) && (signal < 8000*2))       
187
        {
188
        if(index >= 4)  NewPpmData = 0;  // Null bedeutet: Neue Daten
189
        index = 1;             
190
        }
191
        else
192
        {
193
        if(index < 10)
194
            {
195
            if((signal > 250) && (signal < 687*2))
196
                {
197
                signal -= 962;
198
                // Stabiles Signal
199
                if(abs(signal - PPM_in[index]) < 6) { if(SenderOkay < 200) SenderOkay += 10;}
200
                tmp = (3 * (PPM_in[index]) + signal) / 4;  
201
                if(tmp > signal+1) tmp--; else
202
                if(tmp < signal-1) tmp++;
203
                if(SenderOkay >= 195)  PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3;
204
                else PPM_diff[index] = 0;
205
                PPM_in[index] = tmp;
206
                }
207
            index++;  
208
        }
209
        }
210
}
211
#endif
212