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

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

Subversion Repositories FlightCtrl

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