/trunk/GPS.c |
---|
76,9 → 76,8 |
#include "waypoints.h" |
#include "i2c.h" |
#define GPS_UPDATETIME_MS 200 // 200ms is 5 Hz |
#define M_PI_180 (M_PI / 180.0f) |
#define GPS_UPDATETIME_MS 200 // 200ms is 5 Hz |
typedef enum |
{ |
GPS_FLIGHT_MODE_UNDEF, |
108,17 → 107,9 |
GPS_FlightMode_t FlightMode; |
} __attribute__((packed)) GPS_Parameter_t; |
typedef struct |
{ |
u8 Status; // invalid, newdata, processed |
s32 North; // in cm |
s32 East; // in cm |
s32 Bearing; // in deg |
s32 Distance; // in cm |
} __attribute__((packed)) GPS_Deviation_t; |
GPS_Deviation_t CurrentTargetDeviation; // Deviation from Target |
GPS_Deviation_t CurrentHomeDeviation; // Deviation from Home |
GPS_Deviation_t TargetHomeDeviation; // Deviation from Target to Home |
GPS_Pos_Deviation_t CurrentTargetDeviation; // Deviation from Target |
GPS_Pos_Deviation_t CurrentHomeDeviation; // Deviation from Home |
GPS_Pos_Deviation_t TargetHomeDeviation; // Deviation from Target to Home |
GPS_Stick_t GPS_Stick; |
132,7 → 123,7 |
GPS_Pos_t GPS_HomePosition = {0,0,0, INVALID}; // the home position |
GPS_Pos_t * GPS_pTargetPosition = NULL; // pointer to the actual target position |
u32 GPS_TargetRadius = 0; // catch radius for target area |
Point_t* GPS_pWaypoint = NULL; // pointer to the actual waypoint |
Point_t* GPS_pWaypoint = NULL; // pointer to the actual waypoint |
//------------------------------------------------------------- |
// Update GPSParamter |
154,22 → 145,8 |
return 0; |
} |
//------------------------------------------------------------ |
// copy GPS position from source position to target position |
u8 GPS_CopyPosition(GPS_Pos_t * pGPSPosSrc, GPS_Pos_t* pGPSPosTgt) |
{ |
return 0; |
} |
//------------------------------------------------------------ |
// clear position data |
u8 GPS_ClearPosition(GPS_Pos_t * pGPSPos) |
{ |
return 0; |
} |
//------------------------------------------------------------ |
void GPS_Neutral(void) |
{ |
} |
180,42 → 157,7 |
} |
//------------------------------------------------------------ |
// calculate the bearing to target position from its deviation |
s32 DirectionToTarget_N_E(float northdev, float eastdev) |
{ |
return 0; |
} |
//------------------------------------------------------------ |
// Rescale xy-vector length if length limit is violated |
// returns vector len after scaling |
s32 GPS_LimitXY(s32 *x, s32 *y, s32 limit) |
{ |
return 0; |
} |
//------------------------------------------------------------ |
// transform the integer deg into float radians |
inline double RadiansFromGPS(s32 deg) |
{ |
return 0.0; |
} |
//------------------------------------------------------------ |
// transform the integer deg into float deg |
inline double DegFromGPS(s32 deg) |
{ |
return 0.0; |
} |
//------------------------------------------------------------ |
// calculate the deviation from the current position to the target position |
u8 GPS_CalculateDeviation(GPS_Pos_t * pCurrentPos, GPS_Pos_t * pTargetPos, GPS_Deviation_t* pDeviationFromTarget) |
{ |
return 0; |
} |
void GPS_Navigation(gps_data_t *pGPS_Data, GPS_Stick_t* pGPS_Stick) |
{ |
return; |
/trunk/gpspos.h |
---|
0,0 → 1,45 |
#ifndef _GPS_POS_H |
#define _GPS_POS_H |
#define INVALID 0x00 |
#define NEWDATA 0x01 |
#define PROCESSED 0x02 |
typedef struct |
{ |
s32 Longitude; // in 1E-7 deg |
s32 Latitude; // in 1E-7 deg |
s32 Altitude; // in mm |
u8 Status; // validity of data |
} __attribute__((packed)) GPS_Pos_t; |
typedef struct |
{ |
u8 Status; // invalid, newdata, processed |
s32 North; // in cm deviation from target |
s32 East; // in cm deviation from target |
s32 Bearing; // in deg to target |
s32 Distance; // in cm to target |
} __attribute__((packed)) GPS_Pos_Deviation_t; |
/* |
// transform the integer 1E-7 deg into float radians |
float GPSPos_ToRadians(s32 deg); |
// transform the integer 1E-7 deg into float deg |
float GPSPos_ToDeg(s32 deg); |
*/ |
// clear GPS position data |
u8 GPSPos_Clear(GPS_Pos_t* pGPSPos); |
// copy GPS position from source position to target position |
u8 GPSPos_Copy(GPS_Pos_t* pGPSPosSrc, GPS_Pos_t* pGPSPosTgt); |
// calculate the deviation from the source position to the target position |
u8 GPSPos_Deviation(GPS_Pos_t* pReferencePos, GPS_Pos_t* pTargetPos, GPS_Pos_Deviation_t* pDeviation); |
// Move the gps position according to north and east shift in cm |
u8 GPSPos_ShiftCartesian(GPS_Pos_t* pGPSPos, s32 NorthShift, s32 EastShift); |
// Move the gps position according to the direction of bearing in deg by the given distance in cm |
u8 GPSPos_ShiftGeodetic(GPS_Pos_t* pGPSPos, s32 Bearing, s32 Distance); |
// Move Pos so that distance to ReferencePos is limited to MaxDistance in cm |
// returns 1 if Pos has been moved to fit the limit |
u8 GPSPos_CatchDistance(GPS_Pos_t* pPos, GPS_Pos_t* pReferencePos, s32 MaxDistance); |
#endif //_GPS_POS_H |
/trunk/main.c |
---|
55,7 → 55,7 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//#define MCLK96MHZ |
const unsigned long _Main_Crystal = 25000; |
#include <stdio.h> |
//#include <stdio.h> |
#include "91x_lib.h" |
#include "led.h" |
#include "uart0.h" |
357,7 → 357,7 |
newErrorCode = 24; |
DebugOut.StatusRed |= AMPEL_BL; |
} |
else if(NCFlags & NC_FLAG_RANGE_LIMIT && (FC.StatusFlags & FC_STATUS_FLY) && !ErrorCode) |
else if((NCFlags & NC_FLAG_RANGE_LIMIT) && (FC.StatusFlags & FC_STATUS_FLY) && !ErrorCode) |
{ |
LED_RED_ON; |
sprintf(ErrorMSG,"ERR:GPS range "); |
587,7 → 587,6 |
UART1_PutString("\r\n"); |
CompassValueErrorCount = 0; |
//PointList_ReadFromFile(1); |
for (;;) // the endless main loop |
{ |
PollingTimeout = 5; |
/trunk/mymath.c |
---|
37,8 → 37,8 |
s16 c_arccos2(s32 a,s32 b) |
{ |
if(a>b) return(0); |
return(arccos64[64 * a / b]); |
if(a>b) return(0); |
return(arccos64[64 * a / b]); |
} |
// cosinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
/trunk/uart1.c |
---|
490,7 → 490,6 |
if((pPoint->Position.Status == INVALID) && (pPoint->Index == 0)) |
{ |
//PointList_SaveToFile(1); // save to file before clearing the list |
PointList_Clear(); |
GPS_pWaypoint = PointList_WPBegin(); |
UART1_Request_WritePoint = 0; // return new point count |
529,6 → 528,15 |
NCParams_SetValue(SerialMsg.pData[1], &value); |
} |
break; |
case 2: |
// read wp list from file |
if(PointList_ReadFromFile(SerialMsg.pData[2])) |
{ //and move all wp relative so that 1st wp gets current positition |
PointList_Move(1, &GPSData.Position); |
} |
else PointList_Clear(); |
break; |
default: |
break; |
/trunk/uart1.h |
---|
98,7 → 98,7 |
typedef struct |
{ |
u8 Version; // version of the data structure |
GPS_Pos_t CurrentPosition; // see ubx.h for details |
GPS_Pos_t CurrentPosition; // see gpspos.h for details |
GPS_Pos_t TargetPosition; |
GPS_PosDev_t TargetPositionDeviation; |
GPS_Pos_t HomePosition; |
/trunk/ubx.c |
---|
295,7 → 295,6 |
// update message cycle time |
GPSData.MsgCycleTime = (u16)(UbxSol.itow-last_itow); |
last_itow = UbxSol.itow; // update last itow |
// DebugOut.Analog[16] = GPSData.MsgCycleTime; |
// NAV SOL |
GPSData.Flags = (GPSData.Flags & 0xf0) | (UbxSol.Flags & 0x0f); // we take only the lower bits |
GPSData.NumOfSats = UbxSol.numSV; |
324,19 → 323,6 |
UbxVelNed.Status = PROCESSED; // ready for new data |
} // EOF all itow are equal |
} // EOF all ubx messages received |
//++++++++ |
//GPSData.Position.Longitude = 1517409123L; // Hamilton, Australia |
//GPSData.Position.Latitude = -329294773L; // Hamilton, Australia |
//++++++++ |
//GPSData.Position.Longitude =-1556010020L; // Alaska |
//GPSData.Position.Latitude = 629581270L; // Alaska |
//++++++++ |
//GPSData.Position.Longitude =-584343419L; // Buenos aires |
//GPSData.Position.Latitude = -345464421L; // Buenos aires |
//++++++++ |
//GPSData.Position.Longitude =1683362691L; // Neuseeland |
//GPSData.Position.Latitude = -465945926L; // Neuseeland |
} |
/trunk/ubx.h |
---|
1,6 → 1,8 |
#ifndef __UBX_H |
#define __UBX_H |
#include "buffer.h" |
#include "gpspos.h" |
// Satfix types for GPSData.SatFix |
#define SATFIX_NONE 0x00 |
17,21 → 19,9 |
#define FLAG_TOWSET 0x08 // (is Time of Week valid) |
#define FLAG_GPS_NAVIGATION_ACT 0x10 // to FC -> NC is ready to navigate |
#define INVALID 0x00 |
#define NEWDATA 0x01 |
#define PROCESSED 0x02 |
typedef struct |
{ |
s32 Longitude; // in 1E-7 deg |
s32 Latitude; // in 1E-7 deg |
s32 Altitude; // in mm |
u8 Status;// validity of data |
} __attribute__((packed)) GPS_Pos_t; |
typedef struct |
{ |
u16 MsgCycleTime; // time in ms since last gps data |
GPS_Pos_t Position; // Lat/Lon/Alt |
u8 Flags; // Status Flags |
/trunk/waypoints.c |
---|
69,8 → 69,8 |
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 POICount = 0; |
u8 PointCount = 0; // number of points in the list can be maximal equal to MAX_LIST_LEN |
u8 POICount = 0; // number of point of interest in the list |
u8 WPActive = FALSE; |
338,7 → 338,7 |
if(set) |
{ |
WPActive = TRUE; |
PointList_WPBegin(); // uopdates POI index |
PointList_WPBegin(); // updates POI index |
} |
else |
{ |
660,5 → 660,40 |
} // EOF if(Fat16_IsValid()) |
else UART1_PutString("no file system found!\r\n"); |
return(retval); |
} |
} |
// move actual point list to ref pos., the point in the list marked by index gets the RefPos afterwards |
u8 PointList_Move(u8 RefIndex, GPS_Pos_t* pRefPos) |
{ |
u8 retval = 0; |
GPS_Pos_t RefPos_old; |
GPS_Pos_Deviation_t RefDeviation; |
// check inputs for plausibility; |
if((RefIndex == 0) || (RefIndex > PointCount)) return(retval); |
if(pRefPos == NULL) return(retval); |
if(pRefPos->Status == INVALID) return(retval); |
// try to copy the old reference in point list to a local buffer |
if(GPSPos_Copy(&(PointList[RefIndex-1].Position), &RefPos_old)) |
{ |
u8 i; |
// for each point position in the list |
for(i = 0; i < PointCount; i++) |
{ |
retval = 0; |
// calculate deviation form old ref, i.e the north and east shift of each point in the list from the reference position |
if(!GPSPos_Deviation(&(PointList[i].Position), &RefPos_old, &RefDeviation)) break; |
// copy of the new reference position into this list place |
if(!GPSPos_Copy(pRefPos, &(PointList[i].Position))) break; |
// move new reference according to the deviation of the old reference |
retval = GPSPos_ShiftCartesian(&(PointList[i].Position), RefDeviation.North, RefDeviation.East); |
if(!retval) break; |
} |
} // else ref pos old not copied! |
if(!retval) PointList_Clear(); |
return(retval); |
} |
/trunk/waypoints.h |
---|
17,8 → 17,8 |
u8 Index; // to indentify different waypoints, workaround for bad communications PC <-> NC |
u8 Type; // typeof Waypoint |
u8 WP_EventChannelValue; // Will be transferred to the FC and can be used as Poti value there |
u8 AltitudeRate; // rate to change the setpoint |
u8 Speed; // rate to change the Position(0 = max) |
u8 AltitudeRate; // rate to change the setpoint in steps of 0.1m/s |
u8 Speed; // rate to change the Position(0 = max) in steps of 0.1m/s |
u8 CamAngle; // Camera servo angle in degree (255 -> POI-Automatic) |
u8 Name[4]; // Name of that point (ASCII) |
u8 reserve[2]; // reserve |
48,6 → 48,8 |
u8 PointList_SaveToFile(u8 place); |
// load actual point list from SD card |
u8 PointList_ReadFromFile(u8 place); |
// move actual point list to ref pos., the point in the list marked by RefIndex get the RefPos afterwards |
u8 PointList_Move(u8 RefIndex, GPS_Pos_t* pRefPos); |
#endif // _WAYPOINTS_H |