WebSVN - FlightCtrl - Blame - /branches/V0.68d Code Redesign killagreg/GPS.c

Rev	Author	Line No.	Line
726	killagreg	1	#include <inttypes.h>
1	ingob	2
726	killagreg	3	#include "fc.h"
		4	#include "ubx.h"
		5	#include "mymath.h"
727	killagreg	6	#include "timer0.h"
741	killagreg	7	#include "uart.h"
701	killagreg	8
741	killagreg	9	#define TSK_IDLE 0
		10	#define TSK_HOLD 1
		11	#define TSK_HOME 2
		12
		13
726	killagreg	14	int16_t GPS_Pitch = 0;
		15	int16_t GPS_Roll = 0;
		16
746	killagreg	17	int32_t GPS_P_Factor = 0, GPS_D_Factor = 0;
		18	int32_t GPSPosDev_North = 0, GPSPosDev_East = 0;
726	killagreg	19
746	killagreg	20
726	killagreg	21	typedef struct
1	ingob	22	{
726	killagreg	23	int32_t Northing;
		24	int32_t Easting;
		25	uint8_t Status;
1	ingob	26
726	killagreg	27	} GPS_Pos_t;
		28
		29	// GPS coordinates for hold position
741	killagreg	30	GPS_Pos_t HoldPosition = {0,0, INVALID};
		31	// GPS coordinates for home position
		32	GPS_Pos_t HomePosition = {0,0, INVALID};
726	killagreg	33
		34
		35	// ---------------------------------------------------------------------------------
		36
747	killagreg	37	// set home position to current hold positon
746	killagreg	38	void GPS_SetHomePosition(void)
		39	{
		40	HomePosition.Northing = HoldPosition.Northing;
		41	HomePosition.Easting = HoldPosition.Easting;
		42	HomePosition.Status = HoldPosition.Status;
		43	if(HomePosition.Status == VALID) BeepTime = 1000; // signal if new home position was set
		44	}
726	killagreg	45
746	killagreg	46
747	killagreg	47	// disable GPS control sticks
746	killagreg	48	void GPS_Neutral(void)
1	ingob	49	{
746	killagreg	50	GPS_Pitch = 0;
		51	GPS_Roll = 0;
		52	}
		53
