WebSVN - NaviCtrl - Blame - Rev 930 - /tags/V2.22a/waypoints.c

Rev	Author	Line No.	Line
41	ingob	1	/#######################################################################################/
		2	/* !!! THIS IS NOT FREE SOFTWARE !!! */
		3	/#######################################################################################/
		4	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		5	// + www.MikroKopter.com
		6	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
360	holgerb	7	// + Software Nutzungsbedingungen (english version: see below)
		8	// + der Fa. HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland - nachfolgend Lizenzgeber genannt -
		9	// + Der Lizenzgeber r�umt dem Kunden ein nicht-ausschlie�liches, zeitlich und r�umlich* unbeschr�nktes Recht ein, die im den
516	holgerb	10	// + Mikrocontroller verwendete Firmware f�r die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool
360	holgerb	11	// + - nachfolgend Software genannt - nur f�r private Zwecke zu nutzen.
		12	// + Der Einsatz dieser Software ist nur auf oder mit Produkten des Lizenzgebers zul�ssig.
41	ingob	13	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
360	holgerb	14	// + Die vom Lizenzgeber gelieferte Software ist urheberrechtlich gesch�tzt. Alle Rechte an der Software sowie an sonstigen im
		15	// + Rahmen der Vertragsanbahnung und Vertragsdurchf�hrung �berlassenen Unterlagen stehen im Verh�ltnis der Vertragspartner ausschlie�lich dem Lizenzgeber zu.
		16	// + Die in der Software enthaltenen Copyright-Vermerke, Markenzeichen, andere Rechtsvorbehalte, Seriennummern sowie
		17	// + sonstige der Programmidentifikation dienenden Merkmale d�rfen vom Kunden nicht ver�ndert oder unkenntlich gemacht werden.
		18	// + Der Kunde trifft angemessene Vorkehrungen f�r den sicheren Einsatz der Software. Er wird die Software gr�ndlich auf deren
		19	// + Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
		20	// + Die Haftung des Lizenzgebers wird - soweit gesetzlich zul�ssig - begrenzt in H�he des typischen und vorhersehbaren
516	holgerb	21	// + Schadens. Die gesetzliche Haftung bei Personensch�den und nach dem Produkthaftungsgesetz bleibt unber�hrt. Dem Lizenzgeber steht jedoch der Einwand
360	holgerb	22	// + des Mitverschuldens offen.
		23	// + Der Kunde trifft angemessene Vorkehrungen f�r den Fall, dass die Software ganz oder teilweise nicht ordnungsgem�� arbeitet.
		24	// + Er wird die Software gr�ndlich auf deren Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
		25	// + Der Kunde wird er seine Daten vor Einsatz der Software nach dem Stand der Technik sichern.
		26	// + Der Kunde ist dar�ber unterrichtet, dass der Lizenzgeber seine Daten im zur Vertragsdurchf�hrung erforderlichen Umfang
		27	// + und auf Grundlage der Datenschutzvorschriften erhebt, speichert, verarbeitet und, sofern notwendig, an Dritte �bermittelt.
		28	// + *) Die r�umliche Nutzung bezieht sich nur auf den Einsatzort, nicht auf die Reichweite der programmierten Software.
		29	// + #### ENDE DER NUTZUNGSBEDINGUNGEN ####'
		30	// + Hinweis: Informationen �ber erweiterte Nutzungsrechte (wie z.B. Nutzung f�r nicht-private Zwecke) sind auf Anfrage per Email an info(@)hisystems.de verf�gbar.
41	ingob	31	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
360	holgerb	32	// + Software LICENSING TERMS
41	ingob	33	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
360	holgerb	34	// + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -
516	holgerb	35	// + The Licensor grants the customer a non-exclusive license to use the microcontroller firmware of the Flight-Ctrl, Navi-Ctrl, BL-Ctrl, and MK3Mag hardware
360	holgerb	36	// + (the Software) exclusively for private purposes. The License is unrestricted with respect to time and territory*.
		37	// + The Software may only be used with the Licensor's products.
		38	// + The Software provided by the Licensor is protected by copyright. With respect to the relationship between the parties to this
		39	// + agreement, all rights pertaining to the Software and other documents provided during the preparation and execution of this
		40	// + agreement shall be the property of the Licensor.
		41	// + The information contained in the Software copyright notices, trademarks, other legal reservations, serial numbers and other
		42	// + features that can be used to identify the program may not be altered or defaced by the customer.
		43	// + The customer shall be responsible for taking reasonable precautions
		44	// + for the safe use of the Software. The customer shall test the Software thoroughly regarding its suitability for the
		45	// + intended purpose before implementing it for actual operation. The Licensor's liability shall be limited to the extent of typical and
		46	// + foreseeable damage to the extent permitted by law, notwithstanding statutory liability for bodily injury and product
		47	// + liability. However, the Licensor shall be entitled to the defense of contributory negligence.
		48	// + The customer will take adequate precautions in the case, that the software is not working properly. The customer will test
		49	// + the software for his purpose before any operational usage. The customer will backup his data before using the software.
		50	// + The customer understands that the Licensor collects, stores and processes, and, where required, forwards, customer data
		51	// + to third parties to the extent necessary for executing the agreement, subject to applicable data protection and privacy regulations.
		52	// + *) The territory aspect only refers to the place where the Software is used, not its programmed range.
		53	// + #### END OF LICENSING TERMS ####
		54	// + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.
41	ingob	55	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
490	killagreg	56	#include <ctype.h>
		57	#include <stdio.h>
516	holgerb	58	#include <stdlib.h>
41	ingob	59	#include <string.h>
		60	#include "91x_lib.h"
		61	#include "waypoints.h"
		62	#include "uart1.h"
490	killagreg	63	#include "fat16.h"
504	holgerb	64	#include "main.h"
513	killagreg	65	#include "spi_slave.h"
41	ingob	66
496	killagreg	67
		68	WPL_Store_t WPL_Store;
		69
280	killagreg	70	Point_t PointList[MAX_LIST_LEN];
		71	u8 WPIndex = 0; // list index of GPS point representig the current WP, can be maximal WPCount
		72	u8 POIIndex = 0; // list index of GPS Point representing the current POI, can be maximal WPCount
		73	u8 WPCount = 0; // number of waypoints
