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

Subversion Repositories NaviCtrl

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