WebSVN - FlightCtrl - Blame - /branches/V0.69k Code Redesign killagreg/gps.c

Rev	Author	Line No.	Line
886	killagreg	1	#include <inttypes.h>
		2	#include <stdlib.h>
		3	#include "fc.h"
		4	#include "ubx.h"
		5	#include "mymath.h"
		6	#include "timer0.h"
907	killagreg	7	//#include "uart.h"
886	killagreg	8	#include "rc.h"
		9	#include "eeprom.h"
		10
		11	#define TSK_IDLE 0
		12	#define TSK_HOLD 1
		13	#define TSK_HOME 2
		14
		15	#define GPS_STICK_SENSE 15 // must be at least in a range where 90% of the trimming does not switch of the GPS function
		16	#define GPS_STICK_LIMIT 45 // limit of gps stick control to avoid critical flight attitudes
		17	#define GPS_POSDEV_INTEGRAL_LIMIT 32000 // limit for the position error integral
		18	#define GPS_P_LIMIT 25
		19
		20
		21	uint8_t GPS_P_Factor = 0, GPS_I_Factor = 0, GPS_D_Factor = 0;
		22
		23
		24
		25	typedef struct
		26	{
		27	int32_t Longitude;
		28	int32_t Latitude;
		29	int32_t Altitude;
		30	uint8_t Status;
		31	} GPS_Pos_t;
		32
		33	// GPS coordinates for hold position
		34	GPS_Pos_t HoldPosition = {0,0,0,INVALID};
		35	// GPS coordinates for home position
		36	GPS_Pos_t HomePosition = {0,0,0,INVALID};
		37
		38
		39	// ---------------------------------------------------------------------------------
		40
