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

Subversion Repositories NaviCtrl

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