Subversion Repositories FlightCtrl

Rev

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

Rev Author Line No. Line
1538 killagreg 1
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2
// + Copyright (c) Holger Buss, Ingo Busker
3
// + Nur für den privaten Gebrauch
4
// + www.MikroKopter.com
5
// + porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
6
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
7
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
8
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
9
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
10
// + bzgl. der Nutzungsbedingungen aufzunehmen.
11
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
12
// + Verkauf von Luftbildaufnahmen, usw.
13
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
14
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
15
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
16
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
17
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
18
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
19
// + eindeutig als Ursprung verlinkt werden
20
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
21
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
22
// + Benutzung auf eigene Gefahr
23
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
24
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
25
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
26
// + mit unserer Zustimmung zulässig
27
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
28
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
29
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
30
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
31
// + this list of conditions and the following disclaimer.
32
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
33
// +     from this software without specific prior written permission.
34
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
35
// +     for non-commercial use (directly or indirectly)
36
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
37
// +     with our written permission
38
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
39
// +     clearly linked as origin
40
// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
41
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
42
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
43
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
44
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
45
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
46
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
47
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
48
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
49
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
50
// +  POSSIBILITY OF SUCH DAMAGE.
51
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
52
#include <stdlib.h>
53
#include <avr/interrupt.h>
54
 
55
#include "rc.h"
56
#include "eeprom.h"
57
#include "main.h"
58
#include "fc.h"
59
#include "uart0.h"
60
 
61
 
62
//#define ACT_S3D_SUMSIGNAL
63
 
64
volatile uint8_t RC_Channels;   // number of received channels
65
volatile int16_t PPM_in[MAX_CHANNELS]; //PPM24 supports 12 channels per frame
66
volatile int16_t PPM_diff[MAX_CHANNELS];
67
volatile uint8_t NewPpmData = 1;
68
volatile uint8_t RC_Quality = 0;
69
volatile uint8_t RC_RSSI = 0; // Received Signal Strength Indication
70
 
71
 
72
 
73
/***************************************************************/
74
/*  16bit timer 1 is used to decode the PPM-Signal            */
75
/***************************************************************/
76
void RC_Init (void)
77
{
78
        uint8_t sreg = SREG;
79
 
80
        // disable all interrupts before reconfiguration
81
        cli();
82
 
83
        // PPM-signal is connected to the Input Capture Pin (PD6) of timer 1
84
        DDRD &= ~(1<<DDD6);
85
        PORTD |= (1<<PORTD6);
86
 
87
        // Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5)
88
        // set as output
89
        DDRD |= (1<<DDD5)|(1<<DDD4);
90
        // low level
91
        PORTD &= ~((1<<PORTD5)|(1<<PORTD4));
92
 
93
        // PD3 can't be used if 2nd UART is activated
94
        // because TXD1 is at that port
95
        if(CPUType != ATMEGA644P)
96
        {
97
                DDRD |= (1<<PORTD3);
98
                PORTD &= ~(1<<PORTD3);
99
        }
100
 
101
        // Timer/Counter1 Control Register A, B, C
102
 
103
        // Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)
104
        // Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0)
105
        TCCR1A &= ~((1<<COM1A1)|(1<<COM1A0)|(1<<COM1B1)|(1<<COM1B0)|(1<<WGM11)|(1<<WGM10));
106
#ifndef ACT_S3D_SUMSIGNAL
107
        // Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)
108
        // Enable input capture noise cancler (bit: ICNC1=1)
109
        // Trigger on positive edge of the input capture pin (bit: ICES1=1),
110
        // Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2µs
111
        // The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
112
        TCCR1B &= ~((1<<WGM13)|(1<<WGM12)|(1<<CS12));
113
        TCCR1B |= (1<<CS11)|(1<<CS10)|(1<<ICES1)|(1<<ICNC1);
114
#else
115
        // Set clock source to SYSCLK/8 (bit: CS12=0, CS11=1, CS10=0)
116
        // Enable input capture noise cancler (bit: ICNC1=1)
117
        // Trigger on positive edge of the input capture pin (bit: ICES1=1),
118
        // Therefore the counter incremets at a clock of 20 MHz/8 = 2.5 MHz or 0.4µs
119
    // The longest period is 0xFFFF / 2.5 MHz = 0.026214 s.
120
        TCCR1B &= ~((1<<WGM13)|(1<<WGM12)|(1<<CS12)|(1<<CS10));
121
        TCCR1B |= (1<<CS11)|(1<<ICES1)|(1<<ICNC1);
122
#endif
123
        TCCR1C &= ~((1<<FOC1A)|(1<<FOC1B));
124
 
125
        // Timer/Counter1 Interrupt Mask Register
126
 
127
        // Enable Input Capture Interrupt (bit: ICIE1=1)
128
        // Disable Output Compare A & B Match Interrupts and Overflow Interrupt (bit: OCIE1B=0, OICIE1A=0, TOIE1=0)
129
        TIMSK1 &= ~((1<<OCIE1B)|(1<<OCIE1A)|(1<<TOIE1));
130
 
131
    PPM_INPUT_ON; //enabled by default
132
 
133
    SREG = sreg;
134
}
135
 
