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
762	killagreg	13	#define GPS_STICK_SENSE 12
		14	#define GPS_STICK_LIMIT 45
764	killagreg	15	#define GPS_D_LIMIT_DIST 500 // for position deviations grater than 500 cm (5m) 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
762	killagreg	78	//Calculate PD-components of the controller (negative sign for compensation)
		79	P_North = -(GPS_P_Factor * GPSPosDev_North)/2048;
		80	D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512;
		81	P_East = -(GPS_P_Factor * GPSPosDev_East)/2048;
		82	D_East = -(GPS_D_Factor * GPSInfo.veleast)/512;
746	killagreg	83
764	killagreg	84	if( (abs(GPSPosDev_North) > GPS_D_LIMIT_DIST) \|\| (abs(GPSPosDev_East) > GPS_D_LIMIT_DIST) )
		85	{
		86	if (D_North > GPS_D_LIMIT) D_North = GPS_D_LIMIT;
		87	else if (D_North < -GPS_D_LIMIT) D_North = -GPS_D_LIMIT;
		88	if (D_East > GPS_D_LIMIT) D_East = GPS_D_LIMIT;
		89	else if (D_East < -GPS_D_LIMIT) D_East = -GPS_D_LIMIT;
		90	}
		91
762	killagreg	92	// PD-controller
		93	PD_North = P_North + D_North;
		94	PD_East = P_East + D_East;
746	killagreg	95
762	killagreg	96	// GPS to pitch and roll settings
746	killagreg	97
762	killagreg	98	//A positive pitch angle moves head downwards (flying forward).
		99	//A positive roll angle tilts left side downwards (flying left).
		100
		101	// If compass heading is 0 the head of the copter is in north direction.
		102	// A positive pitch angle will fly to north and a positive roll angle will fly to west.
		103	// In case of a positive north deviation/velocity the
		104	// copter should fly to south (negative pitch).
		105	// In case of a positive east position deviation and a positive east velocity the
		106	// copter should fly to west (positive roll).
		107
		108	// The influence of the GPS_Pitch and GPS_Roll variable is contrarily to the stick values
		109	// in the fc.c. Therefore a positive north deviation/velocity should result in a positive
		110	// GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll.
		111
		112	// rotation transformation to compass heading to match any copter orientation
		113	if (CompassHeading < 0) // no valid compass data
		114	{ // disable GPS
		115	GPS_Neutral();
		116	}
		117	else
		118	{
		119	coscompass = (int32_t)c_cos_8192(CompassHeading);
		120	sincompass = (int32_t)c_sin_8192(CompassHeading);
		121	GPS_Roll = (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192);
		122	GPS_Pitch = -1(int16_t)((sincompass PD_East + coscompass * PD_North) / 8192);
		123	}
		124	// limit GPS controls
		125	if (GPS_Pitch > GPS_STICK_LIMIT) GPS_Pitch = GPS_STICK_LIMIT;
764	killagreg	126	else if (GPS_Pitch < -GPS_STICK_LIMIT) GPS_Pitch = -GPS_STICK_LIMIT;
762	killagreg	127	if (GPS_Roll > GPS_STICK_LIMIT) GPS_Roll = GPS_STICK_LIMIT;
764	killagreg	128	else if (GPS_Roll < -GPS_STICK_LIMIT) GPS_Roll = -GPS_STICK_LIMIT;
746	killagreg	129	}
762	killagreg	130	else // invalid input data
746	killagreg	131	{
762	killagreg	132	BeepTime = 50;
		133	GPS_Neutral();
746	killagreg	134	}
		135	}
		136
		137
