WebSVN - NaviCtrl - Blame - Rev 878 - /tags/V2.20h/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
878	holgerb	525	#define WP_FILE_VERSION_COMPATIBLE 3
		526	#define WP_FILE_VERSION_COMPATIBLE2 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);
		617	// write cam angle in degree (255 -> POI-Automatic)
		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;
498	killagreg	660
		661	// clear point list first
		662	PointList_Clear();
517	holgerb	663	HeadingOld = -1; // updates the direction if the new direction is the same like last time
767	holgerb	664	UART1_PutString("\r\n Read ");
516	holgerb	665	UART1_PutString(filename);
490	killagreg	666	UART1_PutString("...");
		667
		668	if(Fat16_IsValid())
		669	{ // check if wpl file is existing
513	killagreg	670	fp = fopen_(filename, 'r'); // try to open the file
490	killagreg	671	if(fp == NULL)
		672	{
		673	UART1_PutString("ERROR: Reading waypoint file!\r\n");
496	killagreg	674	return(retval);
490	killagreg	675	}
		676	// read all lines from file
		677	while(fgets_(wpline, LINE_MAX, fp) != 0)
		678	{
		679	if ( // ignorelines starting with \r,\n,' ',';','#'
		680	(wpline[0] != '\n') &&
		681	(wpline[0] != '\r') &&
		682	(wpline[0] != ' ' ) &&
		683	(wpline[0] != ';' ) &&
		684	(wpline[0] != '#' )
		685	)
		686	{
		687	// check for section line found
		688	if(wpline[0] == '[')
		689	{
		690	// next section found
		691	IsGeneralSection = 0;
		692	IsPointSection = 0;
		693
		694	name = strtok(&wpline[1], "]");
		695	if(name != NULL) // if section name
		696	{
		697	// check section type
		698	for(i=0; name[i]; i++) name[i] = toupper(name[i]); // convert to upper case
		699
		700	if(strncmp(name, "POINT", 5) == 0)
		701	{
		702	IsPointSection = (u8)atoi(&name[5]);
		703	PointCount++;
		704	}
		705	else if(strcmp(name, "GENERAL") == 0)
		706	{
		707	IsGeneralSection = 1;
		708	}
		709	else
		710	{
		711	UART1_PutString("Unknown section: ");
		712	UART1_PutString(name);
		713	UART1_PutString("\r\n");
		714	}
		715	}
		716	} // EOF section line
		717	else
		718	{ // look for key entrys of each sections
		719	name = strtok(wpline, "="); // get name
		720	value = strtok(NULL, "="); // get value
		721	if ((name != NULL) && (value != NULL))
		722	{
		723	for(i=0; name[i]; i++) name[i] = toupper(name[i]); // convert to upper case
		724	if(IsPointSection && (IsPointSection <= WPNumber))
		725	{
		726	if(strcmp(name, "LATITUDE") == 0)
		727	{
		728	PointList[IsPointSection-1].Position.Latitude = (s32)(atof(value) * 1E7);
		729	}
		730	else if(strcmp(name, "LONGITUDE") == 0)
		731	{
		732	PointList[IsPointSection-1].Position.Longitude = (s32)(atof(value) * 1E7);
		733	}
		734	else if(strcmp(name, "RADIUS") == 0)
		735	{
		736	PointList[IsPointSection-1].ToleranceRadius = (u8)atoi(value);
		737	}
		738	else if(strcmp(name, "ALTITUDE") == 0)
		739	{
		740	PointList[IsPointSection-1].Position.Altitude = (s32)(atof(value) * 100.0); // in cm
		741	PointList[IsPointSection-1].Position.Status = NEWDATA;
		742	}
		743	else if(strcmp(name, "CLIMBRATE") == 0)
		744	{
		745	PointList[IsPointSection-1].AltitudeRate = (u8)atoi(value);
		746	}
		747	else if(strcmp(name, "DELAYTIME") == 0)
		748	{
		749	PointList[IsPointSection-1].HoldTime = (u8)atoi(value);
		750	}
		751	else if(strcmp(name, "WP_EVENT_CHANNEL_VALUE") == 0)
		752	{
		753	PointList[IsPointSection-1].WP_EventChannelValue = (u8)atoi(value);
		754	}
		755	else if(strcmp(name, "HEADING") == 0)
		756	{
		757	PointList[IsPointSection-1].Heading = (s16)atoi(value);
		758	}
		759	else if(strcmp(name, "SPEED") == 0)
		760	{
519	holgerb	761	if(use_preset_speed) PointList[IsPointSection-1].Speed = use_preset_speed;
		762	else PointList[IsPointSection-1].Speed = (u8)atoi(value);
490	killagreg	763	}
		764	else if(strcmp(name, "CAM-NICK") == 0)
		765	{
		766	PointList[IsPointSection-1].CamAngle = (u8)atoi(value);
		767	}
		768	else if(strcmp(name, "TYPE") == 0)
		769	{
		770	PointList[IsPointSection-1].Type = (u8)atoi(value);
		771	if(PointList[IsPointSection-1].Type > 0) PointList[IsPointSection-1].Type--; // index shift
		772	else PointList[IsPointSection-1].Type = POINT_TYPE_INVALID;
516	holgerb	773
490	killagreg	774	switch(PointList[IsPointSection-1].Type)
		775	{
785	holgerb	776	case POINT_TYPE_LAND:
		777	FsPointCnt++; // break fehlt absichtlich
490	killagreg	778	case POINT_TYPE_WP:
		779	// this works only if altitude key is set before point type key in WPL file !!
516	holgerb	780	PointList[IsPointSection-1].Position.Altitude /= 10; // dm only for WPs
490	killagreg	781	WPCount++;
		782	break;
601	holgerb	783	case POINT_TYPE_FS:
		784	FsPointCnt++;
		785	break;
490	killagreg	786	case POINT_TYPE_POI:
		787	POICount++;
		788	break;
802	holgerb	789	case POINT_TYPE_FLYZONE:
		790	FlyzonePointCnt++;
		791	break;
490	killagreg	792	}
		793	}
		794	else if(strcmp(name, "PREFIX") == 0)
		795	{
		796	strncpy(PointList[IsPointSection-1].Name, value, 4);
		797	PointList[IsPointSection-1].Name[3] = 0; // Terminate string
		798	}
642	holgerb	799	else if(strcmp(name, "AUTOTRIGGER") == 0)
		800	{
		801	PointList[IsPointSection-1].AutoPhotoDistance = (u8)atoi(value);
		802	}
490	killagreg	803	else
		804	{
		805	UART1_PutString("Unknown key: ");
		806	UART1_PutString(name);
		807	UART1_PutString("\r\n");
516	holgerb	808	}
490	killagreg	809	} // EOF point section
		810	else if(IsGeneralSection)
		811	{
		812	if(strcmp(name, "NUMBEROFWAYPOINTS") == 0)
516	holgerb	813	{
490	killagreg	814	WPNumber = (u8)atoi(value);
		815	if(!WPNumber) // no waypoints in file
		816	{
516	holgerb	817	return(WPL_NO_WAYPOINTS); // we are done here
490	killagreg	818	}
		819	else if(WPNumber > MAX_LIST_LEN) // number o points larger than ram list
		820	{
514	holgerb	821	UART1_PutString("To many points!");
497	killagreg	822	return(WPL_ERROR);
516	holgerb	823	}
490	killagreg	824	}
		825	else if (strcmp(name, "FILEVERSION") == 0)
		826	{
878	holgerb	827	if(((u8)atoi(value) != WP_FILE_VERSION_COMPATIBLE) && ((u8)atoi(value) != WP_FILE_VERSION_COMPATIBLE2))
490	killagreg	828	{
496	killagreg	829	PointList_Clear();
490	killagreg	830	UART1_PutString("Bad file version!\r\n");
516	holgerb	831	return(WPL_ERROR);
490	killagreg	832	}
		833	}
495	killagreg	834	else if (strcmp(name, "NAME") == 0)
		835	{
513	killagreg	836	if(listname)
498	killagreg	837	{
513	killagreg	838	u8 len = strlen(value);
		839	if(len)
		840	{
		841	if(value[len-1] == '\r')
		842	{
		843	value[len-1] = 0;
540	holgerb	844	// len--;
513	killagreg	845	}
		846	}
		847	if(len > listnamelen) len = listnamelen;
		848	if(len)
		849	{
		850	value[len-1] = 0; // terminate string
		851	if(listname) memcpy(listname, value, len);
		852	}
		853	NewWPL_Name = 1;
498	killagreg	854	}
495	killagreg	855	}
490	killagreg	856	else
		857	{
		858	UART1_PutString("Unknown key: ");
		859	UART1_PutString(name);
		860	UART1_PutString("\r\n");
		861	}
		862	} // EOF general section
		863	} // EOF valid key entry
		864	} // EOF key entry line
		865	} // valid line
		866	} // EOF loop over all lines
		867	fclose_(fp);
		868	NaviData.WaypointNumber = WPCount;
516	holgerb	869	retval = WPL_OK;
		870	UART1_PutString("ok\r\n");
490	killagreg	871	} // EOF if(Fat16_IsValid())
516	holgerb	872	else
497	killagreg	873	{
		874	UART1_PutString("no file system found!\r\n");
		875	retval = WPL_NO_SDCARD_FOUND;
516	holgerb	876	}
490	killagreg	877	return(retval);
491	killagreg	878	}
490	killagreg	879
513	killagreg	880	// load actual point list from SD card
		881	u8 PointList_ReadFromFile(WPL_Store_t * pWPL_Store)
		882	{
516	holgerb	883	u8 filename[30];
513	killagreg	884
		885	pWPL_Store->Name[0] = 0; // clear current list name
		886
		887	// user absolute path, i.e. leading /
		888	if(pWPL_Store->Index == 0) // index 0 looks for a default WPL file in the root
		889	{
516	holgerb	890	sprintf(filename, "/default.wpl");
513	killagreg	891	}
		892	else
		893	{
		894	sprintf(filename, "/WPL/list_%03d.wpl", pWPL_Store->Index);
		895	}
519	holgerb	896	return PointList_Load(filename, pWPL_Store->Name, sizeof(pWPL_Store->Name),0);
513	killagreg	897	}
		898
		899	// save actual point list to SD card
		900	u8 PointList_WriteToFile(WPL_Store_t * pWPL_Store)
		901	{
		902	u8 filename[30];
516	holgerb	903
		904
513	killagreg	905	if(PointCount == 0) return(WPL_NO_WAYPOINTS);
516	holgerb	906	// user absolute path, i.e. leading /
513	killagreg	907	if(pWPL_Store->Index == 0)
		908	{
		909	sprintf(filename, "/default.wpl");
		910	}
		911	else
		912	{
		913	sprintf(filename, "/WPL/list_%03d.wpl", pWPL_Store->Index);
		914	}
		915	return PointList_Save(filename, pWPL_Store->Name, pWPL_Store->OverwriteFile);
		916	}
		917
		918
