WebSVN - NaviCtrl - Blame - Rev 802 - /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);
		157	return(wn);
		158	}
		159
		160
280	killagreg	161	u8 PointList_GetCount(void)
41	ingob	162	{
280	killagreg	163	return PointCount; // number of points in the list
41	ingob	164	}
		165
295	killagreg	166	Point_t* PointList_GetAt(u8 index)
41	ingob	167	{
295	killagreg	168	if((index > 0) && (index <= PointCount)) return(&(PointList[index-1])); // return pointer to this waypoint
		169	else return(NULL);
		170	}
		171
		172	u8 PointList_SetAt(Point_t* pPoint)
		173	{
		174	// if index is in range
296	killagreg	175	if((pPoint->Index > 0) && (pPoint->Index <= MAX_LIST_LEN))
41	ingob	176	{
802	holgerb	177	UpdateFlyzoneTimer = 2; // makes an update of the flightzone in some milliseconds
295	killagreg	178	// check list entry before update
		179	switch(PointList[pPoint->Index-1].Type)
277	killagreg	180	{
295	killagreg	181	case POINT_TYPE_INVALID: // was invalid
		182	switch(pPoint->Type)
		183	{
		184	default:
		185	case POINT_TYPE_INVALID:
		186	// nothing to do
		187	break;
		188
785	holgerb	189	case POINT_TYPE_LAND:
		190	FsPointCnt++; // break fehlt absichtlich
295	killagreg	191	case POINT_TYPE_WP:
		192	WPCount++;
		193	PointCount++;
		194	break;
516	holgerb	195
295	killagreg	196	case POINT_TYPE_POI:
		197	POICount++;
		198	PointCount++;
		199	break;
601	holgerb	200	case POINT_TYPE_FS:
		201	FsPointCnt++;
		202	PointCount++;
		203	break;
802	holgerb	204	case POINT_TYPE_FLYZONE:
		205	FlyzonePointCnt++;
		206	PointCount++;
		207	break;
295	killagreg	208	}
516	holgerb	209	break;
		210
785	holgerb	211	case POINT_TYPE_LAND: // was a landing point
295	killagreg	212	switch(pPoint->Type)
		213	{
		214	case POINT_TYPE_INVALID:
		215	WPCount--;
		216	PointCount--;
		217	break;
		218	default:
785	holgerb	219	case POINT_TYPE_LAND:
		220	break;
295	killagreg	221	case POINT_TYPE_WP:
802	holgerb	222	FsPointCnt--; // the landing point is also a Failsafe-Pt
785	holgerb	223	break;
		224	case POINT_TYPE_FS:
		225	FsPointCnt++;
		226	WPCount--;
		227	break;
		228	case POINT_TYPE_POI:
		229	POICount++;
		230	WPCount--;
		231	break;
802	holgerb	232	case POINT_TYPE_FLYZONE:
		233	FlyzonePointCnt++;
		234	WPCount--;
		235	break;
785	holgerb	236	}
		237	break;
		238	case POINT_TYPE_WP:
		239	switch(pPoint->Type)
		240	{
		241	case POINT_TYPE_INVALID:
		242	WPCount--;
		243	PointCount--;
		244	break;
		245
		246	default:
		247	case POINT_TYPE_LAND:
802	holgerb	248	FsPointCnt++; // the landing point is also a Failsafe-Pt
785	holgerb	249	break;
		250	case POINT_TYPE_WP:
295	killagreg	251	//nothing to do
		252	break;
601	holgerb	253	case POINT_TYPE_FS:
		254	FsPointCnt++;
		255	WPCount--;
		256	break;
295	killagreg	257	case POINT_TYPE_POI:
		258	POICount++;
		259	WPCount--;
		260	break;
802	holgerb	261	case POINT_TYPE_FLYZONE:
		262	FlyzonePointCnt++;
		263	WPCount--;
		264	break;
295	killagreg	265	}
		266	break;
802	holgerb	267	case POINT_TYPE_FLYZONE:
		268	switch(pPoint->Type)
		269	{
		270	case POINT_TYPE_INVALID:
		271	FlyzonePointCnt--;
		272	PointCount--;
		273	break;
		274	default:
		275	case POINT_TYPE_LAND:
		276	FsPointCnt++; // the landing point is also a Failsafe-Pt
		277	break;
		278	case POINT_TYPE_WP:
		279	WPCount++;
		280	FlyzonePointCnt--;
		281	break;
		282	case POINT_TYPE_FLYZONE:
		283	//nothing to do
		284	break;
		285	case POINT_TYPE_FS:
		286	FsPointCnt++;
		287	FlyzonePointCnt--;
		288	break;
		289	case POINT_TYPE_POI:
		290	POICount++;
		291	FlyzonePointCnt--;
		292	break;
		293	}
		294	break;
516	holgerb	295
295	killagreg	296	case POINT_TYPE_POI: // was a poi
		297	switch(pPoint->Type)
		298	{
		299	case POINT_TYPE_INVALID:
		300	POICount--;
		301	PointCount--;
		302	break;
802	holgerb	303	case POINT_TYPE_FS:
		304	FsPointCnt++;
		305	POICount--;
		306	break;
295	killagreg	307
785	holgerb	308	case POINT_TYPE_LAND:
		309	FsPointCnt++; // break fehlt absichtlich
295	killagreg	310	case POINT_TYPE_WP:
		311	WPCount++;
		312	POICount--;
		313	break;
802	holgerb	314	case POINT_TYPE_FLYZONE:
		315	FlyzonePointCnt++;
601	holgerb	316	POICount--;
		317	break;
295	killagreg	318	case POINT_TYPE_POI:
		319	default:
		320	// nothing to do
		321	break;
		322	}
601	holgerb	323	case POINT_TYPE_FS: // was a Failsafe
		324	switch(pPoint->Type)
		325	{
		326	case POINT_TYPE_INVALID:
		327	FsPointCnt--;
		328	PointCount--;
		329	break;
785	holgerb	330	case POINT_TYPE_LAND:
802	holgerb	331	FsPointCnt++; // (wird gleich wieder abgezogen)
601	holgerb	332	case POINT_TYPE_WP:
		333	WPCount++;
		334	FsPointCnt--;
		335	break;
		336	case POINT_TYPE_POI:
		337	POICount++;
		338	FsPointCnt--;
		339	break;
802	holgerb	340	case POINT_TYPE_FLYZONE:
		341	FlyzonePointCnt++;
		342	FsPointCnt--;
		343	break;
601	holgerb	344	case POINT_TYPE_FS:
		345	break;
		346	default:
		347	// nothing to do
		348	break;
		349	}
516	holgerb	350	break;
277	killagreg	351	}
516	holgerb	352	memcpy(&PointList[pPoint->Index-1], pPoint, sizeof(Point_t)); // copy data to list entry
295	killagreg	353	NaviData.WaypointNumber = WPCount;
		354	return pPoint->Index;
41	ingob	355	}
296	killagreg	356	else return(0);
41	ingob	357	}
		358
