Subversion Repositories FlightCtrl

Rev

Rev 762 | Rev 800 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 762 Rev 766
1
/*#######################################################################################
1
/*#######################################################################################
2
Decodieren eines RC Summen Signals
2
Decodieren eines RC Summen Signals
3
#######################################################################################*/
3
#######################################################################################*/
4
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
4
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5
// + Copyright (c) 04.2007 Holger Buss
5
// + Copyright (c) 04.2007 Holger Buss
6
// + only for non-profit use
6
// + only for non-profit use
7
// + www.MikroKopter.com
7
// + www.MikroKopter.com
8
// + see the File "License.txt" for further Informations
8
// + see the File "License.txt" for further Informations
9
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
9
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
10
 
10
 
11
#include <stdlib.h>
11
#include <stdlib.h>
12
#include <avr/io.h>
12
#include <avr/io.h>
13
#include <avr/interrupt.h>
13
#include <avr/interrupt.h>
14
 
14
 
15
#include "rc.h"
15
#include "rc.h"
16
#include "main.h"
16
#include "main.h"
17
 
17
 
18
volatile int16_t PPM_in[15]; //PPM24 supports 12 channels per frame
18
volatile int16_t PPM_in[15]; //PPM24 supports 12 channels per frame
19
volatile int16_t PPM_diff[15];
19
volatile int16_t PPM_diff[15];
20
volatile uint8_t NewPpmData = 1;
20
volatile uint8_t NewPpmData = 1;
21
volatile uint8_t SenderOkay = 0;
21
volatile uint8_t SenderOkay = 0;
22
 
22
 
23
/***************************************************************/
23
/***************************************************************/
24
/*  16bit timer 1 is used to decode the PPM-Signal            */
24
/*  16bit timer 1 is used to decode the PPM-Signal            */
25
/***************************************************************/
25
/***************************************************************/
26
void RC_Init (void)
26
void RC_Init (void)
27
{
27
{
28
        uint8_t sreg = SREG;
28
        uint8_t sreg = SREG;
29
 
29
 
30
        // disable all interrupts before reconfiguration
30
        // disable all interrupts before reconfiguration
31
        cli();
31
        cli();
32
 
32
 
33
        // PPM-signal is connected to the Input Capture Pin (PD6) of timer 1
33
        // PPM-signal is connected to the Input Capture Pin (PD6) of timer 1
34
        DDRD &= ~(1<<DDD6);
34
        DDRD &= ~(1<<DDD6);
35
        PORTD |= (1<<PORTD6);
35
        PORTD |= (1<<PORTD6);
36
 
36
 
37
        // Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5)
37
        // Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5)
38
        // set as output
38
        // set as output
39
        DDRD |= (1<<DDD5)|(1<<DDD4);
39
        DDRD |= (1<<DDD5)|(1<<DDD4);
40
        // low level
40
        // low level
41
        PORTD &= ~((1<<PORTD5)|(1<<PORTD4));
41
        PORTD &= ~((1<<PORTD5)|(1<<PORTD4));
42
 
42
 
43
        // PD3 can't be used in FC 1.1 if 2nd UART is activated
43
        // PD3 can't be used in FC 1.1 if 2nd UART is activated
44
        // becouse TXD1 is at that port
44
        // because TXD1 is at that port
45
        if(BoardRelease == 10)
45
        if(BoardRelease == 10)
46
        {
46
        {
47
                DDRD |= (1<<PORTD3);
47
                DDRD |= (1<<PORTD3);
48
                PORTD &= ~(1<<PORTD3);
48
                PORTD &= ~(1<<PORTD3);
49
        }
49
        }
50
 
50
 
51
        // Timer/Counter1 Control Register A, B, C
51
        // Timer/Counter1 Control Register A, B, C
52
 
52
 
53
        // Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)
53
        // Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)
54
        // Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0)
54
        // Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0)
55
        // Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)
55
        // Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)
56
        // Enable input capture noise cancler (bit: ICNC1=1)
56
        // Enable input capture noise cancler (bit: ICNC1=1)
57
        // Trigger on positive edge of the input capture pin (bit: ICES1=1),
57
        // Trigger on positive edge of the input capture pin (bit: ICES1=1),
58
        // Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2µs
58
        // Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2µs
59
    // The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
59
    // The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
60
        TCCR1A &= ~((1<<COM1A1)|(1<<COM1A0)|(1<<COM1B1)|(1<<COM1B0)|(1<<WGM11)|(1<<WGM10));
60
        TCCR1A &= ~((1<<COM1A1)|(1<<COM1A0)|(1<<COM1B1)|(1<<COM1B0)|(1<<WGM11)|(1<<WGM10));
61
        TCCR1B &= ~((1<<WGM13)|(1<<WGM12)|(1<<CS12));
61
        TCCR1B &= ~((1<<WGM13)|(1<<WGM12)|(1<<CS12));
62
        TCCR1B |= (1<<CS11)|(1<<CS10)|(1<<ICES1)|(1<<ICNC1);
62
        TCCR1B |= (1<<CS11)|(1<<CS10)|(1<<ICES1)|(1<<ICNC1);
63
        TCCR1C &= ~((1<<FOC1A)|(1<<FOC1B));
63
        TCCR1C &= ~((1<<FOC1A)|(1<<FOC1B));
64
 
64
 
65
        // Timer/Counter1 Interrupt Mask Register
65
        // Timer/Counter1 Interrupt Mask Register
66
 
66
 
67
        // Enable Input Capture Interrupt (bit: ICIE1=1)
67
        // Enable Input Capture Interrupt (bit: ICIE1=1)
68
        // Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0)
68
        // Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0)
69
        // Disable Overflow Interrupt (bit: TOIE1=0)
69
        // Disable Overflow Interrupt (bit: TOIE1=0)
70
        TIMSK1 &= ~((1<<OCIE1B)|(1<<OCIE1A)|(1<<TOIE1));
70
        TIMSK1 &= ~((1<<OCIE1B)|(1<<OCIE1A)|(1<<TOIE1));
71
    TIMSK1 |= (1<<ICIE1);
71
    TIMSK1 |= (1<<ICIE1);
72
 
72
 
73
    SREG = sreg;
73
    SREG = sreg;
74
}
74
}
75
 
