Subversion Repositories FlightCtrl

Rev

Rev 1612 | Only display areas with differences | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1612 Rev 1821
1
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2
// This code has been derived from the implementation of Stefan Engelke.
2
// This code has been derived from the implementation of Stefan Engelke.
3
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
3
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
4
/*
4
/*
5
    Copyright (c) 2008 Stefan Engelke <stefan@tinkerer.eu>
5
 Copyright (c) 2008 Stefan Engelke <stefan@tinkerer.eu>
6
 
6
 
7
    Permission is hereby granted, free of charge, to any person
7
 Permission is hereby granted, free of charge, to any person
8
    obtaining a copy of this software and associated documentation
8
 obtaining a copy of this software and associated documentation
9
    files (the "Software"), to deal in the Software without
9
 files (the "Software"), to deal in the Software without
10
    restriction, including without limitation the rights to use, copy,
10
 restriction, including without limitation the rights to use, copy,
11
    modify, merge, publish, distribute, sublicense, and/or sell copies
11
 modify, merge, publish, distribute, sublicense, and/or sell copies
12
    of the Software, and to permit persons to whom the Software is
12
 of the Software, and to permit persons to whom the Software is
13
    furnished to do so, subject to the following conditions:
13
 furnished to do so, subject to the following conditions:
14
 
14
 
15
    The above copyright notice and this permission notice shall be
15
 The above copyright notice and this permission notice shall be
16
    included in all copies or substantial portions of the Software.
16
 included in all copies or substantial portions of the Software.
17
 
17
 
18
    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
18
 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
19
    EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
19
 EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
20
    MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
20
 MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
21
    NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
21
 NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
22
    HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
22
 HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
23
    WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
23
 WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
24
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
24
 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
25
    DEALINGS IN THE SOFTWARE.
25
 DEALINGS IN THE SOFTWARE.
26
 
26
 
27
    $Id: rcdsl.c 60 2008-08-21 07:50:48Z taser $
27
 $Id: rcdsl.c 60 2008-08-21 07:50:48Z taser $
28
 
28
 
29
    RCDSL.H and RCDSL.C is an INOFFICIAL implementation of the
29
 RCDSL.H and RCDSL.C is an INOFFICIAL implementation of the
30
    communication protocol used by DSL receivers of Act Europe.
30
 communication protocol used by DSL receivers of Act Europe.
31
    The DSL receivers have a serial communication port to connect
31
 The DSL receivers have a serial communication port to connect
32
    two receivers in diversity mode. Each receiver is sending the
32
 two receivers in diversity mode. Each receiver is sending the
33
    received servo signals periodically over this port. This fact
33
 received servo signals periodically over this port. This fact
34
    can be used to connect the receiver to the control unit of the
34
 can be used to connect the receiver to the control unit of the
35
    model via UART instead of evaluating the PPM signal.
35
 model via UART instead of evaluating the PPM signal.
36
 
36
 
37
    If you have any questions, fell free to send me an e-mail.
37
 If you have any questions, fell free to send me an e-mail.
38
 
38
 
39
*/
39
 */
40
 
-
 
41
 
40
 