491	killagreg	74	u8 PointCount = 0; // number of points in the list can be maximal equal to MAX_LIST_LEN
		75	u8 POICount = 0; // number of point of interest in the list
601	holgerb	76	u8 FsPointCnt = 0;
802	holgerb	77	u8 FlyzonePointCnt = 0;
517	holgerb	78	s16 HeadingOld = -1;
577	holgerb	79	u32 SD_WaypointTimeout = 5; // Setting on SD-Card
41	ingob	80
296	killagreg	81	u8 WPActive = FALSE;
280	killagreg	82
		83	u8 PointList_Init(void)
41	ingob	84	{
516	holgerb	85	return PointList_Clear();
41	ingob	86	}
		87
280	killagreg	88	u8 PointList_Clear(void)
41	ingob	89	{
		90	u8 i;
227	killagreg	91	WPIndex = 0; // real list position are 1 ,2, 3 ...
		92	POIIndex = 0; // real list position are 1 ,2, 3 ...
802	holgerb	93
280	killagreg	94	WPCount = 0; // no waypoints
601	holgerb	95	FsPointCnt = 0;
516	holgerb	96	POICount = 0;
280	killagreg	97	PointCount = 0; // no contents
802	holgerb	98	FlyzonePointCnt = 0;
		99
296	killagreg	100	WPActive = FALSE;
280	killagreg	101	NaviData.WaypointNumber = WPCount;
		102	NaviData.WaypointIndex = 0;
		103	for(i = 0; i < MAX_LIST_LEN; i++)
41	ingob	104	{
280	killagreg	105	PointList[i].Position.Status = INVALID;
		106	PointList[i].Position.Latitude = 0;
		107	PointList[i].Position.Longitude = 0;
		108	PointList[i].Position.Altitude = 0;
		109	PointList[i].Heading = 361; // invalid value
		110	PointList[i].ToleranceRadius = 0; // in meters, if the MK is within that range around the target, then the next target is triggered
		111	PointList[i].HoldTime = 0; // in seconds, if the was once in the tolerance area around a WP, this time defines the delay before the next WP is triggered
490	killagreg	112	PointList[i].Event_Flag = 0; // future implementation
		113	PointList[i].Index = 0;
286	killagreg	114	PointList[i].Type = POINT_TYPE_INVALID;
490	killagreg	115	PointList[i].WP_EventChannelValue = 0;
286	killagreg	116	PointList[i].AltitudeRate = 0; // no change of setpoint
490	killagreg	117	PointList[i].Speed = 0;
		118	PointList[i].CamAngle = 0;
		119	PointList[i].Name[0] = 0;
41	ingob	120	}
504	holgerb	121	ClearWLP_Name();
516	holgerb	122	return TRUE;
41	ingob	123	}
		124
802	holgerb	125
		126	s32 CheckPositionInFlyzone(s32 Longitude, s32 Latitude, u8 UpdateTimer)
		127	{
		128	#define POINTS 31
		129	static Vec2D_t poly[POINTS+1];
		130	Vec2D_t check_pos;
		131	u32 z = 0;
		132	u32 i;
		133	s32 wn;
		134	if(FlyzonePointCnt < 3) return(100); // too small - return 100
		135
		136	check_pos.x = Longitude >> 8;
		137	check_pos.y = Latitude >> 8;
		138
		139	if(UpdateTimer == 0)
		140	{
		141	for(i=0; i < PointCount; i++)
		142	{
		143	if(PointList[i].Type == POINT_TYPE_FLYZONE)
		144	{
		145	poly[z].x = PointList[i].Position.Longitude >> 8;
		146	poly[z].y = PointList[i].Position.Latitude >> 8;
		147	z++;
		148	if(z >= FlyzonePointCnt) break; // we found them all
		149	if(z >= POINTS) break; // too many
		150	}
		151	}
		152	// close polygon
		153	poly[z].x = poly[0].x;
		154	poly[z].y = poly[0].y;
		155	}
		156	wn = PointPolyCheck(check_pos,poly,FlyzonePointCnt);
828	holgerb	157	if(wn == 1 \|\| wn == -1) return(1);
		158	else return(0); // gekreuzte Polygone k�nnen 2 oder -2 zur�ck geben
802	holgerb	159	}
		160
		161
280	killagreg	162	u8 PointList_GetCount(void)
41	ingob	163	{
280	killagreg	164	return PointCount; // number of points in the list
41	ingob	165	}
		166