75
 
76
 
76
 
77
/********************************************************************/
77
/********************************************************************/
78
/*         Every time a positive edge is detected at PD6            */
78
/*         Every time a positive edge is detected at PD6            */
79
/********************************************************************/
79
/********************************************************************/
80
/*                               t-Frame
80
/*                               t-Frame
81
       <----------------------------------------------------------------------->
81
       <----------------------------------------------------------------------->
82
         ____   ______   _____   ________                ______    sync gap      ____
82
         ____   ______   _____   ________                ______    sync gap      ____
83
        |    | |      | |     | |        |              |      |                |
83
        |    | |      | |     | |        |              |      |                |
84
        |    | |      | |     | |        |              |      |                |
84
        |    | |      | |     | |        |              |      |                |
85
     ___|    |_|      |_|     |_|        |_.............|      |________________|
85
     ___|    |_|      |_|     |_|        |_.............|      |________________|
86
        <-----><-------><------><-------->              <------>                <---
86
        <-----><-------><------><-------->              <------>                <---
87
          t0       t1      t2       t4                     tn                     t0
87
          t0       t1      t2       t4                     tn                     t0
88
 
88
 
89
The PPM-Frame length is 22.5 ms.
89
The PPM-Frame length is 22.5 ms.
90
Channel high pulse width range is 0.7 ms to 1.7 ms completed by an 0.3 ms low pulse.
90
Channel high pulse width range is 0.7 ms to 1.7 ms completed by an 0.3 ms low pulse.
91
The mininimum time delay of two events coding a channel is ( 0.7 + 0.3) ms = 1 ms.
91
The mininimum time delay of two events coding a channel is ( 0.7 + 0.3) ms = 1 ms.
92
The maximum time delay of two events coding a chanel is ( 1.7 + 0.3) ms = 2 ms.
92
The maximum time delay of two events coding a chanel is ( 1.7 + 0.3) ms = 2 ms.
93
The minimum duration of all channels at minimum value is  8 * 1 ms = 8 ms.
93
The minimum duration of all channels at minimum value is  8 * 1 ms = 8 ms.
94
The maximum duration of all channels at maximum value is  8 * 2 ms = 16 ms.
94
The maximum duration of all channels at maximum value is  8 * 2 ms = 16 ms.
95
The remaining time of (22.5 - 8 ms) ms = 14.5 ms  to (22.5 - 16 ms) ms = 6.5 ms is
95
The remaining time of (22.5 - 8 ms) ms = 14.5 ms  to (22.5 - 16 ms) ms = 6.5 ms is
96
the syncronization gap.
96
the syncronization gap.
97
*/
97
*/
98
ISR(TIMER1_CAPT_vect) // typical rate of 1 ms to 2 ms
98
ISR(TIMER1_CAPT_vect) // typical rate of 1 ms to 2 ms
99
{
99
{
100
        static uint16_t oldICR1 = 0;
100
        static uint16_t oldICR1 = 0;
101
    int16_t signal = 0, tmp;
101
    int16_t signal = 0, tmp;
102
        static int16_t index;
102
        static int16_t index;
103
 
103
 
104
        // 16bit Input Capture Register ICR1 contains the timer value TCNT1
104
        // 16bit Input Capture Register ICR1 contains the timer value TCNT1
105
        // at the time the edge was detected
105
        // at the time the edge was detected
106
 
106
 
107
        // calculate the time delay to the previous event time which is stored in oldICR1
107
        // calculate the time delay to the previous event time which is stored in oldICR1
108
        signal = (uint16_t) ICR1 - oldICR1;
108
        signal = (uint16_t) ICR1 - oldICR1;
109
        oldICR1 = ICR1;
109
        oldICR1 = ICR1;
110
 
110
 
111
 
111
 
112
    //sync gap? (3.52 ms < signal < 25.6 ms)
112
    //sync gap? (3.52 ms < signal < 25.6 ms)
113
        if((signal > 1100) && (signal < 8000))
113
        if((signal > 1100) && (signal < 8000))
114
        {
114
        {
115
                // if a sync gap happens and there where at least 4 channels decoded before
115
                // if a sync gap happens and there where at least 4 channels decoded before
116
                // then the NewPpmData flag is reset indicating valid data in the PPM_in[] array.
116
                // then the NewPpmData flag is reset indicating valid data in the PPM_in[] array.
117
                if(index >= 4)  NewPpmData = 0;  // Null means NewData for the first 4 channels
117
                if(index >= 4)  NewPpmData = 0;  // Null means NewData for the first 4 channels
118
                // synchronize channel index
118
                // synchronize channel index
119
                index = 1;
119
                index = 1;
120
        }
120
        }
121
        else // within the PPM frame
121
        else // within the PPM frame
122
        {
122
        {
123
        if(index < 14) // PPM24 supports 12 channels
123
        if(index < 14) // PPM24 supports 12 channels
124
        {
124
        {
125
                        // check for valid signal length (0.8 ms < signal < 2.1984 ms)
125
                        // check for valid signal length (0.8 ms < signal < 2.1984 ms)
126
                        // signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625
126
                        // signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625
127
            if((signal > 250) && (signal < 687))
127
            if((signal > 250) && (signal < 687))
128
            {
128
            {
129
                                // shift signal to zero symmetric range  -154 to 159
129
                                // shift signal to zero symmetric range  -154 to 159
130
                signal -= 466; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms)
130
                signal -= 466; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms)
131
                // check for stable signal
131
                // check for stable signal
132
                // the deviation of the current signal level from the average must be less than 6 (aprox. 1%)
132
                // the deviation of the current signal level from the average must be less than 6 (aprox. 1%)
133
                if(abs(signal - PPM_in[index]) < 6)
133
                if(abs(signal - PPM_in[index]) < 6)
134
                {
134
                {
135
                                        // a good signal condition increases SenderOkay by 10
135
                                        // a good signal condition increases SenderOkay by 10
136
                                        // SignalOkay is decremented every 2 ms in main.c
136
                                        // SignalOkay is decremented every 2 ms in main.c
137
                                        // this variable is a level for the average rate of a noiseless rc signal
137
                                        // this variable is a level for the average rate of a noiseless rc signal
138
                                        if(SenderOkay < 200) SenderOkay += 10;
138
                                        if(SenderOkay < 200) SenderOkay += 10;
139
                                }
139
                                }
140
                                // calculate exponential history for signal
140
                                // calculate exponential history for signal
141
                tmp = (3 * (PPM_in[index]) + signal) / 4;
141
                tmp = (3 * (PPM_in[index]) + signal) / 4;
142
                if(tmp > signal+1) tmp--; else
142
                if(tmp > signal+1) tmp--; else
143
                if(tmp < signal-1) tmp++;
143
                if(tmp < signal-1) tmp++;
144
                // calculate signal difference on good signal level
144
                // calculate signal difference on good signal level
145
                if(SenderOkay >= 195)  PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction
145
                if(SenderOkay >= 195)  PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction
146
                else PPM_diff[index] = 0;
146
                else PPM_diff[index] = 0;
147
                PPM_in[index] = tmp; // update channel value
147
                PPM_in[index] = tmp; // update channel value
148
            }
148
            }
149
            index++; // next channel
149
            index++; // next channel
150
            // demux sum signal for channels 5 to 7 to J3, J4, J5
150
            // demux sum signal for channels 5 to 7 to J3, J4, J5
151
                if(index == 5) PORTD |= (1<<PORTD5); else PORTD &= ~(1<<PORTD5);
151
                if(index == 5) PORTD |= (1<<PORTD5); else PORTD &= ~(1<<PORTD5);
152
                if(index == 6) PORTD |= (1<<PORTD4); else PORTD &= ~(1<<PORTD4);
152
                if(index == 6) PORTD |= (1<<PORTD4); else PORTD &= ~(1<<PORTD4);
153
                if(BoardRelease == 10)
153
                if(BoardRelease == 10)
154
                {
154
                {
155
                                if(index == 7) PORTD |= (1<<PORTD3); else PORTD &= ~(1<<PORTD3);
155
                                if(index == 7) PORTD |= (1<<PORTD3); else PORTD &= ~(1<<PORTD3);
156
                }
156
                }
157
        }
157
        }
158
        }
158
        }
159
}
159
}
160
 
160
 
161
 
161
 
162
 
162
 
163
 
163
 
164
 
164
 
165
 
165