227	killagreg	359	// returns the pointer to the first waypoint within the list
785	holgerb	360	Point_t* PointList_WPBegin(u32 start)
41	ingob	361	{
786	holgerb	362	u8 i, wp_nr = 0;
227	killagreg	363	WPIndex = 0; // set list position invalid
295	killagreg	364
283	holgerb	365	if(WPActive == FALSE) return(NULL);
295	killagreg	366
277	killagreg	367	POIIndex = 0; // set invalid POI
785	holgerb	368	if(PointCount > start)
152	killagreg	369	{
225	killagreg	370	// search for first wp in list
786	holgerb	371	for(i = 0; i <MAX_LIST_LEN; i++)
225	killagreg	372	{
785	holgerb	373	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	374	{
227	killagreg	375	WPIndex = i + 1;
786	holgerb	376	wp_nr++; // count the number of scipped WPs to make sure that the Index in the NaviData is correct
		377	if(i >= start) break;
225	killagreg	378	}
		379	}
277	killagreg	380	if(WPIndex) // found a WP in the list
		381	{
786	holgerb	382	NaviData.WaypointIndex = wp_nr;
549	holgerb	383	NewWaypointsReceived = 1; // activates the Waypoint list as soon as CH is started
277	killagreg	384	// update index to POI
280	killagreg	385	if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
516	holgerb	386	else POIIndex = 0;
277	killagreg	387	}
		388	else // some points in the list but no WP found
		389	{
280	killagreg	390	NaviData.WaypointIndex = 0;
277	killagreg	391	//Check for an existing POI
295	killagreg	392	for(i = 0; i < MAX_LIST_LEN; i++)
277	killagreg	393	{
280	killagreg	394	if((PointList[i].Type == POINT_TYPE_POI) && (PointList[i].Position.Status != INVALID))
277	killagreg	395	{
		396	POIIndex = i + 1;
		397	break;
		398	}
		399	}
		400	}
152	killagreg	401	}
277	killagreg	402	else // no point in the list
152	killagreg	403	{
516	holgerb	404	POIIndex = 0;
		405	NaviData.WaypointIndex = 0;
227	killagreg	406	}
295	killagreg	407
		408	if(WPIndex) return(&(PointList[WPIndex-1]));
		409	else return(NULL);
41	ingob	410	}
		411