42
/*
41
/*
43
Connection of DSL to SV1 of FC:
42
 Connection of DSL to SV1 of FC:
44
( DSL Pin1 is on side of channel 4 )
43
 ( DSL Pin1 is on side of channel 4 )
45
 
44
 
46
1. GND <--> pin 7 (GND)
45
 1. GND <--> pin 7 (GND)
47
2. TXD <--> pin 3 (RXD1 Atmega644p)
46
 2. TXD <--> pin 3 (RXD1 Atmega644p)
48
3. RXD <--> pin 4 (TXD1 Atmega644p) optional
47
 3. RXD <--> pin 4 (TXD1 Atmega644p) optional
49
4. 5V  <--> pin 2 (5V)
48
 4. 5V  <--> pin 2 (5V)
50
 
49
 
51
Do not connect the receiver via PPM-Sumsignal output the same time.
50
 Do not connect the receiver via PPM-Sumsignal output the same time.
52
 
51
 
53
Data are send at every 20 ms @ 38400 Baud 8-N-1
52
 Data are send at every 20 ms @ 38400 Baud 8-N-1
54
 
53
 
55
Data Frame: |0xFF|0xFF|0x1F|FREQALLOC|??|RSSI|VBAT|??|CRC|10|CH0D1|CH0D0|CH1D1|CH1D0|CRC| ...etc
54
 Data Frame: |0xFF|0xFF|0x1F|FREQALLOC|??|RSSI|VBAT|??|CRC|10|CH0D1|CH0D0|CH1D1|CH1D0|CRC| ...etc
56
 
55
 
57
FREQALLOC = 35, 40, 72
56
 FREQALLOC = 35, 40, 72
58
RSSI = 0.. 255 // Received signal strength indicator
57
 RSSI = 0.. 255 // Received signal strength indicator
59
VBAT = 0...255 // supply voltage (0.0V.. 7.8V)
58
 VBAT = 0...255 // supply voltage (0.0V.. 7.8V)
60
 
59
 
61
Servo Pair:   |0x1X|CHXD1|CHXD0|CHX+1D1|CHX+1D0|CRC|
60
 Servo Pair:   |0x1X|CHXD1|CHXD0|CHX+1D1|CHX+1D0|CRC|
62
X is channel index of 1 servo value
61
 X is channel index of 1 servo value
63
D1D0 is servo value as u16 in range of 7373 (1ms) to 14745 (2ms)
62
 D1D0 is servo value as u16 in range of 7373 (1ms) to 14745 (2ms)
64
there are 8 channels submitted, i.e 4 servo pairs
63
 there are 8 channels submitted, i.e 4 servo pairs
65
 
64
 
66
 
65
 
67
Frame examples with signel received
66
 Frame examples with signel received
68
 
67
 
69
FFFF 1F23F079A304AD 1036012B1E6F 122AFB2AECB2 142B4D2B4404 1636872B33CE
68
 FFFF 1F23F079A304AD 1036012B1E6F 122AFB2AECB2 142B4D2B4404 1636872B33CE
70
FFFF 1F23F079A304AD 1036002B1F6F 122AFE2AEBB0 142B4B2B4406 1636872B33CE
69
 FFFF 1F23F079A304AD 1036002B1F6F 122AFE2AEBB0 142B4B2B4406 1636872B33CE
71
FFFF 1F23F079A304AD 1035FF2B226E 122AFC2AEAB3 142B4E2B4304 1636882B33CD
70
 FFFF 1F23F079A304AD 1035FF2B226E 122AFC2AEAB3 142B4E2B4304 1636882B33CD
72
FFFF 1F23F079A304AD 1036022B1E6E 122AFB2AEEB0 142B4A2B4506 1636872B33CE
71
 FFFF 1F23F079A304AD 1036022B1E6E 122AFB2AEEB0 142B4A2B4506 1636872B33CE
73
FFFF 1F23F079A304AD 1036022B1E6E 122AFE2AEBB0 142B4B2B4406 1636882B33CD
72
 FFFF 1F23F079A304AD 1036022B1E6E 122AFE2AEBB0 142B4B2B4406 1636882B33CD
74
FFFF 1F23F079A304AD 1036012B1E6F 122AFD2AEAB2 142B4E2B4403 1636862B33CF
73
 FFFF 1F23F079A304AD 1036012B1E6F 122AFD2AEAB2 142B4E2B4403 1636862B33CF
75
FFFF 1F23F079A304AD 1036032B1D6E 122AFD2AEBB1 142B4C2B4504 1636862B33CF
74
 FFFF 1F23F079A304AD 1036032B1D6E 122AFD2AEBB1 142B4C2B4504 1636862B33CF
76
 
75
 
77
Frame examples with no signal received
76
 Frame examples with no signal received
78
 
77
 
79
FFFF 1F23F000A30426
78
 FFFF 1F23F000A30426
80
FFFF 1F23F000A30426
79
 FFFF 1F23F000A30426
81
FFFF 1F23F000A30426
80
 FFFF 1F23F000A30426
82
FFFF 1F23F000A30426
81
 FFFF 1F23F000A30426
83
FFFF 1F23F000A30426
82
 FFFF 1F23F000A30426
84
FFFF 1F23F000A30426
83
 FFFF 1F23F000A30426
85
FFFF 1F23F000A30426
84
 FFFF 1F23F000A30426
86
*/
85
 */