295	killagreg	167	Point_t* PointList_GetAt(u8 index)
41	ingob	168	{
295	killagreg	169	if((index > 0) && (index <= PointCount)) return(&(PointList[index-1])); // return pointer to this waypoint
		170	else return(NULL);
		171	}
		172
		173	u8 PointList_SetAt(Point_t* pPoint)
		174	{
		175	// if index is in range
296	killagreg	176	if((pPoint->Index > 0) && (pPoint->Index <= MAX_LIST_LEN))
41	ingob	177	{
802	holgerb	178	UpdateFlyzoneTimer = 2; // makes an update of the flightzone in some milliseconds
295	killagreg	179	// check list entry before update
		180	switch(PointList[pPoint->Index-1].Type)
277	killagreg	181	{
295	killagreg	182	case POINT_TYPE_INVALID: // was invalid
		183	switch(pPoint->Type)
		184	{
		185	default:
		186	case POINT_TYPE_INVALID:
		187	// nothing to do
		188	break;
		189
785	holgerb	190	case POINT_TYPE_LAND:
		191	FsPointCnt++; // break fehlt absichtlich
295	killagreg	192	case POINT_TYPE_WP:
		193	WPCount++;
		194	PointCount++;
		195	break;
516	holgerb	196
295	killagreg	197	case POINT_TYPE_POI:
		198	POICount++;
		199	PointCount++;
		200	break;
601	holgerb	201	case POINT_TYPE_FS:
		202	FsPointCnt++;
		203	PointCount++;
		204	break;
802	holgerb	205	case POINT_TYPE_FLYZONE:
		206	FlyzonePointCnt++;
		207	PointCount++;
		208	break;
295	killagreg	209	}
516	holgerb	210	break;
		211
785	holgerb	212	case POINT_TYPE_LAND: // was a landing point
295	killagreg	213	switch(pPoint->Type)
		214	{
		215	case POINT_TYPE_INVALID:
		216	WPCount--;
		217	PointCount--;
		218	break;
		219	default:
785	holgerb	220	case POINT_TYPE_LAND:
		221	break;
295	killagreg	222	case POINT_TYPE_WP:
802	holgerb	223	FsPointCnt--; // the landing point is also a Failsafe-Pt
785	holgerb	224	break;
		225	case POINT_TYPE_FS:
		226	FsPointCnt++;
		227	WPCount--;
		228	break;
		229	case POINT_TYPE_POI:
		230	POICount++;
		231	WPCount--;
		232	break;
802	holgerb	233	case POINT_TYPE_FLYZONE:
		234	FlyzonePointCnt++;
		235	WPCount--;
		236	break;
785	holgerb	237	}
		238	break;
		239	case POINT_TYPE_WP:
		240	switch(pPoint->Type)
		241	{
		242	case POINT_TYPE_INVALID:
		243	WPCount--;
		244	PointCount--;
		245	break;
		246
		247	default:
		248	case POINT_TYPE_LAND:
802	holgerb	249	FsPointCnt++; // the landing point is also a Failsafe-Pt
785	holgerb	250	break;
		251	case POINT_TYPE_WP:
295	killagreg	252	//nothing to do
		253	break;
601	holgerb	254	case POINT_TYPE_FS:
		255	FsPointCnt++;
		256	WPCount--;
		257	break;
295	killagreg	258	case POINT_TYPE_POI:
		259	POICount++;
		260	WPCount--;
		261	break;
802	holgerb	262	case POINT_TYPE_FLYZONE:
		263	FlyzonePointCnt++;
		264	WPCount--;
		265	break;
295	killagreg	266	}
		267	break;
802	holgerb	268	case POINT_TYPE_FLYZONE:
		269	switch(pPoint->Type)
		270	{
		271	case POINT_TYPE_INVALID:
		272	FlyzonePointCnt--;
		273	PointCount--;
		274	break;
		275	default:
		276	case POINT_TYPE_LAND:
		277	FsPointCnt++; // the landing point is also a Failsafe-Pt
		278	break;
		279	case POINT_TYPE_WP:
		280	WPCount++;
		281	FlyzonePointCnt--;
		282	break;
		283	case POINT_TYPE_FLYZONE:
		284	//nothing to do
		285	break;
		286	case POINT_TYPE_FS:
		287	FsPointCnt++;
		288	FlyzonePointCnt--;
		289	break;
		290	case POINT_TYPE_POI:
		291	POICount++;
		292	FlyzonePointCnt--;
		293	break;
		294	}
		295	break;
516	holgerb	296
295	killagreg	297	case POINT_TYPE_POI: // was a poi
		298	switch(pPoint->Type)
		299	{
		300	case POINT_TYPE_INVALID:
		301	POICount--;
		302	PointCount--;
		303	break;
802	holgerb	304	case POINT_TYPE_FS:
		305	FsPointCnt++;
		306	POICount--;
		307	break;
295	killagreg	308
785	holgerb	309	case POINT_TYPE_LAND:
		310	FsPointCnt++; // break fehlt absichtlich
295	killagreg	311	case POINT_TYPE_WP:
		312	WPCount++;
		313	POICount--;
		314	break;
802	holgerb	315	case POINT_TYPE_FLYZONE:
		316	FlyzonePointCnt++;
601	holgerb	317	POICount--;
		318	break;
295	killagreg	319	case POINT_TYPE_POI:
		320	default:
		321	// nothing to do
		322	break;
		323	}
601	holgerb	324	case POINT_TYPE_FS: // was a Failsafe
		325	switch(pPoint->Type)
		326	{
		327	case POINT_TYPE_INVALID:
		328	FsPointCnt--;
		329	PointCount--;
		330	break;
785	holgerb	331	case POINT_TYPE_LAND:
802	holgerb	332	FsPointCnt++; // (wird gleich wieder abgezogen)
601	holgerb	333	case POINT_TYPE_WP:
		334	WPCount++;
		335	FsPointCnt--;
		336	break;
		337	case POINT_TYPE_POI:
		338	POICount++;
		339	FsPointCnt--;
		340	break;
802	holgerb	341	case POINT_TYPE_FLYZONE:
		342	FlyzonePointCnt++;
		343	FsPointCnt--;
		344	break;
601	holgerb	345	case POINT_TYPE_FS:
		346	break;
		347	default:
		348	// nothing to do
		349	break;
		350	}
516	holgerb	351	break;
277	killagreg	352	}
516	holgerb	353	memcpy(&PointList[pPoint->Index-1], pPoint, sizeof(Point_t)); // copy data to list entry
295	killagreg	354	NaviData.WaypointNumber = WPCount;
		355	return pPoint->Index;
41	ingob	356	}
296	killagreg	357	else return(0);
41	ingob	358	}
		359
227	killagreg	360	// returns the pointer to the first waypoint within the list
785	holgerb	361	Point_t* PointList_WPBegin(u32 start)
41	ingob	362	{
786	holgerb	363	u8 i, wp_nr = 0;
227	killagreg	364	WPIndex = 0; // set list position invalid
295	killagreg	365
283	holgerb	366	if(WPActive == FALSE) return(NULL);
295	killagreg	367
277	killagreg	368	POIIndex = 0; // set invalid POI
785	holgerb	369	if(PointCount > start)
152	killagreg	370	{
225	killagreg	371	// search for first wp in list
786	holgerb	372	for(i = 0; i <MAX_LIST_LEN; i++)
225	killagreg	373	{
785	holgerb	374	if(((PointList[i].Type == POINT_TYPE_WP)\|\|(PointList[i].Type == POINT_TYPE_LAND)) && (PointList[i].Position.Status != INVALID)) // jump over POIs and FS-Positions
225	killagreg	375	{
227	killagreg	376	WPIndex = i + 1;
786	holgerb	377	wp_nr++; // count the number of scipped WPs to make sure that the Index in the NaviData is correct
		378	if(i >= start) break;
225	killagreg	379	}
		380	}
277	killagreg	381	if(WPIndex) // found a WP in the list
		382	{
786	holgerb	383	NaviData.WaypointIndex = wp_nr;
549	holgerb	384	NewWaypointsReceived = 1; // activates the Waypoint list as soon as CH is started
277	killagreg	385	// update index to POI
280	killagreg	386	if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
516	holgerb	387	else POIIndex = 0;
277	killagreg	388	}
		389	else // some points in the list but no WP found
		390	{
280	killagreg	391	NaviData.WaypointIndex = 0;
277	killagreg	392	//Check for an existing POI
295	killagreg	393	for(i = 0; i < MAX_LIST_LEN; i++)
277	killagreg	394	{
280	killagreg	395	if((PointList[i].Type == POINT_TYPE_POI) && (PointList[i].Position.Status != INVALID))
277	killagreg	396	{
		397	POIIndex = i + 1;
		398	break;
		399	}
		400	}
		401	}
152	killagreg	402	}
277	killagreg	403	else // no point in the list
152	killagreg	404	{
516	holgerb	405	POIIndex = 0;
		406	NaviData.WaypointIndex = 0;
227	killagreg	407	}
295	killagreg	408
		409	if(WPIndex) return(&(PointList[WPIndex-1]));
		410	else return(NULL);
41	ingob	411	}
		412