136
 
137
/********************************************************************/
138
/*         Every time a positive edge is detected at PD6            */
139
/********************************************************************/
140
/*                               t-Frame
141
       <----------------------------------------------------------------------->
142
         ____   ______   _____   ________                ___   ________________   ____
143
        |    | |      | |     | |        |              |   | |                | |
144
        |    | |      | |     | |        |              |   | |    sync gap    | |
145
     ___|    |_|      |_|     |_|        |_............_|   |_|                |_|
146
        <-----><-------><------><-------->              <----->                   <---
147
          t0       t1      t2       t4                     tn                       t0
148
 
149
The PPM-Frame length is 22.5 ms.
150
Channel high pulse width range is 0.7 ms to 1.7 ms completed by an 0.3 ms low pulse.
151
The mininimum time delay of two events coding a channel is ( 0.7 + 0.3) ms = 1 ms.
152
The maximum time delay of two events coding a chanel is ( 1.7 + 0.3) ms = 2 ms.
153
The minimum duration of all channels at minimum value is  8 * 1 ms = 8 ms.
154
The maximum duration of all channels at maximum value is  8 * 2 ms = 16 ms.
155
The remaining time of (22.5 - 8 ms) ms = 14.5 ms  to (22.5 - 16 ms) ms = 6.5 ms is
156
the syncronization gap.
157
*/
158
 
159
#ifndef ACT_S3D_SUMSIGNAL
160
 