87
 
86
 
88
#include <stdlib.h>
87
#include <stdlib.h>
89
#include "dsl.h"
88
#include "dsl.h"
90
#include "rc.h"
89
#include "rc.h"
91
#include "uart0.h"
90
#include "uart0.h"
92
 
91
 
93
uint8_t dsl_RSSI = 0;
92
uint8_t dsl_RSSI = 0;
94
uint8_t dsl_Battery = 0;
93
uint8_t dsl_Battery = 0;
95
uint8_t dsl_Allocation = 0;
94
uint8_t dsl_Allocation = 0;
96
uint8_t PacketBuffer[6];
95
uint8_t PacketBuffer[6];
97
//uint8_t Jitter = 0; // same measurement as RC_Quality in rc.c
96
//uint8_t Jitter = 0; // same measurement as RC_Quality in rc.c
98
 
97
 
99
typedef union
-
 
100
{
98
typedef union {
101
        int16_t Servo[2];
99
        int16_t Servo[2];
102
        uint8_t  byte[4];
100
        uint8_t byte[4];
103
} ChannelPair_t;
101
} ChannelPair_t;
104
 
102
 
105
ChannelPair_t ChannelPair;
103
ChannelPair_t ChannelPair;
106
 
-
 
107
 
104
 
108
// This function is called, when a new servo signal is properly received.
105
// This function is called, when a new servo signal is properly received.
109
// Parameters: servo  - servo number (0-9)
106
// Parameters: servo  - servo number (0-9)
110
//             signal - servo signal between 7373 (1ms) and 14745 (2ms)
107
//             signal - servo signal between 7373 (1ms) and 14745 (2ms)
111
void dsl_new_signal(uint8_t channel, int16_t signal)
108
void dsl_new_signal(uint8_t channel, int16_t signal) {
112
{
-
 
113
    int16_t tmp;
109
        int16_t tmp;
114
    uint8_t index = channel + 1; // mk channels start with 1
110
        uint8_t index = channel + 1; // mk channels start with 1
115
 
111
 
116
        //RC_Quality    = (212 * (uint16_t)dsl_RSSI) / 128; // have to be scaled approx. by a factor of 1.66 to get 200 at full level
112
        //RC_Quality    = (212 * (uint16_t)dsl_RSSI) / 128; // have to be scaled approx. by a factor of 1.66 to get 200 at full level
117
        //if(RC_Quality > 255) RC_Quality = 255;
113
        //if(RC_Quality > 255) RC_Quality = 255;
118
 
114
 
119
    // signal from  DSL-receiver is between 7373 (1ms) und 14745 (2ms).
115
        // signal from  DSL-receiver is between 7373 (1ms) und 14745 (2ms).
120
    signal-= 11059;     // shift to neutral
116
        signal -= 11059; // shift to neutral
121
    signal/= 24;        // scale to mk rc resolution
117
        signal /= 24; // scale to mk rc resolution
122
 
118
 
123
        if(abs(signal-PPM_in[index]) < 6)
-
 
124
        {
119
        if (abs(signal-PPM_in[index]) < 6) {
-
 
120
                if (RC_Quality < 200)
-
 
121
                        RC_Quality += 10;
125
                if(RC_Quality < 200) RC_Quality +=10;
122
                else
126
                else RC_Quality = 200;
123
                        RC_Quality = 200;
127
        }
124
        }
128
 
125
 
129
        // calculate exponential history for signal
126
        // calculate exponential history for signal
130
        tmp = (3 * (PPM_in[index]) + signal) / 4;
127
        tmp = (3 * (PPM_in[index]) + signal) / 4;
131
        if(tmp > signal+1) tmp--; else
128
        if (tmp > signal + 1)
-
 
129
                tmp--;
132
        if(tmp < signal-1) tmp++;
130
        else if (tmp < signal - 1)
-
 
131
                tmp++;
133
        // calculate signal difference on good signal level
132
        // calculate signal difference on good signal level
-
 
133
        if (RC_Quality >= 195)
134
        if(RC_Quality >= 195)  PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for noise reduction
134
                PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for noise reduction
-
 
135
        else
135
        else PPM_diff[index] = 0;
136
                PPM_diff[index] = 0;
136
        PPM_in[index] = tmp; // update channel value
137
        PPM_in[index] = tmp; // update channel value
137
 
138
 
138
    if(index == 4)
-
 
139
    {
139
        if (index == 4) {
140
                NewPpmData = 0;
140
                NewPpmData = 0;
141
        }
141
        }
142
}
142
}
143
 
