569 lines
15 KiB
C++
569 lines
15 KiB
C++
#include <string.h>
|
|
#include <stdlib.h>
|
|
#include <math.h>
|
|
|
|
#include "gpio.h"
|
|
|
|
#include "uart.h"
|
|
#include "tick.h"
|
|
#include "ori.h"
|
|
|
|
#include "filt.h"
|
|
|
|
#include "gps.h"
|
|
|
|
//FilterGPS filt_x, filt_y, filt_z;
|
|
|
|
/*void GPS_Navigation(bool valid, ORI_Data& data, float p[3])
|
|
{
|
|
filt_x.Update(valid, p[0], data.Iner.X/1000);
|
|
filt_y.Update(valid, p[1], data.Iner.Y/1000);
|
|
filt_z.Update(valid, p[2], data.Iner.Z/1000);
|
|
|
|
data.Speed.X=filt_x.Speed.Value;
|
|
data.Speed.Y=filt_y.Speed.Value;
|
|
data.Speed.Z=filt_z.Speed.Value;
|
|
|
|
data.Pos.X=filt_x.Position.Value;
|
|
data.Pos.Y=filt_y.Position.Value;
|
|
data.Pos.Z=filt_z.Position.Value;
|
|
}*/
|
|
//------------------------------------------------------------------------------
|
|
|
|
float GPS_LocalDistance(Point p1, Point p2, float& dx, float& dy) // light formula
|
|
{
|
|
const float pi = 3.14159265359f;
|
|
const float er = 6371000.0f; // Radius of the earth in m
|
|
|
|
float lat = p1.Latitude - p2.Latitude;
|
|
float lon = p1.Longitude - p2.Longitude;
|
|
|
|
float y = er/360.0f * (lat*pi*2.0f); // lat
|
|
dy = y;
|
|
|
|
float l = ((float)(p1.Latitude + p2.Latitude)) / 2.0f;
|
|
float r = cosf(l*pi/180.0f) * er;
|
|
float x = r/360.0f * (lon*pi*2.0f); // long
|
|
dx = x;
|
|
|
|
float d = sqrtf(x*x + y*y);
|
|
return d;
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
float GPS_GlobalDistance(Point p1, Point p2) // Haversine formula
|
|
{
|
|
const float pi = 3.14159265359f;
|
|
const float er = 6371000.0f; // Radius of the earth in m
|
|
|
|
float dLat = ((float)(p2.Latitude - p1.Latitude))*pi/180.0f;
|
|
float dLon = ((float)(p2.Longitude - p1.Longitude))*pi/180.0f;
|
|
|
|
float lat1 = p1.Latitude, lat2 = p2.Latitude;
|
|
|
|
float sdlat=sinf(dLat/2.0f);
|
|
float sdlon=sinf(dLon/2.0f);
|
|
|
|
float a = (sdlat*sdlat) + cosf((lat1)*pi/180.0f) * cosf((lat2)*pi/180.0f) * (sdlon*sdlon);
|
|
float c = 2.0f * atan2f(sqrtf(a), sqrtf(1.0f-a));
|
|
float d = er * c; // Distance in m
|
|
return d;
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
struct GPS_Head
|
|
{
|
|
static const unsigned long Size = 7;
|
|
const char* NMEA;
|
|
};
|
|
|
|
struct GPS_GNRMC // $GNRMC,hhmmss.sss,A,ggmm.mm,P,gggmm.mm,J,v.v,b.b,ddmmyy,x.x,n,m*hh/r/n
|
|
{
|
|
static constexpr GPS_Head Head = {"$GNRMC,"}; // Recommended minimum specific Transit data
|
|
//---
|
|
char* Time; // UTC of position fix
|
|
char* Valid; // Data status (V=navigation receiver warning)
|
|
char* Latitude; // Latitude of fix
|
|
char* Pole; // N or S
|
|
char* Longitude; // Longitude of fix
|
|
char* J; // E or W
|
|
char* HorSpeed; // Speed over ground in knots
|
|
char* Angle; // Track made good in degrees True
|
|
char* Date; // UT date
|
|
char* Mag; // Magnetic variation degrees (Easterly var. subtracts from true course)
|
|
char* MagAng; // Magnetic declination direction, E (east) or W (west)
|
|
char* Mode; // Mode indication (A=autonomous positioning, D=differential, E=estimation, N=invalid data)
|
|
//---
|
|
// Checksum
|
|
};
|
|
|
|
struct GPS_GNGGA // $GNGGA,hhmmss.ss,ggmm.mm,a,gggmm.mm,a,x,xx,x.x,x.x,M,x.x,M,x.x,xxxx*hh/r/n
|
|
{
|
|
static constexpr GPS_Head Head = {"$GNGGA,"}; // Global Positioning System Fix Data
|
|
//---
|
|
char* Time; // UTC of position
|
|
char* Latitude; // latitude of position
|
|
char* Pole; // N or S
|
|
char* Longitude; // Longitude of position
|
|
char* J; // E or W
|
|
char* Quality; // GPS Quality indicator (0=no fix, 1=GPS fix, 2=Dif. GPS fix)
|
|
char* Satellites; // Number of satellites in use [not those in view]
|
|
char* HDOP; // Horizontal dilution of position
|
|
char* Altitude; // Antenna altitude above/below mean sea level (geoid)
|
|
char* UnitsAlt; // Meters (Antenna height unit)
|
|
char* Geoidal; // Geoidal separation (Diff. between WGS-84 earth ellipsoid andmean sea level. -=geoid is below WGS-84 ellipsoid)
|
|
char* UnitsGeoidal; // Meters (Units of geoidal separation)
|
|
char* LastUpdate; // Age in seconds since last update from diff. reference station
|
|
char* StationID; // Diff. reference station ID#
|
|
//---
|
|
// Checksum
|
|
};
|
|
|
|
struct GPS_GNGSA // $GNGSA,mode,fix,PRN(11),PDOP,HDOP,VDOP,*hh/r/n
|
|
{
|
|
static constexpr GPS_Head Head = {"$GNGSA,"}; // Global Positioning System Fix Data
|
|
//---
|
|
char* Mode; // M = Manual, A = Automatic
|
|
char* FixType; // 1 = not available, 2 = 2D, 3 = 3D
|
|
char* ID_1; // 01 to 32 for GPS, 33 to 64 for SBAS, 64+ for GLONASS
|
|
char* ID_2;
|
|
char* ID_3;
|
|
char* ID_4;
|
|
char* ID_5;
|
|
char* ID_6;
|
|
char* ID_7;
|
|
char* ID_8;
|
|
char* ID_9;
|
|
char* ID_10;
|
|
char* ID_11;
|
|
char* ID_12;
|
|
char* PDOP;
|
|
char* HDOP;
|
|
char* VDOP;
|
|
//---
|
|
// Checksum
|
|
};
|
|
|
|
struct GPS_GPGSV // $GPGSV,count,index,visible,[ID,elevation,azimuth,SNR]*hh/r/n
|
|
{
|
|
static constexpr GPS_Head Head = {"$GPGSV,"}; // Global Positioning System Fix Data
|
|
//---
|
|
char* Count; // M = Manual, A = Automatic
|
|
char* Index; // 1 = not available, 2 = 2D, 3 = 3D
|
|
char* Visible; // 01 to 32 for GPS, 33 to 64 for SBAS, 64+ for GLONASS
|
|
|
|
char* ID_1;
|
|
char* Elevation_1;
|
|
char* Azimuth_1;
|
|
char* SNR_1;
|
|
|
|
char* ID_2;
|
|
char* Elevation_2;
|
|
char* Azimuth_2;
|
|
char* SNR_2;
|
|
|
|
char* ID_3;
|
|
char* Elevation_3;
|
|
char* Azimuth_3;
|
|
char* SNR_3;
|
|
|
|
char* ID_4;
|
|
char* Elevation_4;
|
|
char* Azimuth_4;
|
|
char* SNR_4;
|
|
|
|
//---
|
|
// Checksum
|
|
};
|
|
|
|
struct GPS_GLGSV // $GLGSV,count,index,visible,[ID,elevation,azimuth,SNR]*hh/r/n
|
|
{
|
|
static constexpr GPS_Head Head = {"$GLGSV,"}; // Global Positioning System Fix Data
|
|
//---
|
|
char* Count; // M = Manual, A = Automatic
|
|
char* Index; // 1 = not available, 2 = 2D, 3 = 3D
|
|
char* Visible; // 01 to 32 for GPS, 33 to 64 for SBAS, 64+ for GLONASS
|
|
|
|
char* ID_1;
|
|
char* Elevation_1;
|
|
char* Azimuth_1;
|
|
char* SNR_1;
|
|
|
|
char* ID_2;
|
|
char* Elevation_2;
|
|
char* Azimuth_2;
|
|
char* SNR_2;
|
|
|
|
char* ID_3;
|
|
char* Elevation_3;
|
|
char* Azimuth_3;
|
|
char* SNR_3;
|
|
|
|
char* ID_4;
|
|
char* Elevation_4;
|
|
char* Azimuth_4;
|
|
char* SNR_4;
|
|
|
|
//---
|
|
// Checksum
|
|
};
|
|
|
|
enum class GPS_PROTO {GNRMC, GNGGA, GNGSA, GPGSV, GLGSV, COUNT};
|
|
static const long GPS_ProtCount = (long)GPS_PROTO::COUNT;
|
|
static GPS_Head GPS_Protocols[GPS_ProtCount] = { GPS_GNRMC::Head, GPS_GNGGA::Head, GPS_GNGSA::Head, GPS_GPGSV::Head, GPS_GLGSV::Head };
|
|
|
|
struct GPS_Info
|
|
{
|
|
GPS_PROTO Type;
|
|
|
|
static const unsigned long Length = 256;
|
|
char Data[Length];
|
|
|
|
static const unsigned long Count = 32;
|
|
char* Param[Count];
|
|
|
|
long Begin = 0;
|
|
long Index = 0;
|
|
long Size = 0;
|
|
|
|
unsigned char CRC8;
|
|
};
|
|
|
|
|
|
static GPS_Info Info;
|
|
|
|
void (*GPS_CallBack)(GPS_PROTO Proto, void* Data)=0;
|
|
|
|
//------------------------------------------------------------------------------------------------------------------------------
|
|
/*static unsigned long UARTt_Recv(void* Data, unsigned long Size, bool WaitAll=false) // For Check ONLY !!!!!!!!!!!!!!!!
|
|
{
|
|
static int begin=0;
|
|
|
|
const char test[]="$GNGGA,122013.10,4719.45110,N,03846.54105,E,1,12,0.66,35.9,M,17.6,M,,*7A\r\n";
|
|
unsigned long size=strlen(test);
|
|
|
|
memcpy(Data, test, Size<size ? Size : size);
|
|
//memcpy(Data, test+begin, 1);
|
|
|
|
begin++;
|
|
|
|
if(begin>=size) begin=0;
|
|
|
|
//return 1;
|
|
return size;
|
|
}*/
|
|
//------------------------------------------------------------------------------------------------------------------------------
|
|
|
|
void GPS_Update()
|
|
{
|
|
char* head = Info.Data;
|
|
|
|
Info.Size += UART1_Recv(head + Info.Size, Info.Length - Info.Size);
|
|
|
|
long size = Info.Size - Info.Index;
|
|
|
|
while (size)
|
|
{
|
|
char* data = head + Info.Index;
|
|
|
|
if (Info.Index && *data == '$')
|
|
{
|
|
memcpy(head, data, size);
|
|
Info.Index = 0;
|
|
Info.Begin = 0;
|
|
Info.Size = size;
|
|
data = head;
|
|
}
|
|
|
|
if (!Info.Begin)
|
|
{
|
|
if (*head != '$')
|
|
{
|
|
Info.Index++;
|
|
size--;
|
|
continue;
|
|
}
|
|
//---
|
|
if (Info.Size < GPS_Head::Size) return;
|
|
//---
|
|
Info.Type = GPS_PROTO::COUNT;
|
|
for (long a = 0; a < GPS_ProtCount; a++)
|
|
{
|
|
const char* nmea = GPS_Protocols[a].NMEA;
|
|
if (memcmp(head, nmea, GPS_Head::Size) == 0)
|
|
{
|
|
Info.CRC8 = 0x0E;
|
|
for (long a = 0; nmea[a]; a++) Info.CRC8 ^= nmea[a];
|
|
Info.Type = (GPS_PROTO)a;
|
|
break;
|
|
}
|
|
}
|
|
//---
|
|
if (Info.Type == GPS_PROTO::COUNT)
|
|
{
|
|
Info.Index++;
|
|
*head = '#';
|
|
size--;
|
|
continue;
|
|
}
|
|
//---
|
|
Info.Index = GPS_Head::Size;
|
|
size = Info.Size - GPS_Head::Size;
|
|
//---
|
|
Info.Param[Info.Begin++] = head + Info.Index;
|
|
//---
|
|
continue;
|
|
}
|
|
//---
|
|
if (Info.Size > Info.Length)
|
|
{
|
|
Info.Begin = 0;
|
|
Info.Index = 0;
|
|
Info.Size = 0;
|
|
return;
|
|
}
|
|
//---
|
|
if (Info.Begin >= Info.Count)
|
|
{
|
|
if (*data == '\n')
|
|
{
|
|
*data = '\0';
|
|
size--;
|
|
char* end;
|
|
unsigned char crc8 = strtol(data-3, &end, 16);
|
|
if ((end == data - 1) && (crc8 == Info.CRC8))
|
|
{
|
|
if (GPS_CallBack) GPS_CallBack(Info.Type, Info.Param);
|
|
//---
|
|
if (size) memcpy(Info.Data, data + 1, size);
|
|
}
|
|
//---
|
|
Info.Begin = 0;
|
|
Info.Index = 0;
|
|
Info.Size = size;
|
|
//---
|
|
continue;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
Info.CRC8 ^= *data;
|
|
//---
|
|
if (*data == ',')
|
|
{
|
|
*data = '\0';
|
|
Info.Param[Info.Begin++] = ++data;
|
|
}
|
|
else if (*data == '*')
|
|
{
|
|
*data++ = '\0';
|
|
while (Info.Begin < Info.Count)
|
|
{
|
|
Info.Param[Info.Begin++] = nullptr;
|
|
}
|
|
}
|
|
}
|
|
//---
|
|
Info.Index++;
|
|
size--;
|
|
}
|
|
|
|
if (!Info.Begin && Info.Index)
|
|
{
|
|
Info.Index = 0;
|
|
Info.Size = 0;
|
|
}
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
static unsigned char GPS_VisibleGPS=0;
|
|
static unsigned char GPS_VisibleGLO=0;
|
|
|
|
static unsigned char Base_Noise[256];
|
|
|
|
Point GetCoordinates_Coord;
|
|
bool GetCoordinates_Valid;
|
|
bool GetCoordinates_Update;
|
|
|
|
Point Base_BeginXYZ;
|
|
|
|
static float Base_Alt=0, Base_Geo=0;
|
|
static unsigned char Base_Used, Base_Fix;
|
|
|
|
static GPS_BaseInfo GPS_BaseInfoData;
|
|
|
|
static void GPS_CallBack_GSV(GPS_PROTO Proto, void* Data)
|
|
{
|
|
if(Proto==GPS_PROTO::GNGGA)
|
|
{
|
|
GetCoordinates_Update=true;
|
|
|
|
GPS_GNGGA* data=(GPS_GNGGA*)Data;
|
|
|
|
GetCoordinates_Valid = (data->Quality[0] && (data->Quality[0]=='1' || data->Quality[0]=='2'));
|
|
|
|
float geo=0;
|
|
if(data->Geoidal[0] && GetCoordinates_Valid) geo=strtof(data->Geoidal, 0);
|
|
|
|
if(data->Altitude[0]) GetCoordinates_Coord.Altitude=strtof(data->Altitude, 0);
|
|
//---
|
|
Base_Geo=GetCoordinates_Coord.Altitude+geo;
|
|
|
|
|
|
Base_Alt=Base_Geo-Base_BeginXYZ.Altitude;
|
|
//---
|
|
GetCoordinates_Coord.Altitude+=geo;
|
|
|
|
if(data->Latitude[0] && GetCoordinates_Valid)
|
|
{
|
|
GetCoordinates_Coord.Latitude=(data->Latitude[0]-'0')*10 + data->Latitude[1]-'0';
|
|
GetCoordinates_Coord.Latitude+=strtod(data->Latitude+2, 0)/60;
|
|
}
|
|
//---
|
|
if(data->Longitude[0] && GetCoordinates_Valid)
|
|
{
|
|
GetCoordinates_Coord.Longitude=(data->Longitude[0]-'0')*100 + (data->Longitude[1]-'0')*10 + data->Longitude[2]-'0';
|
|
GetCoordinates_Coord.Longitude+=strtod(data->Longitude+3, 0)/60;
|
|
}
|
|
|
|
if(data->Satellites[0] && GetCoordinates_Valid) Base_Used=strtoul(data->Satellites, 0, 10);
|
|
|
|
if(data->Quality[0] && GetCoordinates_Valid) Base_Fix=strtoul(data->Quality, 0, 10);
|
|
}
|
|
|
|
if(Proto==GPS_PROTO::GPGSV)
|
|
{
|
|
GPS_GPGSV* data=(GPS_GPGSV*)Data;
|
|
GPS_VisibleGPS = strtoul(data->Visible, 0, 10);
|
|
|
|
if(!data->Index || *data->Index=='1') memset(Base_Noise, 0, sizeof(Base_Noise));
|
|
|
|
long id;
|
|
|
|
if(data->ID_1)
|
|
{
|
|
id=strtoul(data->ID_1, 0, 10);
|
|
if(id<256) Base_Noise[id]=strtoul(data->SNR_1, 0, 10);
|
|
}
|
|
if(data->ID_2)
|
|
{
|
|
id=strtoul(data->ID_2, 0, 10);
|
|
if(id<256) Base_Noise[id]=strtoul(data->SNR_2, 0, 10);
|
|
}
|
|
if(data->ID_3)
|
|
{
|
|
id=strtoul(data->ID_3, 0, 10);
|
|
if(id<256) Base_Noise[id]=strtoul(data->SNR_3, 0, 10);
|
|
}
|
|
if(data->ID_4)
|
|
{
|
|
id=strtoul(data->ID_4, 0, 10);
|
|
if(id<256) Base_Noise[id]=strtoul(data->SNR_4, 0, 10);
|
|
}
|
|
}
|
|
else if(Proto==GPS_PROTO::GLGSV)
|
|
{
|
|
GPS_GLGSV* data=(GPS_GLGSV*)Data;
|
|
GPS_VisibleGLO = strtoul(data->Visible, 0, 10);
|
|
|
|
long id;
|
|
|
|
if(data->ID_1)
|
|
{
|
|
id=strtoul(data->ID_1, 0, 10);
|
|
if(id<256) Base_Noise[id]=strtoul(data->SNR_1, 0, 10);
|
|
}
|
|
if(data->ID_2)
|
|
{
|
|
id=strtoul(data->ID_2, 0, 10);
|
|
if(id<256) Base_Noise[id]=strtoul(data->SNR_2, 0, 10);
|
|
}
|
|
if(data->ID_3)
|
|
{
|
|
id=strtoul(data->ID_3, 0, 10);
|
|
if(id<256) Base_Noise[id]=strtoul(data->SNR_3, 0, 10);
|
|
}
|
|
if(data->ID_4)
|
|
{
|
|
id=strtoul(data->ID_4, 0, 10);
|
|
if(id<256) Base_Noise[id]=strtoul(data->SNR_4, 0, 10);
|
|
}
|
|
|
|
if(*data->Count==*data->Index)
|
|
{
|
|
float noise=0, count=0;
|
|
for(long a=0; a<256; a++)
|
|
{
|
|
if(!Base_Noise[a]) continue;
|
|
noise+=Base_Noise[a];
|
|
count++;
|
|
}
|
|
|
|
if(count) GPS_BaseInfoData.noise=noise/count;
|
|
else GPS_BaseInfoData.noise=0;
|
|
|
|
GPS_BaseInfoData.satVisible=GPS_VisibleGPS+GPS_VisibleGLO;
|
|
}
|
|
}
|
|
else if(Proto==GPS_PROTO::GNRMC)
|
|
{
|
|
GPS_GNRMC* data=(GPS_GNRMC*)Data;
|
|
|
|
GPS_BaseInfoData.lat=GetCoordinates_Coord.Latitude;
|
|
GPS_BaseInfoData.lon=GetCoordinates_Coord.Longitude;
|
|
|
|
if(data->HorSpeed[0]) GPS_BaseInfoData.speed=strtof(data->HorSpeed, 0);
|
|
|
|
if(data->Time[0]) GPS_BaseInfoData.timeUTC=strtof(data->Time, 0);
|
|
|
|
float dx, dy;
|
|
GPS_LocalDistance(Base_BeginXYZ, {GPS_BaseInfoData.lat, GPS_BaseInfoData.lon}, dx, dy);
|
|
|
|
GPS_BaseInfoData.LocalXYZ[0]=dx;
|
|
GPS_BaseInfoData.LocalXYZ[1]=-dy;
|
|
GPS_BaseInfoData.LocalXYZ[2]=Base_Geo-Base_BeginXYZ.Altitude;
|
|
}
|
|
else if(Proto==GPS_PROTO::GNGSA)
|
|
{
|
|
GPS_GNGSA* data=(GPS_GNGSA*)Data;
|
|
|
|
if(data->HDOP[0]) GPS_BaseInfoData.hdop=strtof(data->HDOP, 0);
|
|
if(data->VDOP[0]) GPS_BaseInfoData.vdop=strtof(data->VDOP, 0);
|
|
if(data->PDOP[0]) GPS_BaseInfoData.pdop=strtof(data->PDOP, 0);
|
|
}
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
void GPS_Init()
|
|
{
|
|
UART1_Init(230400);
|
|
GPS_CallBack=GPS_CallBack_GSV;
|
|
memset(Base_Noise, 0, sizeof(Base_Noise));
|
|
}
|
|
|
|
bool GPS_GetCoordinates(Point& Coord, bool& Valid)
|
|
{
|
|
if(!GetCoordinates_Update) return false;
|
|
|
|
GetCoordinates_Update=false;
|
|
|
|
Coord=GetCoordinates_Coord;
|
|
Valid=GetCoordinates_Valid;
|
|
|
|
return true;
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
bool GPS_GetBaseInfo(GPS_BaseInfo& Info)
|
|
{
|
|
Info=GPS_BaseInfoData;
|
|
|
|
Info.realAlt=Base_Alt;
|
|
Info.absAlt=Base_Geo;
|
|
Info.satUsed=Base_Used;
|
|
Info.fixType=Base_Fix;
|
|
|
|
return true;
|
|
}
|
|
//------------------------------------------------------------------------------
|