WebSVN - FlightCtrl - Blame - Rev 2384 - /branches/V2.01a_ACC-HH

Rev	Author	Line No.	Line
1171	hbuss	1	/*#######################################################################################
		2	Decodieren eines RC Summen Signals oder Spektrum Empf�nger-Satellit
		3	#######################################################################################*/
		4
1426	ingob	5	#include "Spektrum.h"
1171	hbuss	6	#include "main.h"
1506	holgerb	7
1320	hbuss	8	unsigned char SpektrumTimer = 0;
1171	hbuss	9
2185	holgerb	10	// Achtung: RECEIVER_SPEKTRUM_DX7EXP oder RECEIVER_SPEKTRUM_DX8EXP wird in der main.h gesetzt
1928	holgerb	11	#if defined (RECEIVER_SPEKTRUM_DX7EXP) \|\| defined (RECEIVER_SPEKTRUM_DX8EXP)
2185	holgerb	12	unsigned char s_excnt = 0; // Bitcounter for Spektrum-Expander
1680	holgerb	13	unsigned char s_exparity = 0; // Parity Bit for Spektrum-Expander
2185	holgerb	14	signed char s_exdata[11]; // Data for Spektrum-Expander
		15
		16	void s_update(unsigned char channel, signed int value) // Channel-Diff numbercrunching and finally assign new stickvalue to PPM_in
		17	{
		18	if(SenderOkay >= 180) PPM_diff[channel] = ((value - PPM_in[channel]) / 3) * 3;
		19	else PPM_diff[channel] = 0;
		20	PPM_in[channel] = value;
		21	}
1506	holgerb	22	#endif
2185	holgerb	23
1171	hbuss	24	//############################################################################
		25	// USART1 initialisation from killagreg
1424	ingob	26	void SpektrumUartInit(void)
1171	hbuss	27	//############################################################################
		28	{
1680	holgerb	29	// -- Start of USART1 initialisation for Spekturm seriell-mode
		30	// USART1 Control and Status Register A, B, C and baud rate register
		31	uint8_t sreg = SREG;
1928	holgerb	32
1680	holgerb	33	uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * 115200) - 1);
1928	holgerb	34
1680	holgerb	35	// disable all interrupts before reconfiguration
		36	cli();
		37	// disable RX-Interrupt
		38	UCSR1B &= ~(1 << RXCIE1);
		39	// disable TX-Interrupt
		40	UCSR1B &= ~(1 << TXCIE1);
		41	// disable DRE-Interrupt
		42	UCSR1B &= ~(1 << UDRIE1);
2309	holgerb	43	/*
1680	holgerb	44	// set direction of RXD1 and TXD1 pins
		45	// set RXD1 (PD2) as an input pin
		46	PORTD \|= (1 << PORTD2);
		47	DDRD &= ~(1 << DDD2);
		48	// set TXD1 (PD3) as an output pin
		49	PORTD \|= (1 << PORTD3);
		50	DDRD \|= (1 << DDD3);
2309	holgerb	51	*/
1680	holgerb	52	// USART0 Baud Rate Register
		53	// set clock divider
		54	UBRR1H = (uint8_t)(ubrr>>8);
		55	UBRR1L = (uint8_t)ubrr;
		56	// enable double speed operation
		57	UCSR1A \|= (1 << U2X1);
		58	// enable receiver and transmitter
		59	//UCSR1B = (1<<RXEN1)\|(1<<TXEN1);
1391	killagreg	60
1680	holgerb	61	UCSR1B = (1<<RXEN1);
		62	// set asynchronous mode
		63	UCSR1C &= ~(1 << UMSEL11);
		64	UCSR1C &= ~(1 << UMSEL10);
		65	// no parity
		66	UCSR1C &= ~(1 << UPM11);
		67	UCSR1C &= ~(1 << UPM10);
		68	// 1 stop bit
		69	UCSR1C &= ~(1 << USBS1);
		70	// 8-bit
		71	UCSR1B &= ~(1 << UCSZ12);
		72	UCSR1C \|= (1 << UCSZ11);
		73	UCSR1C \|= (1 << UCSZ10);
		74	// flush receive buffer explicit
		75	while(UCSR1A & (1<<RXC1)) UDR1;
		76	// enable RX-interrupts at the end
		77	UCSR1B \|= (1 << RXCIE1);
		78	// -- End of USART1 initialisation
		79	// restore global interrupt flags
1928	holgerb	80
1680	holgerb	81	SREG = sreg;
1171	hbuss	82	return;
		83	}