225	killagreg	412	// returns the last waypoint
280	killagreg	413	Point_t* PointList_WPEnd(void)
151	killagreg	414	{
516	holgerb	415
277	killagreg	416	u8 i;
227	killagreg	417	WPIndex = 0; // set list position invalid
277	killagreg	418	POIIndex = 0; // set invalid
283	holgerb	419
280	killagreg	420	if(WPActive == FALSE) return(NULL);
277	killagreg	421
280	killagreg	422	if(PointCount > 0)
151	killagreg	423	{
227	killagreg	424	// search backward!
295	killagreg	425	for(i = 1; i <= MAX_LIST_LEN; i++)
225	killagreg	426	{
785	holgerb	427	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	428	{
295	killagreg	429	WPIndex = MAX_LIST_LEN - i + 1;
225	killagreg	430	break;
		431	}
		432	}
277	killagreg	433	if(WPIndex) // found a WP within the list
		434	{
280	killagreg	435	NaviData.WaypointIndex = WPCount;
		436	if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
516	holgerb	437	else POIIndex = 0;
277	killagreg	438	}
278	killagreg	439	else // list contains some points but no WP in the list
277	killagreg	440	{
		441	// search backward for a POI!
295	killagreg	442	for(i = 1; i <= MAX_LIST_LEN; i++)
277	killagreg	443	{
295	killagreg	444	if((PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_POI) && (PointList[MAX_LIST_LEN - i].Position.Status != INVALID))
516	holgerb	445	{
295	killagreg	446	POIIndex = MAX_LIST_LEN - i + 1;
277	killagreg	447	break;
		448	}
		449	}
516	holgerb	450	NaviData.WaypointIndex = 0;
277	killagreg	451	}
151	killagreg	452	}
277	killagreg	453	else // no point in the list
227	killagreg	454	{
280	killagreg	455	POIIndex = 0;
		456	NaviData.WaypointIndex = 0;
227	killagreg	457	}
295	killagreg	458	if(WPIndex) return(&(PointList[WPIndex-1]));
		459	else return(NULL);
151	killagreg	460	}
		461
41	ingob	462	// returns a pointer to the next waypoint or NULL if the end of the list has been reached
280	killagreg	463	Point_t* PointList_WPNext(void)
41	ingob	464	{
227	killagreg	465	u8 wp_found = 0;
280	killagreg	466	if(WPActive == FALSE) return(NULL);
516	holgerb	467
295	killagreg	468	if(WPIndex < MAX_LIST_LEN) // if there is a next entry in the list
41	ingob	469	{
227	killagreg	470	u8 i;
295	killagreg	471	for(i = WPIndex; i < MAX_LIST_LEN; i++) // start search for next at next list entry
225	killagreg	472	{
785	holgerb	473	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	474	{
227	killagreg	475	wp_found = i+1;
225	killagreg	476	break;
		477	}
601	holgerb	478	/*
		479	else
		480	if((PointList[i].Type == POINT_TYPE_FS) && (PointList[i].Position.Status != INVALID)) // jump over POIs
		481	{
		482	GPSPos_Copy(&(PointList[i].Position), &GPS_FailsafePosition);
		483	}
		484	*/
225	killagreg	485	}
41	ingob	486	}
227	killagreg	487	if(wp_found)
		488	{
		489	WPIndex = wp_found; // update list position
280	killagreg	490	NaviData.WaypointIndex++;
		491	if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
227	killagreg	492	else POIIndex = 0;
280	killagreg	493	return(&(PointList[WPIndex-1])); // return pointer to this waypoint
227	killagreg	494	}
516	holgerb	495	else
280	killagreg	496	{ // no next wp found
516	holgerb	497	NaviData.WaypointIndex = 0;
277	killagreg	498	POIIndex = 0;
		499	return(NULL);
		500	}
516	holgerb	501	}
280	killagreg	502
		503	void PointList_WPActive(u8 set)
		504	{
295	killagreg	505	if(set)
516	holgerb	506	{
295	killagreg	507	WPActive = TRUE;
785	holgerb	508	PointList_WPBegin(0); // updates POI index
295	killagreg	509	}
		510	else
		511	{
		512	WPActive = FALSE;
		513	POIIndex = 0; // disable POI also
		514	}
280	killagreg	515	}
516	holgerb	516
280	killagreg	517	Point_t* PointList_GetPOI(void)
224	killagreg	518	{
516	holgerb	519	return PointList_GetAt(POIIndex);
224	killagreg	520	}
		521
513	killagreg	522
490	killagreg	523	#define LINE_MAX 70
		524	#define WP_FILE_VERSION_COMPATIBLE 3
513	killagreg	525
		526	u8 PointList_Save(u8 * filename, u8* listname, u8 overwride)
