Subversion Repositories NaviCtrl

Compare Revisions

Ignore whitespace Rev 224 → Rev 225

/trunk/uart1.c
90,7 → 90,7
u8 UART1_Request_NaviData = FALSE;
u8 UART1_Request_ErrorMessage = FALSE;
u8 UART1_Request_NewWaypoint = FALSE;
u8 UART1_Request_ReadWaypoint = 255;
u8 UART1_Request_ReadWaypoint = 0;
u8 UART1_Request_Data3D = FALSE;
u8 UART1_Request_Echo = FALSE;
u8 UART1_Request_ParameterId = 0;
437,32 → 437,27
 
case 'w':// Append Waypoint to List
{
static u8 nextIndex = 0x00;
static u8 nextIndex = 0x01;
 
pWaypoint = (Waypoint_t*)SerialMsg.pData;
sprintf(text, "\r\nI=%d, S=%d,T=%d\r\n",pWaypoint->Index, pWaypoint->Position.Status, pWaypoint->Type);
UART1_PutString(text);
 
if (pWaypoint->Index == 0) // is the POI
if(pWaypoint->Position.Status == INVALID)
{
WPList_SetPOI(pWaypoint); //update POI also when invalid
WPList_Clear(); //delete the WP List
WPList_Clear();
GPS_pWaypoint = WPList_Begin();
nextIndex = 0x01;
BeepTime = 300;
UART1_Request_NewWaypoint = TRUE;
// the POI is not a WP therefore the WPNumber is not increased
// and the command returns a 0 as WP number
nextIndex = 0x01;
UART1_Request_NewWaypoint = TRUE; // return new WP number
}
else // normal WP
{
if (pWaypoint->Position.Status == NEWDATA)
{ // app current WP to the list
if (pWaypoint->Index == nextIndex)
{
WPList_Append(pWaypoint);
BeepTime = 500;
nextIndex = pWaypoint->Index+1;
UART1_Request_NewWaypoint = TRUE; // return new WP number
}
else if(pWaypoint->Position.Status == NEWDATA)
{ // app current WP to the list
if (pWaypoint->Index == nextIndex)
{
WPList_Append(pWaypoint);
BeepTime = 500;
nextIndex = pWaypoint->Index+1;
UART1_Request_NewWaypoint = TRUE; // return new WP number
}
}
}
659,10 → 654,10
MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'W', NC_ADDRESS, 1, &WPNumber, sizeof(WPNumber));
UART1_Request_NewWaypoint = FALSE;
}
else if((UART1_Request_ReadWaypoint != 0xFF) && (UART1_tx_buffer.Locked == FALSE))
else if((UART1_Request_ReadWaypoint) && (UART1_tx_buffer.Locked == FALSE))
{
u8 WPNumber = WPList_GetCount();
if (UART1_Request_ReadWaypoint < WPNumber)
if (UART1_Request_ReadWaypoint <= WPNumber)
{
MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'X', NC_ADDRESS, 3, &WPNumber, 1, &UART1_Request_ReadWaypoint, 1, WPList_GetAt(UART1_Request_ReadWaypoint), sizeof(Waypoint_t));
}
670,7 → 665,7
{
MKProtocol_CreateSerialFrame(&UART1_tx_buffer,'X', NC_ADDRESS, 1, &WPNumber, sizeof(WPNumber));
}
UART1_Request_ReadWaypoint = 0xFF;
UART1_Request_ReadWaypoint = 0;
}
else if((UART1_Request_DebugLabel != 0xFF) && (UART1_tx_buffer.Locked == FALSE))
{
/trunk/uart1.h
71,7 → 71,7
s16 Bearing; // course to target in deg
} __attribute__((packed)) GPS_PosDev_t;
 
#define NAVIDATA_VERSION 3
#define NAVIDATA_VERSION 4
 