762	killagreg	138	void GPS_Main(uint8_t ctrl)
746	killagreg	139	{
741	killagreg	140	static uint8_t GPS_Task = TSK_IDLE;
752	killagreg	141	int16_t satbeep;
1	ingob	142
762	killagreg	143	// ctrl enables the gps feature
		144	if(ctrl < 70) GPS_Task = TSK_IDLE;
		145	else if (ctrl < 160) GPS_Task = TSK_HOLD;
		146	else GPS_Task = TSK_HOME; // ctrl >= 160
741	killagreg	147
		148
		149	switch(GPSInfo.status)
726	killagreg	150	{
741	killagreg	151	case INVALID: // invalid gps data
746	killagreg	152	GPS_Neutral();
752	killagreg	153	if(GPS_Task != TSK_IDLE)
		154	{
		155	BeepTime = 100; // beep if signal is neccesary
		156	}
741	killagreg	157	break;
		158	case PROCESSED: // if gps data are already processed
		159	// downcount timeout
		160	if(GPSTimeout) GPSTimeout--;
		161	// if no new data arrived within timeout set current data invalid
		162	// and therefore disable GPS
		163	else
726	killagreg	164	{
746	killagreg	165	GPS_Neutral();
741	killagreg	166	GPSInfo.status = INVALID;
		167	}
		168	break;
		169	case VALID: // new valid data from gps device
		170	// if the gps data quality is sufficient
		171	if (GPSInfo.satfix == SATFIX_3D)
		172	{
		173	switch(GPS_Task) // check what's to do
726	killagreg	174	{
741	killagreg	175	case TSK_IDLE:
		176	// update hold point to current gps position
		177	HoldPosition.Northing = GPSInfo.utmnorth;
		178	HoldPosition.Easting = GPSInfo.utmeast;
		179	HoldPosition.Status = VALID;
		180	// disable gps control
746	killagreg	181	GPS_Neutral();
741	killagreg	182	break; // eof TSK_IDLE
		183	case TSK_HOLD:
762	killagreg	184	if(HoldPosition.Status == VALID)
741	killagreg	185	{
762	killagreg	186	// if sticks are centered (no manual control)
		187	if ((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE))
		188	{
		189	GPS_PDController(&HoldPosition);
		190	}
		191	else // MK controlled by user
		192	{
		193	// update hold point to current gps position
		194	HoldPosition.Northing = GPSInfo.utmnorth;
		195	HoldPosition.Easting = GPSInfo.utmeast;
		196	HoldPosition.Status = GPSInfo.status;
		197	// disable gps control
		198	GPS_Neutral();
		199	}
741	killagreg	200	}
		201	break; // eof TSK_HOLD
746	killagreg	202	case TSK_HOME:
		203	if(HomePosition.Status == VALID)
		204	{
		205	// update hold point to current gps position
762	killagreg	206	// to avoid a flight back if home comming is deactivated
746	killagreg	207	HoldPosition.Northing = GPSInfo.utmnorth;
		208	HoldPosition.Easting = GPSInfo.utmeast;
762	killagreg	209	HoldPosition.Status = GPSInfo.status;
		210	// if sticks are centered (no manual control)
		211	if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE))
		212	{
		213	GPS_PDController(&HomePosition);
		214	}
746	killagreg	215	}
		216	else // bad home position
		217	{
		218	BeepTime = 50; // signal invalid home position
762	killagreg	219	// try to hold at least the position as a fallback option
		220	// if sticks are centered (no manual control)
		221	if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE) && (HoldPosition.Status == VALID))
		222	{
		223	GPS_PDController(&HoldPosition);
		224	}
746	killagreg	225	}
		226	break; // eof TSK_HOME
741	killagreg	227	default: // unhandled task
746	killagreg	228	GPS_Neutral();
741	killagreg	229	break; // eof default
		230	} // eof switch GPS_Task
		231	} // eof 3D-FIX
727	killagreg	232
741	killagreg	233	else // no 3D-SATFIX
		234	{ // disable gps control
746	killagreg	235	GPS_Neutral();
752	killagreg	236	if(GPS_Task != TSK_IDLE)
		237	{
		238	satbeep = 2048 - (int16_t)GPSInfo.satnum * 256; // is zero at 8 sats
		239	if (satbeep < 0) satbeep = 0;
		240	BeepTime = 50 + (uint16_t)satbeep;
		241	}
741	killagreg	242	}
		243	// set current data as processed to avoid further calculations on the same gps data
		244	GPSInfo.status = PROCESSED;
		245	break;
		246	} // eof GPSInfo.status
		247	DebugOut.Analog[14] = GPS_Pitch;
		248	DebugOut.Analog[15] = GPS_Roll;
726	killagreg	249	}
		250

Subversion Repositories FlightCtrl

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