1391	killagreg	84
1171	hbuss	85	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		86	// + Copyright (c) Rainer Walther
		87	// + RC-routines from original MK rc.c (c) H&I
		88	// + Useful infos from Walter: http://www.rcgroups.com/forums/showthread.php?t=714299&page=2
		89	// + only for non-profit use
		90	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		91	//
		92	// 20080808 rw Modified for Spektrum AR6100 (PPM)
		93	// 20080823 rw Add Spektrum satellite receiver on USART1 (644P only)
		94	// 20081213 rw Add support for Spektrum DS9 Air-Tx-Module (9 channels)
		95	// Replace AR6100-coding with original composit-signal routines
		96	//
		97	// ---
		98	// Entweder Summensignal ODER Spektrum-Receiver anschlie�en. Nicht beides gleichzeitig betreiben!
		99	// Binding is not implemented. Bind with external Receiver.
		100	// Servo output J3, J4, J5 not serviced
		101	//
		102	// Anschu� Spektrum Receiver
1680	holgerb	103	// Orange: 3V von der FC (keinesfalls an 5V anschlie�en!)
		104	// Schwarz: GND
		105	// Grau: RXD1 (Pin 3) auf 10-Pol FC-Stecker
1171	hbuss	106	//
		107	// ---
		108	// Satellite-Reciever connected on USART1:
		109	//
		110	// DX7/DX6i: One data-frame at 115200 baud every 22ms.
		111	// DX7se: One data-frame at 115200 baud every 11ms.
1680	holgerb	112	// byte1: unknown
		113	// byte2: unknown
		114	// byte3: and byte4: channel data (FLT-Mode)
		115	// byte5: and byte6: channel data (Roll)
		116	// byte7: and byte8: channel data (Nick)
		117	// byte9: and byte10: channel data (Gier)
		118	// byte11: and byte12: channel data (Gear Switch)
		119	// byte13: and byte14: channel data (Gas)
		120	// byte15: and byte16: channel data (AUX2)
1171	hbuss	121	//
		122	// DS9 (9 Channel): One data-frame at 115200 baud every 11ms, alternating frame 1/2 for CH1-7 / CH8-9
		123	// 1st Frame:
1680	holgerb	124	// byte1: unknown
		125	// byte2: unknown
		126	// byte3: and byte4: channel data
		127	// byte5: and byte6: channel data
		128	// byte7: and byte8: channel data
		129	// byte9: and byte10: channel data
		130	// byte11: and byte12: channel data
		131	// byte13: and byte14: channel data
		132	// byte15: and byte16: channel data
1171	hbuss	133	// 2nd Frame:
1680	holgerb	134	// byte1: unknown
		135	// byte2: unknown
		136	// byte3: and byte4: channel data
		137	// byte5: and byte6: channel data
		138	// byte7: and byte8: 0xffff
		139	// byte9: and byte10: 0xffff
		140	// byte11: and byte12: 0xffff
		141	// byte13: and byte14: 0xffff
		142	// byte15: and byte16: 0xffff
1171	hbuss	143	//
		144	// Each channel data (16 bit= 2byte, first msb, second lsb) is arranged as:
		145	//
		146	// Bits: F 0 C3 C2 C1 C0 D9 D8 D7 D6 D5 D4 D3 D2 D1 D0
		147	//
		148	// 0 means a '0' bit
		149	// F: 1 = indicates beginning of 2nd frame for CH8-9 (DS9 only)
		150	// C3 to C0 is the channel number. 0 to 9 (4 bit, as assigned in the transmitter)
		151	// D9 to D0 is the channel data (10 bit) 0xaa..0x200..0x356 for 100% transmitter-travel
		152	//
		153	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		154
1322	hbuss	155	#define MIN_FRAMEGAP 68 // 7ms
		156	#define MAX_BYTEGAP 3 // 310us
