Subversion Repositories NaviCtrl

Compare Revisions

Ignore whitespace Rev 294 → Rev 295

/trunk/params.c
4,19 → 4,24
#include "waypoints.h"
 
s16 NCParams[255];
u8 NCParamState[255];
 
void NCParams_Init(void)
{
u16 i;
for(i=0; i<256; i++)
{
NCParamState[i] = NCPARAM_STATE_UNDEFINED;
}
NCParams[NCPARAMS_GPS_TARGETSPEED] = 50; // 5.0 m/s
NCParams[NCPARAMS_NEW_COMPASS_DIRECTION_SETPOINT] = -1;
NCParams[NCPARAMS_NEW_CAMERA_ELEVATION] = -30000; // invalid
NCParams[NCPARAMS_ALTITUDE_RATE] = -1;
NCParams[NCPARAMS_ALTITUDE_SETPOINT] = -30000; // in 0,1m -3000 -> invalid
NCParams[NCPARAMS_ALTITUDE_RATE] = -1;
}
 
u8 NCParams_SetValue(u8 id, s16* pvalue)
{
NCParams[id] = *pvalue;
NCParamState[id] = NCRARAM_STATE_VALID;
 
switch(id)
{
32,12 → 37,15
default:
break;
}
return 1;
return NCParamState[id];
}
 
u8 NCParams_GetValue(u8 id, s16* pvalue)
{
if(pvalue == 0) return 0;
*pvalue = NCParams[id];
return 1;
if(NCParamState[id] == NCRARAM_STATE_VALID)
{
*pvalue = NCParams[id];
}
return NCParamState[id];
}
/trunk/params.h
5,12 → 5,14
#define NCPARAMS_GPS_TARGETSPEED 0
#define NCPARAMS_NEW_COMPASS_DIRECTION_SETPOINT 1
#define NCPARAMS_NEW_CAMERA_ELEVATION 2
#define NCPARAMS_ALTITUDE_RATE 3
#define NCPARAMS_ALTITUDE_RATE 3
#define NCPARAMS_ALTITUDE_SETPOINT 4
 
#define NCPARAM_STATE_UNDEFINED 0
#define NCRARAM_STATE_VALID 1
 
extern void NCParams_Init();
extern u8 NCParams_SetValue(u8 id, s16* pvalue);
extern u8 NCParams_GetValue(u8 id, s16* pvalue);
extern s16 NCParams[255];
 
#endif // _PARAMS_H
/trunk/spi_slave.c
313,15 → 313,11
ToFlightCtrl.Param.sInt[4] = -1;
}
 
if(NCParams[NCPARAMS_NEW_CAMERA_ELEVATION] != -30000) // Elevation set via 'j' command
{
ToFlightCtrl.Param.sInt[5] = NCParams[NCPARAMS_NEW_CAMERA_ELEVATION];
}
else
{
if(FC.StatusFlags2 & FC_STATUS2_CAREFREE) ToFlightCtrl.Param.sInt[5] = CAM_Orientation.Elevation; // only, if carefree is active
else ToFlightCtrl.Param.sInt[5] = 0;
}
if(NCRARAM_STATE_VALID != NCParams_GetValue(NCPARAMS_NEW_CAMERA_ELEVATION, &(ToFlightCtrl.Param.sInt[5]))) // Elevation set via 'j' command
{
if(FC.StatusFlags2 & FC_STATUS2_CAREFREE) ToFlightCtrl.Param.sInt[5] = CAM_Orientation.Elevation; // only, if carefree is active
else ToFlightCtrl.Param.sInt[5] = 0;
}
break;
 
