Subversion Repositories FlightCtrl

Rev

Rev 1539 | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
1538 killagreg 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
/*
43
Connection of DSL to SV1 of FC:
44
( DSL Pin1 is on side of channel 4 )
45
 
46
1. GND <--> pin 7 (GND)
47
2. TXD <--> pin 3 (RXD1 Atmega644p)
48
3. RXD <--> pin 4 (TXD1 Atmega644p) optional
49
4. 5V  <--> pin 2 (5V)
50
 
51
If a receiver is connected via PPM input at the same time, the PPM input will be disabled
52
if a stable signal can be captured by the uart.
53
 
54
Data are send at every 20 ms @ 38400 Baud 8-N-1
55
 
56
Data Frame: |0xFF|0xFF|0x1F|FREQALLOC|??|RSSI|VBAT|??|CRC|10|CH0D1|CH0D0|CH1D1|CH1D0|CRC| ...etc
57
 
58
FREQALLOC = 35, 40, 72
59
RSSI = 0.. 255 // Received signal strength indicator
60
VBAT = 0...255 // supply voltage (0.0V.. 7.8V)
61
 
62
Servo Pair:   |0x1X|CHXD1|CHXD0|CHX+1D1|CHX+1D0|CRC|
63
X is channel index of 1 servo value
64
D1D0 is servo value as u16 in range of 7373 (1ms) to 14745 (2ms)
65
there are 8 channels submitted, i.e 4 servo pairs
66
 
67
 
68
Frame examples with signel received
69
 
70
FFFF 1F23F079A304AD 1036012B1E6F 122AFB2AECB2 142B4D2B4404 1636872B33CE
71
FFFF 1F23F079A304AD 1036002B1F6F 122AFE2AEBB0 142B4B2B4406 1636872B33CE
72
FFFF 1F23F079A304AD 1035FF2B226E 122AFC2AEAB3 142B4E2B4304 1636882B33CD
73
FFFF 1F23F079A304AD 1036022B1E6E 122AFB2AEEB0 142B4A2B4506 1636872B33CE
74
FFFF 1F23F079A304AD 1036022B1E6E 122AFE2AEBB0 142B4B2B4406 1636882B33CD
75
FFFF 1F23F079A304AD 1036012B1E6F 122AFD2AEAB2 142B4E2B4403 1636862B33CF
76
FFFF 1F23F079A304AD 1036032B1D6E 122AFD2AEBB1 142B4C2B4504 1636862B33CF
77
 
78
Frame examples with no signal received
79
 
80
FFFF 1F23F000A30426
81
FFFF 1F23F000A30426
82
FFFF 1F23F000A30426
83
FFFF 1F23F000A30426
84
FFFF 1F23F000A30426
85
FFFF 1F23F000A30426
86
FFFF 1F23F000A30426
87
*/
88
 
89
#include <stdlib.h>
90
#include "dsl.h"
91
#include "rc.h"
92
#include "eeprom.h"
93
 
94
uint8_t dsl_RSSI = 0;
95
uint8_t dsl_Battery = 0;
96
uint8_t dsl_Allocation = 0;
97
uint8_t PacketBuffer[6];
98
 
99
 
100
typedef union
101
{
102
        int16_t Servo[2];
103
        uint8_t  byte[4];
104
} ChannelPair_t;
105
 
106
ChannelPair_t ChannelPair;
107
 
108
 
109
// This function is called, when a new servo signal is properly received.
110
// Parameters: servo  - servo number (0-9)
111
//             signal - servo signal between 7373 (1ms) and 14745 (2ms)
112
void dsl_new_signal(uint8_t channel, int16_t signal)
113
{
114
    int16_t tmp;
115
    uint8_t index = channel + 1; // mk channels start with 1
116
 
117
        if(index > 0  &&  index < MAX_CHANNELS)
118
        {
119
                if(RC_Channels < index)  RC_Channels = index;
120
 
121
                // signal from  DSL-receiver is between 7373 (1ms) und 14745 (2ms).
122
                signal-= 11059;     // shift to neutral
123
                signal/= 24;        // scale to mk rc resolution
124
 
125
                if(abs(signal - PPM_in[index]) < 6)
126
                {
127
                        if(RC_Quality < 200)
128
                        {
129
                                RC_Quality += 10;
130
                        }
131
                        else
132
                        {
133
                                RC_Quality = 200;
134
                                PPM_INPUT_OFF; // disable PPM input at ICP
135
                        }
136
                }
137
 
138
                // calculate exponential history for signal
139
                tmp = (3 * (PPM_in[index]) + signal) / 4;
140
                if(tmp > signal+1) tmp--;
141
                else if(tmp < signal-1) tmp++;
142
                // calculate signal difference on good signal level
143
                if(RC_Quality >= 180)  PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for noise reduction
144
                else PPM_diff[index] = 0;
145
                PPM_in[index] = tmp; // update channel value
146
 
147
                if(index == 4)
148
                {
149
                        NewPpmData = 0;
150
                }
151
        }
152
}
153
 
