WebSVN - NaviCtrl - Blame - Rev 146

Rev	Author	Line No.	Line
1	ingob	1	/#######################################################################################/
41	ingob	2	/#######################################################################################/
		3
		4	// IMPORTANT NOTE:
		5
		6	// This is only a dummy implementation for errorfree compiling of the NaviCtrl sources.
		7
		8	// The GPS navigation routines are NOT included !
		9
		10	/#######################################################################################/
		11	/#######################################################################################/
1	ingob	12	/* !!! THIS IS NOT FREE SOFTWARE !!! */
		13	/#######################################################################################/
		14	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		15	// + Copyright (c) 2008 Ingo Busker, Holger Buss
		16	// + Nur f�r den privaten Gebrauch
		17	// + FOR NON COMMERCIAL USE ONLY
		18	// + www.MikroKopter.com
		19	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
41	ingob	20	// + Es gilt f�r das gesamte Projekt (Hardware, Software, Bin�rfiles, Sourcecode und Dokumentation),
		21	// + dass eine Nutzung (auch auszugsweise) nur f�r den privaten (nicht-kommerziellen) Gebrauch zul�ssig ist.
		22	// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
		23	// + bzgl. der Nutzungsbedingungen aufzunehmen.
1	ingob	24	// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Best�ckung und Verkauf von Platinen oder Baus�tzen,
		25	// + Verkauf von Luftbildaufnahmen, usw.
		26	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
41	ingob	27	// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder ver�ffentlicht,
1	ingob	28	// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright m�ssen dann beiliegen
		29	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		30	// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
		31	// + auf anderen Webseiten oder sonstigen Medien ver�ffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
		32	// + eindeutig als Ursprung verlinkt werden
		33	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		34	// + Keine Gew�hr auf Fehlerfreiheit, Vollst�ndigkeit oder Funktion
		35	// + Benutzung auf eigene Gefahr
		36	// + Wir �bernehmen keinerlei Haftung f�r direkte oder indirekte Personen- oder Sachsch�den
		37	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
41	ingob	38	// + Die PORTIERUNG der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
1	ingob	39	// + mit unserer Zustimmung zul�ssig
		40	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		41	// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
		42	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
41	ingob	43	// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
1	ingob	44	// + this list of conditions and the following disclaimer.
		45	// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
		46	// + from this software without specific prior written permission.
41	ingob	47	// + * The use of this project (hardware, software, binary files, sources and documentation) is only permitted
1	ingob	48	// + for non-commercial use (directly or indirectly)
41	ingob	49	// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
1	ingob	50	// + with our written permission
41	ingob	51	// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
		52	// + clearly linked as origin
1	ingob	53	// + * PORTING this software (or part of it) to systems (other than hardware from www.mikrokopter.de) is NOT allowed
		54	//
		55	// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
		56	// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
		57	// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
		58	// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
		59	// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
		60	// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
		61	// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
		62	// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
		63	// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
		64	// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
41	ingob	65	// + POSSIBILITY OF SUCH DAMAGE.
1	ingob	66	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
41	ingob	67	#include <stdio.h>
		68	#include <stdlib.h>
		69	#include <math.h>
		70	#include "91x_lib.h"
1	ingob	71	#include "main.h"
41	ingob	72	#include "uart1.h"
		73	#include "GPS.h"
119	killagreg	74	#include "timer1.h"
41	ingob	75	#include "spi_slave.h"
		76	#include "waypoints.h"
		77	#include "i2c.h"
1	ingob	78
		79
41	ingob	80	#define M_PI_180 (M_PI / 180.0f)
		81	#define GPS_UPDATETIME_MS 200 // 200ms is 5 Hz
		82	typedef enum
		83	{
		84	GPS_FLIGHT_MODE_UNDEF,
		85	GPS_FLIGHT_MODE_FREE,
		86	GPS_FLIGHT_MODE_AID,
		87	GPS_FLIGHT_MODE_WAYPOINT
		88	} GPS_FlightMode_t;
1	ingob	89
41	ingob	90	typedef struct
		91	{
		92	float Gain;
		93	float P;
		94	float I;
		95	float D;
103	killagreg	96	float A;
41	ingob	97	float ACC;
102	killagreg	98	s32 P_Limit;
		99	s32 I_Limit;
		100	s32 D_Limit;
		101	s32 PID_Limit;
73	killagreg	102	u32 BrakingDuration;
41	ingob	103	u8 MinSat;
		104	s8 StickThreshold;
		105	float WindCorrection;
89	killagreg	106	float SpeedCompensation;
102	killagreg	107	s32 OperatingRadius;
41	ingob	108	GPS_FlightMode_t FlightMode;
		109	} __attribute__((packed)) GPS_Parameter_t;