case SPI_NCCMD_VERSION:
343,10 → 339,21
ToFlightCtrl.Param.Int[2] = NaviData.HomePositionDeviation.Distance; // dm //4&5
ToFlightCtrl.Param.sInt[3] = NaviData.HomePositionDeviation.Bearing; // deg //6&7
ToFlightCtrl.Param.Byte[8] = (s8)(FC_WP_EventChannel - 110);
if(NCParams[NCPARAMS_ALTITUDE_RATE] != -1) ToFlightCtrl.Param.Byte[9] = (u8) NCParams[NCPARAMS_ALTITUDE_RATE];
else ToFlightCtrl.Param.Byte[9] = (u8) ToFC_AltitudeRate;
if(NCParams[NCPARAMS_ALTITUDE_SETPOINT] != -30000) ToFlightCtrl.Param.sInt[5] = (s16) NCParams[NCPARAMS_ALTITUDE_SETPOINT];
else ToFlightCtrl.Param.sInt[5] = (s16) ToFC_AltitudeSetpoint;
{
s16 tmp;
if(NCRARAM_STATE_VALID == NCParams_GetValue(NCPARAMS_ALTITUDE_RATE, &tmp))
{
ToFlightCtrl.Param.Byte[9] = (u8)tmp;
}
else
{
ToFlightCtrl.Param.Byte[9] = (u8)ToFC_AltitudeRate;
}
if(NCRARAM_STATE_VALID != NCParams_GetValue(NCPARAMS_ALTITUDE_SETPOINT, &(ToFlightCtrl.Param.sInt[5])))
{
ToFlightCtrl.Param.sInt[5] = (s16) ToFC_AltitudeSetpoint;
}
}
break;
default:
break;
/trunk/uart1.c
91,7 → 91,7
u8 UART1_Request_DebugLabel = 255;
u8 UART1_Request_NaviData = FALSE;
u8 UART1_Request_ErrorMessage = FALSE;
u8 UART1_Request_NewPoint = FALSE;
u8 UART1_Request_WritePoint = 0xFF;
u8 UART1_Request_ReadPoint = 0;
u8 UART1_Request_Data3D = FALSE;
u8 UART1_Request_Echo = FALSE;
404,45 → 404,11
pPoint = (Point_t*)SerialMsg.pData;
if(pPoint->Position.Status == NEWDATA)
{
if(pPoint->Type == POINT_TYPE_POI)
{
PointList_Clear(); // empty List
pPoint->Index = 1; // must be one after empty list
POICount = 0;
PointList_Append(pPoint);
PointCount = 2;
BeepTime = 50;
}
else
if(pPoint->Type == POINT_TYPE_WP)
{
PointList_Clear(); // empty List
PointCount = 1;
pPoint->Index = 2; // No. 1 could be the POI
WPCount = 0;
PointList_Append(pPoint);
PointCount = 2;
BeepTime = 50;
GPS_pWaypoint = PointList_WPBegin();
}
else
if(pPoint->Type == POINT_TYPE_BOTH)
{
PointList_Clear(); // empty List
pPoint->Index = 1;
pPoint->Type = POINT_TYPE_POI;
PointList_Append(pPoint);
pPoint->Index = 2;
pPoint->Type = POINT_TYPE_WP;
PointList_Append(pPoint);
BeepTime = 50;
GPS_pWaypoint = PointList_WPBegin();
}
else
{
PointList_Clear(); // empty List
GPS_pWaypoint = PointList_WPBegin();
}
//PointList_Clear(); // flush the list
pPoint->Index = 1; // must be one after empty list
PointList_SetAt(pPoint); //
GPS_pWaypoint = PointList_WPBegin(); // updates POI index
BeepTime = 50;
}
break;
 
475,7 → 441,7
}
break;
 
case 'w':// Append Point to List
case 'w':// Set point in list at index
{
pPoint = (Point_t*)SerialMsg.pData;
 
483,14 → 449,14
{
PointList_Clear();
GPS_pWaypoint = PointList_WPBegin();
UART1_Request_NewPoint = TRUE; // return new point count
UART1_Request_WritePoint = 0; // return new point count
}
else
{ // app current WP to the list
if(PointList_Append(pPoint))
{ // update WP in list at index
UART1_Request_WritePoint = PointList_SetAt(pPoint);
if(UART1_Request_WritePoint == pPoint->Index)
{
BeepTime = 500;
UART1_Request_NewPoint = TRUE; // return new WP number
}
}
}
677,11 → 643,10
Echo = 0; // reset echo value
UART1_Request_Echo = FALSE;
}
else if(UART1_Request_NewPoint && (UART1_tx_buffer.Locked == FALSE))
else if((UART1_Request_WritePoint!= 0xFF) && (UART1_tx_buffer.Locked == FALSE))
{
u8 PointCount = PointList_GetCount();
MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'W', NC_ADDRESS, 1, &PointCount, sizeof(PointCount));
UART1_Request_NewPoint = FALSE;
MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'W', NC_ADDRESS, 1, &UART1_Request_WritePoint, sizeof(UART1_Request_WritePoint));
UART1_Request_WritePoint = 0xFF;
}
else if((UART1_Request_ReadPoint) && (UART1_tx_buffer.Locked == FALSE))
{
/trunk/waypoints.c
110,38 → 110,86
return PointCount; // number of points in the list
}
 
