Subversion Repositories NaviCtrl

Compare Revisions

Ignore whitespace Rev 276 → Rev 277

/trunk/spi_slave.c
102,8 → 102,8
u8 SPI_CommandCounter = 0;
s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0;
s32 HeadFreeStartAngle = 0;
s32 NewCompassDirectionSetpoint = -1;
s32 FC_WP_EventChannel = 0; // gibt einen Schaltkanal an die FC weiter, wenn der Wegpunkt erreicht wurde
s16 NewCompassDirectionSetpoint = -1;
s16 FC_WP_EventChannel = 0; // gibt einen Schaltkanal an die FC weiter, wenn der Wegpunkt erreicht wurde
 
SPI_Version_t FC_Version;
 
/trunk/spi_slave.h
21,8 → 21,8
extern s32 ToFcGpsZ;
extern s32 ToFC_Rotate_C, ToFC_Rotate_S;
extern s32 HeadFreeStartAngle;
extern s32 NewCompassDirectionSetpoint;
extern s32 FC_WP_EventChannel;
extern s16 NewCompassDirectionSetpoint;
extern s16 FC_WP_EventChannel;
 
typedef struct
{
/trunk/uart1.c
406,6 → 406,7
if(pWaypoint->Position.Status == NEWDATA)
{
WPList_Clear(); // empty WPList
pWaypoint->Index = 1; // must be one after empty list
WPList_Append(pWaypoint);
GPS_pWaypoint = WPList_Begin();
}
452,9 → 453,8
}
else
{ // app current WP to the list
if (pWaypoint->Index == (WPList_GetCount() + 1))
if(WPList_Append(pWaypoint))
{
WPList_Append(pWaypoint);
BeepTime = 500;
UART1_Request_NewWaypoint = TRUE; // return new WP number
}
/trunk/waypoints.c
104,11 → 104,30
 
u8 WPList_Append(Waypoint_t* pwp)
{
if(WPNumber < WPLISTLEN) // there is still some space in the list
if((WPNumber < WPLISTLEN) && (pwp->Index == (WPNumber + 1)) ) // there is still some space in the list and index points to next
{
memcpy(&WPList[WPNumber], pwp, sizeof(Waypoint_t)); // copy wp data to list entry // increment list length
WPNumber++;
NaviData.WaypointNumber = WPNumber;
if(WPNumber == 1) // only for the first entry
{
// update POI index
switch(WPList[WPIndex-1].Type)
{
case POINT_TYPE_WP:
if(WPList[WPIndex-1].Heading < 0) POIIndex = (u8)(-WPList[WPIndex-1].Heading);
else POIIndex = 0;
break;
case POINT_TYPE_POI:
POIIndex = 1;
break;
 
default:
POIIndex = 0;
break;
}
}
return TRUE;
}
else return FALSE;
117,11 → 136,13
// returns the pointer to the first waypoint within the list
Waypoint_t* WPList_Begin(void)
{
u8 i;
WPIndex = 0; // set list position invalid
POIIndex = 0; // set invalid POI
 
if(WPNumber > 0)
{
u8 i;
 
// search for first wp in list
for(i = 0; i < WPNumber; i++)
{
131,31 → 152,48
break;
}
}
if(WPIndex) // found a WP in the list
{
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 // some points in the list but no WP found
{
NaviData.WaypointIndex = WPIndex;
//Check for an existing POI
for(i = 0; i < WPNumber; i++)
{
if((WPList[i].Type == POINT_TYPE_POI) && (WPList[i].Position.Status != INVALID))
{
POIIndex = i + 1;
break;
}
}
return NULL;
}
}
if(WPIndex) // found a WP in the list
else // no point in the list
{
POIIndex = 0;
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
return NULL;
}
else
{
POIIndex = 0;
NaviData.WaypointIndex = WPIndex;
return NULL;
}
}
 
// returns the last waypoint
Waypoint_t* WPList_End(void)
{
u8 i;
WPIndex = 0; // set list position invalid
POIIndex = 0; // set invalid
 
if(WPNumber > 0)
{
// search backward!
u8 i;
for(i = 1; i <= WPNumber; i++)
{
if((WPList[WPNumber - i].Type == POINT_TYPE_WP) && (WPList[WPNumber - i].Position.Status != INVALID))
164,20 → 202,34
break;
}
}
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 // list conbtains some points but no WP in the list
{
// search backward for a POI!
for(i = 1; i <= WPNumber; i++)
{
if((WPList[WPNumber - i].Type == POINT_TYPE_POI) && (WPList[WPNumber - i].Position.Status != INVALID))
{
POIIndex = WPNumber - i + 1;
break;
}
}
NaviData.WaypointIndex = WPIndex;
return NULL;
}
}
if(WPIndex) // found a WP within the list
else // no point in the list
{
NaviData.WaypointIndex = WPIndex;
if(WPList[WPIndex-1].Heading < 0) POIIndex = (u8)(-WPList[WPIndex-1].Heading);
else POIIndex = 0;
return(&(WPList[WPIndex-1]));
POIIndex = 0;
NaviData.WaypointIndex = WPIndex;
return NULL;
}
else
{
POIIndex = 0;
NaviData.WaypointIndex = WPIndex;
return NULL;
}
}
 
// returns a pointer to the next waypoint or NULL if the end of the list has been reached
205,7 → 257,12
else POIIndex = 0;
return(&(WPList[WPIndex-1])); // return pointer to this waypoint
}
else return(NULL);
else
{
NaviData.WaypointIndex = WPIndex;
POIIndex = 0;
return(NULL);
}
}
Waypoint_t* WPList_GetAt(u8 index)
/trunk/waypoints.h
3,7 → 3,7
 
#include "ubx.h"
 
//#define POINT_TYPE_INVALID
#define POINT_TYPE_INVALID 255
#define POINT_TYPE_WP 0
#define POINT_TYPE_POI 1
 
10,24 → 10,33
typedef struct
{
GPS_Pos_t Position; // the gps position of the waypoint, see ubx.h for details
s16 Heading; // orientation, future implementation
s16 Heading; // orientation, 0 no action, 1...360 fix heading, neg. = Index to POI in WP List
u8 ToleranceRadius; // in meters, if the MK is within that range around the target, then the next target is triggered
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 Type; // typeof Waypoint
u8 WP_EventChannelValue; //
u8 WP_EventChannelValue; //
u8 reserve[9]; // reserve
} __attribute__((packed)) Waypoint_t;
 
// Init List, return TRUE on success
u8 WPList_Init(void);
// Clear List, return TRUE on success
u8 WPList_Clear(void);
// Returns number of points in the list
u8 WPList_GetCount(void);
// appends a point to the list, returns TRUE on success
u8 WPList_Append(Waypoint_t* pwp);
// goto the first WP in the list and return pointer to it
Waypoint_t* WPList_Begin(void);
// goto the last WP in the list and return pointer to it
Waypoint_t* WPList_End(void);
// goto next WP in the list and return pointer to it
Waypoint_t* WPList_Next(void);
// return pointer to point at position
Waypoint_t* WPList_GetAt(u8 index);
// returns pointer to actual POI
Waypoint_t* WPList_GetPOI(void);
 
#endif // _WAYPOINTS_H