225	killagreg	413	// returns the last waypoint
280	killagreg	414	Point_t* PointList_WPEnd(void)
151	killagreg	415	{
516	holgerb	416
277	killagreg	417	u8 i;
227	killagreg	418	WPIndex = 0; // set list position invalid
277	killagreg	419	POIIndex = 0; // set invalid
283	holgerb	420
280	killagreg	421	if(WPActive == FALSE) return(NULL);
277	killagreg	422
280	killagreg	423	if(PointCount > 0)
151	killagreg	424	{
227	killagreg	425	// search backward!
295	killagreg	426	for(i = 1; i <= MAX_LIST_LEN; i++)
225	killagreg	427	{
785	holgerb	428	if(((PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_WP) \|\| (PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_LAND)) && (PointList[MAX_LIST_LEN - i].Position.Status != INVALID))
516	holgerb	429	{
295	killagreg	430	WPIndex = MAX_LIST_LEN - i + 1;
225	killagreg	431	break;
		432	}
		433	}
277	killagreg	434	if(WPIndex) // found a WP within the list
		435	{
280	killagreg	436	NaviData.WaypointIndex = WPCount;
		437	if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
516	holgerb	438	else POIIndex = 0;
277	killagreg	439	}
278	killagreg	440	else // list contains some points but no WP in the list
277	killagreg	441	{
		442	// search backward for a POI!
295	killagreg	443	for(i = 1; i <= MAX_LIST_LEN; i++)
277	killagreg	444	{
295	killagreg	445	if((PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_POI) && (PointList[MAX_LIST_LEN - i].Position.Status != INVALID))
516	holgerb	446	{
295	killagreg	447	POIIndex = MAX_LIST_LEN - i + 1;
277	killagreg	448	break;
		449	}
		450	}
516	holgerb	451	NaviData.WaypointIndex = 0;
277	killagreg	452	}
151	killagreg	453	}
277	killagreg	454	else // no point in the list
227	killagreg	455	{
280	killagreg	456	POIIndex = 0;
		457	NaviData.WaypointIndex = 0;
227	killagreg	458	}
295	killagreg	459	if(WPIndex) return(&(PointList[WPIndex-1]));
		460	else return(NULL);
151	killagreg	461	}
		462
41	ingob	463	// returns a pointer to the next waypoint or NULL if the end of the list has been reached
280	killagreg	464	Point_t* PointList_WPNext(void)
41	ingob	465	{
227	killagreg	466	u8 wp_found = 0;
280	killagreg	467	if(WPActive == FALSE) return(NULL);
516	holgerb	468
295	killagreg	469	if(WPIndex < MAX_LIST_LEN) // if there is a next entry in the list
41	ingob	470	{
227	killagreg	471	u8 i;
295	killagreg	472	for(i = WPIndex; i < MAX_LIST_LEN; i++) // start search for next at next list entry
225	killagreg	473	{
785	holgerb	474	if(((PointList[i].Type == POINT_TYPE_WP) \|\| (PointList[i].Type == POINT_TYPE_LAND)) && (PointList[i].Position.Status != INVALID)) // jump over POIs and FS-Positions
225	killagreg	475	{
227	killagreg	476	wp_found = i+1;
225	killagreg	477	break;
		478	}
601	holgerb	479	/*
		480	else
		481	if((PointList[i].Type == POINT_TYPE_FS) && (PointList[i].Position.Status != INVALID)) // jump over POIs
		482	{
		483	GPSPos_Copy(&(PointList[i].Position), &GPS_FailsafePosition);
		484	}
		485	*/
225	killagreg	486	}
41	ingob	487	}
227	killagreg	488	if(wp_found)
		489	{
		490	WPIndex = wp_found; // update list position
280	killagreg	491	NaviData.WaypointIndex++;
		492	if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
227	killagreg	493	else POIIndex = 0;
280	killagreg	494	return(&(PointList[WPIndex-1])); // return pointer to this waypoint
227	killagreg	495	}
516	holgerb	496	else
280	killagreg	497	{ // no next wp found
516	holgerb	498	NaviData.WaypointIndex = 0;
277	killagreg	499	POIIndex = 0;
		500	return(NULL);
		501	}
516	holgerb	502	}
280	killagreg	503
		504	void PointList_WPActive(u8 set)
		505	{
295	killagreg	506	if(set)
516	holgerb	507	{
295	killagreg	508	WPActive = TRUE;
785	holgerb	509	PointList_WPBegin(0); // updates POI index
295	killagreg	510	}
		511	else
		512	{
		513	WPActive = FALSE;
		514	POIIndex = 0; // disable POI also
		515	}
280	killagreg	516	}
516	holgerb	517
280	killagreg	518	Point_t* PointList_GetPOI(void)
224	killagreg	519	{
516	holgerb	520	return PointList_GetAt(POIIndex);
224	killagreg	521	}
		522
513	killagreg	523
490	killagreg	524	#define LINE_MAX 70
930	holgerb	525	#define WP_FILE_VERSION_COMPATIBLE_OLD 3
		526	#define WP_FILE_VERSION_COMPATIBLE 4
513	killagreg	527
		528	u8 PointList_Save(u8 * filename, u8* listname, u8 overwride)