516	holgerb	919	// save actual gps positiin and heading to file
513	killagreg	920	u8 PointList_SaveSinglePoint(WPL_Store_t * pWPL_Store)
		921	{
		922	u8 retval = WPL_ERROR;
		923	u8 filename[30];
		924	Point_t WP;
		925
767	holgerb	926	UART1_PutString("\r\n write single point\r\n");
516	holgerb	927	if(GPSData.Position.Status == INVALID)
		928	{
767	holgerb	929	UART1_PutString("ERROR: No GPS - Fix\r\n");
514	holgerb	930	return(retval);
516	holgerb	931	}
513	killagreg	932
		933	// clear current point list
		934	PointList_Clear();
		935	// prepare WP at current position
519	holgerb	936
737	holgerb	937	if(NCFlags & NC_FLAG_FREE \|\| NaviData.TargetPositionDeviation.Distance_dm > 7*10)
519	holgerb	938	{ // take actual position
		939	GPSPos_Copy(&GPSData.Position, &(WP.Position));
		940	}
		941	else
		942	{ // take last target position
		943	GPSPos_Copy(&NaviData.TargetPosition, &(WP.Position));
		944	}
		945
513	killagreg	946	GPSPos_Copy(&GPSData.Position, &(WP.Position));
		947	// set heading
545	holgerb	948	WP.Heading = (CompassSetpointCorrected/10 + Parameter.CamOrientation * 15) % 360;
513	killagreg	949	if(WP.Heading == 0) WP.Heading = 360;
519	holgerb	950	WP.ToleranceRadius = 120; // 12m
		951	WP.HoldTime = 2;
513	killagreg	952	WP.Index = 1;
		953	WP.Type = POINT_TYPE_WP;
695	holgerb	954	WP.WP_EventChannelValue = 100;
576	holgerb	955	if(FC.StatusFlags & FC_STATUS_FLY && ((FromFC_VarioCharacter != ' ') \|\| (SimulationFlags & SIMULATION_ACTIVE))) // only in flight and if the Altitude control is enabled
516	holgerb	956	{
519	holgerb	957	WP.AltitudeRate = 255; // Auto
		958	WP.Position.Altitude = NaviData.SetpointAltitude / 2;
516	holgerb	959	}
		960	else
514	holgerb	961	{
516	holgerb	962	WP.AltitudeRate = 0;
		963	WP.Position.Altitude = 0;
514	holgerb	964	}
519	holgerb	965	WP.Speed = 50; // beim Laden wird der Wert nochmal neu gesetzt
513	killagreg	966	WP.CamAngle = 0;
521	holgerb	967	WP.Name[0] = 'S';
		968	WP.Name[1] = 'P';
		969	WP.Name[2] = 'T';
		970	WP.Name[3] = 0;
513	killagreg	971	// add this point to wp list
		972	PointList_SetAt(&WP);
514	holgerb	973
		974	sprintf(filename, "/POINT/POINT%03d.wpl", pWPL_Store->Index);
516	holgerb	975	UART1_PutString(filename);
514	holgerb	976	sprintf(pWPL_Store->Name, "POINT%03d ", pWPL_Store->Index);
513	killagreg	977	retval = PointList_Save(filename, pWPL_Store->Name, 1);
514	holgerb	978
		979	// clear current point list
519	holgerb	980	if((NC_GPS_ModeCharacter != 'w') && (NC_GPS_ModeCharacter != 'W')) PointList_Clear();
549	holgerb	981	else // es ist gearde ein Wegpunktflug aktiv -> updaten
519	holgerb	982	{
		983	if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE);
785	holgerb	984	GPS_pWaypoint = PointList_WPBegin(0); // updates POI index
519	holgerb	985	}
		986