747	killagreg	54	// calculates the GPS control sticks values from the position deviation
746	killagreg	55	void GPS_PDController(void)
		56	{
726	killagreg	57	int32_t coscompass, sincompass;
		58	int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0;
		59	int32_t PD_North = 0, PD_East = 0;
746	killagreg	60	//Calculate PD-components of the controller (negative sign for compensation)
		61	P_North = -(GPS_P_Factor * GPSPosDev_North)/2048;
		62	D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512;
		63	P_East = -(GPS_P_Factor * GPSPosDev_East)/2048;
		64	D_East = -(GPS_D_Factor * GPSInfo.veleast)/512;
		65
		66	// PD-controller
		67	PD_North = P_North + D_North;
		68	PD_East = P_East + D_East;
		69
		70	// GPS to pitch and roll settings
		71
		72	//A positive pitch angle moves head downwards (flying forward).
		73	//A positive roll angle tilts left side downwards (flying left).
		74
		75	// If compass heading is 0 the head of the copter is in north direction.
		76	// A positive pitch angle will fly to north and a positive roll angle will fly to west.
		77	// In case of a positive north deviation/velocity the
		78	// copter should fly to south (negative pitch).
		79	// In case of a positive east position deviation and a positive east velocity the
		80	// copter should fly to west (positive roll).
		81
		82	// The influence of the GPS_Pitch and GPS_Roll variable is contrarily to the stick values
		83	// in the fc.c. Therefore a positive north deviation/velocity should result in a positive
		84	// GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll.
		85
		86	// rotation transformation to compass heading to match any copter orientation
		87	if (CompassHeading < 0) // no valid compass data
		88	{ // disable GPS
		89	GPS_Neutral();
		90	}
		91	else
		92	{
		93	coscompass = (int32_t)c_cos_8192(CompassHeading);
		94	sincompass = (int32_t)c_sin_8192(CompassHeading);
		95	GPS_Roll = (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192);
		96	GPS_Pitch = -1(int16_t)((sincompass PD_East + coscompass * PD_North) / 8192);
		97	}
		98	// limit GPS controls
		99	#define GPS_CTRL_LIMIT 35
		100	if (GPS_Pitch > GPS_CTRL_LIMIT) GPS_Pitch = GPS_CTRL_LIMIT;
		101	if (GPS_Pitch < -GPS_CTRL_LIMIT) GPS_Pitch = -GPS_CTRL_LIMIT;
		102	if (GPS_Roll > GPS_CTRL_LIMIT) GPS_Roll = GPS_CTRL_LIMIT;
		103	if (GPS_Roll < -GPS_CTRL_LIMIT) GPS_Roll = -GPS_CTRL_LIMIT;
		104	}
		105
		106
		107	void GPS_Main(void)
		108	{
741	killagreg	109	static uint8_t GPS_Task = TSK_IDLE;
752	killagreg	110	int16_t satbeep;
1	ingob	111
726	killagreg	112	// poti2 enables the gps feature
741	killagreg	113	if(Poti2 < 70) GPS_Task = TSK_IDLE;
		114	else if (Poti2 < 160) GPS_Task = TSK_HOLD;
746	killagreg	115	else GPS_Task = TSK_HOME; // Poti2 >= 160
741	killagreg	116
		117
		118	switch(GPSInfo.status)
726	killagreg	119	{
741	killagreg	120	case INVALID: // invalid gps data
746	killagreg	121	GPS_Neutral();
752	killagreg	122	if(GPS_Task != TSK_IDLE)
		123	{
		124	BeepTime = 100; // beep if signal is neccesary
		125	}
741	killagreg	126	break;
		127	case PROCESSED: // if gps data are already processed
		128	// downcount timeout
		129	if(GPSTimeout) GPSTimeout--;
		130	// if no new data arrived within timeout set current data invalid
		131	// and therefore disable GPS
		132	else
726	killagreg	133	{
746	killagreg	134	GPS_Neutral();
741	killagreg	135	GPSInfo.status = INVALID;
		136	}
		137	break;
		138	case VALID: // new valid data from gps device
		139	// if the gps data quality is sufficient
		140	if (GPSInfo.satfix == SATFIX_3D)
		141	{
		142	switch(GPS_Task) // check what's to do
726	killagreg	143	{
741	killagreg	144	case TSK_IDLE:
		145	// update hold point to current gps position
		146	HoldPosition.Northing = GPSInfo.utmnorth;
		147	HoldPosition.Easting = GPSInfo.utmeast;
		148	HoldPosition.Status = VALID;
		149	// disable gps control
746	killagreg	150	GPS_Neutral();
741	killagreg	151	break; // eof TSK_IDLE
		152	case TSK_HOLD:
		153	// if sticks are centered and hold position is valid enable position hold control
		154	if ((MaxStickPitch < 20) && (MaxStickRoll < 20) && (HoldPosition.Status == VALID))
		155	{
		156	// Calculate deviation from hold position
		157	GPSPosDev_North = GPSInfo.utmnorth - HoldPosition.Northing;
		158	GPSPosDev_East = GPSInfo.utmeast - HoldPosition.Easting;
		159	DebugOut.Analog[12] = GPSPosDev_North;
		160	DebugOut.Analog[13] = GPSPosDev_East;
1	ingob	161
746	killagreg	162	GPS_PDController();
741	killagreg	163	}
		164	else // MK controlled by user
		165	{
		166	// update hold point to current gps position
		167	HoldPosition.Northing = GPSInfo.utmnorth;
		168	HoldPosition.Easting = GPSInfo.utmeast;
		169	HoldPosition.Status = VALID;
		170	// disable gps control
746	killagreg	171	GPS_Neutral();
741	killagreg	172	}
		173	break; // eof TSK_HOLD
746	killagreg	174	case TSK_HOME:
		175	if(HomePosition.Status == VALID)
		176	{
		177	// update hold point to current gps position
		178	HoldPosition.Northing = GPSInfo.utmnorth;
		179	HoldPosition.Easting = GPSInfo.utmeast;
		180	HoldPosition.Status = VALID;
		181
		182	// Calculate deviation from home position
		183	GPSPosDev_North = GPSInfo.utmnorth - HomePosition.Northing;
		184	GPSPosDev_East = GPSInfo.utmeast - HomePosition.Easting;
		185	DebugOut.Analog[12] = GPSPosDev_North;
		186	DebugOut.Analog[13] = GPSPosDev_East;
		187
		188	GPS_PDController();
		189	}
		190	else // bad home position
		191	{
		192	GPS_Neutral();
		193	BeepTime = 50; // signal invalid home position
		194	}
		195	break; // eof TSK_HOME
741	killagreg	196	default: // unhandled task
746	killagreg	197	GPS_Neutral();
741	killagreg	198	break; // eof default
		199	} // eof switch GPS_Task
		200	} // eof 3D-FIX
727	killagreg	201
741	killagreg	202	else // no 3D-SATFIX
		203	{ // disable gps control
746	killagreg	204	GPS_Neutral();
752	killagreg	205	if(GPS_Task != TSK_IDLE)
		206	{
		207	satbeep = 2048 - (int16_t)GPSInfo.satnum * 256; // is zero at 8 sats
		208	if (satbeep < 0) satbeep = 0;
		209	BeepTime = 50 + (uint16_t)satbeep;
		210	}
741	killagreg	211	}
		212	// set current data as processed to avoid further calculations on the same gps data
		213	GPSInfo.status = PROCESSED;
		214	break;
		215	} // eof GPSInfo.status
		216	DebugOut.Analog[14] = GPS_Pitch;
		217	DebugOut.Analog[15] = GPS_Roll;
726	killagreg	218	}
		219

Subversion Repositories FlightCtrl

(root)/branches/V0.68d Code Redesign killagreg/GPS.c @ 752 - Rev