Subversion Repositories NaviCtrl

Compare Revisions

Ignore whitespace Rev 279 → Rev 280

/trunk/GPS.h
11,15 → 11,17
s16 Yaw;
} __attribute__((packed)) GPS_Stick_t;
 
#define CAM_UPDATE_AZIMUTH 0x01
#define CAM_UPDATE_ELEVATION 0x02
typedef struct
{
s16 Azimuth; // angle measured clockwise from north
s16 Elevation; // angle measured upwards from horizont
u8 State; // NEWDATA, PROCESSED, INVALID
u8 UpdateMask;
} __attribute__((packed)) CAM_Orientation_t;
 
extern CAM_Orientation_t CAM_Orientation;
extern Waypoint_t* GPS_pWaypoint;
extern Point_t* GPS_pWaypoint;
 
void GPS_Init(void);
void GPS_Navigation(gps_data_t *pGPS_Data, GPS_Stick_t* pGPS_Stick);
/trunk/params.c
18,11 → 18,11
switch(id)
{
case NCPARAMS_NEW_COMPASS_DIRECTION_SETPOINT:
if(NULL == WPList_GetPOI())
if(NULL == PointList_GetPOI())
{
CAM_Orientation.Azimuth = *pvalue;
CAM_Orientation.Elevation = 0;
CAM_Orientation.State = NEWDATA;
CAM_Orientation.UpdateMask = CAM_UPDATE_AZIMUTH;
}
break;
 
/trunk/spi_slave.c
296,17 → 296,16
ToFlightCtrl.Param.sByte[4] = (s8) ToFcGpsZ;
ToFlightCtrl.Param.Byte[5] = (s8) ToFC_Rotate_C;
ToFlightCtrl.Param.Byte[6] = (s8) ToFC_Rotate_S;
ToFlightCtrl.Param.sInt[5] = CAM_Orientation.Elevation;
if(CAM_Orientation.State == NEWDATA)
if(CAM_Orientation.UpdateMask & CAM_UPDATE_AZIMUTH)
{
ToFlightCtrl.Param.sInt[4] = CAM_Orientation.Azimuth;
CAM_Orientation.State = PROCESSED;
CAM_Orientation.UpdateMask &= ~CAM_UPDATE_AZIMUTH;
}
else
{ // set invalid values
{
ToFlightCtrl.Param.sInt[4] = -1;
}
DebugOut.Analog[24] = ToFlightCtrl.Param.sInt[5];
ToFlightCtrl.Param.sInt[5] = CAM_Orientation.Elevation;
break;
 
case SPI_NCCMD_VERSION:
/trunk/spi_slave.h
30,7 → 30,7
s16 AngleRoll; // RollAngle in 0.1 deg
s16 AccNick;
s16 AccRoll;
s16 GyroHeading; // Heading in 0.1 deg
s16 GyroHeading; // Heading in 0.1 deg
s16 GyroNick;
s16 GyroRoll;
s16 GyroYaw;
/trunk/uart1.c
91,8 → 91,8
u8 UART1_Request_DebugLabel = 255;
u8 UART1_Request_NaviData = FALSE;
u8 UART1_Request_ErrorMessage = FALSE;
u8 UART1_Request_NewWaypoint = FALSE;
u8 UART1_Request_ReadWaypoint = 0;
u8 UART1_Request_NewPoint = FALSE;
u8 UART1_Request_ReadPoint = 0;
u8 UART1_Request_Data3D = FALSE;
u8 UART1_Request_Echo = FALSE;
u8 UART1_Request_ParameterId = 0;
365,7 → 365,7
}
if(UART1_rx_buffer.Locked == FALSE) return;
 
Waypoint_t * pWaypoint = NULL;
Point_t * pPoint = NULL;
SerialMsg_t SerialMsg;
 
// analyze header first
401,14 → 401,14
break;
 
case 's':// new target position
pWaypoint = (Waypoint_t*)SerialMsg.pData;
pPoint = (Point_t*)SerialMsg.pData;
BeepTime = 300;
if(pWaypoint->Position.Status == NEWDATA)
if(pPoint->Position.Status == NEWDATA)
{
WPList_Clear(); // empty WPList
pWaypoint->Index = 1; // must be one after empty list
WPList_Append(pWaypoint);
GPS_pWaypoint = WPList_Begin();
PointList_Clear(); // empty List
pPoint->Index = 1; // must be one after empty list
PointList_Append(pPoint);
GPS_pWaypoint = PointList_WPBegin();
}
break;
 
441,22 → 441,22
}
break;
 