1	ingob	110
41	ingob	111	typedef struct
		112	{
146	killagreg	113	u8 Status; // invalid, newdata, processed
103	killagreg	114	s32 North; // in cm
41	ingob	115	s32 East; // in cm
		116	s32 Bearing; // in deg
102	killagreg	117	s32 Distance; // in cm
41	ingob	118	} __attribute__((packed)) GPS_Deviation_t;
73	killagreg	119	GPS_Deviation_t CurrentTargetDeviation; // Deviation from Target
		120	GPS_Deviation_t CurrentHomeDeviation; // Deviation from Home
		121	GPS_Deviation_t TargetHomeDeviation; // Deviation from Target to Home
41	ingob	122
		123	GPS_Stick_t GPS_Stick;
		124	GPS_Parameter_t GPS_Parameter;
		125
		126	// the gps reference positions
		127	GPS_Pos_t GPS_HoldPosition = {0,0,0, INVALID}; // the hold position
		128	GPS_Pos_t GPS_HomePosition = {0,0,0, INVALID}; // the home position
		129	GPS_Pos_t * GPS_pTargetPosition = NULL; // pointer to the actual target position
92	killagreg	130	u32 GPS_TargetRadius = 0; // catch radius for target area
41	ingob	131	Waypoint_t* GPS_pWaypoint = NULL; // pointer to the actual waypoint
		132
		133	//-------------------------------------------------------------
		134	// Update GPSParamter
		135	void GPS_UpdateParameter(void)
		136	{
133	killagreg	137	#define SWITCH_DELAY 500
		138	static u32 SwitchDelay = 0;
41	ingob	139	static GPS_FlightMode_t FlightMode_Old = GPS_FLIGHT_MODE_UNDEF;
133	killagreg	140
41	ingob	141	// in case of bad receiving conditions
		142	if(FC.RC_Quality < 100)
		143	{ // set fixed parameter
97	killagreg	144	GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_WAYPOINT;
41	ingob	145	GPS_Parameter.Gain = (float) 100;
		146	GPS_Parameter.P = (float) 90;
		147	GPS_Parameter.I = (float) 90;
		148	GPS_Parameter.D = (float) 90;
		149	GPS_Parameter.A = (float) 90;
		150	GPS_Parameter.ACC = (float) 0;
73	killagreg	151	GPS_Parameter.P_Limit = 90;
		152	GPS_Parameter.I_Limit = 90;
		153	GPS_Parameter.D_Limit = 90;
		154	GPS_Parameter.PID_Limit = 200;
		155	GPS_Parameter.BrakingDuration = 0;
89	killagreg	156	GPS_Parameter.SpeedCompensation = (float) 30;
41	ingob	157	GPS_Parameter.MinSat = 6;
		158	GPS_Parameter.StickThreshold = 8;
		159	GPS_Parameter.WindCorrection = 0.0;
		160	GPS_Parameter.OperatingRadius = 0; // forces the aircraft to fly to home positon
73	killagreg	161
41	ingob	162	}
		163	else
		164	{
		165	// update parameter from FC
		166	if(StopNavigation) GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_FREE;
		167	else
		168	{
96	killagreg	169	if(Parameter.NaviGpsModeControl < 50)
		170	{
133	killagreg	171	if(FlightMode_Old == GPS_FLIGHT_MODE_FREE) SetDelay(SWITCH_DELAY);
		172	if(CheckDelay(SwitchDelay))
		173	{
		174	GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_FREE;
		175	NCFlags &= ~(NC_FLAG_PH \| NC_FLAG_CH);
		176	NCFlags \|= NC_FLAG_FREE;
		177	}
96	killagreg	178	}
		179	else if(Parameter.NaviGpsModeControl < 180)
		180	{
133	killagreg	181	if(FlightMode_Old == GPS_FLIGHT_MODE_AID) SetDelay(SWITCH_DELAY);
		182	if(CheckDelay(SwitchDelay))
		183	{
		184	GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_AID;
		185	NCFlags &= ~(NC_FLAG_FREE \| NC_FLAG_CH);
		186	NCFlags \|= NC_FLAG_PH;
		187	}
96	killagreg	188	}
		189	else
		190	{
133	killagreg	191	if(FlightMode_Old == GPS_FLIGHT_MODE_WAYPOINT) SetDelay(SWITCH_DELAY);
		192	if(CheckDelay(SwitchDelay))
		193	{
		194	GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_WAYPOINT;
		195	NCFlags &= ~(NC_FLAG_FREE \| NC_FLAG_PH);
		196	NCFlags \|= NC_FLAG_CH;
		197	}
96	killagreg	198	}
41	ingob	199	}
		200	GPS_Parameter.Gain = (float)Parameter.NaviGpsGain;
		201	GPS_Parameter.P = (float)Parameter.NaviGpsP;
		202	GPS_Parameter.I = (float)Parameter.NaviGpsI;
		203	GPS_Parameter.D = (float)Parameter.NaviGpsD;
		204	GPS_Parameter.A = (float)Parameter.NaviGpsD;
		205	GPS_Parameter.ACC = (float)Parameter.NaviGpsACC;
102	killagreg	206	GPS_Parameter.P_Limit = (s32)Parameter.NaviGpsPLimit;
		207	GPS_Parameter.I_Limit = (s32)Parameter.NaviGpsILimit;
		208	GPS_Parameter.D_Limit = (s32)Parameter.NaviGpsDLimit;
92	killagreg	209	GPS_Parameter.PID_Limit = 2* (u32)Parameter.NaviAngleLimitation;
73	killagreg	210	GPS_Parameter.BrakingDuration = (u32)Parameter.NaviPH_LoginTime;
89	killagreg	211	GPS_Parameter.SpeedCompensation = (float)Parameter.NaviSpeedCompensation;
41	ingob	212	GPS_Parameter.MinSat = (u8)Parameter.NaviGpsMinSat;
		213	GPS_Parameter.StickThreshold = (s8)Parameter.NaviStickThreshold;
		214	GPS_Parameter.WindCorrection = (float)Parameter.NaviWindCorrection;
102	killagreg	215	GPS_Parameter.OperatingRadius = (s32)Parameter.NaviOperatingRadius * 100; // conversion of m to cm
41	ingob	216	}
		217	// FlightMode changed?
103	killagreg	218	if(GPS_Parameter.FlightMode != FlightMode_Old)
		219	{
133	killagreg	220	BeepTime = 200; // beep to indicate that mode has been switched
103	killagreg	221	NCFlags &= ~NC_FLAG_TARGET_REACHED;
128	killagreg	222	// if the mode has changed to free
		223	if(GPS_Parameter.FlightMode == GPS_FLIGHT_MODE_FREE) WPList_Clear(); // clear WPList if mode has changed to Free
103	killagreg	224	}
41	ingob	225	FlightMode_Old = GPS_Parameter.FlightMode;
		226	}
		227
		228	//-------------------------------------------------------------
		229	// This function defines a good GPS signal condition
		230	u8 GPS_IsSignalOK(void)
		231	{
		232	if( (GPSData.Status != INVALID) && (GPSData.SatFix == SATFIX_3D) && (GPSData.NumOfSats >= GPS_Parameter.MinSat)) return(1);
		233	else return(0);
		234	}
		235
