WebSVN - NaviCtrl - Blame - Rev 496 - /tags/V2.10a/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
		10	// + Mikrocontroller verwendete Firmware f�r die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool
		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
		21	// + Schadens. Die gesetzliche Haftung bei Personensch�den und nach dem Produkthaftungsgesetz bleibt unber�hrt. Dem Lizenzgeber steht jedoch der Einwand
		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 -
		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
		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>
		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"
496	killagreg	64	#include "spi_slave.h"
41	ingob	65
496	killagreg	66
		67	WPL_Store_t WPL_Store;
		68
41	ingob	69	// the waypoints list
351	holgerb	70	#define MAX_LIST_LEN 101
227	killagreg	71
280	killagreg	72	Point_t PointList[MAX_LIST_LEN];
		73	u8 WPIndex = 0; // list index of GPS point representig the current WP, can be maximal WPCount
		74	u8 POIIndex = 0; // list index of GPS Point representing the current POI, can be maximal WPCount
		75	u8 WPCount = 0; // number of waypoints
491	killagreg	76	u8 PointCount = 0; // number of points in the list can be maximal equal to MAX_LIST_LEN
		77	u8 POICount = 0; // number of point of interest in the list
41	ingob	78
296	killagreg	79	u8 WPActive = FALSE;
280	killagreg	80
		81	u8 PointList_Init(void)
41	ingob	82	{
280	killagreg	83	return PointList_Clear();
41	ingob	84	}
		85
280	killagreg	86	u8 PointList_Clear(void)
41	ingob	87	{
		88	u8 i;
227	killagreg	89	WPIndex = 0; // real list position are 1 ,2, 3 ...
		90	POIIndex = 0; // real list position are 1 ,2, 3 ...
280	killagreg	91	WPCount = 0; // no waypoints
283	holgerb	92	POICount = 0;
280	killagreg	93	PointCount = 0; // no contents
296	killagreg	94	WPActive = FALSE;
280	killagreg	95	NaviData.WaypointNumber = WPCount;
		96	NaviData.WaypointIndex = 0;
41	ingob	97
280	killagreg	98	for(i = 0; i < MAX_LIST_LEN; i++)
41	ingob	99	{
280	killagreg	100	PointList[i].Position.Status = INVALID;
		101	PointList[i].Position.Latitude = 0;
		102	PointList[i].Position.Longitude = 0;
		103	PointList[i].Position.Altitude = 0;
		104	PointList[i].Heading = 361; // invalid value
		105	PointList[i].ToleranceRadius = 0; // in meters, if the MK is within that range around the target, then the next target is triggered
		106	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	107	PointList[i].Event_Flag = 0; // future implementation
		108	PointList[i].Index = 0;
286	killagreg	109	PointList[i].Type = POINT_TYPE_INVALID;
490	killagreg	110	PointList[i].WP_EventChannelValue = 0;
286	killagreg	111	PointList[i].AltitudeRate = 0; // no change of setpoint
490	killagreg	112	PointList[i].Speed = 0;
		113	PointList[i].CamAngle = 0;
		114	PointList[i].Name[0] = 0;
41	ingob	115	}
		116	return TRUE;
		117	}
		118
280	killagreg	119	u8 PointList_GetCount(void)
41	ingob	120	{
280	killagreg	121	return PointCount; // number of points in the list
41	ingob	122	}
		123