case 'w':// Append Waypoint to List
case 'w':// Append Point to List
{
pWaypoint = (Waypoint_t*)SerialMsg.pData;
pPoint = (Point_t*)SerialMsg.pData;
 
if((pWaypoint->Position.Status == INVALID) && (pWaypoint->Index == 0))
if((pPoint->Position.Status == INVALID) && (pPoint->Index == 0))
{
WPList_Clear();
GPS_pWaypoint = WPList_Begin();
UART1_Request_NewWaypoint = TRUE; // return new WP number
PointList_Clear();
GPS_pWaypoint = PointList_WPBegin();
UART1_Request_NewPoint = TRUE; // return new point count
}
else
{ // app current WP to the list
if(WPList_Append(pWaypoint))
if(PointList_Append(pPoint))
{
BeepTime = 500;
UART1_Request_NewWaypoint = TRUE; // return new WP number
UART1_Request_NewPoint = TRUE; // return new WP number
}
}
}
463,7 → 463,7
break;
 
case 'x':// Read Waypoint from List
UART1_Request_ReadWaypoint = SerialMsg.pData[0];
UART1_Request_ReadPoint = SerialMsg.pData[0];
break;
 
case 'j':// Set/Get NC-Parameter
643,24 → 643,24
Echo = 0; // reset echo value
UART1_Request_Echo = FALSE;
}
else if(UART1_Request_NewWaypoint && (UART1_tx_buffer.Locked == FALSE))
else if(UART1_Request_NewPoint && (UART1_tx_buffer.Locked == FALSE))
{
u8 WPNumber = WPList_GetCount();
MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'W', NC_ADDRESS, 1, &WPNumber, sizeof(WPNumber));
UART1_Request_NewWaypoint = FALSE;
u8 PointCount = PointList_GetCount();
MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'W', NC_ADDRESS, 1, &PointCount, sizeof(PointCount));
UART1_Request_NewPoint = FALSE;
}
else if((UART1_Request_ReadWaypoint) && (UART1_tx_buffer.Locked == FALSE))
else if((UART1_Request_ReadPoint) && (UART1_tx_buffer.Locked == FALSE))
{
u8 WPNumber = WPList_GetCount();
if (UART1_Request_ReadWaypoint <= WPNumber)
u8 PointCount = PointList_GetCount();
if (UART1_Request_ReadPoint <= PointCount)
{
MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'X', NC_ADDRESS, 3, &WPNumber, 1, &UART1_Request_ReadWaypoint, 1, WPList_GetAt(UART1_Request_ReadWaypoint), sizeof(Waypoint_t));
MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'X', NC_ADDRESS, 3, &PointCount, 1, &UART1_Request_ReadPoint, 1, PointList_GetAt(UART1_Request_ReadPoint), sizeof(Point_t));
}
else
{
MKProtocol_CreateSerialFrame(&UART1_tx_buffer,'X', NC_ADDRESS, 1, &WPNumber, sizeof(WPNumber));
MKProtocol_CreateSerialFrame(&UART1_tx_buffer,'X', NC_ADDRESS, 1, &PointCount, sizeof(PointCount));
}
UART1_Request_ReadWaypoint = 0;
UART1_Request_ReadPoint = 0;
}
else if((UART1_Request_DebugLabel != 0xFF) && (UART1_tx_buffer.Locked == FALSE))
{
/trunk/usb.c
99,8 → 99,6
u8 USB_Request_DebugLabel = 255;
u8 USB_Request_NaviData = FALSE;
u8 USB_Request_ErrorMessage = FALSE;
u8 USB_Request_NewWaypoint = FALSE;
u8 USB_Request_ReadWaypoint = 255;
u8 USB_Request_Data3D = FALSE;
u8 USB_Request_Echo = FALSE;
u8 USB_DisplayKeys = 0;
172,7 → 170,7
{
u8 c;
SerialMsg_t SerialMsg;
Waypoint_t * pWaypoint = NULL;
 
// if data in the rxd buffer are not locked immediately return
// if rx buffer is not locked
if(USB_rx_buffer.Locked == FALSE)
204,43 → 202,6
USB_Request_ErrorMessage = TRUE;
break;
 
case 's':// new target position
pWaypoint = (Waypoint_t*)SerialMsg.pData;
BeepTime = 300;
if(pWaypoint->Position.Status == NEWDATA)
{
WPList_Clear(); // empty WPList
WPList_Append(pWaypoint);
GPS_pWaypoint = WPList_Begin();
}
break;
 
case 'w':// Append Waypoint to List
{
pWaypoint = (Waypoint_t*)SerialMsg.pData;
 
if((pWaypoint->Position.Status == INVALID) && (pWaypoint->Index == 0))
{
WPList_Clear();
GPS_pWaypoint = WPList_Begin();
USB_Request_NewWaypoint = TRUE; // return new WP number
}
else
{ // app current WP to the list
if (pWaypoint->Index == (WPList_GetCount() + 1))
{
WPList_Append(pWaypoint);
BeepTime = 500;
USB_Request_NewWaypoint = TRUE; // return new WP number
}
}
}
break;
 
case 'x':// Read Waypoint from List
USB_Request_ReadWaypoint = SerialMsg.pData[0];
break;
 
default:
// unsupported command recieved
break;
494,24 → 455,5
MKProtocol_CreateSerialFrame(&USB_tx_buffer, 'E', NC_ADDRESS, 1, (u8 *)&ErrorMSG, sizeof(ErrorMSG));
USB_Request_ErrorMessage = FALSE;
}
else if(USB_Request_NewWaypoint && (USB_tx_buffer.Locked == FALSE))
{
u8 WPNumber = WPList_GetCount();
MKProtocol_CreateSerialFrame(&USB_tx_buffer, 'W', NC_ADDRESS, 1, &WPNumber, sizeof(WPNumber));
USB_Request_NewWaypoint = FALSE;
}
else if((USB_Request_ReadWaypoint != 0xFF) && (USB_tx_buffer.Locked == FALSE))
{
u8 WPNumber = WPList_GetCount();
if (USB_Request_ReadWaypoint < WPNumber)
{
MKProtocol_CreateSerialFrame(&USB_tx_buffer, 'X', NC_ADDRESS, 3, &WPNumber, 1, &USB_Request_ReadWaypoint, 1, WPList_GetAt(USB_Request_ReadWaypoint), sizeof(Waypoint_t));
}
else
{
MKProtocol_CreateSerialFrame(&USB_tx_buffer,'X', NC_ADDRESS, 1, &WPNumber, sizeof(WPNumber));
}
USB_Request_ReadWaypoint = 0xFF;
}
USB_Transmit(); // output pending bytes in tx buffer
}
/trunk/waypoints.c
61,61 → 61,67
#include "uart1.h"
 