1	ingob	236	//------------------------------------------------------------
41	ingob	237	// Checks for manual control action
		238	u8 GPS_IsManuallyControlled(void)
		239	{
102	killagreg	240	if( ( (abs(FC.StickNick) > GPS_Parameter.StickThreshold) \|\| (abs(FC.StickRoll) > GPS_Parameter.StickThreshold)) && (GPS_Parameter.StickThreshold > 0) && (FC.RC_Quality > 150) )
		241	{
		242	NCFlags \|= NC_FLAG_MANUAL_CONTROL;
		243	return(1);
		244	}
		245	else
		246	{
		247	NCFlags &= ~NC_FLAG_MANUAL_CONTROL;
		248	return(0);
103	killagreg	249	}
41	ingob	250	}
		251
		252	//------------------------------------------------------------
		253	// copy GPS position from source position to target position
		254	u8 GPS_CopyPosition(GPS_Pos_t * pGPSPosSrc, GPS_Pos_t* pGPSPosTgt)
		255	{
		256	u8 retval = 0;
		257	if((pGPSPosSrc == NULL) \|\| (pGPSPosTgt == NULL)) return(retval); // bad pointer
		258	// copy only valid positions
		259	if(pGPSPosSrc->Status != INVALID)
		260	{
		261	// if the source GPS position is not invalid
		262	pGPSPosTgt->Longitude = pGPSPosSrc->Longitude;
		263	pGPSPosTgt->Latitude = pGPSPosSrc->Latitude;
		264	pGPSPosTgt->Altitude = pGPSPosSrc->Altitude;
		265	pGPSPosTgt->Status = NEWDATA; // mark data in target position as new
		266	retval = 1;
		267	}
		268	return(retval);
		269	}
		270
		271	//------------------------------------------------------------
		272	// clear position data
		273	u8 GPS_ClearPosition(GPS_Pos_t * pGPSPos)
		274	{
		275	u8 retval = FALSE;
		276	if(pGPSPos == NULL) return(retval); // bad pointer
		277	else
		278	{
		279	pGPSPos->Longitude = 0;
		280	pGPSPos->Latitude = 0;
		281	pGPSPos->Altitude = 0;
		282	pGPSPos->Status = INVALID;
		283	retval = TRUE;
		284	}
		285	return (retval);
		286	}
		287
		288
		289	//------------------------------------------------------------
97	killagreg	290	void GPS_Neutral(void)
41	ingob	291	{
		292	GPS_Stick.Nick = 0;
		293	GPS_Stick.Roll = 0;
		294	GPS_Stick.Yaw = 0;
		295	}
		296
		297	//------------------------------------------------------------
