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;
1	ingob	110
741	killagreg	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();
741	killagreg	122	if(GPS_Task != TSK_IDLE) BeepTime = 50; // beep if signal is neccesary
		123	break;
		124	case PROCESSED: // if gps data are already processed
		125	// downcount timeout
		126	if(GPSTimeout) GPSTimeout--;
		127	// if no new data arrived within timeout set current data invalid
		128	// and therefore disable GPS
		129	else
726	killagreg	130	{
746	killagreg	131	GPS_Neutral();
741	killagreg	132	GPSInfo.status = INVALID;
		133	}
		134	break;
		135	case VALID: // new valid data from gps device
		136	// if the gps data quality is sufficient
		137	if (GPSInfo.satfix == SATFIX_3D)
		138	{
		139	switch(GPS_Task) // check what's to do
726	killagreg	140	{
741	killagreg	141	case TSK_IDLE:
		142	// update hold point to current gps position
		143	HoldPosition.Northing = GPSInfo.utmnorth;
		144	HoldPosition.Easting = GPSInfo.utmeast;
		145	HoldPosition.Status = VALID;
		146	// disable gps control
746	killagreg	147	GPS_Neutral();
741	killagreg	148	break; // eof TSK_IDLE
		149	case TSK_HOLD:
		150	// if sticks are centered and hold position is valid enable position hold control
		151	if ((MaxStickPitch < 20) && (MaxStickRoll < 20) && (HoldPosition.Status == VALID))
		152	{
		153	// Calculate deviation from hold position
		154	GPSPosDev_North = GPSInfo.utmnorth - HoldPosition.Northing;
		155	GPSPosDev_East = GPSInfo.utmeast - HoldPosition.Easting;
		156	DebugOut.Analog[12] = GPSPosDev_North;
		157	DebugOut.Analog[13] = GPSPosDev_East;
1	ingob	158
746	killagreg	159	GPS_PDController();
741	killagreg	160	}
		161	else // MK controlled by user
		162	{
		163	// update hold point to current gps position
		164	HoldPosition.Northing = GPSInfo.utmnorth;
		165	HoldPosition.Easting = GPSInfo.utmeast;
		166	HoldPosition.Status = VALID;
		167	// disable gps control
746	killagreg	168	GPS_Neutral();
741	killagreg	169	}
		170	break; // eof TSK_HOLD
746	killagreg	171	case TSK_HOME:
		172	if(HomePosition.Status == VALID)
		173	{
		174	// update hold point to current gps position
		175	HoldPosition.Northing = GPSInfo.utmnorth;
		176	HoldPosition.Easting = GPSInfo.utmeast;
		177	HoldPosition.Status = VALID;
		178
		179	// Calculate deviation from home position
		180	GPSPosDev_North = GPSInfo.utmnorth - HomePosition.Northing;
		181	GPSPosDev_East = GPSInfo.utmeast - HomePosition.Easting;
		182	DebugOut.Analog[12] = GPSPosDev_North;
		183	DebugOut.Analog[13] = GPSPosDev_East;
		184
		185	GPS_PDController();
		186	}
		187	else // bad home position
		188	{
		189	GPS_Neutral();
		190	BeepTime = 50; // signal invalid home position
		191	}
		192	break; // eof TSK_HOME
741	killagreg	193	default: // unhandled task
746	killagreg	194	GPS_Neutral();
741	killagreg	195	break; // eof default
		196	} // eof switch GPS_Task
		197	} // eof 3D-FIX
727	killagreg	198
741	killagreg	199	else // no 3D-SATFIX
		200	{ // disable gps control
746	killagreg	201	GPS_Neutral();
741	killagreg	202	if(GPS_Task != TSK_IDLE) BeepTime = 50;
		203	}
		204
		205	// set current data as processed to avoid further calculations on the same gps data
		206	GPSInfo.status = PROCESSED;
		207	break;
		208	} // eof GPSInfo.status
		209	DebugOut.Analog[14] = GPS_Pitch;
		210	DebugOut.Analog[15] = GPS_Roll;
726	killagreg	211	}
		212

Subversion Repositories FlightCtrl

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