143
 
144
// This function is called within dsl_parser(), when a complete
144
// This function is called within dsl_parser(), when a complete
145
// data packet with valid checksum has been received.
145
// data packet with valid checksum has been received.
146
void dsl_decode_packet(void)
146
void dsl_decode_packet(void) {
147
{
-
 
148
    uint8_t  i;
147
        uint8_t i;
149
 
148
 
150
        // check for header condition
149
        // check for header condition
151
        if((PacketBuffer[0] & 0xF0) == 0x10)
150
        if ((PacketBuffer[0] & 0xF0) == 0x10) {
152
        {
-
 
153
                if(PacketBuffer[0] == 0x1F) // separate status frame
151
                if (PacketBuffer[0] == 0x1F) // separate status frame
154
                {
152
                {
155
                        dsl_Allocation  = PacketBuffer[1]; // Get frequency allocation
153
                        dsl_Allocation = PacketBuffer[1]; // Get frequency allocation
156
                        // ??                   = PacketBuffer[2];
154
                        // ??                   = PacketBuffer[2];
157
                        dsl_RSSI                = PacketBuffer[3]; // Get signal quality
155
                        dsl_RSSI = PacketBuffer[3]; // Get signal quality
158
                        dsl_Battery     = PacketBuffer[4]; // Get voltage of battery supply
156
                        dsl_Battery = PacketBuffer[4]; // Get voltage of battery supply
159
                        // ??                   = PacketBuffer[5];
157
                        // ??                   = PacketBuffer[5];
160
                        if(dsl_RSSI == 0)
158
                        if (dsl_RSSI == 0) {
161
                        {
-
 
162
                                RC_Quality = 0;
159
                                RC_Quality = 0;
163
                                for (i = 0; i<5; i++)
160
                                for (i = 0; i < 5; i++) {
164
                                {
-
 
165
                                        PPM_diff[i] = 0;
161
                                        PPM_diff[i] = 0;
166
                                        PPM_in[i] = 0;
162
                                        PPM_in[i] = 0;
167
                                }
163
                                }
168
                        }
164
                        }
169
                }
-
 
170
                else // probably a channel pair
165
                } else // probably a channel pair
171
                {
166
                {
172
                        i = PacketBuffer[0] & 0x0F;   // last 4 bits of the header indicates the channel pair
167
                        i = PacketBuffer[0] & 0x0F; // last 4 bits of the header indicates the channel pair
173
                        if(i < 10)// maximum 12 channels
168
                        if (i < 10)// maximum 12 channels
174
                        {
169
                        {
175
                                // big to little endian
170
                                // big to little endian
176
                                ChannelPair.byte[1] = PacketBuffer[1];
171
                                ChannelPair.byte[1] = PacketBuffer[1];
177
                                ChannelPair.byte[0] = PacketBuffer[2];
172
                                ChannelPair.byte[0] = PacketBuffer[2];
178
                                ChannelPair.byte[3] = PacketBuffer[3];
173
                                ChannelPair.byte[3] = PacketBuffer[3];
179
                                ChannelPair.byte[2] = PacketBuffer[4];
174
                                ChannelPair.byte[2] = PacketBuffer[4];
180
                                dsl_new_signal(i,  ChannelPair.Servo[0]);
175
                                dsl_new_signal(i, ChannelPair.Servo[0]);
181
                                dsl_new_signal(i+1,ChannelPair.Servo[1]);
176
                                dsl_new_signal(i + 1, ChannelPair.Servo[1]);
182
                        }
177
                        }
183
                }
178
                }
184
        } // EOF header condition
179
        } // EOF header condition
185
}
180
}
186
 