1	ingob	298	void GPS_Init(void)
		299	{
110	killagreg	300	UART1_PutString("\r\n GPS init...");
41	ingob	301	UBX_Init();
		302	GPS_Neutral();
		303	GPS_ClearPosition(&GPS_HoldPosition);
		304	GPS_ClearPosition(&GPS_HomePosition);
		305	GPS_pTargetPosition = NULL;
		306	WPList_Init();
		307	GPS_pWaypoint = WPList_Begin();
		308	GPS_UpdateParameter();
110	killagreg	309	UART1_PutString("ok");
41	ingob	310	}
1	ingob	311
41	ingob	312	//------------------------------------------------------------
		313	// calculate the bearing to target position from its deviation
		314	s32 DirectionToTarget_N_E(float northdev, float eastdev)
		315	{
		316	s32 bearing;
		317	bearing = (s32)(atan2(northdev, eastdev) / M_PI_180);
		318	bearing = (270L - bearing)%360L;
		319	return(bearing);
1	ingob	320	}
41	ingob	321
		322
1	ingob	323	//------------------------------------------------------------
41	ingob	324	// Rescale xy-vector length if length limit is violated
102	killagreg	325	// returns vector len after scaling
		326	s32 GPS_LimitXY(s32 x, s32 y, s32 limit)
41	ingob	327	{
102	killagreg	328	s32 dist;
98	killagreg	329
102	killagreg	330	dist = (s32)hypot(x,y); // the length of the vector
99	killagreg	331	if (dist > limit)
41	ingob	332	// if vector length is larger than the given limit
103	killagreg	333	{ // scale vector compontents so that the length is cut off to limit
102	killagreg	334	x = (s32)(( (double)(x) * (double)limit ) / (double)dist);
		335	y = (s32)(( (double)(y) * (double)limit ) / (double)dist);
99	killagreg	336	dist = limit;
41	ingob	337	}
99	killagreg	338	return(dist);
41	ingob	339	}
1	ingob	340
41	ingob	341	//------------------------------------------------------------
		342	// transform the integer deg into float radians
		343	inline double RadiansFromGPS(s32 deg)
1	ingob	344	{
41	ingob	345	return ((double)deg * 1e-7f * M_PI_180); // 1E-7 because deg is the value in � * 1E7
		346	}
1	ingob	347
41	ingob	348	//------------------------------------------------------------
		349	// transform the integer deg into float deg
		350	inline double DegFromGPS(s32 deg)
		351	{
		352	return ((double)deg * 1e-7f); // 1E-7 because deg is the value in � * 1E7
		353	}
1	ingob	354
41	ingob	355	//------------------------------------------------------------
		356	// calculate the deviation from the current position to the target position