490	killagreg	527	{
		528	File_t *fp;
495	killagreg	529	s8 wpline[LINE_MAX];
		530	u8 retval = WPL_ERROR;
513	killagreg	531
767	holgerb	532	UART1_PutString("\r\n Save WPL...");
490	killagreg	533
		534	if(Fat16_IsValid())
		535	{ // check if wpl file is existing
513	killagreg	536	if(fexist_(filename))
495	killagreg	537	{ //file is existent
516	holgerb	538	if(!(overwride))
495	killagreg	539	{
		540	UART1_PutString("Error: file exist!\r\n");
		541	return(WPL_FILEEXIST);
516	holgerb	542	}
495	killagreg	543	}
513	killagreg	544	fp = fopen_(filename, 'w'); // try to open the file
490	killagreg	545	if(fp == NULL)
		546	{
		547	UART1_PutString("ERROR: Creating waypoint file!\r\n");
		548	return(retval);
		549	}
		550	// Create general section and key entries
		551	fputs_("[General]\r\n", fp);
516	holgerb	552	sprintf(wpline, "Name=%s\r\n", listname);
494	killagreg	553	fputs_(wpline, fp);
516	holgerb	554	sprintf(wpline, "FileVersion=%d\r\n", WP_FILE_VERSION_COMPATIBLE);
490	killagreg	555	fputs_(wpline, fp);
		556	sprintf(wpline, "NumberOfWaypoints=%d\r\n", PointCount);
		557	fputs_(wpline, fp);
		558	// dump all points if existent
		559	if(PointCount)
		560	{
		561	u8 i, u8_1;
		562	s32 i32_1, i32_2;
504	holgerb	563	NewWPL_Name = 1;
490	killagreg	564	for (i = 0; i < PointCount; i++)
		565	{
		566	sprintf(wpline, "[Point%d]\r\n",PointList[i].Index);
		567	fputs_(wpline, fp);
		568	// write latitude in deg
		569	if(PointList[i].Position.Latitude < 0) u8_1 = '-';
		570	else u8_1 = '+';
		571	i32_1 = abs(PointList[i].Position.Latitude)/10000000L;
		572	i32_2 = abs(PointList[i].Position.Latitude)%10000000L;
		573	sprintf(wpline, "Latitude=%c%ld.%07ld\r\n", u8_1, i32_1, i32_2);
		574	fputs_(wpline, fp);
		575	// write longitude in deg
		576	if(PointList[i].Position.Longitude < 0) u8_1 = '-';
		577	else u8_1 = '+';
		578	i32_1 = abs(PointList[i].Position.Longitude)/10000000L;
		579	i32_2 = abs(PointList[i].Position.Longitude)%10000000L;
		580	sprintf(wpline, "Longitude=%c%ld.%07ld\r\n", u8_1, i32_1, i32_2);
		581	fputs_(wpline, fp);
		582	// write tolerace radius in m
		583	sprintf(wpline, "Radius=%d\r\n", PointList[i].ToleranceRadius);
		584	fputs_(wpline, fp);
		585	// write altitude in m
		586	if(PointList[i].Position.Altitude < 0) u8_1 = '-';
		587	else u8_1 = '+';
		588	if(PointList[i].Type == POINT_TYPE_POI)
		589	{
		590	i32_1 = abs(PointList[i].Position.Altitude)/100L; // cm --> m
		591	i32_2 = abs(PointList[i].Position.Altitude)%100L;
		592	}
		593	else
		594	{
		595	i32_1 = abs(PointList[i].Position.Altitude)/10L; // dm --> m
		596	i32_2 = abs(PointList[i].Position.Altitude)%10L;
		597	}
		598	sprintf(wpline, "Altitude=%c%ld.%01ld\r\n", u8_1, i32_1, i32_2);
		599	fputs_(wpline, fp);
		600	// write climb rate in 0.1 m/s
		601	sprintf(wpline, "ClimbRate=%d\r\n", PointList[i].AltitudeRate);
		602	fputs_(wpline, fp);
		603	// write hold time in s
		604	sprintf(wpline, "DelayTime=%d\r\n", PointList[i].HoldTime);
		605	fputs_(wpline, fp);
		606	// write event channel value
		607	sprintf(wpline, "WP_Event_Channel_Value=%d\r\n", PointList[i].WP_EventChannelValue);
		608	fputs_(wpline, fp);
		609	// write heading in deg (0= nothing, neg. values index to poi)
		610	sprintf(wpline, "Heading=%d\r\n", PointList[i].Heading);
		611	fputs_(wpline, fp);
		612	// write speed in 0.1 m/s
		613	sprintf(wpline, "Speed=%d\r\n", PointList[i].Speed);
		614	fputs_(wpline, fp);
		615	// write cam angle in degree (255 -> POI-Automatic)
		616	sprintf(wpline, "CAM-Nick=%d\r\n", PointList[i].CamAngle);
		617	fputs_(wpline, fp);
		618	// write point type
		619	sprintf(wpline, "Type=%d\r\n", PointList[i].Type + 1);
516	holgerb	620	fputs_(wpline, fp);
		621	// write prefix
490	killagreg	622	sprintf(wpline, "Prefix=%s\r\n", PointList[i].Name);
		623	fputs_(wpline, fp);
642	holgerb	624	sprintf(wpline, "AutoTrigger=%d\r\n", PointList[i].AutoPhotoDistance);
		625	fputs_(wpline, fp);
490	killagreg	626	} // EOF loop over all points
		627	} // EOF if(PointCount)
		628	if(EOF == fclose_(fp))
		629	{
		630	UART1_PutString("failed!\r\n");
		631	}
		632	else
		633	{
		634	UART1_PutString("ok\r\n");
495	killagreg	635	retval = WPL_OK;
516	holgerb	636	}
490	killagreg	637	} // EOF if(Fat16_IsValid())
516	holgerb	638	else
497	killagreg	639	{
		640	UART1_PutString("no file system found!\r\n");
		641	retval = WPL_NO_SDCARD_FOUND;
		642	}
490	killagreg	643	return(retval);
		644	}
		645