-
 
187
 
181
 
188
// this function should be called within the UART RX ISR
182
// this function should be called within the UART RX ISR
189
void dsl_parser(uint8_t c)
-
 
190
{
183
void dsl_parser(uint8_t c) {
191
        static uint8_t last_c = 0;
184
        static uint8_t last_c = 0;
192
        static uint8_t crc      = 0;
185
        static uint8_t crc = 0;
193
        static uint8_t cnt = 0;
186
        static uint8_t cnt = 0;
194
        static uint8_t packet_len = 0;
187
        static uint8_t packet_len = 0;
195
 
188
 
196
        // check for sync condition
189
        // check for sync condition
197
        if ((c==0xFF) && (last_c==0xFF))
190
        if ((c == 0xFF) && (last_c == 0xFF)) {
198
        {
-
 
199
                cnt = 0; // reset byte counter
191
                cnt = 0; // reset byte counter
200
                crc = 0; // reset checksum
192
                crc = 0; // reset checksum
201
                return;
193
                return;
202
    }
194
        }
203
 
195
 
204
        if(cnt == 0) // begin of a packet
196
        if (cnt == 0) // begin of a packet
205
        {
197
        {
-
 
198
                if (c == 0x1F)
206
                if(c == 0x1F)   packet_len = 5; // a status packet has 5 bytes + crc
199
                        packet_len = 5; // a status packet has 5 bytes + crc
-
 
200
                else
207
                else                    packet_len = 4; // a channel pair packet has 4 bytes + crc
201
                        packet_len = 4; // a channel pair packet has 4 bytes + crc
208
        }
202
        }
209
        if(cnt > packet_len) // packet complete, crc byte received
203
        if (cnt > packet_len) // packet complete, crc byte received
210
        {
204
        {
211
                // calculate checksum
205
                // calculate checksum
212
                crc = ~crc;
206
                crc = ~crc;
213
                if (crc == 0xFF) crc = 0xFE;
207
                if (crc == 0xFF)
-
 
208
                        crc = 0xFE;
214
                // if crc matches decode the packet
209
                // if crc matches decode the packet
-
 
210
                if (c == crc)
215
        if (c == crc) dsl_decode_packet();
211
                        dsl_decode_packet();
216
        // handle next packet
212
                // handle next packet
217
        cnt = 0;
213
                cnt = 0;
218
        crc = 0;
214
                crc = 0;
219
        }
-
 
220
        else // collect channel data bytes
215
        } else // collect channel data bytes
221
        {
216
        {
222
                PacketBuffer[cnt++] = c;
217
                PacketBuffer[cnt++] = c;
223
                crc += c;
218
                crc += c;
224
        }
219
        }
225
        // store last byte for sync check
220
        // store last byte for sync check
226
        last_c = c;
221
        last_c = c;
227
}
222
}
228
 
223