WebSVN - Projects - Blame - /Transportables_Koptertool/branch/PKT/branch/GPL_PKT_V3_6_7f

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

Subversion Repositories Projects

(root)/Transportables_Koptertool/branch/PKT/branch/GPL_PKT_V3_6_7f_FC090b/waypoints/waypoints.c @ 1934 - Rev