u8 PointList_Append(Point_t* pPoint)
Point_t* PointList_GetAt(u8 index)
{
if((PointCount < MAX_LIST_LEN) && (pPoint->Index == (PointCount + 1)) ) // there is still some space in the list and index points to next
if((index > 0) && (index <= PointCount)) return(&(PointList[index-1])); // return pointer to this waypoint
else return(NULL);
}
 
u8 PointList_SetAt(Point_t* pPoint)
{
// if index is in range
if((pPoint->Index > 0) && (pPoint->Index < MAX_LIST_LEN))
{
memcpy(&PointList[PointCount], pPoint, sizeof(Point_t)); // copy data to list entry // increment list length
if(PointList[PointCount].Type == POINT_TYPE_WP) WPCount++;
if(PointList[PointCount].Type == POINT_TYPE_POI) POICount++;
NaviData.WaypointNumber = WPCount;
PointCount++;
if(PointCount == 1) // only for the first entry
// check list entry before update
if(PointList[pPoint->Index-1].Type)
switch(PointList[pPoint->Index-1].Type)
{
// update POI index
switch(PointList[WPIndex-1].Type)
{
case POINT_TYPE_WP:
if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
else POIIndex = 0;
break;
case POINT_TYPE_INVALID: // was invalid
switch(pPoint->Type)
{
default:
case POINT_TYPE_INVALID:
// nothing to do
break;
 
case POINT_TYPE_WP:
WPCount++;
PointCount++;
break;
case POINT_TYPE_POI:
POICount++;
PointCount++;
break;
}
break;
case POINT_TYPE_POI:
POIIndex = 1;
POICount = 1;
break;
case POINT_TYPE_WP: // was a waypoint
switch(pPoint->Type)
{
case POINT_TYPE_INVALID:
WPCount--;
PointCount--;
break;
 
default:
POIIndex = 0;
break;
}
default:
case POINT_TYPE_WP:
//nothing to do
break;
case POINT_TYPE_POI:
POICount++;
WPCount--;
break;
}
break;
case POINT_TYPE_POI: // was a poi
switch(pPoint->Type)
{
case POINT_TYPE_INVALID:
POICount--;
PointCount--;
break;
 
case POINT_TYPE_WP:
WPCount++;
POICount--;
break;
case POINT_TYPE_POI:
default:
// nothing to do
break;
}
break;
}
return TRUE;
memcpy(&PointList[pPoint->Index-1], pPoint, sizeof(Point_t)); // copy data to list entry
NaviData.WaypointNumber = WPCount;
return pPoint->Index;
}
else return FALSE;
else return 0;
}
 
// returns the pointer to the first waypoint within the list
149,12 → 197,14
{
u8 i;
WPIndex = 0; // set list position invalid
 
if(WPActive == FALSE) return(NULL);
 
POIIndex = 0; // set invalid POI
if(PointCount > 0)
{
// search for first wp in list
for(i = 0; i <PointCount; i++)
for(i = 0; i <MAX_LIST_LEN; i++)
{
if((PointList[i].Type == POINT_TYPE_WP) && (PointList[i].Position.Status != INVALID))
{
174,7 → 224,7
{
NaviData.WaypointIndex = 0;
//Check for an existing POI
for(i = 0; i < PointCount; i++)
for(i = 0; i < MAX_LIST_LEN; i++)
{
if((PointList[i].Type == POINT_TYPE_POI) && (PointList[i].Position.Status != INVALID))
{
182,15 → 232,16
break;
}
}
return NULL;
}
}
else // no point in the list
{
POIIndex = 0;
NaviData.WaypointIndex = 0;
return NULL;
NaviData.WaypointIndex = 0;
}
 
if(WPIndex) return(&(PointList[WPIndex-1]));
else return(NULL);
}
 
// returns the last waypoint
206,11 → 257,11
if(PointCount > 0)
{
// search backward!
for(i = 1; i <= PointCount; i++)
for(i = 1; i <= MAX_LIST_LEN; i++)
{
if((PointList[PointCount - i].Type == POINT_TYPE_WP) && (PointList[PointCount - i].Position.Status != INVALID))
if((PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_WP) && (PointList[MAX_LIST_LEN - i].Position.Status != INVALID))
{
WPIndex = PointCount - i + 1;
WPIndex = MAX_LIST_LEN - i + 1;
break;
}
}
219,21 → 270,19
NaviData.WaypointIndex = WPCount;
if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
else POIIndex = 0;
return(&(PointList[WPIndex-1]));
}
else // list contains some points but no WP in the list
{
// search backward for a POI!
for(i = 1; i <= PointCount; i++)
for(i = 1; i <= MAX_LIST_LEN; i++)
{
if((PointList[PointCount - i].Type == POINT_TYPE_POI) && (PointList[PointCount - i].Position.Status != INVALID))
if((PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_POI) && (PointList[MAX_LIST_LEN - i].Position.Status != INVALID))
{
POIIndex = PointCount - i + 1;
POIIndex = MAX_LIST_LEN - i + 1;
break;
}
}
NaviData.WaypointIndex = 0;
return NULL;
NaviData.WaypointIndex = 0;
}
}
else // no point in the list
240,8 → 289,9
{
POIIndex = 0;
NaviData.WaypointIndex = 0;
return NULL;
}
if(WPIndex) return(&(PointList[WPIndex-1]));
else return(NULL);
}
 