73	killagreg	357	u8 GPS_CalculateDeviation(GPS_Pos_t * pCurrentPos, GPS_Pos_t * pTargetPos, GPS_Deviation_t* pDeviationFromTarget)
41	ingob	358	{
73	killagreg	359	double temp1, temp2;
41	ingob	360	// if given pointer is NULL
		361	if((pCurrentPos == NULL) \|\| (pTargetPos == NULL)) goto baddata;
		362	// if positions are invalid
		363	if((pCurrentPos->Status == INVALID) \|\| (pTargetPos->Status == INVALID)) goto baddata;
1	ingob	364
41	ingob	365	// The deviation from the current to the target position along north and east direction is
		366	// simple the lat/lon difference. To convert that angular deviation into an
		367	// arc length the spherical projection has to be considered.
		368	// The mean earth radius is 6371km. Therfore the arc length per latitude degree
		369	// is always 6371km * 2 * Pi / 360deg = 111.2 km/deg.
		370	// The arc length per longitude degree depends on the correspondig latitude and
		371	// is 111.2km * cos(latitude).
1	ingob	372
41	ingob	373	// calculate the shortest longitude deviation from target
		374	temp1 = DegFromGPS(pCurrentPos->Longitude) - DegFromGPS(pTargetPos->Longitude);
		375	// outside an angular difference of -180 deg ... +180 deg its shorter to go the other way around
		376	// In our application we wont fly more than 20.000 km but along the date line this is important.
		377	if(temp1 > 180.0f) temp1 -= 360.0f;
		378	else if (temp1 < -180.0f) temp1 += 360.0f;
73	killagreg	379	temp1 *= cos((RadiansFromGPS(pTargetPos->Latitude) + RadiansFromGPS(pCurrentPos->Latitude))/2);
41	ingob	380	// calculate latitude deviation from target
		381	// this is allways within -180 deg ... 180 deg
		382	temp2 = DegFromGPS(pCurrentPos->Latitude) - DegFromGPS(pTargetPos->Latitude);
		383	// deviation from target position in cm
		384	// i.e. the distance to walk from the target in northern and eastern direction to reach the current position
		385
73	killagreg	386	pDeviationFromTarget->Status = INVALID;
		387	pDeviationFromTarget->North = (s32)(11119492.7f * temp2);
		388	pDeviationFromTarget->East = (s32)(11119492.7f * temp1);
41	ingob	389	// If the position deviation is small enough to neglect the earth curvature
		390	// (this is for our application always fulfilled) the distance to target
		391	// can be calculated by the pythagoras of north and east deviation.
102	killagreg	392	pDeviationFromTarget->Distance = (s32)(11119492.7f * hypot(temp1, temp2));
73	killagreg	393	if (pDeviationFromTarget->Distance == 0L) pDeviationFromTarget->Bearing = 0L;
		394	else pDeviationFromTarget->Bearing = DirectionToTarget_N_E(temp2, temp1);
		395	pDeviationFromTarget->Status = NEWDATA;
41	ingob	396	return TRUE;
		397
		398	baddata:
73	killagreg	399	pDeviationFromTarget->North = 0L;
		400	pDeviationFromTarget->East = 0L;
		401	pDeviationFromTarget->Distance = 0L;
		402	pDeviationFromTarget->Bearing = 0L;
		403	pDeviationFromTarget->Status = INVALID;
41	ingob	404	return FALSE;
		405	}
		406
		407	//------------------------------------------------------------
		408	void GPS_Navigation(void)
		409	{
		410	static u32 beep_rythm;
		411	static u32 GPSDataTimeout = 0;
129	killagreg	412	float compassheading, sin_h, cos_h;
41	ingob	413
		414	// pointer to current target position
		415	static GPS_Pos_t * pTargetPositionOld = NULL;
		416	static Waypoint_t* GPS_pWaypointOld = NULL;
		417
		418	static GPS_Pos_t RangedTargetPosition = {0,0,0, INVALID}; // the limited target position, this is derived from the target position with repect to the operating radius
102	killagreg	419	static s32 OperatingRadiusOld = -1;
41	ingob	420	static u32 WPTime = 0;
		421
129	killagreg	422
		423
		424	// get current heading from the FC gyro compass heading
		425	if(abs(FC.StickYaw) > 20 \|\| FromFlightCtrl.GyroHeading > 3600) compassheading = (float)I2C_Heading.Heading * M_PI_180;
		426	else compassheading = ((float)FromFlightCtrl.GyroHeading * M_PI_180) / 10.0;
		427
		428	sin_h = sin(compassheading);
		429	cos_h = cos(compassheading);
		430
		431
41	ingob	432	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		433	//+ Check for new data from GPS-receiver
		434	//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		435	switch(GPSData.Status)
		436	{
		437	case INVALID: // no gps data available
		438	// do nothing
73	killagreg	439	GPS_Parameter.PID_Limit = 0; // disables PID output
41	ingob	440	break;
		441
		442	case PROCESSED: // the current data have been allready processed
		443	// if no new data are available within the timeout switch to invalid state.
		444	if(CheckDelay(GPSDataTimeout)) GPSData.Status = INVALID;
		445	// wait for new gps data
		446	break;
		447
		448	case NEWDATA: // handle new gps data
		449
		450	// update GPS Parameter from FC-Data via SPI interface
		451	GPS_UpdateParameter();
		452
		453	// wait maximum of 3 times the normal data update time before data timemout
		454	GPSDataTimeout = SetDelay(3 * GPS_UPDATETIME_MS);
		455	beep_rythm++;
		456
		457	// debug
92	killagreg	458	DebugOut.Analog[21] = (s16)GPSData.Speed_North;
		459	DebugOut.Analog[22] = (s16)GPSData.Speed_East;
		460	DebugOut.Analog[31] = (s16)GPSData.NumOfSats;
41	ingob	461
		462	// If GPS signal condition is sufficient for a reliable position measurement
		463	if(GPS_IsSignalOK())
		464	{
73	killagreg	465	// update home deviation info
		466	GPS_CalculateDeviation(&(GPSData.Position), &GPS_HomePosition, &CurrentHomeDeviation);
74	killagreg	467
41	ingob	468	// if the MK is starting or the home position is invalid then store the home position
		469	if((FC.MKFlags & MKFLAG_START) \|\| (GPS_HomePosition.Status == INVALID))
		470	{ // try to update the home position from the current position
		471	if(GPS_CopyPosition(&(GPSData.Position), &GPS_HomePosition))
		472	{
		473	BeepTime = 700; // beep on success
		474	GPS_CopyPosition(&GPS_HomePosition, &(NaviData.HomePosition));
		475	}
		476	GPS_pWaypoint = WPList_Begin(); // go to start of waypoint list, return NULL of the list is empty
		477	}
		478
		479	/* The selected flight mode influences the target position pointer and therefore the behavior */
		480
		481	// check for current flight mode and set the target pointer GPS_pTargetPosition respectively
103	killagreg	482	switch(GPS_Parameter.FlightMode)
41	ingob	483	{
		484	// the GPS control is deactived
		485	case GPS_FLIGHT_MODE_FREE:
96	killagreg	486
73	killagreg	487	GPS_Parameter.PID_Limit = 0; // disables PID output
41	ingob	488	// update hold position
		489	GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition);
		490	// no target position
		491	GPS_pTargetPosition = NULL;
92	killagreg	492	GPS_TargetRadius = 0;
41	ingob	493	break;
		494
		495	// the GPS supports the position hold, if the pilot takes no action
		496	case GPS_FLIGHT_MODE_AID:
96	killagreg	497
41	ingob	498	if(GPS_IsManuallyControlled())
		499	{
73	killagreg	500	GPS_Parameter.PID_Limit = 0; // disables PID output, as long as the manual conrol is active
129	killagreg	501	GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition);
41	ingob	502	GPS_pTargetPosition = NULL;
92	killagreg	503	GPS_TargetRadius = 0;
41	ingob	504	}
		505	else
		506	{
133	killagreg	507	/*
129	killagreg	508	#define PH_MOVE_THRESHOLD 8
		509	if( ((abs(FC.StickNick) > PH_MOVE_THRESHOLD) \|\| (abs(FC.StickRoll) > PH_MOVE_THRESHOLD) ) && (FC.RC_Quality > 150))
		510	{ // indirect control by moving the hold position with rc sticks
		511	// rc sticks have a range of +/-127 counts
		512	// GPS Coordinates are in 1-E7 deg, i.e. 1 counts is 1E-7�/360�*40E+8cm = 1.11 cm
		513	// normal manual activtion threshold is 8 counts that results in a change of the hold position
		514	// of min 81.11cm 5Hz = 8.88 = 44.4cm/s an max 1271.11cm 5Hz = 705cm/s
		515	GPS_HoldPosition.Latitude += (s32)(cos_hFC.StickNick + sin_hFC.StickRoll);
		516	GPS_HoldPosition.Longitude += (s32)((sin_hFC.StickNick - cos_hFC.StickRoll) / cos(RadiansFromGPS(GPS_HoldPosition.Latitude)) );
133	killagreg	517	}*/
		518	// set target position
41	ingob	519	GPS_pTargetPosition = &GPS_HoldPosition;
92	killagreg	520	GPS_TargetRadius = 100; // 1 meter
41	ingob	521	}
		522	break;
		523
		524	// the GPS control is directed to a target position
		525	// given by a waypoint or by the home position
		526	case GPS_FLIGHT_MODE_WAYPOINT:
74	killagreg	527
73	killagreg	528	if(GPS_IsManuallyControlled()) // the human pilot takes the action
41	ingob	529	{
73	killagreg	530	GPS_Parameter.PID_Limit = 0; // disables PID output, as long as the manual conrol is active
		531	GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition); // update hold position
		532	GPS_pTargetPosition = NULL; // set target position invalid
92	killagreg	533	GPS_TargetRadius = 0;
73	killagreg	534	}
		535	else // no manual control -> gps position hold active
		536	{
		537	// waypoint trigger logic
130	killagreg	538	if(GPS_pWaypoint != NULL) // pointer to waypoint exist
41	ingob	539	{
73	killagreg	540	if(GPS_pWaypoint->Position.Status == INVALID) // should never happen
41	ingob	541	{
73	killagreg	542	GPS_pWaypoint = WPList_Next(); // goto to next WP
		543	BeepTime = 255;
41	ingob	544	}
73	killagreg	545	else // waypoint position is valid
		546	{
		547	// check if the pointer to the waypoint has been changed or the data have been updated
		548	if((GPS_pWaypoint != GPS_pWaypointOld) \|\| (GPS_pWaypoint->Position.Status == NEWDATA))
41	ingob	549	{
73	killagreg	550	GPS_pWaypointOld = GPS_pWaypoint;
93	killagreg	551	}
73	killagreg	552	// if WP has been reached once, wait hold time before trigger to next one
92	killagreg	553	if(NCFlags & NC_FLAG_TARGET_REACHED)
73	killagreg	554	{
		555	/* ToDo: Adjust GPS_pWaypoint->Heading, GPS_pWaypoint->Event handling */
		556	if(CheckDelay(WPTime))
		557	{
		558	GPS_pWaypoint = WPList_Next(); // goto to next waypoint, return NULL if end of list has been reached
128	killagreg	559	if(GPS_pWaypoint == NULL) GPS_pWaypoint = WPList_Begin(); // reset WPList to begin
		560	NCFlags &= ~NC_FLAG_TARGET_REACHED;
73	killagreg	561	}
		562	} // EOF if(WPArrived)
92	killagreg	563	else
		564	{
		565	WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // set hold time stamp
		566	}
41	ingob	567	}
130	killagreg	568	}
		569	else // pointer to waypoint does not exist
		570	{
		571	// try to catch the first waypoint from the list
		572	GPS_pWaypoint = WPList_Begin();
		573	}
		574	// EOF waypoint trigger logic
74	killagreg	575
73	killagreg	576	if(GPS_pWaypoint != NULL) // Waypoint exist
41	ingob	577	{
		578	// update the hold position
		579	GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition);
73	killagreg	580	GPS_pTargetPosition = &(GPS_pWaypoint->Position);
102	killagreg	581	GPS_TargetRadius = (s32)(GPS_pWaypoint->ToleranceRadius) * 100L;
92	killagreg	582
41	ingob	583	}
73	killagreg	584	else // no waypoint info available, i.e. the WPList is empty or the end of the list has been reached
		585	{
		586	// fly back to home postion
		587	if(GPS_HomePosition.Status == INVALID)
		588	{
		589	GPS_pTargetPosition = &GPS_HoldPosition; // fall back to hold mode if home position is not available
92	killagreg	590	GPS_TargetRadius = 100;
73	killagreg	591	BeepTime = 255; // beep to indicate missin home position
		592	}
		593	else // the home position is valid
		594	{
		595	// update the hold position
		596	GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition);
		597	// set target to home position
		598	GPS_pTargetPosition = &GPS_HomePosition;
92	killagreg	599	GPS_TargetRadius = 100;
73	killagreg	600	}
		601	}
		602	} // EOF no manual control
