WebSVN - FlightCtrl - Diff - Rev 1612 and 1821 - /branches/dongfang_FC


// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// This code has been derived from the implementation of Stefan Engelke.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*
 Copyright (c) 2008 Stefan Engelke <stefan@tinkerer.eu>
 
 Permission is hereby granted, free of charge, to any person
 obtaining a copy of this software and associated documentation
 files (the "Software"), to deal in the Software without
 restriction, including without limitation the rights to use, copy,
 modify, merge, publish, distribute, sublicense, and/or sell copies
 of the Software, and to permit persons to whom the Software is
 furnished to do so, subject to the following conditions:
 
 The above copyright notice and this permission notice shall be
 included in all copies or substantial portions of the Software.
 
 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
 DEALINGS IN THE SOFTWARE.
 
 $Id: rcdsl.c 60 2008-08-21 07:50:48Z taser $
 
 RCDSL.H and RCDSL.C is an INOFFICIAL implementation of the
 communication protocol used by DSL receivers of Act Europe.
 The DSL receivers have a serial communication port to connect
 two receivers in diversity mode. Each receiver is sending the
 received servo signals periodically over this port. This fact
 can be used to connect the receiver to the control unit of the
 model via UART instead of evaluating the PPM signal.
 
 If you have any questions, fell free to send me an e-mail.
 
 */
 
 
/*
 Connection of DSL to SV1 of FC:
 ( DSL Pin1 is on side of channel 4 )
 
 1. GND <--> pin 7 (GND)
 2. TXD <--> pin 3 (RXD1 Atmega644p)
 3. RXD <--> pin 4 (TXD1 Atmega644p) optional
 4. 5V  <--> pin 2 (5V)
 
 Do not connect the receiver via PPM-Sumsignal output the same time.
 
 Data are send at every 20 ms @ 38400 Baud 8-N-1
 
 Data Frame: |0xFF|0xFF|0x1F|FREQALLOC|??|RSSI|VBAT|??|CRC|10|CH0D1|CH0D0|CH1D1|CH1D0|CRC| ...etc
 
 FREQALLOC = 35, 40, 72
 RSSI = 0.. 255 // Received signal strength indicator
 VBAT = 0...255 // supply voltage (0.0V.. 7.8V)
 
 Servo Pair:   |0x1X|CHXD1|CHXD0|CHX+1D1|CHX+1D0|CRC|
 X is channel index of 1 servo value
 D1D0 is servo value as u16 in range of 7373 (1ms) to 14745 (2ms)
 there are 8 channels submitted, i.e 4 servo pairs
 
 
 Frame examples with signel received
 
 FFFF 1F23F079A304AD 1036012B1E6F 122AFB2AECB2 142B4D2B4404 1636872B33CE
 FFFF 1F23F079A304AD 1036002B1F6F 122AFE2AEBB0 142B4B2B4406 1636872B33CE
 FFFF 1F23F079A304AD 1035FF2B226E 122AFC2AEAB3 142B4E2B4304 1636882B33CD
 FFFF 1F23F079A304AD 1036022B1E6E 122AFB2AEEB0 142B4A2B4506 1636872B33CE
 FFFF 1F23F079A304AD 1036022B1E6E 122AFE2AEBB0 142B4B2B4406 1636882B33CD
 FFFF 1F23F079A304AD 1036012B1E6F 122AFD2AEAB2 142B4E2B4403 1636862B33CF
 FFFF 1F23F079A304AD 1036032B1D6E 122AFD2AEBB1 142B4C2B4504 1636862B33CF
 
 Frame examples with no signal received
 
 FFFF 1F23F000A30426
 FFFF 1F23F000A30426
 FFFF 1F23F000A30426
 FFFF 1F23F000A30426
 FFFF 1F23F000A30426
 FFFF 1F23F000A30426
 FFFF 1F23F000A30426
 */
 
#include <stdlib.h>
#include "dsl.h"
#include "rc.h"
#include "uart0.h"
 
uint8_t dsl_RSSI = 0;
uint8_t dsl_Battery = 0;
uint8_t dsl_Allocation = 0;
uint8_t PacketBuffer[6];
//uint8_t Jitter = 0; // same measurement as RC_Quality in rc.c
 
 
typedef union {
        int16_t Servo[2];
        uint8_t byte[4];
} ChannelPair_t;
 