typedef struct
{
/trunk/waypoints.c
64,10 → 64,10
#define WPLISTLEN 21
#define MAXWPNUMBER (WPLISTLEN-1)
Waypoint_t WPList[WPLISTLEN];
u8 WPIndex = 0;
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 WPList_Init(void)
{
return WPList_Clear();
76,12 → 76,13
u8 WPList_Clear(void)
{
u8 i;
WPIndex = 0; // invalid index
WPNumber = 0; // no contents
WPIndex = 0; // invalid index
POIIndex = 0; // invalid index
WPNumber = 0; // no contents
NaviData.WaypointNumber = WPNumber;
NaviData.WaypointIndex = WPIndex;
 
for(i = 1; i < WPLISTLEN; i++)
for(i = 0; i < WPLISTLEN; i++)
{
WPList[i].Position.Status = INVALID;
WPList[i].Position.Latitude = 0;
104,9 → 105,9
{
if(WPNumber < MAXWPNUMBER) // there is still some space in the list
{
WPNumber++;
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
WPNumber++;
NaviData.WaypointNumber = WPNumber;
return TRUE;
}
113,50 → 114,111
else return FALSE;
}
 
// rewind to the begin of the list, and returns the first waypoint
// returns the first waypoint
Waypoint_t* WPList_Begin(void)
{
if(WPNumber > 0)
{
WPIndex = 1; // reset list index
NaviData.WaypointIndex = WPIndex;
return(&(WPList[WPIndex])); // if list is not empty return pointer to first waypoint in the list
u8 i, wp_found = 0;
// search for first wp in list
for(i = 0; i < WPNumber; i++)
{
if(WPList[i].Type == POINT_TYPE_WP)
{
wp_found = 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
else // empty list
{
WPIndex = 0; // invalid index
NaviData.WaypointIndex = 0;
return NULL; // else return NULL
POIIndex = 0;
NaviData.WaypointIndex = WPIndex;
return NULL;
}
 
}
 
// jump to the end of the list, and returns the last waypoint
// returns the last waypoint
Waypoint_t* WPList_End(void)
{
if(WPNumber > 0)
{
WPIndex = WPNumber;
NaviData.WaypointIndex = WPIndex;
return(&(WPList[WPIndex])); // if list is not empty return pointer to first waypoint in the list
// search backward
u8 i, wp_found = 0;
for(i = 1; i <= WPNumber; i++)
{
if(WPList[WPNumber - i].Type == POINT_TYPE_WP)
{
wp_found = 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;
}
}
else
{
WPIndex = 0;
POIIndex = 0;
NaviData.WaypointIndex = WPIndex;
return NULL; // else return NULL
return NULL;
}
 
}
 
// returns a pointer to the next waypoint or NULL if the end of the list has been reached
Waypoint_t* WPList_Next(void)
{
if(WPIndex < WPNumber) // if the next WPIndex exist
if(WPNumber == 0) return NULL;
if((WPIndex > 0) && (WPIndex < WPNumber)) // if the next WP exist
{
WPIndex++; // goto next
NaviData.WaypointIndex = WPIndex;
return(&(WPList[WPIndex])); // return pointer to this waypoint
u8 i, wp_found = 0;
for(i = WPIndex; i < WPNumber; i++)
{
if(WPList[i].Type == POINT_TYPE_WP) // jump over POIs
{
wp_found = 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;
}
else return(NULL);
}
163,17 → 225,12
Waypoint_t* WPList_GetAt(u8 index)
{
if(index <= WPNumber) return(&(WPList[index])); // return pointer to this waypoint
if((index > 0) && (index <= WPNumber)) return(&(WPList[index-1])); // return pointer to this waypoint
else return(NULL);
}
 
Waypoint_t* WPList_GetPOI(void)
{
if(WPList[0].Position.Status == INVALID) return(NULL);
else return(&(WPList[0]));
return WPList_GetAt(POIIndex);
}
 
void WPList_SetPOI(Waypoint_t* pwp)
{
memcpy(&WPList[0], pwp, sizeof(Waypoint_t)); // copy wp data to list entry
}
/trunk/waypoints.h
3,6 → 3,9
 
#include "ubx.h"
 
#define POINT_TYPE_WP 0
#define POINT_TYPE_POI 1
 
typedef struct
{
GPS_Pos_t Position; // the gps position of the waypoint, see ubx.h for details
11,7 → 14,8
u8 HoldTime; // in seconds, if the was once in the tolerance area around a WP, this time defines the delay before the next WP is triggered
u8 Event_Flag; // future implementation
u8 Index; // to indentify different waypoints, workaround for bad communications PC <-> NC
u8 reserve[11]; // reserve
u8 Type; // typeof Waypoint
u8 reserve[10]; // reserve
} __attribute__((packed)) Waypoint_t;
 
u8 WPList_Init(void);
23,6 → 27,5
Waypoint_t* WPList_Next(void);
Waypoint_t* WPList_GetAt(u8 index);
Waypoint_t* WPList_GetPOI(void);
void WPList_SetPOI(Waypoint_t* pwp);
 
#endif // _WAYPOINTS_H