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

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

Subversion Repositories FlightCtrl

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