/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); |