WebSVN - FlightCtrl - Blame - Rev 1897 - /branches/V0.84a_MartinW

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

Subversion Repositories FlightCtrl

(root)/branches/V0.84a_MartinW_Jeti+/Spektrum.c - Rev 1897