mirror
/
esp32-ogn-tracker
kopia lustrzana https://github.com/pjalocha/esp32-ogn-tracker
1
0
Forkuj 0
Kod Zgłoszenia Projekty Wydania Wiki Aktywność

Improving hndling of high GPS update rates for Stratux

pull/30/head
Pawel Jalocha 2020-12-27 14:09:57 +00:00
rodzic 31c237db84
commit 4a8f0661aa
5 zmienionych plików z 166 dodań i 68 usunięć
Pokaż statystyki Ściągnij plik aktualizacji Ściągnij plik porównania Expand all files Collapse all files

64
main/gps.cpp
Wyświetl plik

@ -53,16 +53,16 @@ static TickType_t PPS_Tick; // [msec] System Tick when the PPS ar
static TickType_t Burst_Tick; // [msec] System Tick when the data burst from GPS started
uint32_t GPS_TimeSinceLock; // [sec] time since the GPS has a lock
uint32_t GPS_FatTime = 0; // [sec] UTC date/time in FAT format
int32_t GPS_Altitude = 0; // [0.1m] last valid altitude
int32_t GPS_Latitude = 0; //
int32_t GPS_Longitude = 0; //
int32_t GPS_Latitude = 0; // [1/60000deg]
int32_t GPS_Longitude = 0; // [1/60000deg]
GPS_Time GPS_DateTime = { 0, 0, 0, 0, 0, 0, 0 } ;
int16_t GPS_GeoidSepar= 0; // [0.1m]
uint16_t GPS_LatCosine = 3000; //
uint16_t GPS_LatCosine = 3000; // [1.0/4096]
uint32_t GPS_Random = 0x12345678; // random number from the LSB of the GPS data
uint16_t GPS_SatSNR = 0; // [0.25dB]
Status GPS_Status;
Status GPS_Status; // GPS status flags
static union
{ uint8_t Flags;
@ -244,14 +244,15 @@ static void GPS_LockEnd(void) // called when GPS looses a
// ----------------------------------------------------------------------------
static void GPS_BurstStart(void) // when GPS starts sending the data on the serial port
{ Burst_Tick=xTaskGetTickCount();
{ GPS_Burst.Active=1;
Burst_Tick=xTaskGetTickCount();
#ifdef DEBUG_PRINT
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
Format_UnsDec(CONS_UART_Write, TimeSync_Time(Burst_Tick)%60, 2);
CONS_UART_Write('.');
Format_UnsDec(CONS_UART_Write, TimeSync_msTime(Burst_Tick), 3);
Format_String(CONS_UART_Write, " -> GPS_BurstStart() GPS:");
Format_Bin(CONS_UART_Write, GPS_Status.Flags);
Format_String(CONS_UART_Write, " -> GPS_BurstStart () GPS:");
Format_Hex(CONS_UART_Write, GPS_Status.Flags);
Format_String(CONS_UART_Write, "\n");
xSemaphoreGive(CONS_Mutex);
#endif
@ -421,7 +422,7 @@ static void GPS_Random_Update(GPS_Position *Pos)
XorShift32(GPS_Random); }
static void GPS_BurstComplete(void) // when GPS has sent the essential data for position fix
{
{ GPS_Burst.Complete=1;
#ifdef WITH_MAVLINK
GPS_Position *GPS = GPS_Pos+GPS_PosIdx;
if(GPS->hasTime && GPS->hasGPS && GPS->hasBaro)
@ -441,7 +442,7 @@ static void GPS_BurstComplete(void) // wh
CONS_UART_Write('.');
Format_UnsDec(CONS_UART_Write, TimeSync_msTime(), 3);
Format_String(CONS_UART_Write, " -> GPS_BurstComplete() GPS:");
Format_Bin(CONS_UART_Write, GPS_Status.Flags);
Format_Hex(CONS_UART_Write, GPS_Status.Flags);
Format_String(CONS_UART_Write, "\nGPS[");
CONS_UART_Write('0'+GPS_PosIdx); CONS_UART_Write(']'); CONS_UART_Write(' ');
Format_String(CONS_UART_Write, Line);
@ -453,7 +454,7 @@ static void GPS_BurstComplete(void) // wh
if(GPS_Pos[GPS_PosIdx].isTimeValid()) // if time is valid already
{ if(GPS_Pos[GPS_PosIdx].isDateValid()) // if date is valid as well
{ uint32_t UnixTime=GPS_Pos[GPS_PosIdx].getUnixTime();
GPS_FatTime=GPS_Pos[GPS_PosIdx].getFatTime();
// GPS_FatTime=GPS_Pos[GPS_PosIdx].getFatTime();
#ifndef WITH_MAVLINK // with MAVlink we sync. with the SYSTEM_TIME message
int32_t msDiff = GPS_Pos[GPS_PosIdx].mSec;
if(msDiff>=500) { msDiff-=1000; UnixTime++; } // [ms]
@ -558,7 +559,18 @@ static void GPS_BurstComplete(void) // wh
}
static void GPS_BurstEnd(void) // when GPS stops sending the data on the serial port
{ }
{
#ifdef DEBUG_PRINT
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
Format_UnsDec(CONS_UART_Write, TimeSync_Time(Burst_Tick)%60, 2);
CONS_UART_Write('.');
Format_UnsDec(CONS_UART_Write, TimeSync_msTime(Burst_Tick), 3);
Format_String(CONS_UART_Write, " -> GPS_BurstEnd () GPS:");
Format_Hex(CONS_UART_Write, GPS_Status.Flags);
Format_String(CONS_UART_Write, "\n");
xSemaphoreGive(CONS_Mutex);
#endif
GPS_Burst.Flags=0; } // clear all flags: active and complete
// ----------------------------------------------------------------------------
@ -598,12 +610,15 @@ static void GPS_NMEA(void) // wh
LED_PCB_Flash(10); // Flash the LED for 2 ms
if(NMEA.isGxGSV()) ProcessGSV(NMEA); // process satellite data
else if(NMEA.isGxRMC())
{ // if(GPS_Burst.GxRMC) { GPS_BurstComplete(); GPS_Burst.Flags=0; GPS_BurstStart(); }
{ int8_t SameTime = GPS_DateTime.ReadTime((const char *)NMEA.ParmPtr(0)); // 1=same time, 0=diff. time, -1=error
if(SameTime==0 && GPS_Burst.GxRMC) { GPS_BurstComplete(); GPS_BurstEnd(); GPS_BurstStart(); }
GPS_Burst.GxRMC=1; }
else if(NMEA.isGxGGA())
{ // if(GPS_Burst.GxGGA) { GPS_BurstComplete(); GPS_Burst.Flags=0; GPS_BurstStart(); }
{ int8_t SameTime = GPS_DateTime.ReadTime((const char *)NMEA.ParmPtr(0)); // 1=same time, 0=diff. time, -1=error
if(SameTime==0 && GPS_Burst.GxGGA) { GPS_BurstComplete(); GPS_BurstEnd(); GPS_BurstStart(); }
GPS_Burst.GxGGA=1; }
else if(NMEA.isGxGSA()) GPS_Burst.GxGSA=1;
else if(NMEA.isGxGSA())
{ GPS_Burst.GxGSA=1; }
GPS_Pos[GPS_PosIdx].ReadNMEA(NMEA); // read position elements from NMEA
#ifdef DEBUG_PRINT
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
@ -618,10 +633,11 @@ static void GPS_NMEA(void) // wh
#endif
#ifndef WITH_GPS_NMEA_PASS
// these NMEA from GPS we want to pass to the console
static uint8_t Count=0;
bool RatePass=0;
Count++; if(Count>=5) { Count=0; RatePass=1; }
if( NMEA.isP() || NMEA.isGxRMC() || NMEA.isGxGGA() || NMEA.isGxGSA() || NMEA.isP() || NMEA.isGxGSV() || (RatePass && (NMEA.isGPTXT())) )
// static uint8_t Count=0;
// bool RatePass=0;
// Count++; if(Count>=5) { Count=0; RatePass=1; }
// if( NMEA.isP() || NMEA.isGxRMC() || NMEA.isGxGGA() || NMEA.isGxGSA() || NMEA.isGxGSV() || NMEA.isGPTXT()) )
if( NMEA.isP() || NMEA.isBD() || NMEA.isGx() )
// we would need to patch the GGA here for the GPS which does not calc. nor correct for GeoidSepar
#endif
{ if(Parameters.Verbose)
@ -1041,17 +1057,17 @@ void vTaskGPS(void* pvParameters)
*/
if(LineIdle==0) // if any bytes were received ?
{ if(!GPS_Burst.Active) GPS_BurstStart(); // if not already started then declare burst started
GPS_Burst.Active=1;
if( (!GPS_Burst.Complete) && (GPS_Burst.GxGGA && GPS_Burst.GxRMC && GPS_Burst.GxGSA) ) // if GGA+RMC+GSA received
{ GPS_Burst.Complete=1; GPS_BurstComplete(); } // declare burst complete
{ GPS_BurstComplete(); } // declare burst complete
}
else if(LineIdle>=GPS_BurstTimeout) // if GPS sends no more data for GPS_BurstTimeout ticks
{ if(GPS_Burst.Active) // if burst was active
{ if(!GPS_Burst.Complete && GPS_Burst.GxGGA && GPS_Burst.GxRMC) GPS_BurstComplete(); // if not complete yet, then declare burst complete
GPS_BurstEnd(); } // declare burst ended
{ if(!GPS_Burst.Complete && GPS_Burst.GxGGA && GPS_Burst.GxRMC)
{ GPS_BurstComplete(); } // if not complete yet, then declare burst complete
}
else if(LineIdle>=1500) // if idle for more than 1.5 sec
{ GPS_Status.Flags=0; }
GPS_Burst.Flags=0; // clear all flags: active and complete
if(GPS_Burst.Flags) GPS_BurstEnd(); // declare burst ended, if not yet done
}
if(NoValidData>=2000) // if no valid data from GPS for 2sec

8
main/gps.h
Wyświetl plik

@ -17,7 +17,7 @@ const uint8_t GPS_PosPipeSize = 4; // number of GPS positions held in
extern uint8_t GPS_PosIdx; // Pipe index, increments with every GPS position received
extern GPS_Position GPS_Pos[GPS_PosPipeSize]; // GPS position pipe
extern uint32_t GPS_FatTime; // [2 sec] UTC time in FAT format (for FatFS)
extern GPS_Time GPS_DateTime; // Time and date from the GPS
extern int32_t GPS_Altitude; // [0.1m] altitude (height above Geoid)
extern int32_t GPS_Latitude; // [0.0001/60 deg]
extern int32_t GPS_Longitude; // [0.0001/60 deg]
@ -40,9 +40,9 @@ typedef union
bool RateConfig:1; // navigation rate is configured
bool :1; //
} ;
} Status;
} Status; //
extern Status GPS_Status;
extern Status GPS_Status; // GPS status bits
uint32_t GPS_getBaudRate(void); // [bps]
@ -62,7 +62,7 @@ extern EventGroupHandle_t GPS_Event;
const EventBits_t GPSevt_PPS = 0x01;
const EventBits_t GPSevt_NewPos = 0x02;
extern FlightMonitor Flight;
extern FlightMonitor Flight; // detect/monitor takeoff/flight/landing
#ifdef __cplusplus
extern "C"

18
main/nmea.h
Wyświetl plik

@ -102,6 +102,10 @@ inline uint8_t NMEA_AppendCheckCRNL(char *NMEA, uint8_t Len) { return NMEA_Appen
{ if(Data[1]!='G') return 0;
return Data[2]=='N'; }
uint8_t isBD(void) const // for Beidou GSA and GSV
{ if(Data[1]!='B') return 0;
return Data[2]=='D'; }
uint8_t isGx(void) const // GPS or GLONASS sentence ?
{ return Data[1]=='G'; }
@ -141,6 +145,18 @@ inline uint8_t NMEA_AppendCheckCRNL(char *NMEA, uint8_t Len) { return NMEA_Appen
if(Data[4]!='G') return 0;
return Data[5]=='A'; }
uint8_t isGxVTG(void) const // velocity relative to the ground
{ if(!isGx()) return 0;
if(Data[3]!='V') return 0;
if(Data[4]!='T') return 0;
return Data[5]=='G'; }
uint8_t isGxZDA(void) const // UTC time+date, time zone
{ if(!isGx()) return 0;
if(Data[3]!='Z') return 0;
if(Data[4]!='D') return 0;
return Data[5]=='A'; }
uint8_t isGPGSA(void) const // GPS satellite data
{ if(!isGP()) return 0;
if(Data[3]!='G') return 0;
@ -160,7 +176,7 @@ inline uint8_t NMEA_AppendCheckCRNL(char *NMEA, uint8_t Len) { return NMEA_Appen
return Data[5]=='A'; }
uint8_t isGxGSV(void) const // Satellite data
{ if(!isGx()) return 0;
{ if(!isGx() && !isBD()) return 0; // we include as well $BDGSV
if(Data[3]!='G') return 0;
if(Data[4]!='S') return 0;
return Data[5]=='V'; }

78
main/ogn.h
Wyświetl plik

@ -653,8 +653,6 @@ class GPS_Time
int8_t Year, Month, Day; // Date (UTC) from GPS
int8_t Hour, Min, Sec; // Time-of-day (UTC) from GPS
int16_t mSec; // [ms]
// int8_t FracSec; // [1/100 sec] some GPS-es give second fraction with the time-of-day
// int8_t Spare;
public:
@ -742,10 +740,14 @@ class GPS_Time
{ uint16_t Days = DaysSinceYear2000() + DaysSimce1jan();
return Times60(Times60(Times24((uint32_t)(Days+10957)))) + getDayTime(); }
uint32_t getFatTime(void) const // return timestamp in FAT format
{ uint16_t Date = ((uint16_t)(Year+20)<<9) | ((uint16_t)Month<<5) | Day;
uint16_t Time = ((uint16_t)Hour<<11) | ((uint16_t)Min<<5) | (Sec>>1);
return ((uint32_t)Date<<16) | Time; }
uint16_t getFatDate(void) const // return date in FAT format
{ return ((uint16_t)(Year+20)<<9) | ((uint16_t)Month<<5) | Day; }
uint16_t getFatTime(void) const // return time in FAT format
{ return ((uint16_t)Hour<<11) | ((uint16_t)Min<<5) | (Sec>>1); }
uint32_t getFatDateTime(void) const // return date+time in FAT format
{ return ((uint32_t)getFatDate()<<16) | getFatTime(); }
void setUnixTime(uint32_t Time) // works except for the 1.1.2000
{ uint32_t Days = Time/SecsPerDay; // [day] since 1970
@ -770,6 +772,34 @@ class GPS_Time
uint64_t getUnixTime_ms(void) const
{ return (uint64_t)getUnixTime()*1000 + mSec; }
int8_t ReadTime(const char *Value) // read the Time field: HHMMSS.sss and check if it is a new one or the same one
{ int8_t Prev; int8_t Same=1;
Prev=Hour;
Hour=Read_Dec2(Value); if(Hour<0) return -1; // read hour (two digits), return when invalid
if(Prev!=Hour) Same=0;
Prev=Min;
Min=Read_Dec2(Value+2); if(Min<0) return -1; // read minute (two digits), return when invalid
if(Prev!=Min) Same=0;
Prev=Sec;
Sec=Read_Dec2(Value+4); if(Sec<0) return -1; // read second (two digits), return when invalid
if(Prev!=Sec) Same=0;
int16_t mPrev = mSec;
if(Value[6]=='.') // is there a fraction
{ uint16_t Frac=0; int8_t Len=Read_UnsDec(Frac, Value+7); if(Len<1) return -1; // read the fraction, return when invalid
if(Len==1) mSec = Frac*100;
else if(Len==2) mSec = Frac*10;
else if(Len==3) mSec = Frac;
else if(Len==4) mSec = Frac/10;
else return -1; }
if(mPrev!=mSec) Same=0; // return 0 when time is valid but did not change
return Same; } // return 1 when time did not change (both RMC and GGA were for same time)
int8_t ReadDate(const char *Param) // read the field DDMMYY
{ Day=Read_Dec2(Param); if(Day<0) return -1; // read calendar year (two digits - thus need to be extended to four)
Month=Read_Dec2(Param+2); if(Month<0) return -1; // read calendar month
Year=Read_Dec2(Param+4); if(Year<0) return -1; // read calendar day
return 0; } // return 0 when field valid and was read correctly
private:
static const uint32_t SecsPerMin = 60;
@ -1398,15 +1428,21 @@ class GPS_Position: public GPS_Time
int32_t calcAltitudeExtrapolation(int32_t Time) const // [ms]
{ return Time*ClimbRate/1000; } // [0.1m]
// int32_t calcLatitudeExtrapolation(int32_t Time, int32_t LatSpeed) const // [ms] [0.1m/s]
// { return (Time/10*LatSpeed*177+0x4000)>>15; } // [0.1m]
int32_t calcLatitudeExtrapolation(int32_t Time, int32_t LatSpeed) const // [ms] [0.1m/s]
{ return (Time/10*LatSpeed*177+0x4000)>>15; } // [0.1m]
{ return (Time*LatSpeed*283+0x40000)>>19; } // [0.1m]
int32_t calcLongitudeExtrapolation(int32_t Time, int32_t LonSpeed) const // [ms]
{ int16_t LatCosine = calcLatCosine(calcLatAngle16(Latitude));
return calcLongitudeExtrapolation(Time, LonSpeed, LatCosine); }
// int32_t calcLongitudeExtrapolation(int32_t Time, int32_t LonSpeed, int16_t LatCosine) const // [ms]
// { return (((Time/10*LonSpeed*177+4)>>3))/LatCosine; }
int32_t calcLongitudeExtrapolation(int32_t Time, int32_t LonSpeed, int16_t LatCosine) const // [ms]
{ return (((Time/10*LonSpeed*177+4)>>3))/LatCosine; }
{ return (((Time*LonSpeed*283+64)>>7))/LatCosine; }
// static int32_t calcLatDistance(int32_t Lat1, int32_t Lat2) // [m] distance along latitude
// { return ((int64_t)(Lat2-Lat1)*0x2f684bda+0x80000000)>>32; }
@ -1581,32 +1617,6 @@ class GPS_Position: public GPS_Time
else if(DOP>255) DOP=255;
VDOP=DOP; return 0; }
int8_t ReadTime(const char *Value) // read the Time field: HHMMSS.sss and check if it is a new one or the same one
{ int8_t Prev; int8_t Same=1;
Prev=Hour;
Hour=Read_Dec2(Value); if(Hour<0) return -1; // read hour (two digits), return when invalid
if(Prev!=Hour) Same=0;
Prev=Min;
Min=Read_Dec2(Value+2); if(Min<0) return -1; // read minute (two digits), return when invalid
if(Prev!=Min) Same=0;
Prev=Sec;
Sec=Read_Dec2(Value+4); if(Sec<0) return -1; // read second (two digits), return when invalid
if(Prev!=Sec) Same=0;
int16_t mPrev = mSec;
if(Value[6]=='.') // is there a fraction
{ uint16_t Frac=0; int8_t Len=Read_UnsDec(Frac, Value+7); if(Len<1) return -1; // read the fraction, return when invalid
if(Len==1) mSec = Frac*100;
else if(Len==2) mSec = Frac*10;
else if(Len==3) mSec = Frac; }
if(mPrev!=mSec) Same=0; // return 0 when time is valid but did not change
return Same; } // return 1 when time did not change (both RMC and GGA were for same time)
int8_t ReadDate(const char *Param) // read the field DDMMYY
{ Day=Read_Dec2(Param); if(Day<0) return -1; // read calendar year (two digits - thus need to be extended to four)
Month=Read_Dec2(Param+2); if(Month<0) return -1; // read calendar month
Year=Read_Dec2(Param+4); if(Year<0) return -1; // read calendar day
return 0; } // return 0 when field valid and was read correctly
public:
int8_t static IndexNMEA(uint8_t Index[20], const char *Seq) // index parameters and verify the NMEA checksum

66
main/sdlog.cpp
Wyświetl plik

@ -7,6 +7,14 @@
#include "mbedtls/md5.h"
#include "mbedtls/rsa.h"
#include "mbedtls/platform.h"
#include "mbedtls/x509_csr.h"
#include "mbedtls/entropy.h"
#include "mbedtls/ctr_drbg.h"
#include "mbedtls/ecdsa.h"
#include "mbedtls/sha256.h"
#include "mbedtls/ecp.h"
#include "mbedtls/pk.h"
#include "hal.h"
#include "gps.h"
@ -34,10 +42,13 @@ void Log_Write(char Byte) // write a byt
int Log_Free(void) { return Log_FIFO.Free(); } // how much space left in the buffer
static int Log_Open(void)
{ LogDate=GPS_FatTime>>16; // get the FAT-time date part
int32_t Day = LogDate &0x1F; // get day, month, year
int32_t Month = (LogDate>>5)&0x0F;
int32_t Year = (LogDate>>9)-20;
{ // LogDate=GPS_DateTime.getFatDate(); // get the FAT-time date part
// int32_t Day = LogDate &0x1F; // get day, month, year
// int32_t Month = (LogDate>>5)&0x0F;
// int32_t Year = (LogDate>>9)-20;
int32_t Day = GPS_DateTime.Day; // get day, month, year
int32_t Month = GPS_DateTime.Month;
int32_t Year = GPS_DateTime.Year-20;
uint32_t Date = 0;
if(Year>=0) Date = Day*10000 + Month*100 + Year; // create DDMMYY number for easy printout
strcpy(LogFileName, "/sdcard/CONS/TR000000.LOG");
@ -284,9 +295,54 @@ static void IGC_Check(void) // check if
#ifdef WITH_SDLOG
/*
// Uncomment to force use of a specific curve
#define ECPARAMS MBEDTLS_ECP_DP_SECP192R1
#if !defined(ECPARAMS)
#define ECPARAMS mbedtls_ecp_curve_list()->grp_id
#endif
static mbedtls_ecdsa_context SignCtx;
static mbedtls_pk_context Key;
static mbedtls_x509write_csr Req;
static mbedtls_entropy_context Entropy;
static mbedtls_ctr_drbg_context CtrDrbgCtx;
static int IGC_GenKey(void)
{ const char *Pers = "ecdsa";
mbedtls_x509write_csr_init(&Req);
mbedtls_pk_init(&Key);
mbedtls_ecdsa_init(&SignCtx);
mbedtls_ctr_drbg_init(&CtrDrbgCtx);
mbedtls_entropy_init(&Entropy);
int Ret = mbedtls_ctr_drbg_seed( &CtrDrbgCtx, mbedtls_entropy_func, &Entropy,
(const unsigned char *)Pers, strlen(Pers) );
if(Ret!=0) return Ret;
Ret = mbedtls_ecdsa_genkey(&SignCtx, ECPARAMS, mbedtls_ctr_drbg_random, &CtrDrbgCtx);
return Ret; }
*/
#endif // WITH_SDLOG
// ============================================================================================
#ifdef WITH_SDLOG
extern "C"
void vTaskSDLOG(void* pvParameters)
{ uint32_t ID = getUniqueAddress();
{
/*
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
Format_String(CONS_UART_Write, "vTaskSDLOG() Start generating key pair\n");
xSemaphoreGive(CONS_Mutex);
int Ret=IGC_GenKey();
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
Format_String(CONS_UART_Write, "vTaskSDLOG() End generating key pair => ");
Format_SignDec(CONS_UART_Write, Ret);
Format_String(CONS_UART_Write, "\n");
xSemaphoreGive(CONS_Mutex);
*/
uint32_t ID = getUniqueAddress();
IGC_Serial[2] = Flight.Code36(ID%36); ID/=36;
IGC_Serial[1] = Flight.Code36(ID%36); ID/=36;
IGC_Serial[0] = Flight.Code36(ID%36);