ChannelPair_t ChannelPair;
 
 
// This function is called, when a new servo signal is properly received.
// Parameters: servo  - servo number (0-9)
//             signal - servo signal between 7373 (1ms) and 14745 (2ms)
void dsl_new_signal(uint8_t channel, int16_t signal) {
 
        int16_t tmp;
        uint8_t index = channel + 1; // mk channels start with 1
 
        //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
        //if(RC_Quality > 255) RC_Quality = 255;
 
        // signal from  DSL-receiver is between 7373 (1ms) und 14745 (2ms).
        signal -= 11059; // shift to neutral
        signal /= 24; // scale to mk rc resolution
 
 
        if (abs(signal-PPM_in[index]) < 6) {
                if (RC_Quality < 200)
                        RC_Quality += 10;
                else
                        RC_Quality = 200;
        }
 
        // calculate exponential history for signal
        tmp = (3 * (PPM_in[index]) + signal) / 4;
        if (tmp > signal + 1)
                tmp--;
        else if (tmp < signal - 1)
                tmp++;
        // calculate signal difference on good signal level
        if (RC_Quality >= 195)
                PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for noise reduction
        else
                PPM_diff[index] = 0;
        PPM_in[index] = tmp; // update channel value
 
 
        if (index == 4) {
                NewPpmData = 0;
        }
}
 
// This function is called within dsl_parser(), when a complete
// data packet with valid checksum has been received.
void dsl_decode_packet(void) {
 
        uint8_t i;
 
        // check for header condition
        if ((PacketBuffer[0] & 0xF0) == 0x10) {
 
                if (PacketBuffer[0] == 0x1F) // separate status frame
                {
                        dsl_Allocation = PacketBuffer[1]; // Get frequency allocation
                        // ??                   = PacketBuffer[2];
                        dsl_RSSI = PacketBuffer[3]; // Get signal quality
                        dsl_Battery = PacketBuffer[4]; // Get voltage of battery supply
                        // ??                   = PacketBuffer[5];
                        if (dsl_RSSI == 0) {
 
                                RC_Quality = 0;
                                for (i = 0; i < 5; i++) {
 
                                        PPM_diff[i] = 0;
                                        PPM_in[i] = 0;
                                }
                        }
 
                } else // probably a channel pair
                {
                        i = PacketBuffer[0] & 0x0F; // last 4 bits of the header indicates the channel pair
                        if (i < 10)// maximum 12 channels
                        {
                                // big to little endian
                                ChannelPair.byte[1] = PacketBuffer[1];
                                ChannelPair.byte[0] = PacketBuffer[2];
                                ChannelPair.byte[3] = PacketBuffer[3];
                                ChannelPair.byte[2] = PacketBuffer[4];
                                dsl_new_signal(i, ChannelPair.Servo[0]);
                                dsl_new_signal(i + 1, ChannelPair.Servo[1]);
                        }
                }
        } // EOF header condition
}
 
 
// this function should be called within the UART RX ISR
 
void dsl_parser(uint8_t c) {
        static uint8_t last_c = 0;
        static uint8_t crc = 0;
        static uint8_t cnt = 0;
        static uint8_t packet_len = 0;
 
        // check for sync condition
        if ((c == 0xFF) && (last_c == 0xFF)) {
 
                cnt = 0; // reset byte counter
                crc = 0; // reset checksum
                return;
        }
 
        if (cnt == 0) // begin of a packet
        {
                if (c == 0x1F)
                        packet_len = 5; // a status packet has 5 bytes + crc
                else
                        packet_len = 4; // a channel pair packet has 4 bytes + crc
        }
        if (cnt > packet_len) // packet complete, crc byte received
        {
                // calculate checksum
                crc = ~crc;
                if (crc == 0xFF)
                        crc = 0xFE;
                // if crc matches decode the packet
                if (c == crc)
                        dsl_decode_packet();
                // handle next packet
                cnt = 0;
                crc = 0;
 
        } else // collect channel data bytes
        {
                PacketBuffer[cnt++] = c;
                crc += c;
        }
        // store last byte for sync check
        last_c = c;
}
 

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

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/dsl.c @ 1981 - Rev 1612 → 1821