WebSVN - FlightCtrl - Diff - Rev 1171 and 1172 - /branches/Znib/V0.82a/Spektrum.c


/*#######################################################################################
Decodieren eines RC Summen Signals oder Spektrum Empfänger-Satellit
#######################################################################################*/
 
#include "Spectrum.h"
#include "main.h"
 
//############################################################################
// zum Decodieren des Spektrum Satelliten wird USART1 benutzt.
// USART1 initialisation from killagreg
void Uart1Init(void)
//############################################################################
    {
        // -- Start of USART1 initialisation for Spekturm seriell-mode
        // USART1 Control and Status Register A, B, C and baud rate register
        uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * 115200) - 1);
        // disable RX-Interrupt
        UCSR1B &= ~(1 << RXCIE1);
        // disable TX-Interrupt
        UCSR1B &= ~(1 << TXCIE1);
        // disable DRE-Interrupt
        UCSR1B &= ~(1 << UDRIE1);
        // set direction of RXD1 and TXD1 pins
        // set RXD1 (PD2) as an input pin
        PORTD |= (1 << PORTD2);
        DDRD &= ~(1 << DDD2);
        // USART0 Baud Rate Register
        // set clock divider
        UBRR1H = (uint8_t)(ubrr>>8);
        UBRR1L = (uint8_t)ubrr;
        // enable double speed operation
        UCSR1A |= (1 << U2X1);
        // enable receiver and transmitter
        //UCSR1B = (1<<RXEN1)|(1<<TXEN1);
        UCSR1B = (1<<RXEN1);
        // set asynchronous mode
        UCSR1C &= ~(1 << UMSEL11);
        UCSR1C &= ~(1 << UMSEL10);
        // no parity
        UCSR1C &= ~(1 << UPM11);
        UCSR1C &= ~(1 << UPM10);
        // 1 stop bit
        UCSR1C &= ~(1 << USBS1);
        // 8-bit
        UCSR1B &= ~(1 << UCSZ12);
        UCSR1C |=  (1 << UCSZ11);
        UCSR1C |=  (1 << UCSZ10);
        // flush receive buffer explicit
        while(UCSR1A & (1<<RXC1)) UDR1;
        // enable RX-interrupts at the end
        UCSR1B |= (1 << RXCIE1);
        // -- End of USART1 initialisation
  return;
 }
       
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) Rainer Walther
// + RC-routines from original MK rc.c (c) H&I
// + Useful infos from Walter: http://www.rcgroups.com/forums/showthread.php?t=714299&page=2
// + only for non-profit use
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//
// 20080808 rw Modified for Spektrum AR6100 (PPM)
// 20080823 rw Add Spektrum satellite receiver on USART1 (644P only)
// 20081213 rw Add support for Spektrum DS9 Air-Tx-Module (9 channels)
//             Replace AR6100-coding with original composit-signal routines
//
// ---
// Entweder Summensignal ODER Spektrum-Receiver anschließen. Nicht beides gleichzeitig betreiben!
// Binding is not implemented. Bind with external Receiver.
// Servo output J3, J4, J5 not serviced
//
// Anschuß Spektrum Receiver
//              Orange:         3V von der FC (keinesfalls an 5V anschließen!)
//              Schwarz:        GND
//              Grau:           RXD1 (Pin 3) auf 10-Pol FC-Stecker
//
// ---
// Satellite-Reciever connected on USART1:
//
// DX7/DX6i: One data-frame at 115200 baud every 22ms.
// DX7se:    One data-frame at 115200 baud every 11ms.
//              byte1:  unknown
//      byte2:  unknown
//      byte3:  and byte4:  channel data        (FLT-Mode)
//      byte5:  and byte6:  channel data        (Roll)
//      byte7:  and byte8:  channel data        (Nick)
//      byte9:  and byte10: channel data        (Gier)
//      byte11: and byte12: channel data        (Gear Switch)
//      byte13: and byte14: channel data        (Gas)
//      byte15: and byte16: channel data        (AUX2)
//
// DS9 (9 Channel): One data-frame at 115200 baud every 11ms, alternating frame 1/2 for CH1-7 / CH8-9
//  1st Frame:
//              byte1:  unknown
//      byte2:  unknown
//              byte3:  and byte4:  channel data
//      byte5:  and byte6:  channel data
//      byte7:  and byte8:  channel data
//      byte9:  and byte10: channel data
//      byte11: and byte12: channel data
//      byte13: and byte14: channel data
//      byte15: and byte16: channel data 
//  2nd Frame:
//              byte1:  unknown
//      byte2:  unknown
//              byte3:  and byte4:  channel data
//      byte5:  and byte6:  channel data
//      byte7:  and byte8:  0xffff
//      byte9:  and byte10: 0xffff
//      byte11: and byte12: 0xffff
//      byte13: and byte14: 0xffff
//      byte15: and byte16: 0xffff
//
// Each channel data (16 bit= 2byte, first msb, second lsb) is arranged as:
//
// Bits: F 0 C3 C2 C1 C0 D9 D8 D7 D6 D5 D4 D3 D2 D1 D0
//
// 0 means a '0' bit
// F: 1 = indicates beginning of 2nd frame for CH8-9 (DS9 only)
// C3 to C0 is the channel number. 0 to 9 (4 bit, as assigned in the transmitter)
// D9 to D0 is the channel data (10 bit) 0xaa..0x200..0x356 for 100% transmitter-travel
//
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
//############################################################################
//Diese Routine startet und inizialisiert den USART1 für seriellen Spektrum satellite reciever
SIGNAL(USART1_RX_vect)
//############################################################################
{
static unsigned int Sync=0, FrameCnt=0, ByteHigh=0, ReSync=1, Frame2=0, FrameTimer;
        unsigned int Channel, index;
        signed int signal, tmp;
        int bCheckDelay;
        uint8_t c;
       
        c = UDR1; // get data byte
       
        if (ReSync == 1)
            {
                // wait for beginning of new frame
                ReSync = 0;
               
                FrameTimer = SetDelay(7);  // minimum 7ms zwischen den frames
                FrameCnt = 0;
                Sync = 0;
                ByteHigh = 0;
                }
  else
  {    
        bCheckDelay = CheckDelay(FrameTimer);
        if ( Sync == 0 )
            {
                if(bCheckDelay)
                    {
                        // nach einer Pause von mind. 7ms erstes Sync-Character gefunden
                        // Zeichen ignorieren, da Bedeutung unbekannt
                        Sync = 1;
                        FrameCnt ++;
                        }
                else
                        {
                        // Zeichen kam vor Ablauf der 7ms Sync-Pause
                        // warten auf erstes Sync-Zeichen
                        }
                }
        else if((Sync == 1) && !bCheckDelay)
            {
                // zweites Sync-Character ignorieren, Bedeutung unbekannt
                Sync = 2;
                FrameCnt ++;
                }
        else if((Sync == 2) && !bCheckDelay)
            {
                // Datenbyte high
                ByteHigh = c;
               
                if (FrameCnt == 2)
                    {
                    // is 1st Byte of Channel-data
 
                        // Frame 1 with Channel 1-7 comming next
                        Frame2 = 0;
                        if(ByteHigh & 0x80)
                            {
                                // DS9: Frame 2 with Channel 8-9 comming next
                                Frame2 = 1;
                                }
                        }      
                Sync = 3;
                FrameCnt ++;
                }
        else if((Sync == 3) && !bCheckDelay)
            {
                // Datenbyte low
               
                // High-Byte for next channel comes next
                Sync = 2;
                FrameCnt ++;
               
                index = (ByteHigh >> 2) & 0x0f;
                index ++;
                Channel = (ByteHigh << 8) | c;
                signal = Channel & 0x3ff;
                signal -= 0x200;                // Offset, range 0x000..0x3ff?
                signal = signal/3;              // scaling to fit PPM resolution
               
                if(index >= 0  &&  index <= 10)
 
                    {
 
                // Stabiles Signal
 
 
 
 
                if(abs(signal - PPM_in[index]) < 6) { if(SenderOkay < 200) SenderOkay += 10; else SenderOkay = 200;}
                tmp = (3 * (PPM_in[index]) + signal) / 4;  
 
 
 
 
 
                if(tmp > signal+1) tmp--; else
 
 
 
 
 
 
                if(tmp < signal-1) tmp++;
 
 
 
                if(SenderOkay >= 180)  PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3;
 
 
                else PPM_diff[index] = 0;
                PPM_in[index] = tmp;
                        }
                }
        else
                {
                // hier stimmt was nicht: neu synchronisieren
                ReSync = 1;
                FrameCnt = 0;
                Frame2 = 0;
                }
               
        // 16 Bytes per frame
        if(FrameCnt >= 16)
            {
                // Frame complete
                if(Frame2 == 0)
                        {
                        // Null bedeutet: Neue Daten
                        // nur beim ersten Frame (CH 0-7) setzen
                        NewPpmData = 0;  
                        }
               
                // new frame next, nach fruehestens 7ms erwartet
                FrameCnt = 0;
                Frame2 = 0;
                Sync = 0;
                }
 
        // Zeit bis zum nächsten Zeichen messen
        FrameTimer = SetDelay(7);
   }
}  
 
 
 