1320	hbuss	157
1419	ingob	158
1171	hbuss	159	//############################################################################
1481	holgerb	160	// Wird im UART-Interrupt aufgerufen
1171	hbuss	161	//############################################################################
1419	ingob	162	void SpektrumParser(unsigned char c)
1171	hbuss	163	{
1320	hbuss	164	static unsigned char Sync=0, FrameCnt=0, ByteHigh=0, ReSync=1, Frame2=0;
1680	holgerb	165	unsigned int Channel, index = 0;
		166	signed int signal = 0, tmp;
		167	int bCheckDelay;
		168	// c = UDR1; // get data byte
		169	if(ReSync == 1)
		170	{
		171	// wait for beginning of new frame
		172	ReSync = 0;
		173	SpektrumTimer = MIN_FRAMEGAP;
		174	FrameCnt = 0;
		175	Sync = 0;
		176	ByteHigh = 0;
		177	}
1171	hbuss	178	else
1391	killagreg	179	{
1680	holgerb	180	if(!SpektrumTimer) bCheckDelay = 1; else bCheckDelay = 0;//CheckDelay(FrameTimer);
		181	if ( Sync == 0 )
		182	{
		183	if(bCheckDelay)
		184	{
		185	// nach einer Pause von mind. 7ms erstes Sync-Character gefunden
		186	// Zeichen ignorieren, da Bedeutung unbekannt
		187	Sync = 1;
		188	FrameCnt ++;
		189	SpektrumTimer = MAX_BYTEGAP;
		190	}
		191	else
		192	{
		193	// Zeichen kam vor Ablauf der 7ms Sync-Pause
		194	// warten auf erstes Sync-Zeichen
		195	SpektrumTimer = MIN_FRAMEGAP;
		196	FrameCnt = 0;
		197	Sync = 0;
		198	ByteHigh = 0;
		199	}
		200	}
		201	else if((Sync == 1) && !bCheckDelay)
		202	{
		203	// zweites Sync-Character ignorieren, Bedeutung unbekannt
		204	Sync = 2;
		205	FrameCnt ++;
		206	SpektrumTimer = MAX_BYTEGAP;
		207	}
		208	else if((Sync == 2) && !bCheckDelay)
		209	{
		210	SpektrumTimer = MAX_BYTEGAP;
		211	// Datenbyte high
		212	ByteHigh = c;
		213	if (FrameCnt == 2)
		214	{
		215	// is 1st Byte of Channel-data
		216	// Frame 1 with Channel 1-7 comming next
		217	Frame2 = 0;
		218	if(ByteHigh & 0x80)
		219	{
		220	// DS9: Frame 2 with Channel 8-9 comming next
		221	Frame2 = 1;
		222	}
		223	}
		224	Sync = 3;
		225	FrameCnt ++;
		226	}
		227	else if((Sync == 3) && !bCheckDelay)
		228	{
		229	// Datenbyte low
		230	// High-Byte for next channel comes next
		231	SpektrumTimer = MAX_BYTEGAP;
		232	Sync = 2;
		233	FrameCnt ++;
		234	Channel = ((unsigned int)ByteHigh << 8) \| c;
		235	if(EE_Parameter.Receiver == RECEIVER_SPEKTRUM)
		236	{
		237	signal = Channel & 0x3ff;
		238	signal -= 0x200; // Offset, range 0x000..0x3ff?
		239	signal = signal/3; // scaling to fit PPM resolution
		240	index = (ByteHigh >> 2) & 0x0f;
		241	}
		242	else
		243	if(EE_Parameter.Receiver == RECEIVER_SPEKTRUM_HI_RES)
		244	{
		245	signal = Channel & 0x7ff;
		246	signal -= 0x400; // Offset, range 0x000..0x7ff?
		247	signal = signal/6; // scaling to fit PPM resolution
		248	index = (ByteHigh >> 3) & 0x0f;
1928	holgerb	249	}
1680	holgerb	250	else
		251	//if(EE_Parameter.Receiver == RECEIVER_SPEKTRUM_LOW_RES)
		252	{
		253	signal = Channel & 0x3ff;
		254	signal -= 360; // Offset, range 0x000..0x3ff?
		255	signal = signal/2; // scaling to fit PPM resolution
		256	index = (ByteHigh >> 2) & 0x0f;
		257	}
1391	killagreg	258
1680	holgerb	259	index++;
		260	if(index < 13)
		261	{
		262	// Stabiles Signal
1928	holgerb	263	#if defined (RECEIVER_SPEKTRUM_DX7EXP) \|\| defined (RECEIVER_SPEKTRUM_DX8EXP)
2185	holgerb	264	if (index == 2) index = 4; // Analog channel reassigment (2 <-> 4) for logical numbering (1,2,3,4)
1928	holgerb	265	else if (index == 4) index = 2;
1506	holgerb	266	#endif
2011	holgerb	267	if(abs(signal - PPM_in[index]) < 6)
		268	{
2185	holgerb	269	if(EE_Parameter.FailsafeChannel == 0 \|\| PPM_in[EE_Parameter.FailsafeChannel] < 100) // forces Failsafe if the receiver doesn't have 'signal loss' on Failsafe
		270	{
2011	holgerb	271	if(SenderOkay < 200) SenderOkay += 10;
		272	else
		273	{
1928	holgerb	274	SenderOkay = 200;
		275	TIMSK1 &= ~_BV(ICIE1); // disable PPM-Input
2011	holgerb	276	}
2185	holgerb	277	}
		278	}
1928	holgerb	279	tmp = (3 * (PPM_in[index]) + signal) / 4;
		280	if(tmp > signal+1) tmp--; else
		281	if(tmp < signal-1) tmp++;
		282
		283	#ifdef RECEIVER_SPEKTRUM_DX7EXP
2185	holgerb	284	if(index == 6) // FLIGHT-MODE - The channel used for our data uplink
1928	holgerb	285	{
2185	holgerb	286	if (signal > 100) // SYNC received
1928	holgerb	287	{
2185	holgerb	288	if (s_exdata[s_excnt] == 125) s_exparity = ~s_exparity; // Bit = 1 -> Re-Invert parity bit
1928	holgerb	289	if ((s_excnt == 6 && ((s_exparity != 0 && s_exdata[s_excnt] == -125) \|\| (s_exparity == 0 && s_exdata[s_excnt] == 125))) \|\| (s_excnt == 9 && ((s_exparity == 0 && s_exdata[s_excnt] == -125) \|\| (s_exparity != 0 && s_exdata[s_excnt] == 125)))) // Parity check
		290	{
2185	holgerb	291	if (s_exdata[1] == 125 && s_exdata[2] == -125) s_update(5,-125); // Reconstruct tripole Flight-Mode value (CH5)
		292	else if (s_exdata[1] == -125 && s_exdata[2] == -125) s_update(5,0); // Reconstruct tripole Flight-Mode value (CH5)
		293	else if (s_exdata[1] == -125 && s_exdata[2] == 125) s_update(5,125); // Reconstruct tripole Flight-Mode value (CH5)
		294	s_update(6,s_exdata[3]); // Elevator (CH6)
		295	s_update(11,s_exdata[4]); // Aileron (CH11)
		296	s_update(12,s_exdata[5]); // Rudder (CH12)
1680	holgerb	297
2185	holgerb	298	if (s_excnt == 9) // New Mode (12 Channels)
1928	holgerb	299	{
2185	holgerb	300	if (s_exdata[7] == 125) s_update(8,PPM_in[8]+5); // Hover Pitch UP (CH8)
		301	if (s_exdata[8] == 125) s_update(8,PPM_in[8]-5); // Hover Pitch DN (CH8)
		302	if (PPM_in[8] < -125) PPM_in[8] = -125; // Range-Limit
		303	else if (PPM_in[8] > 125) PPM_in[8] = 125; // Range-Limit
		304	s_update(10,s_exdata[6]); // AUX2 (CH10)
1928	holgerb	305	}
		306	}
1680	holgerb	307
2185	holgerb	308	s_excnt = 0; // Reset bitcounter
		309	s_exparity = 0; // Reset parity bit
1928	holgerb	310	}
1506	holgerb	311
2185	holgerb	312	if (signal < 10) s_exdata[++s_excnt] = -125; // Bit = 0 -> value = -125 (min)
1928	holgerb	313	if (s_excnt == 10) s_excnt = 0; // Overflow protection
		314	if (signal < -100)
		315	{
2185	holgerb	316	s_exdata[s_excnt] = 125; // Bit = 1 -> value = 125 (max)
		317	s_exparity = ~s_exparity; // Bit = 1 -> Invert parity bit
1928	holgerb	318	}
1680	holgerb	319
1928	holgerb	320	}
		321
2185	holgerb	322	if (index < 5 ) s_update(index,tmp); // Update normal potis (CH1-4)
		323	else if (index == 5) s_update(7,signal); // Gear (CH7)
		324	else if (index == 7) s_update(9,signal); // Hover Throttle (CH9)
		325
1928	holgerb	326	#elif defined RECEIVER_SPEKTRUM_DX8EXP
2185	holgerb	327	if(index == 6) // FLIGHT-MODE - The channel used for our data uplink
1928	holgerb	328	{
2185	holgerb	329	if (signal > 100) // SYNC received
1928	holgerb	330	{
		331	if (s_exdata[s_excnt] == 125) s_exparity = ~s_exparity; // Bit = 1 -> Re-Invert parity bit
		332	if (s_excnt == 9 && ((s_exparity == 0 && s_exdata[s_excnt] == -125) \|\| (s_exparity != 0 && s_exdata[s_excnt] == 125))) // Parity check
		333	{
2185	holgerb	334	if (s_exdata[1] == 125 && s_exdata[2] == -125) s_update(5,-125); // Reconstruct tripole Flight-Mode value (CH5)
		335	else if (s_exdata[1] == -125 && s_exdata[2] == -125) s_update(5,0); // Reconstruct tripole Flight-Mode value (CH5)
		336	else if (s_exdata[1] == -125 && s_exdata[2] == 125) s_update(5,125); // Reconstruct tripole Flight-Mode value (CH5)
1928	holgerb	337
2185	holgerb	338	if (s_exdata[3] == 125 && s_exdata[6] == -125) s_update(6,125); // Reconstruct tripole Elev D/R value (CH6)
		339	else if (s_exdata[3] == -125 && s_exdata[6] == -125) s_update(6,0); // Reconstruct tripole Elev D/R value (CH6)
		340	else if (s_exdata[3] == -125 && s_exdata[6] == 125) s_update(6,-125); // Reconstruct tripole Elev D/R value (CH6)
1928	holgerb	341
		342
2185	holgerb	343	if (s_exdata[7] == 125 && s_exdata[8] == -125) s_update(9,-125); // Reconstruct tripole AIL D/R value (CH9)
		344	else if (s_exdata[7] == -125 && s_exdata[8] == -125) s_update(9,0); // Reconstruct tripole AIL D/R value (CH9)
		345	else if (s_exdata[7] == -125 && s_exdata[8] == 125) s_update(9,125); // Reconstruct tripole AIL D/R value (CH9)
1928	holgerb	346
2185	holgerb	347	s_update(10,s_exdata[5]); // Gear (CH10)
		348	s_update(12,s_exdata[4]); // Mix (CH12)
1928	holgerb	349	}
		350
		351	s_excnt = 0; // Reset bitcounter
		352	s_exparity = 0; // Reset parity bit
		353	}
		354
		355	if (signal < 10) s_exdata[++s_excnt] = -125; // Bit = 0 -> value = -125 (min)
		356	if (s_excnt == 10) s_excnt = 0; // Overflow protection
		357	if (signal < -100)
		358	{
		359	s_exdata[s_excnt] = 125; // Bit = 1 -> value = 125 (max)
		360	s_exparity = ~s_exparity; // Bit = 1 -> Invert parity bit
		361	}
		362
		363	}
2185	holgerb	364
		365	if (index < 5 ) s_update(index,tmp); // Update normal potis (CH1-4)
		366	else if (index == 7) s_update(7,signal); // R Trim (CH7)
		367	else if (index == 5) s_update(8,signal); // AUX2 (CH8)
		368	else if (index == 8) s_update(11,signal); // AUX3 (CH11)
		369
		370	#else
1928	holgerb	371	if(SenderOkay >= 180) PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3;
		372	else PPM_diff[index] = 0;
		373	PPM_in[index] = tmp;
1506	holgerb	374	#endif
1928	holgerb	375	}
1506	holgerb	376	else if(index > 17) ReSync = 1; // hier stimmt was nicht: neu synchronisieren
1680	holgerb	377	}
		378	else
		379	{
		380	// hier stimmt was nicht: neu synchronisieren
		381	ReSync = 1;
		382	FrameCnt = 0;
		383	Frame2 = 0;
		384	// new frame next, nach fruehestens 7ms erwartet
		385	SpektrumTimer = MIN_FRAMEGAP;
		386	}
1391	killagreg	387
1680	holgerb	388	// 16 Bytes eingetroffen -> Komplett
		389	if(FrameCnt >= 16)
		390	{
		391	// Frame complete
		392	if(Frame2 == 0)
		393	{
		394	// Null bedeutet: Neue Daten
		395	// nur beim ersten Frame (CH 0-7) setzen
		396	if(!ReSync) NewPpmData = 0;
		397	}
		398	FrameCnt = 0;
		399	Frame2 = 0;
		400	Sync = 0;
		401	SpektrumTimer = MIN_FRAMEGAP;
		402	}
1171	hbuss	403	}
1391	killagreg	404	}
1928	holgerb	405
2185	holgerb	406

Subversion Repositories FlightCtrl

(root)/branches/V2.01a_ACC-HH_MartinR/Spektrum.c @ 2707 - Rev 2384