// the waypoints list
#define WPLISTLEN 31
#define MAX_LIST_LEN 31
 
Waypoint_t WPList[WPLISTLEN];
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
Point_t PointList[MAX_LIST_LEN];
u8 WPIndex = 0; // list index of GPS point representig the current WP, can be maximal WPCount
u8 POIIndex = 0; // list index of GPS Point representing the current POI, can be maximal WPCount
u8 WPCount = 0; // number of waypoints
u8 PointCount = 0; // number of wp in the list can be maximal equal to MAX_LIST_LEN
 
u8 WPList_Init(void)
u8 WPActive = TRUE;
 
u8 PointList_Init(void)
{
return WPList_Clear();
return PointList_Clear();
}
 
u8 WPList_Clear(void)
u8 PointList_Clear(void)
{
u8 i;
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;
WPCount = 0; // no waypoints
PointCount = 0; // no contents
WPActive = TRUE;
NaviData.WaypointNumber = WPCount;
NaviData.WaypointIndex = 0;
 
for(i = 0; i < WPLISTLEN; i++)
for(i = 0; i < MAX_LIST_LEN; i++)
{
WPList[i].Position.Status = INVALID;
WPList[i].Position.Latitude = 0;
WPList[i].Position.Longitude = 0;
WPList[i].Position.Altitude = 0;
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;
PointList[i].Position.Status = INVALID;
PointList[i].Position.Latitude = 0;
PointList[i].Position.Longitude = 0;
PointList[i].Position.Altitude = 0;
PointList[i].Heading = 361; // invalid value
PointList[i].ToleranceRadius = 0; // in meters, if the MK is within that range around the target, then the next target is triggered
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
PointList[i].Event_Flag = 0; // future implementation
PointList[i].Type = POINT_TYPE_INVALID;
}
return TRUE;
}
 