41	ingob	603	break;
		604
		605	case GPS_FLIGHT_MODE_UNDEF:
		606	default:
73	killagreg	607	GPS_Parameter.PID_Limit = 0; // disables PID output
41	ingob	608	// update hold position
		609	GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition);
		610	// no target position
		611	GPS_pTargetPosition = NULL;
92	killagreg	612	GPS_TargetRadius = 0;
41	ingob	613	break;
		614
		615	}// EOF GPS Mode Handling
		616
		617
		618	/* Calculation of range target based on the real target */
		619
		620	// if no target position exist clear the ranged target position
		621	if(GPS_pTargetPosition == NULL) GPS_ClearPosition(&RangedTargetPosition);
		622	else
		623	{ // if the target position has been changed or the value has been updated or the OperatingRadius has changed
		624	if((GPS_pTargetPosition != pTargetPositionOld) \|\| (GPS_pTargetPosition->Status == NEWDATA) \|\| (GPS_Parameter.OperatingRadius != OperatingRadiusOld) )
		625	{
73	killagreg	626	BeepTime = 255; // beep to indicate setting of a new target position
92	killagreg	627	NCFlags &= ~NC_FLAG_TARGET_REACHED; // clear target reached flag
41	ingob	628	// calculate deviation of new target position from home position
73	killagreg	629	if(GPS_CalculateDeviation(GPS_pTargetPosition, &GPS_HomePosition, &TargetHomeDeviation))
41	ingob	630	{
		631	// check distance from home position
73	killagreg	632	if(TargetHomeDeviation.Distance > GPS_Parameter.OperatingRadius)
41	ingob	633	{
		634	//calculate ranged target position to be within the operation radius area
92	killagreg	635	NCFlags \|= NC_FLAG_RANGE_LIMIT;
41	ingob	636
102	killagreg	637	TargetHomeDeviation.North = (s32)(((float)TargetHomeDeviation.North * (float)GPS_Parameter.OperatingRadius) / (float)TargetHomeDeviation.Distance);
		638	TargetHomeDeviation.East = (s32)(((float)TargetHomeDeviation.East * (float)GPS_Parameter.OperatingRadius) / (float)TargetHomeDeviation.Distance);
		639	TargetHomeDeviation.Distance = GPS_Parameter.OperatingRadius;
73	killagreg	640
41	ingob	641	RangedTargetPosition.Status = INVALID;
		642	RangedTargetPosition.Latitude = GPS_HomePosition.Latitude;
73	killagreg	643	RangedTargetPosition.Latitude += (s32)((float)TargetHomeDeviation.North / 1.11194927f);
41	ingob	644	RangedTargetPosition.Longitude = GPS_HomePosition.Longitude;
73	killagreg	645	RangedTargetPosition.Longitude += (s32)((float)TargetHomeDeviation.East / (1.11194927f * cos(RadiansFromGPS(GPS_HomePosition.Latitude))) );
41	ingob	646	RangedTargetPosition.Altitude = GPS_pTargetPosition->Altitude;
103	killagreg	647	RangedTargetPosition.Status = NEWDATA;
41	ingob	648	}
		649	else
		650	{ // the target is located within the operation radius area
		651	// simple copy the loaction to the ranged target position
		652	GPS_CopyPosition(GPS_pTargetPosition, &RangedTargetPosition);
92	killagreg	653	NCFlags &= ~NC_FLAG_RANGE_LIMIT;
41	ingob	654	}
		655	}
		656	else
		657	{ // deviation could not be determined
		658	GPS_ClearPosition(&RangedTargetPosition);
		659	}
		660	GPS_pTargetPosition->Status = PROCESSED; // mark current target as processed!
		661	}
		662	}
		663	OperatingRadiusOld = GPS_Parameter.OperatingRadius;
		664	// remember last target position pointer
		665	pTargetPositionOld = GPS_pTargetPosition;
		666
		667	/* Calculate position deviation from ranged target */
		668
		669	// calculate deviation of current position to ranged target position in cm