// returns a pointer to the next waypoint or NULL if the end of the list has been reached
250,10 → 300,10
u8 wp_found = 0;
if(WPActive == FALSE) return(NULL);
if(WPIndex < PointCount) // if there is a next entry in the list
if(WPIndex < MAX_LIST_LEN) // if there is a next entry in the list
{
u8 i;
for(i = WPIndex; i < PointCount; i++) // start search for next at next list entry
for(i = WPIndex; i < MAX_LIST_LEN; i++) // start search for next at next list entry
{
if((PointList[i].Type == POINT_TYPE_WP) && (PointList[i].Position.Status != INVALID)) // jump over POIs
{
280,16 → 330,18
 
void PointList_WPActive(u8 set)
{
if(set) WPActive = TRUE;
else WPActive = FALSE;
if(set)
{
WPActive = TRUE;
PointList_WPBegin(); // uopdates POI index
}
else
{
WPActive = FALSE;
POIIndex = 0; // disable POI also
}
}
Point_t* PointList_GetAt(u8 index)
{
if((index > 0) && (index <= PointCount)) return(&(PointList[index-1])); // return pointer to this waypoint
else return(NULL);
}
 
Point_t* PointList_GetPOI(void)
{
return PointList_GetAt(POIIndex);
/trunk/waypoints.h
6,14 → 6,7
#define POINT_TYPE_INVALID 255
#define POINT_TYPE_WP 0
#define POINT_TYPE_POI 1
#define POINT_TYPE_BOTH 2 // only command 's'
 
extern u8 WPIndex; // list index of GPS point representig the current WP, can be maximal WPCount
extern u8 POIIndex; // list index of GPS Point representing the current POI, can be maximal WPCount
extern u8 WPCount; // number of waypoints
extern u8 PointCount; // number of wp in the list can be maximal equal to MAX_LIST_LEN
extern u8 POICount;
 
typedef struct
{
GPS_Pos_t Position; // the gps position of the waypoint, see ubx.h for details
28,10 → 21,6
u8 reserve[8]; // reserve
} __attribute__((packed)) Point_t;
 
extern Point_t* GPS_pPoi; // pointer to the actual point of interrest
extern u8 POICount;
 
 
// Init List, return TRUE on success
u8 PointList_Init(void);
// Clear List, return TRUE on success
40,8 → 29,8
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 PointList_Append(Point_t* pwp);
// set a point in the list at index, returns its index on success, else 0
u8 PointList_SetAt(Point_t* pPoint);
// goto the first WP in the list and return pointer to it
Point_t* PointList_WPBegin(void);
// goto the last WP in the list and return pointer to it
48,7 → 37,7
Point_t* PointList_WPEnd(void);
// goto next WP in the list and return pointer to it
Point_t* PointList_WPNext(void);
// disables waypoint function
// enables/disables waypoint function
void PointList_WPActive(u8 set);
// returns pointer to actual POI
Point_t* PointList_GetPOI(void);