295	killagreg	124	Point_t* PointList_GetAt(u8 index)
41	ingob	125	{
295	killagreg	126	if((index > 0) && (index <= PointCount)) return(&(PointList[index-1])); // return pointer to this waypoint
		127	else return(NULL);
		128	}
		129
		130	u8 PointList_SetAt(Point_t* pPoint)
		131	{
		132	// if index is in range
296	killagreg	133	if((pPoint->Index > 0) && (pPoint->Index <= MAX_LIST_LEN))
41	ingob	134	{
295	killagreg	135	// check list entry before update
		136	switch(PointList[pPoint->Index-1].Type)
277	killagreg	137	{
295	killagreg	138	case POINT_TYPE_INVALID: // was invalid
		139	switch(pPoint->Type)
		140	{
		141	default:
		142	case POINT_TYPE_INVALID:
		143	// nothing to do
		144	break;
		145
		146	case POINT_TYPE_WP:
		147	WPCount++;
		148	PointCount++;
		149	break;
		150
		151	case POINT_TYPE_POI:
		152	POICount++;
		153	PointCount++;
		154	break;
		155	}
		156	break;
277	killagreg	157
490	killagreg	158	case POINT_TYPE_WP: // was a waypoint
295	killagreg	159	switch(pPoint->Type)
		160	{
		161	case POINT_TYPE_INVALID:
		162	WPCount--;
		163	PointCount--;
		164	break;
277	killagreg	165
295	killagreg	166	default:
		167	case POINT_TYPE_WP:
		168	//nothing to do
		169	break;
		170
		171	case POINT_TYPE_POI:
		172	POICount++;
		173	WPCount--;
		174	break;
		175	}
		176	break;
		177
		178	case POINT_TYPE_POI: // was a poi
		179	switch(pPoint->Type)
		180	{
		181	case POINT_TYPE_INVALID:
		182	POICount--;
		183	PointCount--;
		184	break;
		185
		186	case POINT_TYPE_WP:
		187	WPCount++;
		188	POICount--;
		189	break;
		190
		191	case POINT_TYPE_POI:
		192	default:
		193	// nothing to do
		194	break;
		195	}
		196	break;
277	killagreg	197	}
295	killagreg	198	memcpy(&PointList[pPoint->Index-1], pPoint, sizeof(Point_t)); // copy data to list entry
		199	NaviData.WaypointNumber = WPCount;
		200	return pPoint->Index;
41	ingob	201	}
296	killagreg	202	else return(0);
41	ingob	203	}
		204
227	killagreg	205	// returns the pointer to the first waypoint within the list
280	killagreg	206	Point_t* PointList_WPBegin(void)
41	ingob	207	{
277	killagreg	208	u8 i;
227	killagreg	209	WPIndex = 0; // set list position invalid
295	killagreg	210
283	holgerb	211	if(WPActive == FALSE) return(NULL);
295	killagreg	212
277	killagreg	213	POIIndex = 0; // set invalid POI
280	killagreg	214	if(PointCount > 0)
152	killagreg	215	{
225	killagreg	216	// search for first wp in list
295	killagreg	217	for(i = 0; i <MAX_LIST_LEN; i++)
225	killagreg	218	{
280	killagreg	219	if((PointList[i].Type == POINT_TYPE_WP) && (PointList[i].Position.Status != INVALID))
225	killagreg	220	{
227	killagreg	221	WPIndex = i + 1;
225	killagreg	222	break;
		223	}
		224	}
277	killagreg	225	if(WPIndex) // found a WP in the list
		226	{
280	killagreg	227	NaviData.WaypointIndex = 1;
277	killagreg	228	// update index to POI
280	killagreg	229	if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
296	killagreg	230	else POIIndex = 0;
277	killagreg	231	}
		232	else // some points in the list but no WP found
		233	{
280	killagreg	234	NaviData.WaypointIndex = 0;
277	killagreg	235	//Check for an existing POI
295	killagreg	236	for(i = 0; i < MAX_LIST_LEN; i++)
277	killagreg	237	{
280	killagreg	238	if((PointList[i].Type == POINT_TYPE_POI) && (PointList[i].Position.Status != INVALID))
277	killagreg	239	{
		240	POIIndex = i + 1;
		241	break;
		242	}
		243	}
		244	}
152	killagreg	245	}
277	killagreg	246	else // no point in the list
152	killagreg	247	{
277	killagreg	248	POIIndex = 0;
295	killagreg	249	NaviData.WaypointIndex = 0;
227	killagreg	250	}
295	killagreg	251
		252	if(WPIndex) return(&(PointList[WPIndex-1]));
		253	else return(NULL);
41	ingob	254	}
		255
