You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
280 lines
7.8 KiB
280 lines
7.8 KiB
#include "GPSAnalyse.h"
|
|
|
|
GPSAnalyse::GPSAnalyse()
|
|
{
|
|
_GPS_Str.clear();
|
|
}
|
|
|
|
|
|
void GPSAnalyse::setSerialPtr(HardwareSerial &serial)
|
|
{
|
|
_serial = &serial;
|
|
_xSemaphore = xSemaphoreCreateMutex();
|
|
}
|
|
|
|
GPSAnalyse::~GPSAnalyse()
|
|
{
|
|
|
|
}
|
|
|
|
void GPSAnalyse::run(void *data)
|
|
{
|
|
GPSReadBuff = (char*)calloc(1024,sizeof(char));
|
|
Serial.println("GPS Task");
|
|
while (1)
|
|
{
|
|
if (_serial->available())
|
|
{
|
|
|
|
memset(GPSReadBuff, 1024, sizeof(char));
|
|
_serial->readBytes(GPSReadBuff, _serial->available());
|
|
_GPS_Str.concat(GPSReadBuff);
|
|
xSemaphoreTake(_xSemaphore, portMAX_DELAY);
|
|
Analyse();
|
|
xSemaphoreGive(_xSemaphore);
|
|
delay(1);
|
|
}
|
|
else
|
|
{
|
|
delay(5);
|
|
}
|
|
}
|
|
}
|
|
|
|
void GPSAnalyse::upDate()
|
|
{
|
|
xSemaphoreTake(_xSemaphore, portMAX_DELAY);
|
|
memcpy(&s_GNRMC,&_s_GNRMC,sizeof(GNRMC_t));
|
|
memcpy(&s_GNGAS,&_s_GNGAS,sizeof(GNGSA_t));
|
|
memcpy(&s_GPGSV,&_s_GPGSV,sizeof(GPGSV_t));
|
|
xSemaphoreGive(_xSemaphore);
|
|
}
|
|
|
|
void GPSAnalyse::AnaGPRMC(String str)
|
|
{
|
|
str = str.substring(str.indexOf("GNRMC"), str.length());
|
|
if (str.indexOf('*') != -1)
|
|
{
|
|
int indexsub = str.indexOf('*');
|
|
String sumstr = str.substring(indexsub + 1, str.length());
|
|
str = str.substring(0, indexsub);
|
|
//Serial.println(sumstr);
|
|
}
|
|
|
|
int index = 0, last_index = 0, pamindex = 1;
|
|
|
|
while (str.indexOf(',', index) != -1)
|
|
{
|
|
index = str.indexOf(',', index + 1);
|
|
last_index = str.indexOf(',', index + 1);
|
|
//Serial.printf("index:%d,%d\n", index, last_index);
|
|
if (index != -1)
|
|
{
|
|
last_index = (last_index == -1) ? str.length() : last_index;
|
|
if ((last_index - index) > 1)
|
|
{
|
|
String pamstr = str.substring(index + 1, last_index);
|
|
#ifdef DEBUG_GPS
|
|
Serial.printf("%d:", pamindex);
|
|
Serial.println(pamstr);
|
|
#endif
|
|
_s_GNRMC.pamstr[pamindex] = pamstr;
|
|
}
|
|
else
|
|
{
|
|
_s_GNRMC.pamstr[pamindex].clear();
|
|
}
|
|
|
|
pamindex++;
|
|
}
|
|
}
|
|
|
|
_s_GNRMC.Utc = _s_GNRMC.pamstr[1];
|
|
if (_s_GNRMC.pamstr[2].length() != 0)
|
|
{
|
|
_s_GNRMC.State = _s_GNRMC.pamstr[2].charAt(0);
|
|
}
|
|
|
|
{
|
|
_s_GNRMC.Latitude = _s_GNRMC.pamstr[3].substring(0, 2).toDouble() + _s_GNRMC.pamstr[3].substring(2, 4).toDouble() / 60 + _s_GNRMC.pamstr[3].substring(5, 9).toDouble() / 600000;
|
|
}
|
|
//_s_GNRMC.Latitude = _s_GNRMC.pamstr[3].toDouble();
|
|
_s_GNRMC.LatitudeMark = _s_GNRMC.pamstr[4].charAt(0);
|
|
{
|
|
_s_GNRMC.Longitude = _s_GNRMC.pamstr[5].substring(0, 3).toDouble() + _s_GNRMC.pamstr[5].substring(3, 5).toDouble() / 60 + _s_GNRMC.pamstr[5].substring(6, 10).toDouble() / 600000;
|
|
}
|
|
//_s_GNRMC.Longitude = _s_GNRMC.pamstr[5].toDouble();
|
|
_s_GNRMC.LongitudeMark = _s_GNRMC.pamstr[6].charAt(0);
|
|
_s_GNRMC.TrackSpeed = _s_GNRMC.pamstr[7].toFloat();
|
|
_s_GNRMC.TrackAngle = _s_GNRMC.pamstr[8].toFloat();
|
|
_s_GNRMC.Date = _s_GNRMC.pamstr[9];
|
|
_s_GNRMC.Magnetic = _s_GNRMC.pamstr[10].toFloat();
|
|
_s_GNRMC.Declination = _s_GNRMC.pamstr[11].charAt(0);
|
|
_s_GNRMC.mode = _s_GNRMC.pamstr[12].charAt(0);
|
|
}
|
|
|
|
void GPSAnalyse::AnaGNGAS(String str)
|
|
{
|
|
str = str.substring(str.indexOf("GNGSA"), str.length());
|
|
|
|
if (str.indexOf('*') != -1)
|
|
{
|
|
int indexsub = str.indexOf('*');
|
|
String sumstr = str.substring(indexsub + 1, str.length());
|
|
str = str.substring(0, indexsub);
|
|
//Serial.println(sumstr);
|
|
}
|
|
|
|
int index = 0, last_index = 0, pamindex = 1;
|
|
while (str.indexOf(',', index) != -1)
|
|
{
|
|
index = str.indexOf(',', index + 1);
|
|
last_index = str.indexOf(',', index + 1);
|
|
if (index != -1)
|
|
{
|
|
last_index = (last_index == -1) ? str.length() : last_index;
|
|
if ((last_index - index) > 1)
|
|
{
|
|
String pamstr = str.substring(index + 1, last_index);
|
|
#ifdef DEBUG_GPS
|
|
Serial.printf("%d:", pamindex);
|
|
Serial.println(pamstr);
|
|
#endif
|
|
_s_GNGAS.pamstr[pamindex] = pamstr;
|
|
}
|
|
else
|
|
{
|
|
_s_GNGAS.pamstr[pamindex].clear();
|
|
}
|
|
pamindex++;
|
|
}
|
|
}
|
|
|
|
_s_GNGAS.mode2 = _s_GNGAS.pamstr[1].charAt(0);
|
|
_s_GNGAS.mode1 = _s_GNGAS.pamstr[2].toInt();
|
|
for (size_t i = 0; i < 12; i++)
|
|
{
|
|
_s_GNGAS.PINMap[i] = _s_GNGAS.pamstr[3 + i].toInt();
|
|
}
|
|
_s_GNGAS.PDOP = _s_GNGAS.pamstr[15].toFloat();
|
|
_s_GNGAS.HDOP = _s_GNGAS.pamstr[16].toFloat();
|
|
_s_GNGAS.VDOP = _s_GNGAS.pamstr[17].toFloat();
|
|
}
|
|
|
|
void GPSAnalyse::AnaGPGSV(String str)
|
|
{
|
|
//Serial.println(str);
|
|
str = str.substring(str.indexOf("GPGSV"), str.length());
|
|
|
|
if (str.indexOf('*') != -1)
|
|
{
|
|
int indexsub = str.indexOf('*');
|
|
String sumstr = str.substring(indexsub + 1, str.length());
|
|
str = str.substring(0, indexsub);
|
|
//Serial.println(sumstr);
|
|
}
|
|
|
|
int index = 0, last_index = 0, pamindex = 1;
|
|
while (str.indexOf(',', index) != -1)
|
|
{
|
|
index = str.indexOf(',', index + 1);
|
|
last_index = str.indexOf(',', index + 1);
|
|
if (index != -1)
|
|
{
|
|
last_index = (last_index == -1) ? str.length() : last_index;
|
|
if ((last_index - index) > 1)
|
|
{
|
|
String pamstr = str.substring(index + 1, last_index);
|
|
#ifdef DEBUG_GPS
|
|
Serial.printf("%d:", pamindex);
|
|
Serial.println(pamstr);
|
|
#endif
|
|
_s_GPGSV.pamstr[pamindex] = pamstr;
|
|
}
|
|
else
|
|
{
|
|
_s_GPGSV.pamstr[pamindex].clear();
|
|
}
|
|
pamindex++;
|
|
}
|
|
}
|
|
int SatelliteSize = (pamindex - 4) / 4;
|
|
//Serial.printf("Number%d\n", SatelliteSize);
|
|
|
|
_s_GPGSV.size = _s_GPGSV.pamstr[1].toInt();
|
|
_s_GPGSV.Number = _s_GPGSV.pamstr[2].toInt();
|
|
_s_GPGSV.SatelliteSize = _s_GPGSV.pamstr[3].toInt();
|
|
|
|
if (_s_GPGSV.Number == 1)
|
|
{
|
|
for (size_t i = 0; i < 32; i++)
|
|
{
|
|
_s_GPGSV.Satellite[i].flag = false;
|
|
}
|
|
}
|
|
for (size_t i = 0; i < SatelliteSize; i++)
|
|
{
|
|
int id = _s_GPGSV.pamstr[4 + (i * 4) + 0].toInt();
|
|
if( id >= 32 ) continue;
|
|
if(( 7 + (i * 4)) > 50 )
|
|
{
|
|
break;
|
|
}
|
|
_s_GPGSV.Satellite[id].elevation = _s_GPGSV.pamstr[4 + (i * 4) + 1].toInt();
|
|
_s_GPGSV.Satellite[id].Azimuth = _s_GPGSV.pamstr[4 + (i * 4) + 2].toInt();
|
|
_s_GPGSV.Satellite[id].SNR = (_s_GPGSV.pamstr[4 + (i * 4) + 3].length() == 0) ? -1 : _s_GPGSV.pamstr[4 + (i * 4) + 3].toInt();
|
|
_s_GPGSV.Satellite[id].flag = true;
|
|
}
|
|
|
|
if (_s_GPGSV.Number == _s_GPGSV.size)
|
|
{
|
|
for (size_t i = 0; i < 32; i++)
|
|
{
|
|
if (_s_GPGSV.Satellite[i].flag == true)
|
|
{
|
|
#ifdef DEBUG_GPS
|
|
Serial.printf("ID %d:%d,%d,%d\n", i, _s_GPGSV.Satellite[i].elevation, _s_GPGSV.Satellite[i].Azimuth, _s_GPGSV.Satellite[i].SNR);
|
|
#endif
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void GPSAnalyse::Analyse()
|
|
{
|
|
while (_GPS_Str.indexOf('\r') != -1)
|
|
{
|
|
int index = _GPS_Str.indexOf('\r');
|
|
String str = _GPS_Str.substring(0, index);
|
|
_GPS_Str = _GPS_Str.substring(index + 3, _GPS_Str.length());
|
|
|
|
str.trim();
|
|
|
|
//Serial.println(str);
|
|
|
|
if (str.indexOf("GNRMC") != -1)
|
|
{
|
|
AnaGPRMC(str);
|
|
}
|
|
else if (str.indexOf("GPVTG") != -1)
|
|
{
|
|
}
|
|
else if (str.indexOf("GPGGA") != -1)
|
|
{
|
|
|
|
}
|
|
else if (str.indexOf("GNGSA") != -1)
|
|
{
|
|
//Serial.print("GNGSA:");
|
|
//AnaGNGAS(str);
|
|
}
|
|
else if (str.indexOf("GPGSV") != -1)
|
|
{
|
|
//Serial.print("GPGSV:");
|
|
AnaGPGSV(str);
|
|
}
|
|
else if (str.indexOf("GPGLL") != -1)
|
|
{
|
|
}
|
|
}
|
|
}
|