Subversion Repositories NaviCtrl

Compare Revisions

Ignore whitespace Rev 490 → Rev 491

/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