WebSVN - FlightCtrl - Blame - Rev 2013 - /branches/V0.86d_ACC-HH_HR

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

Subversion Repositories FlightCtrl

(root)/branches/V0.86d_ACC-HH_HR_MartinR/Spektrum.c - Rev 2013