911	killagreg	41	// checks nick and roll sticks for manual control
886	killagreg	42	uint8_t IsManualControlled(void)
		43	{
911	killagreg	44	if ( (abs(PPM_in[ParamSet.ChannelAssignment[CH_NICK]]) < GPS_STICK_SENSE) && (abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) < GPS_STICK_SENSE)) return 0;
886	killagreg	45	else return 1;
		46	}
		47
		48	// set home position to current positon
		49	void GPS_SetHomePosition(void)
		50	{
		51	if( ((GPSInfo.status == VALID) \|\| (GPSInfo.status == PROCESSED)) && GPSInfo.satfix == SATFIX_3D)
		52	{
		53	HomePosition.Longitude = GPSInfo.longitude;
		54	HomePosition.Latitude = GPSInfo.latitude;
		55	HomePosition.Altitude = GPSInfo.altitude;
		56	HomePosition.Status = VALID;
		57	BeepTime = 1000; // signal if new home position was set
		58	}
		59	else
		60	{
		61	HomePosition.Status = INVALID;
		62	}
		63	}
		64
		65	// set hold position to current positon
		66	void GPS_SetHoldPosition(void)
		67	{
		68	if( ((GPSInfo.status == VALID) \|\| (GPSInfo.status == PROCESSED)) && GPSInfo.satfix == SATFIX_3D)
		69	{
		70	HoldPosition.Longitude = GPSInfo.longitude;
		71	HoldPosition.Latitude = GPSInfo.latitude;
		72	HoldPosition.Altitude = GPSInfo.altitude;
		73	HoldPosition.Status = VALID;
		74	}
		75	else
		76	{
		77	HoldPosition.Status = INVALID;
		78	}
		79	}
		80
		81	// clear home position
		82	void GPS_ClearHomePosition(void)
		83	{
		84	HomePosition.Status = INVALID;
		85	}
		86
		87	// disable GPS control sticks
		88	void GPS_Neutral(void)
		89	{
911	killagreg	90	GPS_Nick = 0;
886	killagreg	91	GPS_Roll = 0;
		92	}
		93
		94	// calculates the GPS control stick values from the deviation to target position
		95	// if the pointer to the target positin is NULL or is the target position invalid
		96	// then the P part of the controller is deactivated.
		97	void GPS_PIDController(GPS_Pos_t *pTargetPos)
		98	{
911	killagreg	99	int32_t temp, temp1, PID_Nick, PID_Roll;
886	killagreg	100	int32_t coscompass, sincompass;
		101	int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm
		102	int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0;
		103	int32_t PID_North = 0, PID_East = 0;
		104	static int32_t cos_target_latitude = 1;
		105	static int32_t GPSPosDevIntegral_North = 0, GPSPosDevIntegral_East = 0;
		106	static GPS_Pos_t *pLastTargetPos = 0;
		107
		108	// if GPS data and Compass are ok
		109	if((GPSInfo.status == VALID) && (GPSInfo.satfix == SATFIX_3D) && (CompassHeading >= 0) )
		110	{
		111
		112	if(pTargetPos != NULL) // if there is a target position
		113	{
		114	if(pTargetPos->Status != INVALID) // and the position data are valid
		115	{
		116	// if the target data are updated or the target pointer has changed
		117	if ((pTargetPos->Status != PROCESSED) \|\| (pTargetPos != pLastTargetPos) )
		118	{
		119	// reset error integral
		120	GPSPosDevIntegral_North = 0;
		121	GPSPosDevIntegral_East = 0;
		122	// recalculate latitude projection
		123	cos_target_latitude = (int32_t)c_cos_8192((int16_t)(pTargetPos->Latitude/10000000L));
		124	// remember last target pointer
		125	pLastTargetPos = pTargetPos;
		126	// mark data as processed
		127	pTargetPos->Status = PROCESSED;
		128	}
		129	// calculate position deviation from latitude and longitude differences
		130	GPSPosDev_North = (GPSInfo.latitude - pTargetPos->Latitude); // to calculate real cm we would need *111/100 additionally
		131	GPSPosDev_East = (GPSInfo.longitude - pTargetPos->Longitude); // to calculate real cm we would need *111/100 additionally
		132	// calculate latitude projection
		133	GPSPosDev_East *= cos_target_latitude;
		134	GPSPosDev_East /= 8192;
		135	}
		136	else // no valid target position available
		137	{
		138	// reset error
		139	GPSPosDev_North = 0;
		140	GPSPosDev_East = 0;
		141	// reset error integral
		142	GPSPosDevIntegral_North = 0;
		143	GPSPosDevIntegral_East = 0;
		144	}
		145	}
		146	else // no target position available
		147	{
		148	// reset error
		149	GPSPosDev_North = 0;
		150	GPSPosDev_East = 0;
		151	// reset error integral
		152	GPSPosDevIntegral_North = 0;
		153	GPSPosDevIntegral_East = 0;
		154	}
		155
		156	//Calculate PID-components of the controller (negative sign for compensation)
		157
		158	// P-Part
		159	P_North = -((int32_t)GPS_P_Factor * GPSPosDev_North)/2048;
		160	P_East = -((int32_t)GPS_P_Factor * GPSPosDev_East)/2048;
		161
		162	// I-Part
		163	I_North = -((int32_t)GPS_I_Factor * GPSPosDevIntegral_North)/8192;
		164	I_East = -((int32_t)GPS_I_Factor * GPSPosDevIntegral_East)/8192;
		165
		166	// combine P- & I-Part
		167	PID_North = P_North + I_North;
		168	PID_East = P_East + I_East;
		169
		170	//limit PI-Part to limit the max velocity
		171	//temp1 = ((int32_t)GPS_D_Factor * MAX_VELOCITY)/512; // the PI-Part limit
		172	temp = (int32_t)c_sqrt(PID_NorthPID_North + PID_EastPID_East); // the current PI-Part
		173	if(temp > GPS_P_LIMIT) // P-Part limit is reached
		174	{
		175	// normalize P-part components to the P-Part limit
		176	PID_North = (PID_North * GPS_P_LIMIT)/temp;
		177	PID_East = (PID_East * GPS_P_LIMIT)/temp;
		178	}
		179	else // PI-Part under its limit
		180	{
		181	// update position error integrals
		182	GPSPosDevIntegral_North += GPSPosDev_North/16;
		183	if( GPSPosDevIntegral_North > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_North = GPS_POSDEV_INTEGRAL_LIMIT;
		184	else if (GPSPosDevIntegral_North < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_North = -GPS_POSDEV_INTEGRAL_LIMIT;
		185	GPSPosDevIntegral_East += GPSPosDev_East/16;
		186	if( GPSPosDevIntegral_East > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = GPS_POSDEV_INTEGRAL_LIMIT;
		187	else if (GPSPosDevIntegral_East < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = -GPS_POSDEV_INTEGRAL_LIMIT;
		188	}
		189
		190	// D-Part
		191	D_North = -((int32_t)GPS_D_Factor * GPSInfo.velnorth)/512;
		192	D_East = -((int32_t)GPS_D_Factor * GPSInfo.veleast)/512;
		193
		194
		195	// combine PI- and D-Part
		196	PID_North += D_North;
		197	PID_East += D_East;
		198
911	killagreg	199	// GPS to nick and roll settings
886	killagreg	200
911	killagreg	201	// A positive nick angle moves head downwards (flying forward).
886	killagreg	202	// A positive roll angle tilts left side downwards (flying left).
		203	// If compass heading is 0 the head of the copter is in north direction.
911	killagreg	204	// A positive nick angle will fly to north and a positive roll angle will fly to west.
886	killagreg	205	// In case of a positive north deviation/velocity the
911	killagreg	206	// copter should fly to south (negative nick).
886	killagreg	207	// In case of a positive east position deviation and a positive east velocity the
		208	// copter should fly to west (positive roll).
911	killagreg	209	// The influence of the GPS_Nick and GPS_Roll variable is contrarily to the stick values
886	killagreg	210	// in the fc.c. Therefore a positive north deviation/velocity should result in a positive
911	killagreg	211	// GPS_Nick and a positive east deviation/velocity should result in a negative GPS_Roll.
886	killagreg	212
		213	coscompass = (int32_t)c_cos_8192(CompassHeading);
		214	sincompass = (int32_t)c_sin_8192(CompassHeading);
		215	PID_Roll = (coscompass * PID_East - sincompass * PID_North) / 8192;
911	killagreg	216	PID_Nick = -1((sincompass PID_East + coscompass * PID_North) / 8192);
886	killagreg	217
		218	// limit resulting GPS control vector
911	killagreg	219	temp = (int32_t)c_sqrt(PID_RollPID_Roll + PID_NickPID_Nick);
886	killagreg	220	if (temp > GPS_STICK_LIMIT)
		221	{
		222	// normalize control vector components to the limit
		223	PID_Roll = (PID_Roll * GPS_STICK_LIMIT)/temp;
911	killagreg	224	PID_Nick = (PID_Nick * GPS_STICK_LIMIT)/temp;
886	killagreg	225	}
		226
		227	GPS_Roll = (int16_t)PID_Roll;
911	killagreg	228	GPS_Nick = (int16_t)PID_Nick;
886	killagreg	229
		230	}
		231	else // invalid GPS data or bad compass reading
		232	{
		233	GPS_Neutral(); // do nothing
		234	// reset error integral
		235	GPSPosDevIntegral_North = 0;
		236	GPSPosDevIntegral_East = 0;
		237	}
		238	}
		239
		240
		241
		242
		243	void GPS_Main(uint8_t ctrl)
		244	{
		245	static uint8_t GPS_Task = TSK_IDLE;
		246	static uint8_t GPS_P_Delay = 0;
		247	int16_t satbeep;
		248
		249	// ctrl enables the gps feature
		250	if(ctrl < 70) GPS_Task = TSK_IDLE;
		251	else if (ctrl < 160) GPS_Task = TSK_HOLD;
		252	else GPS_Task = TSK_HOME; // ctrl >= 160
		253
		254
		255	switch(GPSInfo.status)
		256	{
		257	case INVALID: // invalid gps data
		258	GPS_Neutral();
		259	if(GPS_Task != TSK_IDLE)
		260	{
		261	BeepTime = 100; // beep if signal is neccesary
		262	}
		263	break;
		264	case PROCESSED: // if gps data are already processed do nothing
		265	// downcount timeout
		266	if(GPSTimeout) GPSTimeout--;
		267	// if no new data arrived within timeout set current data invalid
		268	// and therefore disable GPS
		269	else
		270	{
		271	GPS_Neutral();
		272	GPSInfo.status = INVALID;
		273	}
		274	break;
		275	case VALID: // new valid data from gps device
		276	// if the gps data quality is good
		277	if (GPSInfo.satfix == SATFIX_3D)
		278	{
		279	switch(GPS_Task) // check what's to do
		280	{
		281	case TSK_IDLE:
		282	// update hold position to current gps position
		283	GPS_SetHoldPosition(); // can get invalid if gps signal is bad
		284	// disable gps control
		285	GPS_Neutral();
		286	break; // eof TSK_IDLE
		287	case TSK_HOLD:
		288	if(HoldPosition.Status != INVALID)
		289	{
		290	if( IsManualControlled() ) // MK controlled by user
		291	{
		292	// update hold point to current gps position
		293	GPS_SetHoldPosition();
		294	// disable gps control
		295	GPS_Neutral();
		296	GPS_P_Delay = 0;
		297	}
		298	else // GPS control active
		299	{
		300	if(GPS_P_Delay<7)
		301	{ // delayed activation of P-Part for 8 cycles (8*0.25s = 2s)
		302	GPS_P_Delay++;
		303	GPS_SetHoldPosition(); // update hold point to current gps position
		304	GPS_PIDController(NULL); // activates only the D-Part
		305	}
		306	else GPS_PIDController(&HoldPosition);// activates the P&D-Part
		307	}
		308	}
		309	else // invalid Hold Position
		310	{ // try to catch a valid hold position from gps data input
		311	GPS_SetHoldPosition();
		312	GPS_Neutral();
		313	}
		314	break; // eof TSK_HOLD
		315	case TSK_HOME:
		316	if(HomePosition.Status != INVALID)
		317	{
		318	// update hold point to current gps position
		319	// to avoid a flight back if home comming is deactivated
		320	GPS_SetHoldPosition();
		321	if( IsManualControlled() ) // MK controlled by user
		322	{
		323	GPS_Neutral();
		324	}
		325	else // GPS control active
		326	{
		327	GPS_PIDController(&HomePosition);
		328	}
		329	}
		330	else // bad home position
		331	{
		332	BeepTime = 50; // signal invalid home position
		333	// try to hold at least the position as a fallback option
		334
		335	if (HoldPosition.Status != INVALID)
		336	{
		337	if( IsManualControlled() ) // MK controlled by user
		338	{
		339	GPS_Neutral();
		340	}
		341	else // GPS control active
		342	{
		343	GPS_PIDController(&HoldPosition);
		344	}
		345	}
		346	else
		347	{ // try to catch a valid hold position
		348	GPS_SetHoldPosition();
		349	GPS_Neutral();
		350	}
		351	}
		352	break; // eof TSK_HOME
		353	default: // unhandled task
		354	GPS_Neutral();
		355	break; // eof default
		356	} // eof switch GPS_Task
		357	} // eof 3D-FIX
		358	else // no 3D-SATFIX
		359	{ // disable gps control
		360	GPS_Neutral();
		361	if(GPS_Task != TSK_IDLE)
		362	{
		363	satbeep = 1600 - (int16_t)GPSInfo.satnum * 200; // is zero at 8 sats
		364	if (satbeep < 0) satbeep = 0;
		365	BeepTime = 50 + (uint16_t)satbeep; // max 1650 * 0.1 ms =
		366	}
		367	}
		368	// set current data as processed to avoid further calculations on the same gps data
		369	GPSInfo.status = PROCESSED;
		370	break;
		371	} // eof GPSInfo.status
		372	}
		373

Subversion Repositories FlightCtrl

(root)/branches/V0.69k Code Redesign killagreg/gps.c @ 1236 - Rev