513	killagreg	987	return(retval);
		988	}
		989	// load target gps posititon and heading from file
		990	u8 PointList_LoadSinglePoint(WPL_Store_t * pWPL_Store)
		991	{
519	holgerb	992	u8 filename[30], result = 0;
516	holgerb	993
514	holgerb	994	sprintf(filename, "/POINT/POINT%03d.wpl", pWPL_Store->Index);
513	killagreg	995	pWPL_Store->Name[0] = 0; // clear current list name
519	holgerb	996	result = PointList_Load(filename, pWPL_Store->Name, sizeof(pWPL_Store->Name),Parameter.SingleWpSpeed);
		997	if(result) UART1_Request_ReadPoint = 1; // Sends Point 1 to the PC
		998	return(result);
513	killagreg	999	}
		1000
		1001
516	holgerb	1002
504	holgerb	1003	void ClearWLP_Name(void)
		1004	{
513	killagreg	1005	u8 i;
		1006	for(i=0; i<sizeof(WPL_Store.Name);i++) WPL_Store.Name[i] = 0;
		1007	NewWPL_Name = 1;
504	holgerb	1008	}
513	killagreg	1009
491	killagreg	1010	// move actual point list to ref pos., the point in the list marked by index gets the RefPos afterwards
503	holgerb	1011	u8 PointList_Move(u8 RefIndex, GPS_Pos_t* pRefPos, u16 RotationAngle)
491	killagreg	1012	{
		1013	u8 retval = 0;
513	killagreg	1014	s32 altitude;
		1015	GPS_Pos_t OldRefPos;
491	killagreg	1016	GPS_Pos_Deviation_t RefDeviation;
516	holgerb	1017
491	killagreg	1018	// check inputs for plausibility;
516	holgerb	1019	if((RefIndex == 0) \|\| (RefIndex > PointCount)) return(retval);
491	killagreg	1020	if(pRefPos == NULL) return(retval);
		1021	if(pRefPos->Status == INVALID) return(retval);
		1022
513	killagreg	1023	if(GPSPos_Copy(&(PointList[RefIndex-1].Position), &OldRefPos)) // backup old reference position
491	killagreg	1024	{
		1025	u8 i;
513	killagreg	1026	// iterate the position list
491	killagreg	1027	for(i = 0; i < PointCount; i++)
		1028	{
		1029	retval = 0;
513	killagreg	1030	// backup altitude of this point
		1031	altitude = PointList[i].Position.Altitude;
		1032	// calculate deviation form old ref, i.e the north and east shift of each point in the list from the reference position
		1033	if(!GPSPos_Deviation(&(PointList[i].Position), &OldRefPos, &RefDeviation)) break;
491	killagreg	1034	// copy of the new reference position into this list place
		1035	if(!GPSPos_Copy(pRefPos, &(PointList[i].Position))) break;
516	holgerb	1036	// restore former altitude
513	killagreg	1037	PointList[i].Position.Altitude = altitude;
491	killagreg	1038	// move new reference according to the deviation of the old reference
513	killagreg	1039	if(RotationAngle > 0)
		1040	{
737	holgerb	1041	retval = GPSPos_ShiftGeodetic(&(PointList[i].Position), (RefDeviation.Bearing + 180 + RotationAngle)%360, RefDeviation.Distance_cm );
513	killagreg	1042	// Now rotate the heading positions if they are fixed angles
		1043	if(PointList[i].Heading >= 0 && PointList[i].Heading <= 360) PointList[i].Heading = (PointList[i].Heading + RotationAngle) % 360;
		1044	}
		1045	else // no rotation
		1046	{
		1047	// move new reference according to the deviation of the old reference
		1048	retval = GPSPos_ShiftCartesian(&(PointList[i].Position), RefDeviation.North, RefDeviation.East);
		1049	}
516	holgerb	1050	if(!retval) break;
491	killagreg	1051	}
		1052	} // else ref pos old not copied!
		1053	if(!retval) PointList_Clear();
		1054	return(retval);
		1055	}
		1056
516	holgerb	1057
491	killagreg	1058

Subversion Repositories NaviCtrl

(root)/tags/V2.20h/waypoints.c @ 898 - Rev 878