62,11 → 62,11 |
|
// the waypoints list |
#define WPLISTLEN 21 |
#define MAXWPNUMBER (WPLISTLEN-1) |
|
Waypoint_t WPList[WPLISTLEN]; |
u8 WPIndex = 0; // index of GPS point representig the WP |
u8 POIIndex = 0; // index of GPS Point representing the current POI |
u8 WPNumber = 0; |
u8 WPIndex = 0; // list index of GPS point representig the current WP, can be maximal WPNumber |
u8 POIIndex = 0; // list index of GPS Point representing the current POI, can be maximal WPNumber |
u8 WPNumber = 0; // number of wp in the list can be maximal equal to WPLISTLEN |
|
u8 WPList_Init(void) |
{ |
76,8 → 76,8 |
u8 WPList_Clear(void) |
{ |
u8 i; |
WPIndex = 0; // invalid index |
POIIndex = 0; // invalid index |
WPIndex = 0; // real list position are 1 ,2, 3 ... |
POIIndex = 0; // real list position are 1 ,2, 3 ... |
WPNumber = 0; // no contents |
NaviData.WaypointNumber = WPNumber; |
NaviData.WaypointIndex = WPIndex; |
88,10 → 88,11 |
WPList[i].Position.Latitude = 0; |
WPList[i].Position.Longitude = 0; |
WPList[i].Position.Altitude = 0; |
WPList[i].Heading = -1; |
WPList[i].Heading = 361; // invalid value |
WPList[i].ToleranceRadius = 0; // in meters, if the MK is within that range around the target, then the next target is triggered |
WPList[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 |
WPList[i].Event_Flag = 0; // future implementation |
WPList[i].Type = POINT_TYPE_WP; |
} |
return TRUE; |
} |
98,15 → 99,14 |
|
u8 WPList_GetCount(void) |
{ |
return WPNumber; |
return WPNumber; // number of points in the list |
} |
|
u8 WPList_Append(Waypoint_t* pwp) |
{ |
if(WPNumber < MAXWPNUMBER) // there is still some space in the list |
if(WPNumber < WPLISTLEN) // there is still some space in the list |
{ |
memcpy(&WPList[WPNumber], pwp, sizeof(Waypoint_t)); // copy wp data to list entry |
WPList[WPNumber].Position.Status = NEWDATA; // mark as new data // increment list length |
memcpy(&WPList[WPNumber], pwp, sizeof(Waypoint_t)); // copy wp data to list entry // increment list length |
WPNumber++; |
NaviData.WaypointNumber = WPNumber; |
return TRUE; |
114,84 → 114,69 |
else return FALSE; |
} |
|
// returns the first waypoint |
// returns the pointer to the first waypoint within the list |
Waypoint_t* WPList_Begin(void) |
{ |
WPIndex = 0; // set list position invalid |
|
if(WPNumber > 0) |
{ |
u8 i, wp_found = 0; |
u8 i; |
// search for first wp in list |
for(i = 0; i < WPNumber; i++) |
{ |
if(WPList[i].Type == POINT_TYPE_WP) |
if((WPList[i].Type == POINT_TYPE_WP) && (WPList[i].Position.Status != INVALID)) |
{ |
wp_found = 1; |
WPIndex = i + 1; |
break; |
} |
} |
if(wp_found) // found a WP in the list |
{ |
WPIndex = i+1; |
NaviData.WaypointIndex = WPIndex; |
if(WPList[WPIndex-1].Heading < 0) POIIndex = (u8)(-WPList[WPIndex-1].Heading); |
else POIIndex = 0; |
return(&(WPList[WPIndex-1])); // if list is not empty return pointer to first waypoint in the list |
} |
else // found no WP within the list |
{ |
WPIndex = 0; |
POIIndex = 0; |
NaviData.WaypointIndex = WPIndex; |
return NULL; |
} |
} |
else // empty list |
if(WPIndex) // found a WP in the list |
{ |
WPIndex = 0; // invalid index |
NaviData.WaypointIndex = WPIndex; |
// update index to POI |
if(WPList[WPIndex-1].Heading < 0) POIIndex = (u8)(-WPList[WPIndex-1].Heading); |
else POIIndex = 0; |
return(&(WPList[WPIndex-1])); // if list is not empty return pointer to first waypoint in the list |
} |
else |
{ |
POIIndex = 0; |
NaviData.WaypointIndex = WPIndex; |
return NULL; |
} |
|
} |
|
// returns the last waypoint |
Waypoint_t* WPList_End(void) |
{ |
WPIndex = 0; // set list position invalid |
if(WPNumber > 0) |
{ |
// search backward |
u8 i, wp_found = 0; |
// search backward! |
u8 i; |
for(i = 1; i <= WPNumber; i++) |
{ |
if(WPList[WPNumber - i].Type == POINT_TYPE_WP) |
if((WPList[WPNumber - i].Type == POINT_TYPE_WP) && (WPList[WPNumber - i].Position.Status != INVALID)) |
{ |
wp_found = 1; |
WPIndex = WPNumber - i + 1; |
break; |
} |
} |
if(wp_found) // found a WP within the list |
{ |
WPIndex = WPNumber - i + 1; |
NaviData.WaypointIndex = WPIndex; |
if(WPList[WPIndex-1].Heading < 0) POIIndex = (u8)(-WPList[WPIndex-1].Heading); |
else POIIndex = 0; |
return(&(WPList[WPIndex-1])); |
} |
else |
{ |
WPIndex = 0; |
POIIndex = 0; |
NaviData.WaypointIndex = WPIndex; |
return NULL; |
} |
} |
if(WPIndex) // found a WP within the list |
{ |
NaviData.WaypointIndex = WPIndex; |
if(WPList[WPIndex-1].Heading < 0) POIIndex = (u8)(-WPList[WPIndex-1].Heading); |
else POIIndex = 0; |
return(&(WPList[WPIndex-1])); |
} |
else |
{ |
WPIndex = 0; |
POIIndex = 0; |
NaviData.WaypointIndex = WPIndex; |
return NULL; |
return NULL; |
} |
} |
|
198,28 → 183,28 |
// returns a pointer to the next waypoint or NULL if the end of the list has been reached |
Waypoint_t* WPList_Next(void) |
{ |
if(WPNumber == 0) return NULL; |
if((WPIndex > 0) && (WPIndex < WPNumber)) // if the next WP exist |
u8 wp_found = 0; |
|
if(WPIndex < WPNumber) // if there is a next entry in the list |
{ |
u8 i, wp_found = 0; |
for(i = WPIndex; i < WPNumber; i++) |
u8 i; |
for(i = WPIndex; i < WPNumber; i++) // start search for next at next list entry |
{ |
if(WPList[i].Type == POINT_TYPE_WP) // jump over POIs |
if((WPList[i].Type == POINT_TYPE_WP) && (WPList[i].Position.Status != INVALID)) // jump over POIs |
{ |
wp_found = 1; |
wp_found = i+1; |
break; |
} |
} |
if(wp_found) |
{ |
WPIndex = i+1; |
NaviData.WaypointIndex = WPIndex; |
if(WPList[WPIndex-1].Heading < 0) POIIndex = (u8)(-WPList[WPIndex-1].Heading); |
else POIIndex = 0; |
return(&(WPList[WPIndex-1])); // return pointer to this waypoint |
} |
else return NULL; |
} |
if(wp_found) |
{ |
WPIndex = wp_found; // update list position |
NaviData.WaypointIndex = WPIndex; |
if(WPList[WPIndex-1].Heading < 0) POIIndex = (u8)(-WPList[WPIndex-1].Heading); |
else POIIndex = 0; |
return(&(WPList[WPIndex-1])); // return pointer to this waypoint |
} |
else return(NULL); |
} |
|