519	holgerb	646	u8 PointList_Load(u8 * filename, u8* listname, u8 listnamelen, u8 use_preset_speed)
490	killagreg	647	{
		648	File_t *fp;
496	killagreg	649	s8 wpline[LINE_MAX];
		650	u8 retval = WPL_ERROR;
516	holgerb	651
490	killagreg	652	s8 name, value;
		653	u8 i;
		654
		655	u8 IsGeneralSection = 0;
		656	u8 IsPointSection = 0;
		657	u8 WPNumber = 0;
498	killagreg	658
		659	// clear point list first
		660	PointList_Clear();
517	holgerb	661	HeadingOld = -1; // updates the direction if the new direction is the same like last time
767	holgerb	662	UART1_PutString("\r\n Read ");
516	holgerb	663	UART1_PutString(filename);
490	killagreg	664	UART1_PutString("...");
		665
		666	if(Fat16_IsValid())
		667	{ // check if wpl file is existing
513	killagreg	668	fp = fopen_(filename, 'r'); // try to open the file
490	killagreg	669	if(fp == NULL)
		670	{
		671	UART1_PutString("ERROR: Reading waypoint file!\r\n");
496	killagreg	672	return(retval);
490	killagreg	673	}
		674	// read all lines from file
		675	while(fgets_(wpline, LINE_MAX, fp) != 0)
		676	{
		677	if ( // ignorelines starting with \r,\n,' ',';','#'
		678	(wpline[0] != '\n') &&
		679	(wpline[0] != '\r') &&
		680	(wpline[0] != ' ' ) &&
		681	(wpline[0] != ';' ) &&
		682	(wpline[0] != '#' )
		683	)
		684	{
		685	// check for section line found
		686	if(wpline[0] == '[')
		687	{
		688	// next section found
		689	IsGeneralSection = 0;
		690	IsPointSection = 0;
		691
		692	name = strtok(&wpline[1], "]");
		693	if(name != NULL) // if section name
		694	{
		695	// check section type
		696	for(i=0; name[i]; i++) name[i] = toupper(name[i]); // convert to upper case
		697
		698	if(strncmp(name, "POINT", 5) == 0)
		699	{
		700	IsPointSection = (u8)atoi(&name[5]);
		701	PointCount++;
		702	}
		703	else if(strcmp(name, "GENERAL") == 0)
		704	{
		705	IsGeneralSection = 1;
		706	}
		707	else
		708	{
		709	UART1_PutString("Unknown section: ");
		710	UART1_PutString(name);
		711	UART1_PutString("\r\n");
		712	}
		713	}
		714	} // EOF section line
		715	else
		716	{ // look for key entrys of each sections
		717	name = strtok(wpline, "="); // get name
		718	value = strtok(NULL, "="); // get value
		719	if ((name != NULL) && (value != NULL))
		720	{
		721	for(i=0; name[i]; i++) name[i] = toupper(name[i]); // convert to upper case
		722	if(IsPointSection && (IsPointSection <= WPNumber))
		723	{
		724	if(strcmp(name, "LATITUDE") == 0)
		725	{
		726	PointList[IsPointSection-1].Position.Latitude = (s32)(atof(value) * 1E7);
		727	}
		728	else if(strcmp(name, "LONGITUDE") == 0)
		729	{
		730	PointList[IsPointSection-1].Position.Longitude = (s32)(atof(value) * 1E7);
		731	}
		732	else if(strcmp(name, "RADIUS") == 0)
		733	{
		734	PointList[IsPointSection-1].ToleranceRadius = (u8)atoi(value);
		735	}
		736	else if(strcmp(name, "ALTITUDE") == 0)
		737	{
		738	PointList[IsPointSection-1].Position.Altitude = (s32)(atof(value) * 100.0); // in cm
		739	PointList[IsPointSection-1].Position.Status = NEWDATA;
		740	}
		741	else if(strcmp(name, "CLIMBRATE") == 0)
		742	{
		743	PointList[IsPointSection-1].AltitudeRate = (u8)atoi(value);
		744	}
		745	else if(strcmp(name, "DELAYTIME") == 0)
		746	{
		747	PointList[IsPointSection-1].HoldTime = (u8)atoi(value);
		748	}
		749	else if(strcmp(name, "WP_EVENT_CHANNEL_VALUE") == 0)
		750	{
		751	PointList[IsPointSection-1].WP_EventChannelValue = (u8)atoi(value);
		752	}
		753	else if(strcmp(name, "HEADING") == 0)
		754	{
		755	PointList[IsPointSection-1].Heading = (s16)atoi(value);
		756	}
		757	else if(strcmp(name, "SPEED") == 0)
		758	{
519	holgerb	759	if(use_preset_speed) PointList[IsPointSection-1].Speed = use_preset_speed;
		760	else PointList[IsPointSection-1].Speed = (u8)atoi(value);
490	killagreg	761	}
		762	else if(strcmp(name, "CAM-NICK") == 0)
		763	{
		764	PointList[IsPointSection-1].CamAngle = (u8)atoi(value);
		765	}
		766	else if(strcmp(name, "TYPE") == 0)
		767	{
		768	PointList[IsPointSection-1].Type = (u8)atoi(value);
		769	if(PointList[IsPointSection-1].Type > 0) PointList[IsPointSection-1].Type--; // index shift
		770	else PointList[IsPointSection-1].Type = POINT_TYPE_INVALID;
516	holgerb	771
490	killagreg	772	switch(PointList[IsPointSection-1].Type)
		773	{
785	holgerb	774	case POINT_TYPE_LAND:
		775	FsPointCnt++; // break fehlt absichtlich
490	killagreg	776	case POINT_TYPE_WP:
		777	// this works only if altitude key is set before point type key in WPL file !!
516	holgerb	778	PointList[IsPointSection-1].Position.Altitude /= 10; // dm only for WPs
490	killagreg	779	WPCount++;
		780	break;
601	holgerb	781	case POINT_TYPE_FS:
		782	FsPointCnt++;
		783	break;
490	killagreg	784	case POINT_TYPE_POI:
		785	POICount++;
		786	break;
802	holgerb	787	case POINT_TYPE_FLYZONE:
		788	FlyzonePointCnt++;
		789	break;
490	killagreg	790	}
		791	}
		792	else if(strcmp(name, "PREFIX") == 0)
		793	{
		794	strncpy(PointList[IsPointSection-1].Name, value, 4);
		795	PointList[IsPointSection-1].Name[3] = 0; // Terminate string
		796	}
642	holgerb	797	else if(strcmp(name, "AUTOTRIGGER") == 0)
		798	{
		799	PointList[IsPointSection-1].AutoPhotoDistance = (u8)atoi(value);
		800	}
490	killagreg	801	else
		802	{
		803	UART1_PutString("Unknown key: ");
		804	UART1_PutString(name);
		805	UART1_PutString("\r\n");
516	holgerb	806	}
490	killagreg	807	} // EOF point section
		808	else if(IsGeneralSection)
		809	{
		810	if(strcmp(name, "NUMBEROFWAYPOINTS") == 0)
516	holgerb	811	{
490	killagreg	812	WPNumber = (u8)atoi(value);
		813	if(!WPNumber) // no waypoints in file
		814	{
516	holgerb	815	return(WPL_NO_WAYPOINTS); // we are done here
490	killagreg	816	}
		817	else if(WPNumber > MAX_LIST_LEN) // number o points larger than ram list
		818	{
514	holgerb	819	UART1_PutString("To many points!");
497	killagreg	820	return(WPL_ERROR);
516	holgerb	821	}
490	killagreg	822	}
		823	else if (strcmp(name, "FILEVERSION") == 0)
		824	{
		825	if((u8)atoi(value) != WP_FILE_VERSION_COMPATIBLE)
		826	{
496	killagreg	827	PointList_Clear();
490	killagreg	828	UART1_PutString("Bad file version!\r\n");
516	holgerb	829	return(WPL_ERROR);
490	killagreg	830	}
		831	}
495	killagreg	832	else if (strcmp(name, "NAME") == 0)
		833	{
513	killagreg	834	if(listname)
498	killagreg	835	{
513	killagreg	836	u8 len = strlen(value);
		837	if(len)
		838	{
		839	if(value[len-1] == '\r')
		840	{
		841	value[len-1] = 0;
540	holgerb	842	// len--;
513	killagreg	843	}
		844	}
		845	if(len > listnamelen) len = listnamelen;
		846	if(len)
		847	{
		848	value[len-1] = 0; // terminate string
		849	if(listname) memcpy(listname, value, len);
		850	}
		851	NewWPL_Name = 1;
498	killagreg	852	}
495	killagreg	853	}
490	killagreg	854	else
		855	{
		856	UART1_PutString("Unknown key: ");
		857	UART1_PutString(name);
		858	UART1_PutString("\r\n");
		859	}
		860	} // EOF general section
		861	} // EOF valid key entry
		862	} // EOF key entry line
		863	} // valid line
		864	} // EOF loop over all lines
		865	fclose_(fp);
		866	NaviData.WaypointNumber = WPCount;
516	holgerb	867	retval = WPL_OK;
		868	UART1_PutString("ok\r\n");
490	killagreg	869	} // EOF if(Fat16_IsValid())
516	holgerb	870	else
497	killagreg	871	{
		872	UART1_PutString("no file system found!\r\n");
		873	retval = WPL_NO_SDCARD_FOUND;
516	holgerb	874	}
490	killagreg	875	return(retval);
491	killagreg	876	}
490	killagreg	877
513	killagreg	878	// load actual point list from SD card
		879	u8 PointList_ReadFromFile(WPL_Store_t * pWPL_Store)
		880	{
516	holgerb	881	u8 filename[30];
513	killagreg	882
		883	pWPL_Store->Name[0] = 0; // clear current list name
		884
		885	// user absolute path, i.e. leading /
		886	if(pWPL_Store->Index == 0) // index 0 looks for a default WPL file in the root
		887	{
516	holgerb	888	sprintf(filename, "/default.wpl");
513	killagreg	889	}
		890	else
		891	{
		892	sprintf(filename, "/WPL/list_%03d.wpl", pWPL_Store->Index);
		893	}
519	holgerb	894	return PointList_Load(filename, pWPL_Store->Name, sizeof(pWPL_Store->Name),0);
513	killagreg	895	}
		896
		897	// save actual point list to SD card
		898	u8 PointList_WriteToFile(WPL_Store_t * pWPL_Store)
		899	{
		900	u8 filename[30];
516	holgerb	901
		902
513	killagreg	903	if(PointCount == 0) return(WPL_NO_WAYPOINTS);
516	holgerb	904	// user absolute path, i.e. leading /
513	killagreg	905	if(pWPL_Store->Index == 0)
		906	{
		907	sprintf(filename, "/default.wpl");
		908	}
		909	else
		910	{
		911	sprintf(filename, "/WPL/list_%03d.wpl", pWPL_Store->Index);
		912	}
		913	return PointList_Save(filename, pWPL_Store->Name, pWPL_Store->OverwriteFile);
		914	}
		915
		916