490	killagreg	529	{
		530	File_t *fp;
495	killagreg	531	s8 wpline[LINE_MAX];
		532	u8 retval = WPL_ERROR;
513	killagreg	533
767	holgerb	534	UART1_PutString("\r\n Save WPL...");
490	killagreg	535
		536	if(Fat16_IsValid())
		537	{ // check if wpl file is existing
513	killagreg	538	if(fexist_(filename))
495	killagreg	539	{ //file is existent
516	holgerb	540	if(!(overwride))
495	killagreg	541	{
		542	UART1_PutString("Error: file exist!\r\n");
		543	return(WPL_FILEEXIST);
516	holgerb	544	}
495	killagreg	545	}
513	killagreg	546	fp = fopen_(filename, 'w'); // try to open the file
490	killagreg	547	if(fp == NULL)
		548	{
		549	UART1_PutString("ERROR: Creating waypoint file!\r\n");
		550	return(retval);
		551	}
		552	// Create general section and key entries
		553	fputs_("[General]\r\n", fp);
516	holgerb	554	sprintf(wpline, "Name=%s\r\n", listname);
494	killagreg	555	fputs_(wpline, fp);
516	holgerb	556	sprintf(wpline, "FileVersion=%d\r\n", WP_FILE_VERSION_COMPATIBLE);
490	killagreg	557	fputs_(wpline, fp);
		558	sprintf(wpline, "NumberOfWaypoints=%d\r\n", PointCount);
		559	fputs_(wpline, fp);
		560	// dump all points if existent
		561	if(PointCount)
		562	{
		563	u8 i, u8_1;
		564	s32 i32_1, i32_2;
504	holgerb	565	NewWPL_Name = 1;
490	killagreg	566	for (i = 0; i < PointCount; i++)
		567	{
		568	sprintf(wpline, "[Point%d]\r\n",PointList[i].Index);
		569	fputs_(wpline, fp);
		570	// write latitude in deg
		571	if(PointList[i].Position.Latitude < 0) u8_1 = '-';
		572	else u8_1 = '+';
		573	i32_1 = abs(PointList[i].Position.Latitude)/10000000L;
		574	i32_2 = abs(PointList[i].Position.Latitude)%10000000L;
		575	sprintf(wpline, "Latitude=%c%ld.%07ld\r\n", u8_1, i32_1, i32_2);
		576	fputs_(wpline, fp);
		577	// write longitude in deg
		578	if(PointList[i].Position.Longitude < 0) u8_1 = '-';
		579	else u8_1 = '+';
		580	i32_1 = abs(PointList[i].Position.Longitude)/10000000L;
		581	i32_2 = abs(PointList[i].Position.Longitude)%10000000L;
		582	sprintf(wpline, "Longitude=%c%ld.%07ld\r\n", u8_1, i32_1, i32_2);
		583	fputs_(wpline, fp);
		584	// write tolerace radius in m
		585	sprintf(wpline, "Radius=%d\r\n", PointList[i].ToleranceRadius);
		586	fputs_(wpline, fp);
		587	// write altitude in m
		588	if(PointList[i].Position.Altitude < 0) u8_1 = '-';
		589	else u8_1 = '+';
		590	if(PointList[i].Type == POINT_TYPE_POI)
		591	{
		592	i32_1 = abs(PointList[i].Position.Altitude)/100L; // cm --> m
		593	i32_2 = abs(PointList[i].Position.Altitude)%100L;
		594	}
		595	else
		596	{
		597	i32_1 = abs(PointList[i].Position.Altitude)/10L; // dm --> m
		598	i32_2 = abs(PointList[i].Position.Altitude)%10L;
		599	}
		600	sprintf(wpline, "Altitude=%c%ld.%01ld\r\n", u8_1, i32_1, i32_2);
		601	fputs_(wpline, fp);
		602	// write climb rate in 0.1 m/s
		603	sprintf(wpline, "ClimbRate=%d\r\n", PointList[i].AltitudeRate);
		604	fputs_(wpline, fp);
		605	// write hold time in s
		606	sprintf(wpline, "DelayTime=%d\r\n", PointList[i].HoldTime);
		607	fputs_(wpline, fp);
		608	// write event channel value
		609	sprintf(wpline, "WP_Event_Channel_Value=%d\r\n", PointList[i].WP_EventChannelValue);
		610	fputs_(wpline, fp);
		611	// write heading in deg (0= nothing, neg. values index to poi)
		612	sprintf(wpline, "Heading=%d\r\n", PointList[i].Heading);
		613	fputs_(wpline, fp);
		614	// write speed in 0.1 m/s
		615	sprintf(wpline, "Speed=%d\r\n", PointList[i].Speed);
		616	fputs_(wpline, fp);
930	holgerb	617	// write cam angle in degree (121 -> POI-Automatic)
490	killagreg	618	sprintf(wpline, "CAM-Nick=%d\r\n", PointList[i].CamAngle);
		619	fputs_(wpline, fp);
		620	// write point type
		621	sprintf(wpline, "Type=%d\r\n", PointList[i].Type + 1);
516	holgerb	622	fputs_(wpline, fp);
		623	// write prefix
490	killagreg	624	sprintf(wpline, "Prefix=%s\r\n", PointList[i].Name);
		625	fputs_(wpline, fp);
642	holgerb	626	sprintf(wpline, "AutoTrigger=%d\r\n", PointList[i].AutoPhotoDistance);
		627	fputs_(wpline, fp);
490	killagreg	628	} // EOF loop over all points
		629	} // EOF if(PointCount)
		630	if(EOF == fclose_(fp))
		631	{
		632	UART1_PutString("failed!\r\n");
		633	}
		634	else
		635	{
		636	UART1_PutString("ok\r\n");
495	killagreg	637	retval = WPL_OK;
516	holgerb	638	}
490	killagreg	639	} // EOF if(Fat16_IsValid())
516	holgerb	640	else
497	killagreg	641	{
		642	UART1_PutString("no file system found!\r\n");
		643	retval = WPL_NO_SDCARD_FOUND;
		644	}
490	killagreg	645	return(retval);
		646	}
		647