225	killagreg	256	// returns the last waypoint
280	killagreg	257	Point_t* PointList_WPEnd(void)
151	killagreg	258	{
277	killagreg	259
		260	u8 i;
227	killagreg	261	WPIndex = 0; // set list position invalid
277	killagreg	262	POIIndex = 0; // set invalid
283	holgerb	263
280	killagreg	264	if(WPActive == FALSE) return(NULL);
277	killagreg	265
280	killagreg	266	if(PointCount > 0)
151	killagreg	267	{
227	killagreg	268	// search backward!
295	killagreg	269	for(i = 1; i <= MAX_LIST_LEN; i++)
225	killagreg	270	{
295	killagreg	271	if((PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_WP) && (PointList[MAX_LIST_LEN - i].Position.Status != INVALID))
225	killagreg	272	{
295	killagreg	273	WPIndex = MAX_LIST_LEN - i + 1;
225	killagreg	274	break;
		275	}
		276	}
277	killagreg	277	if(WPIndex) // found a WP within the list
		278	{
280	killagreg	279	NaviData.WaypointIndex = WPCount;
		280	if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
277	killagreg	281	else POIIndex = 0;
		282	}
278	killagreg	283	else // list contains some points but no WP in the list
277	killagreg	284	{
		285	// search backward for a POI!
295	killagreg	286	for(i = 1; i <= MAX_LIST_LEN; i++)
277	killagreg	287	{
295	killagreg	288	if((PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_POI) && (PointList[MAX_LIST_LEN - i].Position.Status != INVALID))
277	killagreg	289	{
295	killagreg	290	POIIndex = MAX_LIST_LEN - i + 1;
277	killagreg	291	break;
		292	}
		293	}
295	killagreg	294	NaviData.WaypointIndex = 0;
277	killagreg	295	}
151	killagreg	296	}
277	killagreg	297	else // no point in the list
227	killagreg	298	{
280	killagreg	299	POIIndex = 0;
		300	NaviData.WaypointIndex = 0;
227	killagreg	301	}
295	killagreg	302	if(WPIndex) return(&(PointList[WPIndex-1]));
		303	else return(NULL);
151	killagreg	304	}
		305
41	ingob	306	// returns a pointer to the next waypoint or NULL if the end of the list has been reached
280	killagreg	307	Point_t* PointList_WPNext(void)
41	ingob	308	{
227	killagreg	309	u8 wp_found = 0;
280	killagreg	310	if(WPActive == FALSE) return(NULL);
227	killagreg	311
295	killagreg	312	if(WPIndex < MAX_LIST_LEN) // if there is a next entry in the list
41	ingob	313	{
227	killagreg	314	u8 i;
295	killagreg	315	for(i = WPIndex; i < MAX_LIST_LEN; i++) // start search for next at next list entry
225	killagreg	316	{
280	killagreg	317	if((PointList[i].Type == POINT_TYPE_WP) && (PointList[i].Position.Status != INVALID)) // jump over POIs
225	killagreg	318	{
227	killagreg	319	wp_found = i+1;
225	killagreg	320	break;
		321	}
		322	}
41	ingob	323	}
227	killagreg	324	if(wp_found)
		325	{
		326	WPIndex = wp_found; // update list position
280	killagreg	327	NaviData.WaypointIndex++;
		328	if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
227	killagreg	329	else POIIndex = 0;
280	killagreg	330	return(&(PointList[WPIndex-1])); // return pointer to this waypoint
227	killagreg	331	}
277	killagreg	332	else
280	killagreg	333	{ // no next wp found
		334	NaviData.WaypointIndex = 0;
277	killagreg	335	POIIndex = 0;
		336	return(NULL);
		337	}
41	ingob	338	}
280	killagreg	339
		340	void PointList_WPActive(u8 set)
		341	{
295	killagreg	342	if(set)
		343	{
		344	WPActive = TRUE;
491	killagreg	345	PointList_WPBegin(); // updates POI index
295	killagreg	346	}
		347	else
		348	{
		349	WPActive = FALSE;
		350	POIIndex = 0; // disable POI also
		351	}
280	killagreg	352	}
41	ingob	353
280	killagreg	354	Point_t* PointList_GetPOI(void)
224	killagreg	355	{
280	killagreg	356	return PointList_GetAt(POIIndex);
224	killagreg	357	}
		358
490	killagreg	359	#define LINE_MAX 70
		360	#define WP_FILE_VERSION_COMPATIBLE 3
		361	// save actual point list to SD card