154
// This function is called within dsl_parser(), when a complete
155
// data packet with valid checksum has been received.
156
void dsl_decode_packet(void)
157
{
158
    uint8_t  i;
159
 
160
        // check for header condition
161
        if((PacketBuffer[0] & 0xF0) == 0x10)
162
        {
163
                if(PacketBuffer[0] == 0x1F) // separate status frame
164
                {
165
                        dsl_Allocation  = PacketBuffer[1]; // Get frequency allocation
166
                        // ??                   = PacketBuffer[2];
167
                        dsl_RSSI                = PacketBuffer[3]; // Get signal quality
168
                        dsl_Battery     = PacketBuffer[4]; // Get voltage of battery supply
169
                        // ??                   = PacketBuffer[5];
170
                        RC_RSSI                 = dsl_RSSI;
171
                        PPM_in[0] = RC_RSSI;
172
                        if(RC_RSSI == 0)
173
                        {   // set to neutral attitude commands
174
                                PPM_diff[ParamSet.ChannelAssignment[CH_NICK]] = 0;
175
                                PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] = 0;
176
                                PPM_diff[ParamSet.ChannelAssignment[CH_YAW]]  = 0;
177
                                PPM_in[ParamSet.ChannelAssignment[CH_NICK]] = 0;
178
                                PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] = 0;
179
                                PPM_in[ParamSet.ChannelAssignment[CH_YAW]]  = 0;
180
                        }
181
                }
182
                else // probably a channel pair
183
                {
184
                        i = PacketBuffer[0] & 0x0F;   // last 4 bits of the header indicates the channel pair
185
                        if(i < 10)// maximum 12 channels
186
                        {
187
                                // big to little endian
188
                                ChannelPair.byte[1] = PacketBuffer[1];
189
                                ChannelPair.byte[0] = PacketBuffer[2];
190
                                ChannelPair.byte[3] = PacketBuffer[3];
191
                                ChannelPair.byte[2] = PacketBuffer[4];
192
                                dsl_new_signal(i,  ChannelPair.Servo[0]);
193
                                dsl_new_signal(i+1,ChannelPair.Servo[1]);
194
                        }
195
                }
196
        } // EOF header condition
197
}
198
 
199
 
200
// this function should be called within the UART RX ISR
201
void dsl_parser(uint8_t c)
202
{
203
        static uint8_t last_c = 0;
204
        static uint8_t crc      = 0;
205
        static uint8_t cnt = 0;
206
        static uint8_t packet_len = 0;
207
 
208
        // check for sync condition
209
        if ((c==0xFF) && (last_c==0xFF))
210
        {
211
                cnt = 0; // reset byte counter
212
                crc = 0; // reset checksum
213
                return;
214
    }
215
 
216
        if(cnt == 0) // begin of a packet
217
        {
218
                if(c == 0x1F)   packet_len = 5; // a status packet has 5 bytes + crc
219
                else                    packet_len = 4; // a channel pair packet has 4 bytes + crc
220
        }
221
        if(cnt > packet_len) // packet complete, crc byte received
222
        {
223
                // calculate checksum
224
                crc = ~crc;
225
                if (crc == 0xFF) crc = 0xFE;
226
                // if crc matches decode the packet
227
        if (c == crc) dsl_decode_packet();
228
        // handle next packet
229
        cnt = 0;
230
        crc = 0;
231
        }
232
        else // collect channel data bytes
233
        {
234
                PacketBuffer[cnt++] = c;
235
                crc += c;
236
        }
237
        // store last byte for sync check
238
        last_c = c;
239
}