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

Subversion Repositories FlightCtrl

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