495	killagreg	362	u8 PointList_SaveToFile(WPL_Store_t * pWPL_Store)
490	killagreg	363	{
		364	File_t *fp;
495	killagreg	365	s8 wpline[LINE_MAX];
		366	u8 retval = WPL_ERROR;
		367
490	killagreg	368
496	killagreg	369	// user absolute path, i.e. leading /
		370	if(pWPL_Store->Index == 0)
		371	{
		372	sprintf(wpline, "/default.wpl");
		373	}
		374	else
		375	{
		376	sprintf(wpline, "/WPL/list_%03d.wpl", pWPL_Store->Index);
		377	}
490	killagreg	378	UART1_PutString("\n\r Save WPL...");
		379
		380	if(Fat16_IsValid())
		381	{ // check if wpl file is existing
495	killagreg	382	if(fexist_(wpline))
		383	{ //file is existent
		384	if(!(pWPL_Store->OverwriteFile))
		385	{
		386	UART1_PutString("Error: file exist!\r\n");
		387	return(WPL_FILEEXIST);
		388	}
		389	}
490	killagreg	390	fp = fopen_(wpline, 'w'); // try to open the file
		391	if(fp == NULL)
		392	{
		393	UART1_PutString("ERROR: Creating waypoint file!\r\n");
		394	return(retval);
		395	}
		396	// Create general section and key entries
		397	fputs_("[General]\r\n", fp);
495	killagreg	398	sprintf(wpline, "Name=%s\r\n", pWPL_Store->Name);
494	killagreg	399	fputs_(wpline, fp);
495	killagreg	400	sprintf(wpline, "PositionType=%d\r\n", pWPL_Store->Type);
		401	fputs_(wpline, fp);
490	killagreg	402	sprintf(wpline, "FileVersion=%d\r\n", WP_FILE_VERSION_COMPATIBLE);
		403	fputs_(wpline, fp);
		404	sprintf(wpline, "NumberOfWaypoints=%d\r\n", PointCount);
		405	fputs_(wpline, fp);
		406	// dump all points if existent
		407	if(PointCount)
		408	{
		409	u8 i, u8_1;
		410	s32 i32_1, i32_2;
		411
		412	for (i = 0; i < PointCount; i++)
		413	{
		414	sprintf(wpline, "[Point%d]\r\n",PointList[i].Index);
		415	fputs_(wpline, fp);
		416	// write latitude in deg
		417	if(PointList[i].Position.Latitude < 0) u8_1 = '-';
		418	else u8_1 = '+';
		419	i32_1 = abs(PointList[i].Position.Latitude)/10000000L;
		420	i32_2 = abs(PointList[i].Position.Latitude)%10000000L;
		421	sprintf(wpline, "Latitude=%c%ld.%07ld\r\n", u8_1, i32_1, i32_2);
		422	fputs_(wpline, fp);
		423	// write longitude in deg
		424	if(PointList[i].Position.Longitude < 0) u8_1 = '-';
		425	else u8_1 = '+';
		426	i32_1 = abs(PointList[i].Position.Longitude)/10000000L;
		427	i32_2 = abs(PointList[i].Position.Longitude)%10000000L;
		428	sprintf(wpline, "Longitude=%c%ld.%07ld\r\n", u8_1, i32_1, i32_2);
		429	fputs_(wpline, fp);
		430	// write tolerace radius in m
		431	sprintf(wpline, "Radius=%d\r\n", PointList[i].ToleranceRadius);
		432	fputs_(wpline, fp);
		433	// write altitude in m
		434	if(PointList[i].Position.Altitude < 0) u8_1 = '-';
		435	else u8_1 = '+';
		436	if(PointList[i].Type == POINT_TYPE_POI)
		437	{
		438	i32_1 = abs(PointList[i].Position.Altitude)/100L; // cm --> m
		439	i32_2 = abs(PointList[i].Position.Altitude)%100L;
		440	}
		441	else
		442	{
		443	i32_1 = abs(PointList[i].Position.Altitude)/10L; // dm --> m
		444	i32_2 = abs(PointList[i].Position.Altitude)%10L;
		445	}
		446	sprintf(wpline, "Altitude=%c%ld.%01ld\r\n", u8_1, i32_1, i32_2);
		447	fputs_(wpline, fp);
		448	// write climb rate in 0.1 m/s
		449	sprintf(wpline, "ClimbRate=%d\r\n", PointList[i].AltitudeRate);
		450	fputs_(wpline, fp);
		451	// write hold time in s
		452	sprintf(wpline, "DelayTime=%d\r\n", PointList[i].HoldTime);
		453	fputs_(wpline, fp);
		454	// write event channel value
		455	sprintf(wpline, "WP_Event_Channel_Value=%d\r\n", PointList[i].WP_EventChannelValue);
		456	fputs_(wpline, fp);
		457	// write heading in deg (0= nothing, neg. values index to poi)
		458	sprintf(wpline, "Heading=%d\r\n", PointList[i].Heading);
		459	fputs_(wpline, fp);
		460	// write speed in 0.1 m/s
		461	sprintf(wpline, "Speed=%d\r\n", PointList[i].Speed);
		462	fputs_(wpline, fp);
		463	// write cam angle in degree (255 -> POI-Automatic)
		464	sprintf(wpline, "CAM-Nick=%d\r\n", PointList[i].CamAngle);
		465	fputs_(wpline, fp);
		466	// write point type
		467	sprintf(wpline, "Type=%d\r\n", PointList[i].Type + 1);
		468	fputs_(wpline, fp);
		469	// write prefix
		470	//if(PointList[i].Type == POINT_TYPE_WP) u8_1 = 'P';
		471	//else u8_1 = '0';
		472	sprintf(wpline, "Prefix=%s\r\n", PointList[i].Name);
		473	fputs_(wpline, fp);
		474	} // EOF loop over all points
		475	} // EOF if(PointCount)
		476	if(EOF == fclose_(fp))
		477	{
		478	UART1_PutString("failed!\r\n");
		479	}
		480	else
		481	{
		482	UART1_PutString("ok\r\n");
495	killagreg	483	retval = WPL_OK;
490	killagreg	484	}
		485	} // EOF if(Fat16_IsValid())
492	killagreg	486	else UART1_PutString("no file system found!\r\n");
490	killagreg	487	return(retval);
		488	}
		489
		490	// load actual point list from SD card
