WebSVN - Projects - Blame - /Transportables_Koptertool/branch/PKT_V363a_V088n

Rev	Author	Line No.	Line
1734	-	1	/#######################################################################################/
		2	/* !!! THIS IS NOT FREE SOFTWARE !!! */
		3	/#######################################################################################/
		4	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		5	// + Copyright (c) 2008 Ingo Busker, Holger Buss
		6	// + Nur f�r den privaten Gebrauch / NON-COMMERCIAL USE ONLY
		7	// + FOR NON COMMERCIAL USE ONLY
		8	// + www.MikroKopter.com
		9	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		10	// + Es gilt f�r das gesamte Projekt (Hardware, Software, Bin�rfiles, Sourcecode und Dokumentation),
		11	// + dass eine Nutzung (auch auszugsweise) nur f�r den privaten (nicht-kommerziellen) Gebrauch zul�ssig ist.
		12	// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
		13	// + bzgl. der Nutzungsbedingungen aufzunehmen.
		14	// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Best�ckung und Verkauf von Platinen oder Baus�tzen,
		15	// + Verkauf von Luftbildaufnahmen, usw.
		16	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		17	// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder ver�ffentlicht,
		18	// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright m�ssen dann beiliegen
		19	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		20	// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
		21	// + auf anderen Webseiten oder sonstigen Medien ver�ffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
		22	// + eindeutig als Ursprung verlinkt werden
		23	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		24	// + Keine Gew�hr auf Fehlerfreiheit, Vollst�ndigkeit oder Funktion
		25	// + Benutzung auf eigene Gefahr
		26	// + Wir �bernehmen keinerlei Haftung f�r direkte oder indirekte Personen- oder Sachsch�den
		27	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		28	// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
		29	// + mit unserer Zustimmung zul�ssig
		30	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		31	// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
		32	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		33	// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
		34	// + this list of conditions and the following disclaimer.
		35	// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
		36	// + from this software without specific prior written permission.
		37	// + * The use of this project (hardware, software, binary files, sources and documentation) is only permitted
		38	// + for non-commercial use (directly or indirectly)
		39	// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
		40	// + with our written permission
		41	// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
		42	// + clearly linked as origin
		43	// + * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
		44	//
		45	// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
		46	// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
		47	// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
		48	// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
		49	// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
		50	// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
		51	// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
		52	// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
		53	// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
		54	// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
		55	// + POSSIBILITY OF SUCH DAMAGE.
		56	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
		57
		58	#include <string.h>
		59	#include "91x_lib.h"
		60	#include "waypoints.h"
		61	#include "uart1.h"
		62
		63	// the waypoints list
		64	#define MAX_LIST_LEN 31
		65
		66	Point_t PointList[MAX_LIST_LEN];
		67	u8 WPIndex = 0; // list index of GPS point representig the current WP, can be maximal WPCount
		68	u8 POIIndex = 0; // list index of GPS Point representing the current POI, can be maximal WPCount
		69	u8 WPCount = 0; // number of waypoints
		70	u8 PointCount = 0; // number of wp in the list can be maximal equal to MAX_LIST_LEN
		71	u8 POICount = 0;
		72
		73	u8 WPActive = FALSE;
		74
		75	u8 PointList_Init(void)
		76	{
		77	return PointList_Clear();
		78	}
		79
		80	u8 PointList_Clear(void)
		81	{
		82	u8 i;
		83	WPIndex = 0; // real list position are 1 ,2, 3 ...
		84	POIIndex = 0; // real list position are 1 ,2, 3 ...
		85	WPCount = 0; // no waypoints
		86	POICount = 0;
		87	PointCount = 0; // no contents
		88	WPActive = FALSE;
		89	NaviData.WaypointNumber = WPCount;
		90	NaviData.WaypointIndex = 0;
		91
		92	for(i = 0; i < MAX_LIST_LEN; i++)
		93	{
		94	PointList[i].Position.Status = INVALID;
		95	PointList[i].Position.Latitude = 0;
		96	PointList[i].Position.Longitude = 0;
		97	PointList[i].Position.Altitude = 0;
		98	PointList[i].Heading = 361; // invalid value
		99	PointList[i].ToleranceRadius = 0; // in meters, if the MK is within that range around the target, then the next target is triggered
		100	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
		101	PointList[i].Type = POINT_TYPE_INVALID;
		102	PointList[i].Event_Flag = 0; // future implementation
		103	PointList[i].AltitudeRate = 0; // no change of setpoint
		104	}
		105	return TRUE;
		106	}
		107
		108	u8 PointList_GetCount(void)
		109	{
		110	return PointCount; // number of points in the list
		111	}
		112
		113	Point_t* PointList_GetAt(u8 index)
		114	{
		115	if((index > 0) && (index <= PointCount)) return(&(PointList[index-1])); // return pointer to this waypoint
		116	else return(NULL);
		117	}
		118
		119	u8 PointList_SetAt(Point_t* pPoint)
		120	{
		121	// if index is in range
		122	if((pPoint->Index > 0) && (pPoint->Index <= MAX_LIST_LEN))
		123	{
		124	// check list entry before update
		125	switch(PointList[pPoint->Index-1].Type)
		126	{
		127	case POINT_TYPE_INVALID: // was invalid
		128	switch(pPoint->Type)
		129	{
		130	default:
		131	case POINT_TYPE_INVALID:
		132	// nothing to do
		133	break;
		134
		135	case POINT_TYPE_WP:
		136	WPCount++;
		137	PointCount++;
		138	break;
		139
		140	case POINT_TYPE_POI:
		141	POICount++;
		142	PointCount++;
		143	break;
		144	}
		145	break;
		146
		147	case POINT_TYPE_WP: // was a waypoint
		148	switch(pPoint->Type)
		149	{
		150	case POINT_TYPE_INVALID:
		151	WPCount--;
		152	PointCount--;
		153	break;
		154
		155	default:
		156	case POINT_TYPE_WP:
		157	//nothing to do
		158	break;
		159
		160	case POINT_TYPE_POI:
		161	POICount++;
		162	WPCount--;
		163	break;
		164	}
		165	break;
		166
		167	case POINT_TYPE_POI: // was a poi
		168	switch(pPoint->Type)
		169	{
		170	case POINT_TYPE_INVALID:
		171	POICount--;
		172	PointCount--;
		173	break;
		174
		175	case POINT_TYPE_WP:
		176	WPCount++;
		177	POICount--;
		178	break;
		179
		180	case POINT_TYPE_POI:
		181	default:
		182	// nothing to do
		183	break;
		184	}
		185	break;
		186	}
		187	memcpy(&PointList[pPoint->Index-1], pPoint, sizeof(Point_t)); // copy data to list entry
		188	NaviData.WaypointNumber = WPCount;
		189	return pPoint->Index;
		190	}
		191	else return(0);
		192	}
		193
		194	// returns the pointer to the first waypoint within the list
		195	Point_t* PointList_WPBegin(void)
		196	{
		197	u8 i;
		198	WPIndex = 0; // set list position invalid
		199
		200	if(WPActive == FALSE) return(NULL);
		201
		202	POIIndex = 0; // set invalid POI
		203	if(PointCount > 0)
		204	{
		205	// search for first wp in list
		206	for(i = 0; i <MAX_LIST_LEN; i++)
		207	{
		208	if((PointList[i].Type == POINT_TYPE_WP) && (PointList[i].Position.Status != INVALID))
		209	{
		210	WPIndex = i + 1;
		211	break;
		212	}
		213	}
		214	if(WPIndex) // found a WP in the list
		215	{
		216	NaviData.WaypointIndex = 1;
		217	// update index to POI
		218	if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
		219	else POIIndex = 0;
		220	}
		221	else // some points in the list but no WP found
		222	{
		223	NaviData.WaypointIndex = 0;
		224	//Check for an existing POI
		225	for(i = 0; i < MAX_LIST_LEN; i++)
		226	{
		227	if((PointList[i].Type == POINT_TYPE_POI) && (PointList[i].Position.Status != INVALID))
		228	{
		229	POIIndex = i + 1;
		230	break;
		231	}
		232	}
		233	}
		234	}
		235	else // no point in the list
		236	{
		237	POIIndex = 0;
		238	NaviData.WaypointIndex = 0;
		239	}
		240
		241	if(WPIndex) return(&(PointList[WPIndex-1]));
		242	else return(NULL);
		243	}
		244
		245	// returns the last waypoint
		246	Point_t* PointList_WPEnd(void)
		247	{
		248
		249	u8 i;
		250	WPIndex = 0; // set list position invalid
		251	POIIndex = 0; // set invalid
		252
		253	if(WPActive == FALSE) return(NULL);
		254
		255	if(PointCount > 0)
		256	{
		257	// search backward!
		258	for(i = 1; i <= MAX_LIST_LEN; i++)
		259	{
		260	if((PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_WP) && (PointList[MAX_LIST_LEN - i].Position.Status != INVALID))
		261	{
		262	WPIndex = MAX_LIST_LEN - i + 1;
		263	break;
		264	}
		265	}
		266	if(WPIndex) // found a WP within the list
		267	{
		268	NaviData.WaypointIndex = WPCount;
		269	if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
		270	else POIIndex = 0;
		271	}
		272	else // list contains some points but no WP in the list
		273	{
		274	// search backward for a POI!
		275	for(i = 1; i <= MAX_LIST_LEN; i++)
		276	{
		277	if((PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_POI) && (PointList[MAX_LIST_LEN - i].Position.Status != INVALID))
		278	{
		279	POIIndex = MAX_LIST_LEN - i + 1;
		280	break;
		281	}
		282	}
		283	NaviData.WaypointIndex = 0;
		284	}
		285	}
		286	else // no point in the list
		287	{
		288	POIIndex = 0;
		289	NaviData.WaypointIndex = 0;
		290	}
		291	if(WPIndex) return(&(PointList[WPIndex-1]));
		292	else return(NULL);
		293	}
		294
		295	// returns a pointer to the next waypoint or NULL if the end of the list has been reached
		296	Point_t* PointList_WPNext(void)
		297	{
		298	u8 wp_found = 0;
		299	if(WPActive == FALSE) return(NULL);
		300
		301	if(WPIndex < MAX_LIST_LEN) // if there is a next entry in the list
		302	{
		303	u8 i;
		304	for(i = WPIndex; i < MAX_LIST_LEN; i++) // start search for next at next list entry
		305	{
		306	if((PointList[i].Type == POINT_TYPE_WP) && (PointList[i].Position.Status != INVALID)) // jump over POIs
		307	{
		308	wp_found = i+1;
		309	break;
		310	}
		311	}
		312	}
		313	if(wp_found)
		314	{
		315	WPIndex = wp_found; // update list position
		316	NaviData.WaypointIndex++;
		317	if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
		318	else POIIndex = 0;
		319	return(&(PointList[WPIndex-1])); // return pointer to this waypoint
		320	}
		321	else
		322	{ // no next wp found
		323	NaviData.WaypointIndex = 0;
		324	POIIndex = 0;
		325	return(NULL);
		326	}
		327	}
		328
		329	void PointList_WPActive(u8 set)
		330	{
		331	if(set)
		332	{
		333	WPActive = TRUE;
		334	PointList_WPBegin(); // uopdates POI index
		335	}
		336	else
		337	{
		338	WPActive = FALSE;
		339	POIIndex = 0; // disable POI also
		340	}
		341	}
		342
		343	Point_t* PointList_GetPOI(void)
		344	{
		345	return PointList_GetAt(POIIndex);
		346	}
		347

Subversion Repositories Projects

(root)/Transportables_Koptertool/branch/PKT_V363a_V088n_MartinR/waypoints.c @ 2475 - Rev