516	holgerb	917	// save actual gps positiin and heading to file
513	killagreg	918	u8 PointList_SaveSinglePoint(WPL_Store_t * pWPL_Store)
		919	{
		920	u8 retval = WPL_ERROR;
		921	u8 filename[30];
		922	Point_t WP;
		923
767	holgerb	924	UART1_PutString("\r\n write single point\r\n");
516	holgerb	925	if(GPSData.Position.Status == INVALID)
		926	{
767	holgerb	927	UART1_PutString("ERROR: No GPS - Fix\r\n");
514	holgerb	928	return(retval);
516	holgerb	929	}
513	killagreg	930
		931	// clear current point list
		932	PointList_Clear();
		933	// prepare WP at current position
519	holgerb	934
737	holgerb	935	if(NCFlags & NC_FLAG_FREE \|\| NaviData.TargetPositionDeviation.Distance_dm > 7*10)
519	holgerb	936	{ // take actual position
		937	GPSPos_Copy(&GPSData.Position, &(WP.Position));
		938	}
		939	else
		940	{ // take last target position
		941	GPSPos_Copy(&NaviData.TargetPosition, &(WP.Position));
		942	}
		943
513	killagreg	944	GPSPos_Copy(&GPSData.Position, &(WP.Position));
		945	// set heading
545	holgerb	946	WP.Heading = (CompassSetpointCorrected/10 + Parameter.CamOrientation * 15) % 360;
513	killagreg	947	if(WP.Heading == 0) WP.Heading = 360;
519	holgerb	948	WP.ToleranceRadius = 120; // 12m
		949	WP.HoldTime = 2;
513	killagreg	950	WP.Index = 1;
		951	WP.Type = POINT_TYPE_WP;
695	holgerb	952	WP.WP_EventChannelValue = 100;
576	holgerb	953	if(FC.StatusFlags & FC_STATUS_FLY && ((FromFC_VarioCharacter != ' ') \|\| (SimulationFlags & SIMULATION_ACTIVE))) // only in flight and if the Altitude control is enabled
516	holgerb	954	{
519	holgerb	955	WP.AltitudeRate = 255; // Auto
		956	WP.Position.Altitude = NaviData.SetpointAltitude / 2;
516	holgerb	957	}
		958	else
514	holgerb	959	{
516	holgerb	960	WP.AltitudeRate = 0;
		961	WP.Position.Altitude = 0;
514	holgerb	962	}
519	holgerb	963	WP.Speed = 50; // beim Laden wird der Wert nochmal neu gesetzt
513	killagreg	964	WP.CamAngle = 0;
521	holgerb	965	WP.Name[0] = 'S';
		966	WP.Name[1] = 'P';
		967	WP.Name[2] = 'T';
		968	WP.Name[3] = 0;
513	killagreg	969	// add this point to wp list
		970	PointList_SetAt(&WP);
514	holgerb	971
		972	sprintf(filename, "/POINT/POINT%03d.wpl", pWPL_Store->Index);
516	holgerb	973	UART1_PutString(filename);
514	holgerb	974	sprintf(pWPL_Store->Name, "POINT%03d ", pWPL_Store->Index);
513	killagreg	975	retval = PointList_Save(filename, pWPL_Store->Name, 1);
514	holgerb	976
		977	// clear current point list
519	holgerb	978	if((NC_GPS_ModeCharacter != 'w') && (NC_GPS_ModeCharacter != 'W')) PointList_Clear();
549	holgerb	979	else // es ist gearde ein Wegpunktflug aktiv -> updaten
519	holgerb	980	{
		981	if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE);
785	holgerb	982	GPS_pWaypoint = PointList_WPBegin(0); // updates POI index
519	holgerb	983	}
		984
