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

Subversion Repositories NaviCtrl

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