Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

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