WebSVN - Projects - Blame - Rev 2136 - /Transportables_Koptertool/PKT/trunk/waypoints/waypoints.c

Rev	Author	Line No.	Line
2136	-	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 <stdlib.h>
		59	#include <avr/io.h>
		60	#include <stdint.h>
		61	#include <string.h>
		62	#include <stdbool.h>
		63	//#include "91x_lib.h"
		64	#include "waypoints.h"
		65	#include "../mk-data-structs.h"
		66	#include "../uart/uart1.h"
		67	#include "../eeprom/eeprom.h"
		68	#include "../main.h"
		69
		70	#include <util/delay.h>
		71
		72	#ifdef USE_WAYPOINTS
		73
		74	// the waypoints list
		75
		76
		77
		78	NaviData_t *NaviData;
		79
		80	//Point_t PointList[MAX_LIST_LEN];
		81	uint8_t WPIndex = 0; // list index of GPS point representig the current WP, can be maximal WPCount
		82	uint8_t POIIndex = 0; // list index of GPS Point representing the current POI, can be maximal WPCount
		83	uint8_t WPCount = 0; // number of waypoints
		84	uint8_t PointCount = 0; // number of wp in the list can be maximal equal to MAX_LIST_LEN
		85	uint8_t POICount = 0;
		86
		87	uint8_t WPActive = false;
		88
		89	uint8_t PointList_Init(void)
		90	{
		91	return PointList_Clear();
		92	}
		93
		94	uint8_t PointList_Clear(void)
		95	{
		96	uint8_t i;
		97	WPIndex = 0; // real list position are 1 ,2, 3 ...
		98	POIIndex = 0; // real list position are 1 ,2, 3 ...
		99	WPCount = 0; // no waypoints
		100	POICount = 0;
		101	PointCount = 0; // no contents
		102	WPActive = false;
		103	NaviData->WaypointNumber = WPCount;
		104	NaviData->WaypointIndex = 0;
		105
		106	for(i = 0; i < MAX_LIST_LEN; i++)
		107	{
		108	Config.PointList[i].Position.Status = INVALID;
		109	Config.PointList[i].Position.Latitude = 0;
		110	Config.PointList[i].Position.Longitude = 0;
		111	Config.PointList[i].Position.Altitude = 0;
		112	Config.PointList[i].Heading = 361; // invalid value
		113	Config.PointList[i].ToleranceRadius = 0; // in meters, if the MK is within that range around the target, then the next target is triggered
		114	Config.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
		115	Config.PointList[i].Type = POINT_TYPE_INVALID;
		116	Config.PointList[i].Event_Flag = 0; // future implementation
		117	Config.PointList[i].AltitudeRate = 0; // no change of setpoint
		118	}
		119	return true;
		120	}
		121
		122	uint8_t PointList_GetCount(void)
		123	{
		124	return PointCount; // number of points in the list
		125	}
		126
		127	Point_t* PointList_GetAt(uint8_t index)
		128	{
		129	if((index > 0) && (index <= PointCount)) return(&(Config.PointList[index-1])); // return pointer to this waypoint
		130	else return(NULL);
		131	}
		132
		133	uint8_t PointList_SetAt(Point_t* pPoint)
		134	{
		135	// if index is in range
		136	if((pPoint->Index > 0) && (pPoint->Index <= MAX_LIST_LEN))
		137	{
		138	// check list entry before update
		139	switch(Config.PointList[pPoint->Index-1].Type)
		140	{
		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;
		153
		154	case POINT_TYPE_POI:
		155	POICount++;
		156	PointCount++;
		157	break;
		158	}
		159	break;
		160
		161	case POINT_TYPE_WP: // was a waypoint
		162	switch(pPoint->Type)
		163	{
		164	case POINT_TYPE_INVALID:
		165	WPCount--;
		166	PointCount--;
		167	break;
		168
		169	default:
		170	case POINT_TYPE_WP:
		171	//nothing to do
		172	break;
		173
		174	case POINT_TYPE_POI:
		175	POICount++;
		176	WPCount--;
		177	break;
		178	}
		179	break;
		180
		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;
		193
		194	case POINT_TYPE_POI:
		195	default:
		196	// nothing to do
		197	break;
		198	}
		199	break;
		200	}
		201	memcpy(&Config.PointList[pPoint->Index-1], pPoint, sizeof(Point_t)); // copy data to list entry
		202	NaviData->WaypointNumber = WPCount;
		203	return pPoint->Index;
		204	}
		205	else return(0);
		206	}
		207
		208	// returns the pointer to the first waypoint within the list
		209	Point_t* PointList_WPBegin(void)
		210	{
		211	uint8_t i;
		212	WPIndex = 0; // set list position invalid
		213
		214	if(WPActive == false) return(NULL);
		215
		216	POIIndex = 0; // set invalid POI
		217	if(PointCount > 0)
		218	{
		219	// search for first wp in list
		220	for(i = 0; i <MAX_LIST_LEN; i++)
		221	{
		222	if((Config.PointList[i].Type == POINT_TYPE_WP) && (Config.PointList[i].Position.Status != INVALID))
		223	{
		224	WPIndex = i + 1;
		225	break;
		226	}
		227	}
		228	if(WPIndex) // found a WP in the list
		229	{
		230	NaviData->WaypointIndex = 1;
		231	// update index to POI
		232	if(Config.PointList[WPIndex-1].Heading < 0) POIIndex = (uint8_t)(-Config.PointList[WPIndex-1].Heading);
		233	else POIIndex = 0;
		234	}
		235	else // some points in the list but no WP found
		236	{
		237	NaviData->WaypointIndex = 0;
		238	//Check for an existing POI
		239	for(i = 0; i < MAX_LIST_LEN; i++)
		240	{
		241	if((Config.PointList[i].Type == POINT_TYPE_POI) && (Config.PointList[i].Position.Status != INVALID))
		242	{
		243	POIIndex = i + 1;
		244	break;
		245	}
		246	}
		247	}
		248	}
		249	else // no point in the list
		250	{
		251	POIIndex = 0;
		252	NaviData->WaypointIndex = 0;
		253	}
		254
		255	if(WPIndex) return(&(Config.PointList[WPIndex-1]));
		256	else return(NULL);
		257	}
		258
		259	// returns the last waypoint
		260	Point_t* PointList_WPEnd(void)
		261	{
		262
		263	uint8_t i;
		264	WPIndex = 0; // set list position invalid
		265	POIIndex = 0; // set invalid
		266
		267	if(WPActive == false) return(NULL);
		268
		269	if(PointCount > 0)
		270	{
		271	// search backward!
		272	for(i = 1; i <= MAX_LIST_LEN; i++)
		273	{
		274	if((Config.PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_WP) && (Config.PointList[MAX_LIST_LEN - i].Position.Status != INVALID))
		275	{
		276	WPIndex = MAX_LIST_LEN - i + 1;
		277	break;
		278	}
		279	}
		280	if(WPIndex) // found a WP within the list
		281	{
		282	NaviData->WaypointIndex = WPCount;
		283	if(Config.PointList[WPIndex-1].Heading < 0) POIIndex = (uint8_t)(-Config.PointList[WPIndex-1].Heading);
		284	else POIIndex = 0;
		285	}
		286	else // list contains some points but no WP in the list
		287	{
		288	// search backward for a POI!
		289	for(i = 1; i <= MAX_LIST_LEN; i++)
		290	{
		291	if((Config.PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_POI) && (Config.PointList[MAX_LIST_LEN - i].Position.Status != INVALID))
		292	{
		293	POIIndex = MAX_LIST_LEN - i + 1;
		294	break;
		295	}
		296	}
		297	NaviData->WaypointIndex = 0;
		298	}
		299	}
		300	else // no point in the list
		301	{
		302	POIIndex = 0;
		303	NaviData->WaypointIndex = 0;
		304	}
		305	if(WPIndex) return(&(Config.PointList[WPIndex-1]));
		306	else return(NULL);
		307	}
		308
		309	// returns a pointer to the next waypoint or NULL if the end of the list has been reached
		310	Point_t* PointList_WPNext(void)
		311	{
		312	uint8_t wp_found = 0;
		313	if(WPActive == false) return(NULL);
		314
		315	if(WPIndex < MAX_LIST_LEN) // if there is a next entry in the list
		316	{
		317	uint8_t i;
		318	for(i = WPIndex; i < MAX_LIST_LEN; i++) // start search for next at next list entry
		319	{
		320	if((Config.PointList[i].Type == POINT_TYPE_WP) && (Config.PointList[i].Position.Status != INVALID)) // jump over POIs
		321	{
		322	wp_found = i+1;
		323	break;
		324	}
		325	}
		326	}
		327	if(wp_found)
		328	{
		329	WPIndex = wp_found; // update list position
		330	NaviData->WaypointIndex++;
		331	if(Config.PointList[WPIndex-1].Heading < 0) POIIndex = (uint8_t)(-Config.PointList[WPIndex-1].Heading);
		332	else POIIndex = 0;
		333	return(&(Config.PointList[WPIndex-1])); // return pointer to this waypoint
		334	}
		335	else
		336	{ // no next wp found
		337	NaviData->WaypointIndex = 0;
		338	POIIndex = 0;
		339	return(NULL);
		340	}
		341	}
		342
		343	void PointList_WPActive(uint8_t set)
		344	{
		345	if(set)
		346	{
		347	WPActive = true;
		348	PointList_WPBegin(); // uopdates POI index
		349	}
		350	else
		351	{
		352	WPActive = false;
		353	POIIndex = 0; // disable POI also
		354	}
		355	}
		356
		357	Point_t* PointList_GetPOI(void)
		358	{
		359	return PointList_GetAt(POIIndex);
		360	}
		361
		362	#endif

Subversion Repositories Projects

(root)/Transportables_Koptertool/PKT/trunk/waypoints/waypoints.c @ 2212 - Rev 2136