Rev 1171	Rev 1172
1	/*#######################################################################################	1	/*#######################################################################################
2	Decodieren eines RC Summen Signals oder Spektrum Empfänger-Satellit	2	Decodieren eines RC Summen Signals oder Spektrum Empfänger-Satellit
3	#######################################################################################*/	3	#######################################################################################*/
4		4
5	#include "Spectrum.h"	5	#include "Spectrum.h"
6	#include "main.h"	6	#include "main.h"
7		7
8	//############################################################################	8	//############################################################################
9	// zum Decodieren des Spektrum Satelliten wird USART1 benutzt.	9	// zum Decodieren des Spektrum Satelliten wird USART1 benutzt.
10	// USART1 initialisation from killagreg	10	// USART1 initialisation from killagreg
11	void Uart1Init(void)	11	void Uart1Init(void)
12	//############################################################################	12	//############################################################################
13	{	13	{
14	// -- Start of USART1 initialisation for Spekturm seriell-mode	14	// -- Start of USART1 initialisation for Spekturm seriell-mode
15	// USART1 Control and Status Register A, B, C and baud rate register	15	// USART1 Control and Status Register A, B, C and baud rate register
16	uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * 115200) - 1);	16	uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * 115200) - 1);
17	// disable RX-Interrupt	17	// disable RX-Interrupt
18	UCSR1B &= ~(1 << RXCIE1);	18	UCSR1B &= ~(1 << RXCIE1);
19	// disable TX-Interrupt	19	// disable TX-Interrupt
20	UCSR1B &= ~(1 << TXCIE1);	20	UCSR1B &= ~(1 << TXCIE1);
21	// disable DRE-Interrupt	21	// disable DRE-Interrupt
22	UCSR1B &= ~(1 << UDRIE1);	22	UCSR1B &= ~(1 << UDRIE1);
23	// set direction of RXD1 and TXD1 pins	23	// set direction of RXD1 and TXD1 pins
24	// set RXD1 (PD2) as an input pin	24	// set RXD1 (PD2) as an input pin
25	PORTD \|= (1 << PORTD2);	25	PORTD \|= (1 << PORTD2);
26	DDRD &= ~(1 << DDD2);	26	DDRD &= ~(1 << DDD2);
27	// USART0 Baud Rate Register	27	// USART0 Baud Rate Register
28	// set clock divider	28	// set clock divider
29	UBRR1H = (uint8_t)(ubrr>>8);	29	UBRR1H = (uint8_t)(ubrr>>8);
30	UBRR1L = (uint8_t)ubrr;	30	UBRR1L = (uint8_t)ubrr;
31	// enable double speed operation	31	// enable double speed operation
32	UCSR1A \|= (1 << U2X1);	32	UCSR1A \|= (1 << U2X1);
33	// enable receiver and transmitter	33	// enable receiver and transmitter
34	//UCSR1B = (1<<RXEN1)\|(1<<TXEN1);	34	//UCSR1B = (1<<RXEN1)\|(1<<TXEN1);
35	UCSR1B = (1<<RXEN1);	35	UCSR1B = (1<<RXEN1);
36	// set asynchronous mode	36	// set asynchronous mode
37	UCSR1C &= ~(1 << UMSEL11);	37	UCSR1C &= ~(1 << UMSEL11);
38	UCSR1C &= ~(1 << UMSEL10);	38	UCSR1C &= ~(1 << UMSEL10);
39	// no parity	39	// no parity
40	UCSR1C &= ~(1 << UPM11);	40	UCSR1C &= ~(1 << UPM11);
41	UCSR1C &= ~(1 << UPM10);	41	UCSR1C &= ~(1 << UPM10);
42	// 1 stop bit	42	// 1 stop bit
43	UCSR1C &= ~(1 << USBS1);	43	UCSR1C &= ~(1 << USBS1);
44	// 8-bit	44	// 8-bit
45	UCSR1B &= ~(1 << UCSZ12);	45	UCSR1B &= ~(1 << UCSZ12);
46	UCSR1C \|= (1 << UCSZ11);	46	UCSR1C \|= (1 << UCSZ11);
47	UCSR1C \|= (1 << UCSZ10);	47	UCSR1C \|= (1 << UCSZ10);
48	// flush receive buffer explicit	48	// flush receive buffer explicit
49	while ( UCSR1A & (1<<RXC1) ) UDR1;	49	while(UCSR1A & (1<<RXC1)) UDR1;
50	// enable RX-interrupts at the end	50	// enable RX-interrupts at the end
51	UCSR1B \|= (1 << RXCIE1);	51	UCSR1B \|= (1 << RXCIE1);
52	// -- End of USART1 initialisation	52	// -- End of USART1 initialisation
53	return;	53	return;
54	}	54	}
55		55
56	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	56	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
57	// + Copyright (c) Rainer Walther	57	// + Copyright (c) Rainer Walther
58	// + RC-routines from original MK rc.c (c) H&I	58	// + RC-routines from original MK rc.c (c) H&I
59	// + Useful infos from Walter: http://www.rcgroups.com/forums/showthread.php?t=714299&page=2	59	// + Useful infos from Walter: http://www.rcgroups.com/forums/showthread.php?t=714299&page=2
60	// + only for non-profit use	60	// + only for non-profit use
61	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	61	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
62	//	62	//
63	// 20080808 rw Modified for Spektrum AR6100 (PPM)	63	// 20080808 rw Modified for Spektrum AR6100 (PPM)
64	// 20080823 rw Add Spektrum satellite receiver on USART1 (644P only)	64	// 20080823 rw Add Spektrum satellite receiver on USART1 (644P only)
65	// 20081213 rw Add support for Spektrum DS9 Air-Tx-Module (9 channels)	65	// 20081213 rw Add support for Spektrum DS9 Air-Tx-Module (9 channels)
66	// Replace AR6100-coding with original composit-signal routines	66	// Replace AR6100-coding with original composit-signal routines
67	//	67	//
68	// ---	68	// ---
69	// Entweder Summensignal ODER Spektrum-Receiver anschließen. Nicht beides gleichzeitig betreiben!	69	// Entweder Summensignal ODER Spektrum-Receiver anschließen. Nicht beides gleichzeitig betreiben!
70	// Binding is not implemented. Bind with external Receiver.	70	// Binding is not implemented. Bind with external Receiver.
71	// Servo output J3, J4, J5 not serviced	71	// Servo output J3, J4, J5 not serviced
72	//	72	//
73	// Anschuß Spektrum Receiver	73	// Anschuß Spektrum Receiver
74	// Orange: 3V von der FC (keinesfalls an 5V anschließen!)	74	// Orange: 3V von der FC (keinesfalls an 5V anschließen!)
75	// Schwarz: GND	75	// Schwarz: GND
76	// Grau: RXD1 (Pin 3) auf 10-Pol FC-Stecker	76	// Grau: RXD1 (Pin 3) auf 10-Pol FC-Stecker
77	//	77	//
78	// ---	78	// ---
79	// Satellite-Reciever connected on USART1:	79	// Satellite-Reciever connected on USART1:
80	//	80	//
81	// DX7/DX6i: One data-frame at 115200 baud every 22ms.	81	// DX7/DX6i: One data-frame at 115200 baud every 22ms.
82	// DX7se: One data-frame at 115200 baud every 11ms.	82	// DX7se: One data-frame at 115200 baud every 11ms.
83	// byte1: unknown	83	// byte1: unknown
84	// byte2: unknown	84	// byte2: unknown
85	// byte3: and byte4: channel data (FLT-Mode)	85	// byte3: and byte4: channel data (FLT-Mode)
86	// byte5: and byte6: channel data (Roll)	86	// byte5: and byte6: channel data (Roll)
87	// byte7: and byte8: channel data (Nick)	87	// byte7: and byte8: channel data (Nick)
88	// byte9: and byte10: channel data (Gier)	88	// byte9: and byte10: channel data (Gier)
89	// byte11: and byte12: channel data (Gear Switch)	89	// byte11: and byte12: channel data (Gear Switch)
90	// byte13: and byte14: channel data (Gas)	90	// byte13: and byte14: channel data (Gas)
91	// byte15: and byte16: channel data (AUX2)	91	// byte15: and byte16: channel data (AUX2)
92	//	92	//
93	// DS9 (9 Channel): One data-frame at 115200 baud every 11ms, alternating frame 1/2 for CH1-7 / CH8-9	93	// DS9 (9 Channel): One data-frame at 115200 baud every 11ms, alternating frame 1/2 for CH1-7 / CH8-9
94	// 1st Frame:	94	// 1st Frame:
95	// byte1: unknown	95	// byte1: unknown
96	// byte2: unknown	96	// byte2: unknown
97	// byte3: and byte4: channel data	97	// byte3: and byte4: channel data
98	// byte5: and byte6: channel data	98	// byte5: and byte6: channel data
99	// byte7: and byte8: channel data	99	// byte7: and byte8: channel data
100	// byte9: and byte10: channel data	100	// byte9: and byte10: channel data
101	// byte11: and byte12: channel data	101	// byte11: and byte12: channel data
102	// byte13: and byte14: channel data	102	// byte13: and byte14: channel data
103	// byte15: and byte16: channel data	103	// byte15: and byte16: channel data
104	// 2nd Frame:	104	// 2nd Frame:
105	// byte1: unknown	105	// byte1: unknown
106	// byte2: unknown	106	// byte2: unknown
107	// byte3: and byte4: channel data	107	// byte3: and byte4: channel data
108	// byte5: and byte6: channel data	108	// byte5: and byte6: channel data
109	// byte7: and byte8: 0xffff	109	// byte7: and byte8: 0xffff
110	// byte9: and byte10: 0xffff	110	// byte9: and byte10: 0xffff
111	// byte11: and byte12: 0xffff	111	// byte11: and byte12: 0xffff
112	// byte13: and byte14: 0xffff	112	// byte13: and byte14: 0xffff
113	// byte15: and byte16: 0xffff	113	// byte15: and byte16: 0xffff
114	//	114	//
115	// Each channel data (16 bit= 2byte, first msb, second lsb) is arranged as:	115	// Each channel data (16 bit= 2byte, first msb, second lsb) is arranged as:
116	//	116	//
117	// Bits: F 0 C3 C2 C1 C0 D9 D8 D7 D6 D5 D4 D3 D2 D1 D0	117	// Bits: F 0 C3 C2 C1 C0 D9 D8 D7 D6 D5 D4 D3 D2 D1 D0
118	//	118	//
119	// 0 means a '0' bit	119	// 0 means a '0' bit
120	// F: 1 = indicates beginning of 2nd frame for CH8-9 (DS9 only)	120	// F: 1 = indicates beginning of 2nd frame for CH8-9 (DS9 only)
121	// C3 to C0 is the channel number. 0 to 9 (4 bit, as assigned in the transmitter)	121	// C3 to C0 is the channel number. 0 to 9 (4 bit, as assigned in the transmitter)
122	// D9 to D0 is the channel data (10 bit) 0xaa..0x200..0x356 for 100% transmitter-travel	122	// D9 to D0 is the channel data (10 bit) 0xaa..0x200..0x356 for 100% transmitter-travel
123	//	123	//
124	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	124	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
125		125
126	//############################################################################	126	//############################################################################
127	//Diese Routine startet und inizialisiert den USART1 für seriellen Spektrum satellite reciever	127	//Diese Routine startet und inizialisiert den USART1 für seriellen Spektrum satellite reciever
128	SIGNAL(USART1_RX_vect)	128	SIGNAL(USART1_RX_vect)
129	//############################################################################	129	//############################################################################
130	{	130	{
131	static unsigned int Sync=0, FrameCnt=0, ByteHigh=0, ReSync=1, Frame2=0, FrameTimer;	131	static unsigned int Sync=0, FrameCnt=0, ByteHigh=0, ReSync=1, Frame2=0, FrameTimer;
132	unsigned int Channel, index;	132	unsigned int Channel, index;
133	signed int signal, tmp;	133	signed int signal, tmp;
134	int bCheckDelay;	134	int bCheckDelay;
135	uint8_t c;	135	uint8_t c;
136		136
137	c = UDR1; // get data byte	137	c = UDR1; // get data byte
138		138
139	if (ReSync == 1)	139	if (ReSync == 1)
140	{	140	{
141	// wait for beginning of new frame	141	// wait for beginning of new frame
142	ReSync = 0;	142	ReSync = 0;
143		143
144	FrameTimer = SetDelay(7); // minimum 7ms zwischen den frames	144	FrameTimer = SetDelay(7); // minimum 7ms zwischen den frames
145	FrameCnt = 0;	145	FrameCnt = 0;
146	Sync = 0;	146	Sync = 0;
147	ByteHigh = 0;	147	ByteHigh = 0;
148	}	148	}
149	else	149	else
150	{	150	{
151	bCheckDelay = CheckDelay(FrameTimer);	151	bCheckDelay = CheckDelay(FrameTimer);
152	if ( Sync == 0 )	152	if ( Sync == 0 )
153	{	153	{
154	if ( bCheckDelay )	154	if(bCheckDelay)
155	{	155	{
156	// nach einer Pause von mind. 7ms erstes Sync-Character gefunden	156	// nach einer Pause von mind. 7ms erstes Sync-Character gefunden
157	// Zeichen ignorieren, da Bedeutung unbekannt	157	// Zeichen ignorieren, da Bedeutung unbekannt
158	Sync = 1;	158	Sync = 1;
159	FrameCnt ++;	159	FrameCnt ++;
160	}	160	}
161	else	161	else
162	{	162	{
163	// Zeichen kam vor Ablauf der 7ms Sync-Pause	163	// Zeichen kam vor Ablauf der 7ms Sync-Pause
164	// warten auf erstes Sync-Zeichen	164	// warten auf erstes Sync-Zeichen
165	}	165	}
166	}	166	}
167	else if ( (Sync == 1) && !bCheckDelay )	167	else if((Sync == 1) && !bCheckDelay)
168	{	168	{
169	// zweites Sync-Character ignorieren, Bedeutung unbekannt	169	// zweites Sync-Character ignorieren, Bedeutung unbekannt
170	Sync = 2;	170	Sync = 2;
171	FrameCnt ++;	171	FrameCnt ++;
172	}	172	}
173	else if ( (Sync == 2) && !bCheckDelay )	173	else if((Sync == 2) && !bCheckDelay)
174	{	174	{
175	// Datenbyte high	175	// Datenbyte high
176	ByteHigh = c;	176	ByteHigh = c;
177		177
178	if (FrameCnt == 2)	178	if (FrameCnt == 2)
179	{	179	{
180	// is 1st Byte of Channel-data	180	// is 1st Byte of Channel-data
181		-
182	// Frame 1 with Channel 1-7 comming next	181	// Frame 1 with Channel 1-7 comming next
183	Frame2 = 0;	182	Frame2 = 0;
184	if ( ByteHigh & 0x80 )	183	if(ByteHigh & 0x80)
185	{	184	{
186	// DS9: Frame 2 with Channel 8-9 comming next	185	// DS9: Frame 2 with Channel 8-9 comming next
187	Frame2 = 1;	186	Frame2 = 1;
188	}	187	}
189	}	188	}
190	Sync = 3;	189	Sync = 3;
191	FrameCnt ++;	190	FrameCnt ++;
192	}	191	}
193	else if ( (Sync == 3) && !bCheckDelay )	192	else if((Sync == 3) && !bCheckDelay)
194	{	193	{
195	// Datenbyte low	194	// Datenbyte low
196		195
197	// High-Byte for next channel comes next	196	// High-Byte for next channel comes next
198	Sync = 2;	197	Sync = 2;
199	FrameCnt ++;	198	FrameCnt ++;
200		199
201	index = (ByteHigh >> 2) & 0x0f;	200	index = (ByteHigh >> 2) & 0x0f;
202	index ++;	201	index ++;
203	Channel = (ByteHigh << 8) \| c;	202	Channel = (ByteHigh << 8) \| c;
204	signal = Channel & 0x3ff;	203	signal = Channel & 0x3ff;
205	signal -= 0x200; // Offset, range 0x000..0x3ff?	204	signal -= 0x200; // Offset, range 0x000..0x3ff?
206	signal = signal/3; // scaling to fit PPM resolution	205	signal = signal/3; // scaling to fit PPM resolution
207		206
208	if (index >= 0 && index <= 10)	207	if(index >= 0 && index <= 10)
209	{	-
210		208	{
211	if(abs(signal - PPM_in[index]) < 6)	-
212	{	209	// Stabiles Signal
213	if(SenderOkay < 200)	-
214	{	-
215	SenderOkay += 10;	-
216	}	-
217	}	210	if(abs(signal - PPM_in[index]) < 6) { if(SenderOkay < 200) SenderOkay += 10; else SenderOkay = 200;}
218	tmp = (3 * (PPM_in[index]) + signal) / 4;	211	tmp = (3 * (PPM_in[index]) + signal) / 4;
219	if(tmp > signal+1)	-
220	{	-
221	tmp--;	-
222	}	-
223	else	-
224	{	212	if(tmp > signal+1) tmp--; else
225	if(tmp < signal-1)	-
226	{	-
227	tmp++;	-
228	}	-
229	}	-
230	if(SenderOkay >= 195)	-
231	{	213	if(tmp < signal-1) tmp++;
232	PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3;	-
233	}	-
234	else	-
235	{	214	if(SenderOkay >= 180) PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3;
236	PPM_diff[index] = 0;	-
237	}	-
238		215	else PPM_diff[index] = 0;
239	PPM_in[index] = tmp;	216	PPM_in[index] = tmp;
240	}	217	}
241	}	218	}
242	else	219	else
243	{	220	{
244	// hier stimmt was nicht: neu synchronisieren	221	// hier stimmt was nicht: neu synchronisieren
245	ReSync = 1;	222	ReSync = 1;
246	FrameCnt = 0;	223	FrameCnt = 0;
247	Frame2 = 0;	224	Frame2 = 0;
248	}	225	}
249		226
250	// 16 Bytes per frame	227	// 16 Bytes per frame
251	if (FrameCnt >= 16)	228	if(FrameCnt >= 16)
252	{	229	{
253	// Frame complete	230	// Frame complete
254	if ( Frame2 == 0 )	231	if(Frame2 == 0)
255	{	232	{
256	// Null bedeutet: Neue Daten	233	// Null bedeutet: Neue Daten
257	// nur beim ersten Frame (CH 0-7) setzen	234	// nur beim ersten Frame (CH 0-7) setzen
258	NewPpmData = 0;	235	NewPpmData = 0;
259	}	236	}
260		237
261	// new frame next, nach fruehestens 7ms erwartet	238	// new frame next, nach fruehestens 7ms erwartet
262	FrameCnt = 0;	239	FrameCnt = 0;
263	Frame2 = 0;	240	Frame2 = 0;
264	Sync = 0;	241	Sync = 0;
265	}	242	}
266		-
267	// Zeit bis zum nächsten Zeichen messen	243	// Zeit bis zum nächsten Zeichen messen
268	FrameTimer = SetDelay (7);	244	FrameTimer = SetDelay(7);
269	}	245	}
270	}	246	}
271		247
272		248
273		249

Subversion Repositories FlightCtrl

(root)/branches/Znib/V0.82a/Spektrum.c @ 1804 - Rev 1171 → 1172