73	killagreg	670	if(GPS_CalculateDeviation(&(GPSData.Position), &RangedTargetPosition, &CurrentTargetDeviation))
92	killagreg	671	{ // set target reached flag of we once reached the target point
93	killagreg	672	if(!(NCFlags & NC_FLAG_TARGET_REACHED) && (CurrentTargetDeviation.Distance < GPS_TargetRadius))
92	killagreg	673	{
93	killagreg	674	NCFlags \|= NC_FLAG_TARGET_REACHED; // set target reached flag
92	killagreg	675	}
41	ingob	676	// implement your control code here based
73	killagreg	677	// in the info available in the CurrentTargetDeviation, GPSData and FromFlightCtrl.GyroHeading
41	ingob	678	GPS_Stick.Nick = 0;
		679	GPS_Stick.Roll = 0;
		680	GPS_Stick.Yaw = 0;
		681	}
		682	else // deviation could not be calculated
		683	{ // do nothing on gps sticks!
		684	GPS_Neutral();
103	killagreg	685	NCFlags &= ~NC_FLAG_TARGET_REACHED; // clear target reached
41	ingob	686	}
		687
		688	}// eof if GPSSignal is OK
		689	else // GPSSignal not OK
		690	{
		691	GPS_Neutral();
		692	// beep if signal is not sufficient
		693	if(GPS_Parameter.FlightMode != GPS_FLIGHT_MODE_FREE)
		694	{
		695	if(!(GPSData.Flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) BeepTime = 100;
		696	else if (GPSData.NumOfSats < GPS_Parameter.MinSat && !(beep_rythm % 5)) BeepTime = 10;
		697	}
		698	}
103	killagreg	699	GPSData.Status = PROCESSED; // mark as processed
41	ingob	700	break;
1	ingob	701	}
41	ingob	702
95	killagreg	703	DebugOut.Analog[6] = NCFlags;
73	killagreg	704	DebugOut.Analog[27] = (s16)CurrentTargetDeviation.North;
		705	DebugOut.Analog[28] = (s16)CurrentTargetDeviation.East;