519	holgerb	648	u8 PointList_Load(u8 * filename, u8* listname, u8 listnamelen, u8 use_preset_speed)
490	killagreg	649	{
		650	File_t *fp;
496	killagreg	651	s8 wpline[LINE_MAX];
		652	u8 retval = WPL_ERROR;
516	holgerb	653
490	killagreg	654	s8 name, value;
		655	u8 i;
		656
		657	u8 IsGeneralSection = 0;
		658	u8 IsPointSection = 0;
		659	u8 WPNumber = 0;
930	holgerb	660	u8 fileversion = 0;
498	killagreg	661
		662	// clear point list first
		663	PointList_Clear();
517	holgerb	664	HeadingOld = -1; // updates the direction if the new direction is the same like last time
767	holgerb	665	UART1_PutString("\r\n Read ");
516	holgerb	666	UART1_PutString(filename);
490	killagreg	667	UART1_PutString("...");
		668
		669	if(Fat16_IsValid())
		670	{ // check if wpl file is existing
513	killagreg	671	fp = fopen_(filename, 'r'); // try to open the file
490	killagreg	672	if(fp == NULL)
		673	{
		674	UART1_PutString("ERROR: Reading waypoint file!\r\n");
496	killagreg	675	return(retval);
490	killagreg	676	}
		677	// read all lines from file
		678	while(fgets_(wpline, LINE_MAX, fp) != 0)
		679	{
		680	if ( // ignorelines starting with \r,\n,' ',';','#'
		681	(wpline[0] != '\n') &&
		682	(wpline[0] != '\r') &&
		683	(wpline[0] != ' ' ) &&
		684	(wpline[0] != ';' ) &&
		685	(wpline[0] != '#' )
		686	)
		687	{
		688	// check for section line found
		689	if(wpline[0] == '[')
		690	{
		691	// next section found
		692	IsGeneralSection = 0;
		693	IsPointSection = 0;
		694
		695	name = strtok(&wpline[1], "]");
		696	if(name != NULL) // if section name
		697	{
		698	// check section type
		699	for(i=0; name[i]; i++) name[i] = toupper(name[i]); // convert to upper case
		700
		701	if(strncmp(name, "POINT", 5) == 0)
		702	{
		703	IsPointSection = (u8)atoi(&name[5]);
		704	PointCount++;
		705	}
		706	else if(strcmp(name, "GENERAL") == 0)
		707	{
		708	IsGeneralSection = 1;
		709	}
		710	else
		711	{
		712	UART1_PutString("Unknown section: ");
		713	UART1_PutString(name);
		714	UART1_PutString("\r\n");
		715	}
		716	}
		717	} // EOF section line
		718	else
		719	{ // look for key entrys of each sections
		720	name = strtok(wpline, "="); // get name
		721	value = strtok(NULL, "="); // get value
		722	if ((name != NULL) && (value != NULL))
		723	{
		724	for(i=0; name[i]; i++) name[i] = toupper(name[i]); // convert to upper case
		725	if(IsPointSection && (IsPointSection <= WPNumber))
		726	{
		727	if(strcmp(name, "LATITUDE") == 0)
		728	{
		729	PointList[IsPointSection-1].Position.Latitude = (s32)(atof(value) * 1E7);
		730	}
		731	else if(strcmp(name, "LONGITUDE") == 0)
		732	{
		733	PointList[IsPointSection-1].Position.Longitude = (s32)(atof(value) * 1E7);
		734	}
		735	else if(strcmp(name, "RADIUS") == 0)
		736	{
		737	PointList[IsPointSection-1].ToleranceRadius = (u8)atoi(value);
		738	}
		739	else if(strcmp(name, "ALTITUDE") == 0)
		740	{
		741	PointList[IsPointSection-1].Position.Altitude = (s32)(atof(value) * 100.0); // in cm
		742	PointList[IsPointSection-1].Position.Status = NEWDATA;
		743	}
		744	else if(strcmp(name, "CLIMBRATE") == 0)
		745	{
		746	PointList[IsPointSection-1].AltitudeRate = (u8)atoi(value);
		747	}
		748	else if(strcmp(name, "DELAYTIME") == 0)
		749	{
		750	PointList[IsPointSection-1].HoldTime = (u8)atoi(value);
		751	}
		752	else if(strcmp(name, "WP_EVENT_CHANNEL_VALUE") == 0)
		753	{
		754	PointList[IsPointSection-1].WP_EventChannelValue = (u8)atoi(value);
		755	}
		756	else if(strcmp(name, "HEADING") == 0)
		757	{
		758	PointList[IsPointSection-1].Heading = (s16)atoi(value);
		759	}
		760	else if(strcmp(name, "SPEED") == 0)
		761	{
519	holgerb	762	if(use_preset_speed) PointList[IsPointSection-1].Speed = use_preset_speed;
		763	else PointList[IsPointSection-1].Speed = (u8)atoi(value);
490	killagreg	764	}
		765	else if(strcmp(name, "CAM-NICK") == 0)
		766	{
930	holgerb	767	s8 tmp = (s8)atoi(value);
		768	if(fileversion == WP_FILE_VERSION_COMPATIBLE_OLD) if(tmp == -1) tmp = 121; // 121 is auto since version 4 -> in older versions auto was -1
		769	PointList[IsPointSection-1].CamAngle = tmp;
490	killagreg	770	}
		771	else if(strcmp(name, "TYPE") == 0)
		772	{
		773	PointList[IsPointSection-1].Type = (u8)atoi(value);
		774	if(PointList[IsPointSection-1].Type > 0) PointList[IsPointSection-1].Type--; // index shift
		775	else PointList[IsPointSection-1].Type = POINT_TYPE_INVALID;
516	holgerb	776
490	killagreg	777	switch(PointList[IsPointSection-1].Type)
		778	{
785	holgerb	779	case POINT_TYPE_LAND:
		780	FsPointCnt++; // break fehlt absichtlich
490	killagreg	781	case POINT_TYPE_WP:
		782	// this works only if altitude key is set before point type key in WPL file !!
516	holgerb	783	PointList[IsPointSection-1].Position.Altitude /= 10; // dm only for WPs
490	killagreg	784	WPCount++;
		785	break;
601	holgerb	786	case POINT_TYPE_FS:
		787	FsPointCnt++;
		788	break;
490	killagreg	789	case POINT_TYPE_POI:
		790	POICount++;
		791	break;
802	holgerb	792	case POINT_TYPE_FLYZONE:
		793	FlyzonePointCnt++;
		794	break;
490	killagreg	795	}
		796	}
		797	else if(strcmp(name, "PREFIX") == 0)
		798	{
		799	strncpy(PointList[IsPointSection-1].Name, value, 4);
		800	PointList[IsPointSection-1].Name[3] = 0; // Terminate string
		801	}
642	holgerb	802	else if(strcmp(name, "AUTOTRIGGER") == 0)
		803	{
		804	PointList[IsPointSection-1].AutoPhotoDistance = (u8)atoi(value);
		805	}
490	killagreg	806	else
		807	{
		808	UART1_PutString("Unknown key: ");
		809	UART1_PutString(name);
		810	UART1_PutString("\r\n");
516	holgerb	811	}
490	killagreg	812	} // EOF point section
		813	else if(IsGeneralSection)
		814	{
		815	if(strcmp(name, "NUMBEROFWAYPOINTS") == 0)
516	holgerb	816	{
490	killagreg	817	WPNumber = (u8)atoi(value);
		818	if(!WPNumber) // no waypoints in file
		819	{
516	holgerb	820	return(WPL_NO_WAYPOINTS); // we are done here
490	killagreg	821	}
		822	else if(WPNumber > MAX_LIST_LEN) // number o points larger than ram list
		823	{
514	holgerb	824	UART1_PutString("To many points!");
497	killagreg	825	return(WPL_ERROR);
516	holgerb	826	}
490	killagreg	827	}
		828	else if (strcmp(name, "FILEVERSION") == 0)
		829	{
930	holgerb	830	fileversion = (u8)atoi(value);
		831	if((fileversion != WP_FILE_VERSION_COMPATIBLE) && (fileversion != WP_FILE_VERSION_COMPATIBLE_OLD))
490	killagreg	832	{
496	killagreg	833	PointList_Clear();
490	killagreg	834	UART1_PutString("Bad file version!\r\n");
516	holgerb	835	return(WPL_ERROR);
490	killagreg	836	}
		837	}
495	killagreg	838	else if (strcmp(name, "NAME") == 0)
		839	{
513	killagreg	840	if(listname)
498	killagreg	841	{
513	killagreg	842	u8 len = strlen(value);
		843	if(len)
		844	{
		845	if(value[len-1] == '\r')
		846	{
		847	value[len-1] = 0;
540	holgerb	848	// len--;
513	killagreg	849	}
		850	}
		851	if(len > listnamelen) len = listnamelen;
		852	if(len)
		853	{
		854	value[len-1] = 0; // terminate string
		855	if(listname) memcpy(listname, value, len);
		856	}
		857	NewWPL_Name = 1;
498	killagreg	858	}
495	killagreg	859	}
490	killagreg	860	else
		861	{
		862	UART1_PutString("Unknown key: ");
		863	UART1_PutString(name);
		864	UART1_PutString("\r\n");
		865	}
		866	} // EOF general section
		867	} // EOF valid key entry
		868	} // EOF key entry line
		869	} // valid line
		870	} // EOF loop over all lines
		871	fclose_(fp);
		872	NaviData.WaypointNumber = WPCount;
516	holgerb	873	retval = WPL_OK;
		874	UART1_PutString("ok\r\n");
490	killagreg	875	} // EOF if(Fat16_IsValid())
516	holgerb	876	else
497	killagreg	877	{
		878	UART1_PutString("no file system found!\r\n");
		879	retval = WPL_NO_SDCARD_FOUND;
516	holgerb	880	}
490	killagreg	881	return(retval);
491	killagreg	882	}
490	killagreg	883
513	killagreg	884	// load actual point list from SD card
		885	u8 PointList_ReadFromFile(WPL_Store_t * pWPL_Store)
		886	{
516	holgerb	887	u8 filename[30];
513	killagreg	888
		889	pWPL_Store->Name[0] = 0; // clear current list name
		890
		891	// user absolute path, i.e. leading /
		892	if(pWPL_Store->Index == 0) // index 0 looks for a default WPL file in the root
		893	{
516	holgerb	894	sprintf(filename, "/default.wpl");
513	killagreg	895	}
		896	else
		897	{
		898	sprintf(filename, "/WPL/list_%03d.wpl", pWPL_Store->Index);
		899	}
519	holgerb	900	return PointList_Load(filename, pWPL_Store->Name, sizeof(pWPL_Store->Name),0);
513	killagreg	901	}
		902
		903	// save actual point list to SD card
		904	u8 PointList_WriteToFile(WPL_Store_t * pWPL_Store)
		905	{
		906	u8 filename[30];
516	holgerb	907
		908
513	killagreg	909	if(PointCount == 0) return(WPL_NO_WAYPOINTS);
516	holgerb	910	// user absolute path, i.e. leading /
513	killagreg	911	if(pWPL_Store->Index == 0)
		912	{
		913	sprintf(filename, "/default.wpl");
		914	}
		915	else
		916	{
		917	sprintf(filename, "/WPL/list_%03d.wpl", pWPL_Store->Index);
		918	}
		919	return PointList_Save(filename, pWPL_Store->Name, pWPL_Store->OverwriteFile);
		920	}
		921
		922