495	killagreg	491	u8 PointList_ReadFromFile(WPL_Store_t * pWPL_Store)
490	killagreg	492	{
		493	File_t *fp;
496	killagreg	494	s8 wpline[LINE_MAX];
		495	u8 retval = WPL_ERROR;
490	killagreg	496
		497	s8 name, value;
		498	u8 i;
		499
		500	u8 IsGeneralSection = 0;
		501	u8 IsPointSection = 0;
		502	u8 WPNumber = 0;
		503
		504	// user absolute path, i.e. leading /
496	killagreg	505	if(pWPL_Store->Index == 0) // index 0 looks for a default WPL file in the root
		506	{
		507	sprintf(wpline, "/default.wpl");
		508	}
		509	else
		510	{
		511	sprintf(wpline, "/WPL/list_%03d.wpl", pWPL_Store->Index);
		512	}
490	killagreg	513
		514	UART1_PutString("\n\r Read ");
		515	UART1_PutString(wpline);
		516	UART1_PutString("...");
		517
		518	if(Fat16_IsValid())
		519	{ // check if wpl file is existing
		520	fp = fopen_(wpline, 'r'); // try to open the file
		521	if(fp == NULL)
		522	{
		523	UART1_PutString("ERROR: Reading waypoint file!\r\n");
496	killagreg	524	return(retval);
490	killagreg	525	}
		526	// clear point list first
		527	PointList_Clear();
		528	// read all lines from file
		529	while(fgets_(wpline, LINE_MAX, fp) != 0)
		530	{
		531	if ( // ignorelines starting with \r,\n,' ',';','#'
		532	(wpline[0] != '\n') &&
		533	(wpline[0] != '\r') &&
		534	(wpline[0] != ' ' ) &&
		535	(wpline[0] != ';' ) &&
		536	(wpline[0] != '#' )
		537	)
		538	{
		539	// check for section line found
		540	if(wpline[0] == '[')
		541	{
		542	// next section found
		543	IsGeneralSection = 0;
		544	IsPointSection = 0;
		545
		546	name = strtok(&wpline[1], "]");
		547	if(name != NULL) // if section name
		548	{
		549	// check section type
		550	for(i=0; name[i]; i++) name[i] = toupper(name[i]); // convert to upper case
		551
		552	if(strncmp(name, "POINT", 5) == 0)
		553	{
		554	IsPointSection = (u8)atoi(&name[5]);
		555	PointCount++;
		556	}
		557	else if(strcmp(name, "GENERAL") == 0)
		558	{
		559	IsGeneralSection = 1;
		560	}
		561	else
		562	{
		563	UART1_PutString("Unknown section: ");
		564	UART1_PutString(name);
		565	UART1_PutString("\r\n");
		566	}
		567	}
		568	} // EOF section line
		569	else
		570	{ // look for key entrys of each sections
		571	name = strtok(wpline, "="); // get name
		572	value = strtok(NULL, "="); // get value
		573	if ((name != NULL) && (value != NULL))
		574	{
		575	for(i=0; name[i]; i++) name[i] = toupper(name[i]); // convert to upper case
		576	if(IsPointSection && (IsPointSection <= WPNumber))
		577	{
		578	if(strcmp(name, "LATITUDE") == 0)
		579	{
		580	PointList[IsPointSection-1].Position.Latitude = (s32)(atof(value) * 1E7);
		581	}
		582	else if(strcmp(name, "LONGITUDE") == 0)
		583	{
		584	PointList[IsPointSection-1].Position.Longitude = (s32)(atof(value) * 1E7);
		585	}
		586	else if(strcmp(name, "RADIUS") == 0)
		587	{
		588	PointList[IsPointSection-1].ToleranceRadius = (u8)atoi(value);
		589	}
		590	else if(strcmp(name, "ALTITUDE") == 0)
		591	{
		592	PointList[IsPointSection-1].Position.Altitude = (s32)(atof(value) * 100.0); // in cm
		593	PointList[IsPointSection-1].Position.Status = NEWDATA;
		594	}
		595	else if(strcmp(name, "CLIMBRATE") == 0)
		596	{
		597	PointList[IsPointSection-1].AltitudeRate = (u8)atoi(value);
		598	}
		599	else if(strcmp(name, "DELAYTIME") == 0)
		600	{
		601	PointList[IsPointSection-1].HoldTime = (u8)atoi(value);
		602	}
		603	else if(strcmp(name, "WP_EVENT_CHANNEL_VALUE") == 0)
		604	{
		605	PointList[IsPointSection-1].WP_EventChannelValue = (u8)atoi(value);
		606	}
		607	else if(strcmp(name, "HEADING") == 0)
		608	{
		609	PointList[IsPointSection-1].Heading = (s16)atoi(value);
		610	}
		611	else if(strcmp(name, "SPEED") == 0)
		612	{
		613	PointList[IsPointSection-1].Speed = (u8)atoi(value);
		614	}
		615	else if(strcmp(name, "CAM-NICK") == 0)
		616	{
		617	PointList[IsPointSection-1].CamAngle = (u8)atoi(value);
		618	}
		619	else if(strcmp(name, "TYPE") == 0)
		620	{
		621	PointList[IsPointSection-1].Type = (u8)atoi(value);
		622	if(PointList[IsPointSection-1].Type > 0) PointList[IsPointSection-1].Type--; // index shift
		623	else PointList[IsPointSection-1].Type = POINT_TYPE_INVALID;
		624
		625	switch(PointList[IsPointSection-1].Type)
		626	{
		627	case POINT_TYPE_WP:
		628	// this works only if altitude key is set before point type key in WPL file !!
		629	PointList[IsPointSection-1].Position.Altitude /= 10; // dm only for WPs
		630	WPCount++;
		631	break;
		632
		633	case POINT_TYPE_POI:
		634	POICount++;
		635	break;
		636	}
		637	}
		638	else if(strcmp(name, "PREFIX") == 0)
		639	{
		640	strncpy(PointList[IsPointSection-1].Name, value, 4);
		641	PointList[IsPointSection-1].Name[3] = 0; // Terminate string
		642	}
		643	else
		644	{
		645	UART1_PutString("Unknown key: ");
		646	UART1_PutString(name);
		647	UART1_PutString("\r\n");
		648	}
		649	} // EOF point section
		650	else if(IsGeneralSection)
		651	{
		652	if(strcmp(name, "NUMBEROFWAYPOINTS") == 0)
		653	{
		654	WPNumber = (u8)atoi(value);
		655	if(!WPNumber) // no waypoints in file
		656	{
		657	return(1); // we are done here
		658	}
		659	else if(WPNumber > MAX_LIST_LEN) // number o points larger than ram list
		660	{
		661	UART1_PutString("To much points!");
		662	return(0);
		663	}
		664	}
		665	else if (strcmp(name, "FILEVERSION") == 0)
		666	{
		667	if((u8)atoi(value) != WP_FILE_VERSION_COMPATIBLE)
		668	{
496	killagreg	669	PointList_Clear();
490	killagreg	670	UART1_PutString("Bad file version!\r\n");
496	killagreg	671	return(WPL_ERROR);
490	killagreg	672	}
		673	}
495	killagreg	674	else if (strcmp(name, "NAME") == 0)
		675	{
		676	u8 len = strlen(value);
		677	if(len > 11)
		678	value[11] = 0;
		679	memcpy(pWPL_Store->Name, value, 12);
		680	}
		681	else if (strcmp(name, "POSITIONTYPE") == 0)
		682	{
		683	pWPL_Store->Type = (u8)atoi(value);
		684	}
490	killagreg	685	else
		686	{
		687	UART1_PutString("Unknown key: ");
		688	UART1_PutString(name);
		689	UART1_PutString("\r\n");
		690	}
		691	} // EOF general section
		692	} // EOF valid key entry
		693	} // EOF key entry line
		694	} // valid line
		695	} // EOF loop over all lines
		696	fclose_(fp);
		697	NaviData.WaypointNumber = WPCount;
496	killagreg	698	// if the WPS data are relative to home position
		699	if(pWPL_Store->Type == WPL_STORE_TYPE_REL)
		700	{ // try to shift the wp list
		701	if(PointList_Move(1, &(NaviData.HomePosition))) retval = WPL_OK;
		702	else
		703	{
		704	PointList_Clear(); // to avoid wrong absolute positions
		705	UART1_PutString("no reference position!\r\n");
		706	}
		707
		708	}
		709	else
		710	{ // nothing to do
		711	retval = WPL_OK;
		712	}
		713	if(retval == WPL_OK) UART1_PutString("ok\r\n");
490	killagreg	714	} // EOF if(Fat16_IsValid())
		715	else UART1_PutString("no file system found!\r\n");
		716	return(retval);
491	killagreg	717	}
490	killagreg	718
491	killagreg	719	// move actual point list to ref pos., the point in the list marked by index gets the RefPos afterwards
		720	u8 PointList_Move(u8 RefIndex, GPS_Pos_t* pRefPos)
		721	{
		722	u8 retval = 0;
		723	GPS_Pos_t RefPos_old;
		724	GPS_Pos_Deviation_t RefDeviation;
		725
		726	// check inputs for plausibility;
		727	if((RefIndex == 0) \|\| (RefIndex > PointCount)) return(retval);
		728	if(pRefPos == NULL) return(retval);
		729	if(pRefPos->Status == INVALID) return(retval);
		730
		731	// try to copy the old reference in point list to a local buffer
		732	if(GPSPos_Copy(&(PointList[RefIndex-1].Position), &RefPos_old))
		733	{
		734	u8 i;
		735	// for each point position in the list
		736	for(i = 0; i < PointCount; i++)
		737	{
		738	retval = 0;
		739	// calculate deviation form old ref, i.e the north and east shift of each point in the list from the reference position
		740	if(!GPSPos_Deviation(&(PointList[i].Position), &RefPos_old, &RefDeviation)) break;
		741	// copy of the new reference position into this list place
		742	if(!GPSPos_Copy(pRefPos, &(PointList[i].Position))) break;
		743	// move new reference according to the deviation of the old reference
		744	retval = GPSPos_ShiftCartesian(&(PointList[i].Position), RefDeviation.North, RefDeviation.East);
		745	if(!retval) break;
		746	}
		747	} // else ref pos old not copied!
		748	if(!retval) PointList_Clear();
		749	return(retval);
		750	}
		751
		752
		753

Subversion Repositories NaviCtrl

(root)/tags/V2.10a/waypoints.c @ 660 - Rev 496