161
ISR(TIMER1_CAPT_vect) // typical rate of 1 ms to 2 ms
162
{
163
    int16_t signal = 0, tmp;
164
    uint8_t i;
165
        static uint8_t index = 1;
166
        static uint16_t oldICR1 = 0;
167
 
168
 
169
        // 16bit Input Capture Register ICR1 contains the timer value TCNT1
170
        // at the time the edge was detected
171
 
172
        // calculate the time delay to the previous event time which is stored in oldICR1
173
        // calculatiing the difference of the two uint16_t and converting the result to an int16_t
174
        // implicit handles a timer overflow 65535 -> 0 the right way.
175
        signal = (uint16_t) ICR1 - oldICR1;
176
        oldICR1 = ICR1;
177
 
178
        if(ParamSet.Config2 & CFG2_SENSITIVE_RC)
179
        {
180
                static int16_t old_ppm_in[MAX_CHANNELS];
181
                static int16_t ppm_in[MAX_CHANNELS];
182
                static int16_t ppm_diff[MAX_CHANNELS];
183
                static uint8_t okay_cnt = 0;
184
 
185
                //sync gap? (3.52 ms < signal < 25.6 ms)
186
                if((signal > 1100) && (signal < 8000))
187
                {
188
                        // if a sync gap happens and there where at least 4 channels decoded
189
                        // and the number of channes decoded since the last sync gap matches the expectation
190
                        // then the NewPpmData flag is reset indicating valid data in the PPM_in[] array.
191
                        index--; // no next channel because the sync gap was detected
192
                        if(index >= 4 && index == RC_Channels)
193
                        {
194
                                if(okay_cnt > 10) // at least 10 frames in line ok
195
                                {
196
 
197
                                        for(i=0; i < MAX_CHANNELS; i++)
198
                                        {
199
                                                if(okay_cnt > 30) // at least 30 frames ok
200
                                                {
201
                                                        old_ppm_in[i] = PPM_in[i]; // backup data
202
                                                }
203
                                                PPM_in[i] = ppm_in[i];
204
                                                PPM_diff[i] = ppm_diff[i];
205
                                        }
206
                                        NewPpmData = 0;  // Null means NewData for at least the first 4 channels
207
                                }
208
                                if(okay_cnt < 255) okay_cnt++;
209
                        } // eof if(index >= 4 && index == RC_Channels)
210
                        else
211
                        {
212
                                if(okay_cnt > 100) okay_cnt = 10;
213
                                else okay_cnt = 0;
214
                                RED_ON;
215
                        }
216
                        // store max channels transmitted
217
                        if(!(MKFlags & MKFLAG_MOTOR_RUN)) RC_Channels = index;
218
                        // reset channel index
219
                        index = 1;
220
                }
221
                else // assuming within the PPM frame
222
                {
223
                        if(index < MAX_CHANNELS)
224
                        {
225
                                // check for valid signal length (0.8 ms < signal < 2.1984 ms)
226
                                // signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625
227
                                if((signal > 250) && (signal < 687))
228
                                {
229
                                        // shift signal to zero symmetric range  -154 to 159
230
                                        signal -= 466; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms)
231
                                        // check for stable signal
232
                                        if(abs(signal - ppm_in[index]) < 6)
233
                                        {
234
                                                if(okay_cnt > 25) RC_Quality +=10;
235
                                                else if(okay_cnt > 10) RC_Quality += 2;
236
                                                if(RC_Quality > 200) RC_Quality = 200;
237
                                        }
238
                                        // calculate exponential history for signal
239
                                        tmp = (3 * (ppm_in[index]) + signal) / 4;
240
                                        if(tmp > signal+1) tmp--; else
241
                                        if(tmp < signal-1) tmp++;
242
                                        // calculate signal difference on good signal level
243
                                        if(RC_Quality >= 190)  ppm_diff[index] = ((tmp - ppm_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction
244
                                        else ppm_diff[index] = 0;
245
                                        ppm_in[index] = tmp; // update channel value
246
                                }
247
                                else // invalid signal lenght
248
                                {
249
                                        RED_ON;
250
                                }
251
                                // demux sum signal for channels 5 to 7 to J3, J4, J5
252
                                if(index == 5) J3HIGH; else J3LOW;
253
                                if(index == 6) J4HIGH; else J4LOW;
254
                                if(CPUType != ATMEGA644P) // not used as TXD1
255
                                {
256
                                        if(index == 7) J5HIGH; else J5LOW;
257
                                }
258
                                index++; // next channel
259
                        }
260
                        else // (index >= MAX_CHANNELS)
261
                        {
262
                                RED_ON;
263
                                if(index == MAX_CHANNELS)
264
                                {
265
                                        for(i = 0; i < MAX_CHANNELS; i++)
266
                                        {
267
                                                PPM_in[i] = old_ppm_in[i]; // restore old valid values
268
                                                PPM_diff[i] = 0;
269
                                        }
270
                                        index++;
271
                                }
272
                        } // eof (index >= MAX_CHANNELS)
273
                } // eof within the PPM frame
274
        } // eof sensitive rc
275
        else // old more tolerant version
276
        {
277
                //sync gap? (3.52 ms < signal < 25.6 ms)
278
                if((signal > 1100) && (signal < 8000))
279
                {
280
                        // if a sync gap happens and there where at least 4 channels decoded before
281
                        // then the NewPpmData flag is reset indicating valid data in the PPM_in[] array.
282
                        index--; // no next channel because the sync gap was detected
283
                        RC_Channels = index;
284
                        if(index >= 4)
285
                        {
286
                                NewPpmData = 0;  // Null means NewData for the first 4 channels
287
                        }
288
                        // synchronize channel index
289
                        index = 1;
290
                }
291
                else // within the PPM frame
292
                {
293
                        if(index < MAX_CHANNELS)
294
                        {
295
                                // check for valid signal length (0.8 ms < signal < 2.1984 ms)
296
                                // signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625
297
                                if((signal > 250) && (signal < 687))
298
                                {
299
                                        // shift signal to zero symmetric range  -154 to 159
300
                                        signal -= 466; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms)
301
                                        // check for stable signal
302
                                        if(abs(signal - PPM_in[index]) < 6)
303
                                        {
304
                                                if(RC_Quality < 200) RC_Quality +=10;
305
                                                else RC_Quality = 200;
306
                                        }
307
                                        // calculate exponential history for signal
308
                                        tmp = (3 * (PPM_in[index]) + signal) / 4;
309
                                        if(tmp > signal+1) tmp--; else
310
                                        if(tmp < signal-1) tmp++;
311
                                        // calculate signal difference on good signal level
312
                                        if(RC_Quality >= 195)  PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction
313
                                        else PPM_diff[index] = 0;
314
                                        PPM_in[index] = tmp; // update channel value
315
                                }
316
                                index++; // next channel
317
                                // demux sum signal for channels 5 to 7 to J3, J4, J5
318
                                if(index == 5) J3HIGH; else J3LOW;
319
                                if(index == 6) J4HIGH; else J4LOW;
320
                                if(CPUType != ATMEGA644P) // not used as TXD1
321
                                {
322
                                        if(index == 7) J5HIGH; else J5LOW;
323
                                }
324
                        } // eof (index < MAX_CHANNELS)
325
                } // eof within the PPM frame
326
        } // eof old more tolerant version
327
}
328
 