516	holgerb	923	// save actual gps positiin and heading to file
513	killagreg	924	u8 PointList_SaveSinglePoint(WPL_Store_t * pWPL_Store)
		925	{
		926	u8 retval = WPL_ERROR;
		927	u8 filename[30];
		928	Point_t WP;
		929
767	holgerb	930	UART1_PutString("\r\n write single point\r\n");
516	holgerb	931	if(GPSData.Position.Status == INVALID)
		932	{
767	holgerb	933	UART1_PutString("ERROR: No GPS - Fix\r\n");
514	holgerb	934	return(retval);
516	holgerb	935	}
513	killagreg	936
		937	// clear current point list
		938	PointList_Clear();
		939	// prepare WP at current position
519	holgerb	940
737	holgerb	941	if(NCFlags & NC_FLAG_FREE \|\| NaviData.TargetPositionDeviation.Distance_dm > 7*10)
519	holgerb	942	{ // take actual position
		943	GPSPos_Copy(&GPSData.Position, &(WP.Position));
		944	}
		945	else
		946	{ // take last target position
		947	GPSPos_Copy(&NaviData.TargetPosition, &(WP.Position));
		948	}
		949
513	killagreg	950	GPSPos_Copy(&GPSData.Position, &(WP.Position));
		951	// set heading
545	holgerb	952	WP.Heading = (CompassSetpointCorrected/10 + Parameter.CamOrientation * 15) % 360;
513	killagreg	953	if(WP.Heading == 0) WP.Heading = 360;
519	holgerb	954	WP.ToleranceRadius = 120; // 12m
		955	WP.HoldTime = 2;
513	killagreg	956	WP.Index = 1;
		957	WP.Type = POINT_TYPE_WP;
695	holgerb	958	WP.WP_EventChannelValue = 100;
576	holgerb	959	if(FC.StatusFlags & FC_STATUS_FLY && ((FromFC_VarioCharacter != ' ') \|\| (SimulationFlags & SIMULATION_ACTIVE))) // only in flight and if the Altitude control is enabled
516	holgerb	960	{
519	holgerb	961	WP.AltitudeRate = 255; // Auto
928	holgerb	962	WP.Position.Altitude = FC.SetpointAltimeter_dm; // in dm
516	holgerb	963	}
		964	else
514	holgerb	965	{
516	holgerb	966	WP.AltitudeRate = 0;
		967	WP.Position.Altitude = 0;
514	holgerb	968	}
519	holgerb	969	WP.Speed = 50; // beim Laden wird der Wert nochmal neu gesetzt
513	killagreg	970	WP.CamAngle = 0;
521	holgerb	971	WP.Name[0] = 'S';
		972	WP.Name[1] = 'P';
		973	WP.Name[2] = 'T';
		974	WP.Name[3] = 0;
513	killagreg	975	// add this point to wp list
		976	PointList_SetAt(&WP);
514	holgerb	977
		978	sprintf(filename, "/POINT/POINT%03d.wpl", pWPL_Store->Index);
516	holgerb	979	UART1_PutString(filename);
514	holgerb	980	sprintf(pWPL_Store->Name, "POINT%03d ", pWPL_Store->Index);
513	killagreg	981	retval = PointList_Save(filename, pWPL_Store->Name, 1);
514	holgerb	982
		983	// clear current point list
519	holgerb	984	if((NC_GPS_ModeCharacter != 'w') && (NC_GPS_ModeCharacter != 'W')) PointList_Clear();
549	holgerb	985	else // es ist gearde ein Wegpunktflug aktiv -> updaten
519	holgerb	986	{
		987	if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE);
785	holgerb	988	GPS_pWaypoint = PointList_WPBegin(0); // updates POI index
519	holgerb	989	}
		990