u8 WPList_GetCount(void)
u8 PointList_GetCount(void)
{
return WPNumber; // number of points in the list
return PointCount; // number of points in the list
}
 
u8 WPList_Append(Waypoint_t* pwp)
u8 PointList_Append(Point_t* pPoint)
{
if((WPNumber < WPLISTLEN) && (pwp->Index == (WPNumber + 1)) ) // there is still some space in the list and index points to next
if((PointCount < MAX_LIST_LEN) && (pPoint->Index == (PointCount + 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
memcpy(&PointList[PointCount], pPoint, sizeof(Point_t)); // copy data to list entry // increment list length
if(PointList[PointCount].Type == POINT_TYPE_WP) WPCount++;
NaviData.WaypointNumber = WPCount;
PointCount++;
if(PointCount == 1) // only for the first entry
{
// update POI index
switch(WPList[WPIndex-1].Type)
switch(PointList[WPIndex-1].Type)
{
case POINT_TYPE_WP:
if(WPList[WPIndex-1].Heading < 0) POIIndex = (u8)(-WPList[WPIndex-1].Heading);
if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
else POIIndex = 0;
break;
134,19 → 140,20
}
 
// returns the pointer to the first waypoint within the list
Waypoint_t* WPList_Begin(void)
Point_t* PointList_WPBegin(void)
{
u8 i;
WPIndex = 0; // set list position invalid
POIIndex = 0; // set invalid POI
 
if(WPNumber > 0)
if(WPActive == FALSE) return(NULL);
 
if(PointCount > 0)
{
 
// search for first wp in list
for(i = 0; i < WPNumber; i++)
for(i = 0; i <PointCount; i++)
{
if((WPList[i].Type == POINT_TYPE_WP) && (WPList[i].Position.Status != INVALID))
if((PointList[i].Type == POINT_TYPE_WP) && (PointList[i].Position.Status != INVALID))
{
WPIndex = i + 1;
break;
154,19 → 161,19
}
if(WPIndex) // found a WP in the list
{
NaviData.WaypointIndex = WPIndex;
NaviData.WaypointIndex = 1;
// update index to POI
if(WPList[WPIndex-1].Heading < 0) POIIndex = (u8)(-WPList[WPIndex-1].Heading);
if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
else POIIndex = 0;
return(&(WPList[WPIndex-1])); // if list is not empty return pointer to first waypoint in the list
return(&(PointList[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;
NaviData.WaypointIndex = 0;
//Check for an existing POI
for(i = 0; i < WPNumber; i++)
for(i = 0; i < PointCount; i++)
{
if((WPList[i].Type == POINT_TYPE_POI) && (WPList[i].Position.Status != INVALID))
if((PointList[i].Type == POINT_TYPE_POI) && (PointList[i].Position.Status != INVALID))
{
POIIndex = i + 1;
break;
178,71 → 185,73
else // no point in the list
{
POIIndex = 0;
NaviData.WaypointIndex = WPIndex;
NaviData.WaypointIndex = 0;
return NULL;
}
}
 
// returns the last waypoint
Waypoint_t* WPList_End(void)
Point_t* PointList_WPEnd(void)
{
u8 i;
WPIndex = 0; // set list position invalid
POIIndex = 0; // set invalid
if(WPActive == FALSE) return(NULL);
 
if(WPNumber > 0)
if(PointCount > 0)
{
// search backward!
for(i = 1; i <= WPNumber; i++)
for(i = 1; i <= PointCount; i++)
{
if((WPList[WPNumber - i].Type == POINT_TYPE_WP) && (WPList[WPNumber - i].Position.Status != INVALID))
if((PointList[PointCount - i].Type == POINT_TYPE_WP) && (PointList[PointCount - i].Position.Status != INVALID))
{
WPIndex = WPNumber - i + 1;
WPIndex = PointCount - i + 1;
break;
}
}
if(WPIndex) // found a WP within the list
{
NaviData.WaypointIndex = WPIndex;
if(WPList[WPIndex-1].Heading < 0) POIIndex = (u8)(-WPList[WPIndex-1].Heading);
NaviData.WaypointIndex = WPCount;
if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
else POIIndex = 0;
return(&(WPList[WPIndex-1]));
return(&(PointList[WPIndex-1]));
}
else // list contains some points but no WP in the list
{
// search backward for a POI!
for(i = 1; i <= WPNumber; i++)
for(i = 1; i <= PointCount; i++)
{
if((WPList[WPNumber - i].Type == POINT_TYPE_POI) && (WPList[WPNumber - i].Position.Status != INVALID))
if((PointList[PointCount - i].Type == POINT_TYPE_POI) && (PointList[PointCount - i].Position.Status != INVALID))
{
POIIndex = WPNumber - i + 1;
POIIndex = PointCount - i + 1;
break;
}
}
NaviData.WaypointIndex = WPIndex;
NaviData.WaypointIndex = 0;
return NULL;
}
}
else // no point in the list
{
POIIndex = 0;
NaviData.WaypointIndex = WPIndex;
return NULL;
POIIndex = 0;
NaviData.WaypointIndex = 0;
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)
Point_t* PointList_WPNext(void)
{
u8 wp_found = 0;
if(WPActive == FALSE) return(NULL);
if(WPIndex < WPNumber) // if there is a next entry in the list
if(WPIndex < PointCount) // if there is a next entry in the list
{
u8 i;
for(i = WPIndex; i < WPNumber; i++) // start search for next at next list entry
for(i = WPIndex; i < PointCount; i++) // start search for next at next list entry
{
if((WPList[i].Type == POINT_TYPE_WP) && (WPList[i].Position.Status != INVALID)) // jump over POIs
if((PointList[i].Type == POINT_TYPE_WP) && (PointList[i].Position.Status != INVALID)) // jump over POIs
{
wp_found = i+1;
break;
252,27 → 261,33
if(wp_found)
{
WPIndex = wp_found; // update list position
NaviData.WaypointIndex = WPIndex;
if(WPList[WPIndex-1].Heading < 0) POIIndex = (u8)(-WPList[WPIndex-1].Heading);
NaviData.WaypointIndex++;
if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
else POIIndex = 0;
return(&(WPList[WPIndex-1])); // return pointer to this waypoint
return(&(PointList[WPIndex-1])); // return pointer to this waypoint
}
else
{
NaviData.WaypointIndex = WPIndex;
{ // no next wp found
NaviData.WaypointIndex = 0;
POIIndex = 0;
return(NULL);
}
}
 
void PointList_WPActive(u8 set)
{
if(set) WPActive = TRUE;
else WPActive = FALSE;
}
Waypoint_t* WPList_GetAt(u8 index)
Point_t* PointList_GetAt(u8 index)
{
if((index > 0) && (index <= WPNumber)) return(&(WPList[index-1])); // return pointer to this waypoint
if((index > 0) && (index <= PointCount)) return(&(PointList[index-1])); // return pointer to this waypoint
else return(NULL);
}
 
Waypoint_t* WPList_GetPOI(void)
Point_t* PointList_GetPOI(void)
{
return WPList_GetAt(POIIndex);
return PointList_GetAt(POIIndex);
}
 
/trunk/waypoints.h
18,25 → 18,28
u8 Type; // typeof Waypoint
u8 WP_EventChannelValue; //
u8 reserve[9]; // reserve
} __attribute__((packed)) Waypoint_t;
} __attribute__((packed)) Point_t;
 
// Init List, return TRUE on success
u8 WPList_Init(void);
u8 PointList_Init(void);
// Clear List, return TRUE on success
u8 WPList_Clear(void);
u8 PointList_Clear(void);
// Returns number of points in the list
u8 WPList_GetCount(void);
u8 PointList_GetCount(void);
// return pointer to point at position
Point_t* PointList_GetAt(u8 index);
// appends a point to the list, returns TRUE on success
u8 WPList_Append(Waypoint_t* pwp);
u8 PointList_Append(Point_t* pwp);
// goto the first WP in the list and return pointer to it
Waypoint_t* WPList_Begin(void);
Point_t* PointList_WPBegin(void);
// goto the last WP in the list and return pointer to it
Waypoint_t* WPList_End(void);
Point_t* PointList_WPEnd(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);
Point_t* PointList_WPNext(void);
// disables waypoint function
void PointList_WPActive(u8 set);
// returns pointer to actual POI
Waypoint_t* WPList_GetPOI(void);
Point_t* PointList_GetPOI(void);
 
 
#endif // _WAYPOINTS_H