41	ingob	706	DebugOut.Analog[29] = GPS_Stick.Nick;
		707	DebugOut.Analog[30] = GPS_Stick.Roll;
		708
		709	// update navi data, send back to ground station
73	killagreg	710	GPS_CopyPosition(&(GPSData.Position), &(NaviData.CurrentPosition));
41	ingob	711	GPS_CopyPosition(&RangedTargetPosition, &(NaviData.TargetPosition));
73	killagreg	712	GPS_CopyPosition(&GPS_HomePosition, &(NaviData.HomePosition));
41	ingob	713	NaviData.SatsInUse = GPSData.NumOfSats;
89	killagreg	714	NaviData.TargetPositionDeviation.Distance = (u16)CurrentTargetDeviation.Distance/10; // dm
		715	NaviData.TargetPositionDeviation.Bearing = (s16)CurrentTargetDeviation.Bearing;
		716	NaviData.HomePositionDeviation.Distance = (u16)CurrentHomeDeviation.Distance/10; // dm
		717	NaviData.HomePositionDeviation.Bearing = (s16)CurrentHomeDeviation.Bearing;
41	ingob	718	NaviData.UBat = FC.UBat;
		719	NaviData.GroundSpeed = (u16)GPSData.Speed_Ground;
74	killagreg	720	NaviData.Heading = (s16)(GPSData.Heading/100000L);
73	killagreg	721	NaviData.CompassHeading = (s16)FromFlightCtrl.GyroHeading/10; // in deg
		722	NaviData.AngleNick = FromFlightCtrl.AngleNick / 10; // in deg
		723	NaviData.AngleRoll = FromFlightCtrl.AngleRoll / 10; // in deg
125	killagreg	724	NaviData.RC_Quality = FC.RC_Quality;
		725	NaviData.RC_RSSI = FC.RC_RSSI;
		726	NaviData.MKFlags = FC.MKFlags;
92	killagreg	727	NaviData.NCFlags = NCFlags;
74	killagreg	728	NaviData.OperatingRadius = Parameter.NaviOperatingRadius;
89	killagreg	729	NaviData.TopSpeed = (s16)GPSData.Speed_Top; // in cm/s
97	killagreg	730	NaviData.TargetHoldTime = (u8)(GetDelay(WPTime)/1000); // in s
41	ingob	731	//+++++++++++++++++++++++++++++++++++++++++++++++++++
103	killagreg	732	return;
1	ingob	733	}
		734

Subversion Repositories NaviCtrl

(root)/tags/V0.24c/GPS.c @ 334 - Rev 146