513	killagreg	985	return(retval);
		986	}
		987	// load target gps posititon and heading from file
		988	u8 PointList_LoadSinglePoint(WPL_Store_t * pWPL_Store)
		989	{
519	holgerb	990	u8 filename[30], result = 0;
516	holgerb	991
514	holgerb	992	sprintf(filename, "/POINT/POINT%03d.wpl", pWPL_Store->Index);
513	killagreg	993	pWPL_Store->Name[0] = 0; // clear current list name
519	holgerb	994	result = PointList_Load(filename, pWPL_Store->Name, sizeof(pWPL_Store->Name),Parameter.SingleWpSpeed);
		995	if(result) UART1_Request_ReadPoint = 1; // Sends Point 1 to the PC
		996	return(result);
513	killagreg	997	}
		998
		999
516	holgerb	1000
504	holgerb	1001	void ClearWLP_Name(void)
		1002	{
513	killagreg	1003	u8 i;
		1004	for(i=0; i<sizeof(WPL_Store.Name);i++) WPL_Store.Name[i] = 0;
		1005	NewWPL_Name = 1;
504	holgerb	1006	}
513	killagreg	1007
491	killagreg	1008	// move actual point list to ref pos., the point in the list marked by index gets the RefPos afterwards
503	holgerb	1009	u8 PointList_Move(u8 RefIndex, GPS_Pos_t* pRefPos, u16 RotationAngle)
491	killagreg	1010	{
		1011	u8 retval = 0;
513	killagreg	1012	s32 altitude;
		1013	GPS_Pos_t OldRefPos;
491	killagreg	1014	GPS_Pos_Deviation_t RefDeviation;
516	holgerb	1015
491	killagreg	1016	// check inputs for plausibility;
516	holgerb	1017	if((RefIndex == 0) \|\| (RefIndex > PointCount)) return(retval);
491	killagreg	1018	if(pRefPos == NULL) return(retval);
		1019	if(pRefPos->Status == INVALID) return(retval);
		1020
513	killagreg	1021	if(GPSPos_Copy(&(PointList[RefIndex-1].Position), &OldRefPos)) // backup old reference position
491	killagreg	1022	{
		1023	u8 i;
513	killagreg	1024	// iterate the position list
491	killagreg	1025	for(i = 0; i < PointCount; i++)
		1026	{
		1027	retval = 0;
513	killagreg	1028	// backup altitude of this point
		1029	altitude = PointList[i].Position.Altitude;
		1030	// calculate deviation form old ref, i.e the north and east shift of each point in the list from the reference position
		1031	if(!GPSPos_Deviation(&(PointList[i].Position), &OldRefPos, &RefDeviation)) break;
491	killagreg	1032	// copy of the new reference position into this list place
		1033	if(!GPSPos_Copy(pRefPos, &(PointList[i].Position))) break;
516	holgerb	1034	// restore former altitude
513	killagreg	1035	PointList[i].Position.Altitude = altitude;
491	killagreg	1036	// move new reference according to the deviation of the old reference
513	killagreg	1037	if(RotationAngle > 0)
		1038	{
737	holgerb	1039	retval = GPSPos_ShiftGeodetic(&(PointList[i].Position), (RefDeviation.Bearing + 180 + RotationAngle)%360, RefDeviation.Distance_cm );
513	killagreg	1040	// Now rotate the heading positions if they are fixed angles
		1041	if(PointList[i].Heading >= 0 && PointList[i].Heading <= 360) PointList[i].Heading = (PointList[i].Heading + RotationAngle) % 360;
		1042	}
		1043	else // no rotation
		1044	{
		1045	// move new reference according to the deviation of the old reference
		1046	retval = GPSPos_ShiftCartesian(&(PointList[i].Position), RefDeviation.North, RefDeviation.East);
		1047	}
516	holgerb	1048	if(!retval) break;
491	killagreg	1049	}
		1050	} // else ref pos old not copied!
		1051	if(!retval) PointList_Clear();
		1052	return(retval);
		1053	}
		1054
516	holgerb	1055
491	killagreg	1056

Subversion Repositories NaviCtrl

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