329
#else // ACT_S3D_SUMSIGNAL
330
 
331
ISR(TIMER1_CAPT_vect) // typical rate of 1 ms to 2 ms
332
{
333
        int16_t signal = 0, tmp;
334
        uint8_t i;
335
        static uint8_t index = 1;
336
        static uint16_t oldICR1 = 0;
337
 
338
 
339
        // 16bit Input Capture Register ICR1 contains the timer value TCNT1
340
        // at the time the edge was detected
341
 
342
        // calculate the time delay to the previous event time which is stored in oldICR1
343
        // calculatiing the difference of the two uint16_t and converting the result to an int16_t
344
        // implicit handles a timer overflow 65535 -> 0 the right way.
345
        signal = (uint16_t) ICR1 - oldICR1;
346
        signal /= 2;
347
        oldICR1 = ICR1;
348
        //sync gap? (3.52 ms < signal < 25.6 ms)
349
        if((signal > 1100*2) && (signal < 8000*2))
350
        {
351
                // if a sync gap happens and there where at least 4 channels decoded before
352
                // then the NewPpmData flag is reset indicating valid data in the PPM_in[] array.
353
                index--; // no next channel because the sync gap was detected
354
                RC_Channels = index;
355
                if(index >= 4)
356
                {
357
                        NewPpmData = 0;  // Null means NewData for the first 4 channels
358
                }
359
                // synchronize channel index
360
                index = 1;
361
        }
362
        else // within the PPM frame
363
        {
364
                if(index < MAX_CHANNELS)
365
                {
366
                        // check for valid signal length (0.8 ms < signal < 2.1984 ms)
367
                        // signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625
368
                        if((signal > 250) && (signal < 687*2))
369
                        {
370
                                // shift signal to zero symmetric range  -154 to 159
371
                                signal -= 962; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms)
372
                                // check for stable signal
373
                                if(abs(signal - PPM_in[index]) < 6)
374
                                {
375
                                        if(RC_Quality < 200) RC_Quality +=10;
376
                                        else RC_Quality = 200;
377
                                }
378
                                // calculate exponential history for signal
379
                                tmp = (3 * (PPM_in[index]) + signal) / 4;
380
                                if(tmp > signal+1) tmp--; else
381
                                if(tmp < signal-1) tmp++;
382
                                // calculate signal difference on good signal level
383
                                if(RC_Quality >= 195)  PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction
384
                                else PPM_diff[index] = 0;
385
                                PPM_in[index] = tmp; // update channel value
386
                        }
387
                        index++; // next channel
388
                } // eof (index < MAX_CHANNELS)
389
        } // eof within the PPM frame
390
}
391
#endif
392
 
393
 
394
 
395