513	killagreg	991	return(retval);
		992	}
924	holgerb	993
513	killagreg	994	// load target gps posititon and heading from file
		995	u8 PointList_LoadSinglePoint(WPL_Store_t * pWPL_Store)
		996	{
519	holgerb	997	u8 filename[30], result = 0;
516	holgerb	998
514	holgerb	999	sprintf(filename, "/POINT/POINT%03d.wpl", pWPL_Store->Index);
513	killagreg	1000	pWPL_Store->Name[0] = 0; // clear current list name
519	holgerb	1001	result = PointList_Load(filename, pWPL_Store->Name, sizeof(pWPL_Store->Name),Parameter.SingleWpSpeed);
		1002	if(result) UART1_Request_ReadPoint = 1; // Sends Point 1 to the PC
		1003	return(result);
513	killagreg	1004	}
		1005
		1006
516	holgerb	1007
504	holgerb	1008	void ClearWLP_Name(void)
		1009	{
513	killagreg	1010	u8 i;
		1011	for(i=0; i<sizeof(WPL_Store.Name);i++) WPL_Store.Name[i] = 0;
		1012	NewWPL_Name = 1;
504	holgerb	1013	}
513	killagreg	1014
491	killagreg	1015	// move actual point list to ref pos., the point in the list marked by index gets the RefPos afterwards
503	holgerb	1016	u8 PointList_Move(u8 RefIndex, GPS_Pos_t* pRefPos, u16 RotationAngle)
491	killagreg	1017	{
		1018	u8 retval = 0;
513	killagreg	1019	s32 altitude;
		1020	GPS_Pos_t OldRefPos;
491	killagreg	1021	GPS_Pos_Deviation_t RefDeviation;
516	holgerb	1022
491	killagreg	1023	// check inputs for plausibility;
516	holgerb	1024	if((RefIndex == 0) \|\| (RefIndex > PointCount)) return(retval);
491	killagreg	1025	if(pRefPos == NULL) return(retval);
		1026	if(pRefPos->Status == INVALID) return(retval);
		1027
513	killagreg	1028	if(GPSPos_Copy(&(PointList[RefIndex-1].Position), &OldRefPos)) // backup old reference position
491	killagreg	1029	{
		1030	u8 i;
513	killagreg	1031	// iterate the position list
491	killagreg	1032	for(i = 0; i < PointCount; i++)
		1033	{
		1034	retval = 0;
513	killagreg	1035	// backup altitude of this point
		1036	altitude = PointList[i].Position.Altitude;
		1037	// calculate deviation form old ref, i.e the north and east shift of each point in the list from the reference position
		1038	if(!GPSPos_Deviation(&(PointList[i].Position), &OldRefPos, &RefDeviation)) break;
491	killagreg	1039	// copy of the new reference position into this list place
		1040	if(!GPSPos_Copy(pRefPos, &(PointList[i].Position))) break;
516	holgerb	1041	// restore former altitude
513	killagreg	1042	PointList[i].Position.Altitude = altitude;
491	killagreg	1043	// move new reference according to the deviation of the old reference
513	killagreg	1044	if(RotationAngle > 0)
		1045	{
737	holgerb	1046	retval = GPSPos_ShiftGeodetic(&(PointList[i].Position), (RefDeviation.Bearing + 180 + RotationAngle)%360, RefDeviation.Distance_cm );
513	killagreg	1047	// Now rotate the heading positions if they are fixed angles
		1048	if(PointList[i].Heading >= 0 && PointList[i].Heading <= 360) PointList[i].Heading = (PointList[i].Heading + RotationAngle) % 360;
		1049	}
		1050	else // no rotation
		1051	{
		1052	// move new reference according to the deviation of the old reference
		1053	retval = GPSPos_ShiftCartesian(&(PointList[i].Position), RefDeviation.North, RefDeviation.East);
		1054	}
516	holgerb	1055	if(!retval) break;
491	killagreg	1056	}
		1057	} // else ref pos old not copied!
		1058	if(!retval) PointList_Clear();
		1059	return(retval);
		1060	}
		1061
516	holgerb	1062
491	killagreg	1063

Subversion Repositories NaviCtrl

(root)/tags/V2.22a/waypoints.c @ 935 - Rev 930