WebSVN - FlightCtrl - Blame - Rev 838 - /branches/KalmanFilter MikeW/gps.c

Rev	Author	Line No.	Line
838	MikeW	1	/*
		2	Copyright 2008, by Michael Walter
		3
		4	All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser
		5	General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but
		6	WITHOUT ANY WARRANTY, without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
		7	See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public
		8	License along with this program. If not, see <http://www.gnu.org/licenses/>.
		9
		10	Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that
		11	are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de
		12	unless it is stated otherwise.
		13	*/
		14
		15	#include "main.h"
		16	#include "kafi.h"
		17
		18	#include "mymath.h"
		19	#include <math.h>
		20
		21	int GPSTracking = 0;
		22	int targetPosValid = 0;
		23	int homePosValid = 0;
		24
		25	uint8_t gpsState;
		26
		27	gpsInfo_t actualPos; // measured position (last gps record)
		28	gpsInfo_t targetPos; // measured position (last gps record)
		29	gpsInfo_t homePos; // measured position (last gps record)
		30
		31	NAV_STATUS_t navStatus;
		32	NAV_POSLLH_t navPosLlh;
		33	NAV_POSUTM_t navPosUtm;
		34	NAV_VELNED_t navVelNed;
		35
		36	signed int GPS_Nick = 0;
		37	signed int GPS_Roll = 0;
		38
		39	long distanceNS = 0;
		40	long distanceEW = 0;
		41	long velocityNS = 0;
		42	long velocityEW = 0;
		43	unsigned long maxDistance = 0;
		44
		45	int roll_gain = 0;
		46	int nick_gain = 0;
		47	int nick_gain_p = 0;
		48	int nick_gain_d = 0;
		49	int roll_gain_p = 0;
		50	int roll_gain_d = 0;
		51	int Override = 0;
		52	int TargetGier = 0;
		53
		54	extern int sollGier;
		55
		56	char ubxP, ubxEp, *ubxSp; // pointers to packet currently transfered
		57	uint8_t CK_A, CK_B; // UBX checksum bytes
		58	uint8_t ignorePacket; // true when previous packet was not processed
		59	unsigned short msgLen;
		60	uint8_t msgID;
		61
		62	void GPSscanData (void);
		63	void GPSupdate(void);
		64
		65
		66	/* ****************************************************************************
		67	Functionname: GPSupdate / /!
		68	Description:
		69
		70	@return void
		71	@pre -
		72	@post -
		73	@author Michael Walter
		74	**************************************************************************** */
		75	void GPSupdate(void)
		76	{
		77	float SIN_H, COS_H;
		78	long max_p = 0;
		79	long max_d = 0;
		80
		81	if (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]])> 15 \|\|
		82	abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]])> 10 \|\|
		83	abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]])> 15)
		84	{
		85	Override = 1;
		86	}
		87	else
		88	{
		89	Override = 0;
		90	}
		91
		92	/* Set Home Position */
		93	if ((actualPos.state > 2) &&
		94	(homePosValid == 0))
		95	{
		96	/* Save Current Position */
		97	homePos.north = actualPos.north;
		98	homePos.east = actualPos.east;
		99	homePos.altitude = actualPos.altitude ;
		100	homePosValid = 1;
		101	}
		102
		103	/* Set Target Position */
		104	if ((actualPos.state > 2) &&
		105	(PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] < -100))
		106	{
		107	/* Save Current Position */
		108	targetPos.north = actualPos.north;
		109	targetPos.east = actualPos.east;
		110	targetPos.altitude = actualPos.altitude ;
		111	targetPosValid = 1;
		112
		113	if(BeepMuster == 0xffff)
		114	{
		115	beeptime = 5000;
		116	BeepMuster = 0x0100;
		117	}
		118	}
		119
		120	if ((GPSTracking == 1) &&
		121	(PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] < 100))
		122	{
		123	GPSTracking = 0;
		124	beeptime = 5000;
		125	BeepMuster = 0x0080;
		126	}
		127
		128	/* The System is in Tracking State*/
		129	if ((GPSTracking == 0) &&
		130	(targetPosValid == 1) &&
		131	(PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] > 100) &&
		132	(actualPos.state > 2))
		133	{
		134	GPSTracking = 1;
		135	beeptime = 5000;
		136	BeepMuster = 0x0c00;
		137	}
		138
		139	max_p = (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 170)) / 20);
		140	max_d = (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 170)) / 40);
		141
		142	#if 0
		143	DebugOut.Analog[11] = max_p;
		144	DebugOut.Analog[12] = max_d;
		145	#endif
		146
		147	static int GPSTrackingCycles = 0;
		148	long maxGainPos = 140;
		149	long maxGainVel = 140;
		150
		151	/* Slowly ramp up gain */
		152	if (GPSTrackingCycles < 1000)
		153	{
		154	GPSTrackingCycles++;
		155	}
		156	maxGainPos = (maxGainPos * GPSTrackingCycles) / 1000;
		157	maxGainVel = (maxGainVel * GPSTrackingCycles) / 1000;
		158
		159	if (actualPos.state > 2 )
		160	{
		161	distanceNS = actualPos.north - targetPos.north; // in 0.1m
		162	distanceEW = actualPos.east - targetPos.east; // in 0.1m
		163	velocityNS = actualPos.velNorth; // in 0.1m/s
		164	velocityEW = actualPos.velEast; // in 0.1m/s
		165	maxDistance = sqrt(distanceNS * distanceNS + distanceEW * distanceEW);
		166
		167	// Limit GPS_Nick to 25
		168	nick_gain_p = (long) MAX(-maxGainPos, MIN(maxGainPos, ((distanceNS* max_p)/50))); //m
		169	nick_gain_d = (long) MAX(-maxGainVel, MIN(maxGainVel, ((actualPos.velNorth * max_d)/50))); //m/s
		170	roll_gain_p = (long) MAX(-maxGainPos, MIN(maxGainPos, ((distanceEW * max_p)/50))); //m
		171	roll_gain_d = (long) MAX(-maxGainVel, MIN(maxGainVel, ((actualPos.velEast * max_d)/50))); //m/s
		172
		173	TargetGier = c_atan2(-distanceEW / 8, -distanceNS / 8) * 10;
		174	}
		175
		176	if ((GPSTracking == 1) &&
		177	(actualPos.state > 2) &&
		178	(Override == 0))
		179	{
		180	/* Calculate Distance to Target */
		181	SIN_H = (float) sin(status.Psi);
		182	COS_H =(float) cos(status.Psi);
		183
		184	/* PD-Regler */
		185	nick_gain = nick_gain_p + nick_gain_d;
		186	roll_gain = -(roll_gain_p + roll_gain_d);
		187
		188	GPS_Nick = (int) (COS_H * (float) nick_gain - SIN_H * (float) roll_gain);
		189	GPS_Roll = (int) (SIN_H * (float) nick_gain + COS_H * (float) roll_gain);
		190
		191	GPS_Nick = MAX(-maxGainPos, MIN(maxGainPos, GPS_Nick));
		192	GPS_Roll = MAX(-maxGainPos, MIN(maxGainPos, GPS_Roll));
		193
		194	/* Turn the mikrokopter slowly towards the target position */
		195	{
		196	int OffsetGier;
		197	if (maxDistance / 10 > 2)
		198	{
		199	OffsetGier = 2;
		200	}
		201	else
		202	{
		203	OffsetGier = 0;
		204	}
		205
		206	if (TargetGier < 0)
		207	{
		208	TargetGier += 3600;
		209	}
		210	if (TargetGier > sollGier)
		211	{
		212	if (TargetGier - sollGier < 1800)
		213	{
		214	sollGier += OffsetGier;
		215	}
		216	else
		217	{
		218	sollGier -= OffsetGier;
		219	}
		220	}
		221	else
		222	{
		223	if (sollGier - TargetGier < 1800)
		224	{
		225	sollGier -= OffsetGier;
		226	}
		227	else
		228	{
		229	sollGier += OffsetGier;
		230	}
		231	}
		232	}
		233	#if 0
		234	/* Go round in a square */
		235	if (maxDistance / 10 < 10)
		236	{
		237	static int iState = 0;
		238	switch (iState)
		239	{
		240	case 0:
		241	targetPos.north += 400;
		242	targetPos.east += 0;
		243	GPSTrackingCycles = 0;
		244	iState++;
		245	break;
		246	case 1:
		247	targetPos.north += 0;
		248	targetPos.east += 400;
		249	GPSTrackingCycles = 0;
		250	iState++;
		251	break;
		252	case 2:
		253	targetPos.north -= 400;
		254	targetPos.east += 0;
		255	GPSTrackingCycles = 0;
		256	iState++;
		257	break;
		258	case 3:
		259	targetPos.north += 0;
		260	targetPos.east -= 400;
		261	GPSTrackingCycles = 0;
		262	iState=0;
		263	break;
		264	default:
		265	iState=0;
		266	break;
		267	}
		268	beeptime = 5000;
		269	BeepMuster = 0x0c00;
		270	}
		271	#endif
		272	}
		273	else
		274	{
		275	GPS_Nick = 0;
		276	GPS_Roll = 0;
		277	GPSTrackingCycles = 0;
		278	}
		279	}
		280
		281	/* ****************************************************************************
		282	Functionname: InitGPS / /!
		283	Description:
		284
		285	@return void
		286	@pre -
		287	@post -
		288	@author
		289	**************************************************************************** */
		290	void InitGPS(void)
		291	{
		292	// USART1 Control and Status Register A, B, C and baud rate register
		293	uint8_t sreg = SREG;
		294	//uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART1_BAUD) - 1);
		295
		296	// disable all interrupts before reconfiguration
		297	cli();
		298
		299	// disable RX-Interrupt
		300	UCSR1B &= ~(1 << RXCIE1);
		301	// disable TX-Interrupt
		302	UCSR1B &= ~(1 << TXCIE1);
		303	// disable DRE-Interrupt
		304	UCSR1B &= ~(1 << UDRIE1);
		305
		306	// set direction of RXD1 and TXD1 pins
		307	// set RXD1 (PD2) as an input pin
		308	PORTD \|= (1 << PORTD2);
		309	DDRD &= ~(1 << DDD2);
		310
		311	// set TXD1 (PD3) as an output pin
		312	PORTD \|= (1 << PORTD3);
		313	DDRD \|= (1 << DDD3);
		314
		315	// USART0 Baud Rate Register
		316	// set clock divider
		317	UBRR1H = 0;
		318	UBRR1L = BAUDRATE;
		319
		320	// enable double speed operation
		321	//UCSR1A \|= (1 << U2X1);
		322	UCSR1A = _B0(U2X1);
		323
		324	// enable receiver and transmitter
		325	UCSR1B = (1 << TXEN1) \| (1 << RXEN1);
		326	// set asynchronous mode
		327	UCSR1C &= ~(1 << UMSEL11);
		328	UCSR1C &= ~(1 << UMSEL10);
		329	// no parity
		330	UCSR1C &= ~(1 << UPM11);
		331	UCSR1C &= ~(1 << UPM10);
		332	// 1 stop bit
		333	UCSR1C &= ~(1 << USBS1);
		334	// 8-bit
		335	UCSR1B &= ~(1 << UCSZ12);
		336	UCSR1C \|= (1 << UCSZ11);
		337	UCSR1C \|= (1 << UCSZ10);
		338
		339	// flush receive buffer explicit
		340	while ( UCSR1A & (1<<RXC1) ) UDR1;
		341
		342	// enable interrupts at the end
		343	// enable RX-Interrupt
		344	UCSR1B \|= (1 << RXCIE1);
		345	// enable TX-Interrupt
		346	UCSR1B \|= (1 << TXCIE1);
		347	// enable DRE interrupt
		348	//UCSR1B \|= (1 << UDRIE1);
		349
		350	// restore global interrupt flags
		351	SREG = sreg;
		352
		353	gpsState = GPS_EMPTY;
		354	}
		355
		356
		357	/* ****************************************************************************
		358	Functionname: InitGPS / /!
		359	Description: Copy GPS paket data to actualPos
		360
		361	@return void
		362	@pre -
		363	@post -
		364	@author
		365	**************************************************************************** */
		366	void GPSscanData (void)
		367	{
		368	if (navStatus.packetStatus == 1) // valid packet
		369	{
		370	actualPos.state = navStatus.GPSfix;
		371	if ((actualPos.state > 1) && (GPSTracking == 0))
		372	{
		373	GRN_FLASH;
		374	}
		375	navStatus.packetStatus = 0;
		376	}
		377
		378	if (navPosUtm.packetStatus == 1) // valid packet
		379	{
		380	actualPos.north = navPosUtm.NORTH/10L;
		381	actualPos.east = navPosUtm.EAST/10L;
		382	actualPos.altitude = navPosUtm.ALT/100;
		383	actualPos.ITOW = navPosUtm.ITOW;
		384	navPosUtm.packetStatus = 0;
		385	}
		386	/*
		387	if (navPosLlh.packetStatus == 1)
		388	{
		389	actualPos.longi = navPosLlh.LON;
		390	actualPos.lati = navPosLlh.LAT;
		391	actualPos.height = navPosLlh.HEIGHT;
		392	navPosLlh.packetStatus = 0;
		393	}
		394	*/
		395	if (navVelNed.packetStatus == 1)
		396	{
		397	actualPos.velNorth = navVelNed.VEL_N;
		398	actualPos.velEast = navVelNed.VEL_E;
		399	actualPos.velDown = navVelNed.VEL_D;
		400	actualPos.groundSpeed = navVelNed.GSpeed;
		401	navVelNed.packetStatus = 0;
		402	}
		403	}
		404
		405	/* ****************************************************************************
		406	Functionname: SIGNAL / /!
		407	Description:
		408
		409	@return void
		410	@pre -
		411	@post -
		412	@author
		413	**************************************************************************** */
		414	SIGNAL (SIG_USART1_RECV)
		415	{
		416	uint8_t c;
		417	uint8_t re;
		418
		419	re = (UCSR1A & (_B1(FE1) \| _B1(DOR1))); // any error occured ?
		420	c = UDR1;
		421
		422	if (re == 0)
		423	{
		424	switch (gpsState)
		425	{
		426	case GPS_EMPTY:
		427	if (c == SYNC_CHAR1)
		428	{
		429	gpsState = GPS_SYNC1;
		430	}
		431	break;
		432	case GPS_SYNC1:
		433	if (c == SYNC_CHAR2)
		434	{
		435	gpsState = GPS_SYNC2;
		436	}
		437	else if (c != SYNC_CHAR1)
		438	{
		439	gpsState = GPS_EMPTY;
		440	}
		441	break;
		442	case GPS_SYNC2:
		443	if (c == CLASS_NAV)
		444	{
		445	gpsState = GPS_CLASS;
		446	}
		447	else
		448	{
		449	gpsState = GPS_EMPTY;
		450	}
		451	break;
		452	case GPS_CLASS: // msg ID seen: init packed receive
		453	msgID = c;
		454	CK_A = CLASS_NAV + c;
		455	CK_B = CLASS_NAV + CK_A;
		456	gpsState = GPS_LEN1;
		457
		458	switch (msgID)
		459	{
		460	case MSGID_STATUS:
		461	ubxP = (char*)&navStatus;
		462	ubxEp = (char*)(&navStatus + sizeof(NAV_STATUS_t));
		463	ubxSp = (char*)&navStatus.packetStatus;
		464	ignorePacket = navStatus.packetStatus;
		465	break;
		466	/*
		467	case MSGID_POSLLH:
		468	ubxP = (char*)&navPosLlh;
		469	ubxEp = (char*)(&navPosLlh + sizeof(NAV_POSLLH_t));
		470	ubxSp = (char*)&navPosLlh.packetStatus;
		471	ignorePacket = navPosLlh.packetStatus;
		472	break;
		473	*/
		474	case MSGID_POSUTM:
		475	ubxP = (char*)&navPosUtm;
		476	ubxEp = (char*)(&navPosUtm + sizeof(NAV_POSUTM_t));
		477	ubxSp = (char*)&navPosUtm.packetStatus;
		478	ignorePacket = navPosUtm.packetStatus;
		479	break;
		480	case MSGID_VELNED:
		481	ubxP = (char*)&navVelNed;
		482	ubxEp = (char*)(&navVelNed + sizeof(NAV_VELNED_t));
		483	ubxSp = (char*)&navVelNed.packetStatus;
		484	ignorePacket = navVelNed.packetStatus;
		485	break;
		486	default:
		487	ignorePacket = 1;
		488	ubxSp = (char*)0;
		489	}
		490	break;
		491	case GPS_LEN1: // first len byte
		492	msgLen = c;
		493	CK_A += c;
		494	CK_B += CK_A;
		495	gpsState = GPS_LEN2;
		496	break;
		497	case GPS_LEN2: // second len byte
		498	msgLen = msgLen + (c * 256);
		499	CK_A += c;
		500	CK_B += CK_A;
		501	gpsState = GPS_FILLING; // next data will be stored in packet struct
		502	break;
		503	case GPS_FILLING:
		504	CK_A += c;
		505	CK_B += CK_A;
		506
		507	if ( !ignorePacket && ubxP < ubxEp)
		508	{
		509	*ubxP++ = c;
		510	}
		511
		512	if (--msgLen == 0)
		513	{
		514	gpsState = GPS_CKA;
		515	}
		516	break;
		517	case GPS_CKA:
		518	if (c == CK_A)
		519	{
		520	gpsState = GPS_CKB;
		521	}
		522	else
		523	{
		524	gpsState = GPS_EMPTY;
		525	}
		526	break;
		527	case GPS_CKB:
		528	if (c == CK_B && ubxSp) // No error -> packet received successfully
		529	{
		530	*ubxSp = 1; // set packetStatus in struct
		531	GPSscanData();
		532	}
		533	gpsState = GPS_EMPTY; // ready for next packet
		534	break;
		535	default:
		536	gpsState = GPS_EMPTY; // ready for next packet
		537	}
		538	}
		539	else // discard any data if error occured
		540	{
		541	gpsState = GPS_EMPTY;
		542	}
		543	}
		544
		545
		546
		547

Subversion Repositories FlightCtrl

(root)/branches/KalmanFilter MikeW/gps.c @ 894 - Rev 838