kopia lustrzana https://github.com/pjalocha/esp32-ogn-tracker
Improving hndling of high GPS update rates for Stratux
rodzic
31c237db84
commit
4a8f0661aa
64
main/gps.cpp
64
main/gps.cpp
|
@ -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
|
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_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_Altitude = 0; // [0.1m] last valid altitude
|
||||||
int32_t GPS_Latitude = 0; //
|
int32_t GPS_Latitude = 0; // [1/60000deg]
|
||||||
int32_t GPS_Longitude = 0; //
|
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]
|
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
|
uint32_t GPS_Random = 0x12345678; // random number from the LSB of the GPS data
|
||||||
uint16_t GPS_SatSNR = 0; // [0.25dB]
|
uint16_t GPS_SatSNR = 0; // [0.25dB]
|
||||||
|
|
||||||
Status GPS_Status;
|
Status GPS_Status; // GPS status flags
|
||||||
|
|
||||||
static union
|
static union
|
||||||
{ uint8_t Flags;
|
{ 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
|
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
|
#ifdef DEBUG_PRINT
|
||||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||||
Format_UnsDec(CONS_UART_Write, TimeSync_Time(Burst_Tick)%60, 2);
|
Format_UnsDec(CONS_UART_Write, TimeSync_Time(Burst_Tick)%60, 2);
|
||||||
CONS_UART_Write('.');
|
CONS_UART_Write('.');
|
||||||
Format_UnsDec(CONS_UART_Write, TimeSync_msTime(Burst_Tick), 3);
|
Format_UnsDec(CONS_UART_Write, TimeSync_msTime(Burst_Tick), 3);
|
||||||
Format_String(CONS_UART_Write, " -> GPS_BurstStart() GPS:");
|
Format_String(CONS_UART_Write, " -> GPS_BurstStart () GPS:");
|
||||||
Format_Bin(CONS_UART_Write, GPS_Status.Flags);
|
Format_Hex(CONS_UART_Write, GPS_Status.Flags);
|
||||||
Format_String(CONS_UART_Write, "\n");
|
Format_String(CONS_UART_Write, "\n");
|
||||||
xSemaphoreGive(CONS_Mutex);
|
xSemaphoreGive(CONS_Mutex);
|
||||||
#endif
|
#endif
|
||||||
|
@ -421,7 +422,7 @@ static void GPS_Random_Update(GPS_Position *Pos)
|
||||||
XorShift32(GPS_Random); }
|
XorShift32(GPS_Random); }
|
||||||
|
|
||||||
static void GPS_BurstComplete(void) // when GPS has sent the essential data for position fix
|
static void GPS_BurstComplete(void) // when GPS has sent the essential data for position fix
|
||||||
{
|
{ GPS_Burst.Complete=1;
|
||||||
#ifdef WITH_MAVLINK
|
#ifdef WITH_MAVLINK
|
||||||
GPS_Position *GPS = GPS_Pos+GPS_PosIdx;
|
GPS_Position *GPS = GPS_Pos+GPS_PosIdx;
|
||||||
if(GPS->hasTime && GPS->hasGPS && GPS->hasBaro)
|
if(GPS->hasTime && GPS->hasGPS && GPS->hasBaro)
|
||||||
|
@ -441,7 +442,7 @@ static void GPS_BurstComplete(void) // wh
|
||||||
CONS_UART_Write('.');
|
CONS_UART_Write('.');
|
||||||
Format_UnsDec(CONS_UART_Write, TimeSync_msTime(), 3);
|
Format_UnsDec(CONS_UART_Write, TimeSync_msTime(), 3);
|
||||||
Format_String(CONS_UART_Write, " -> GPS_BurstComplete() GPS:");
|
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[");
|
Format_String(CONS_UART_Write, "\nGPS[");
|
||||||
CONS_UART_Write('0'+GPS_PosIdx); CONS_UART_Write(']'); CONS_UART_Write(' ');
|
CONS_UART_Write('0'+GPS_PosIdx); CONS_UART_Write(']'); CONS_UART_Write(' ');
|
||||||
Format_String(CONS_UART_Write, Line);
|
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].isTimeValid()) // if time is valid already
|
||||||
{ if(GPS_Pos[GPS_PosIdx].isDateValid()) // if date is valid as well
|
{ if(GPS_Pos[GPS_PosIdx].isDateValid()) // if date is valid as well
|
||||||
{ uint32_t UnixTime=GPS_Pos[GPS_PosIdx].getUnixTime();
|
{ 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
|
#ifndef WITH_MAVLINK // with MAVlink we sync. with the SYSTEM_TIME message
|
||||||
int32_t msDiff = GPS_Pos[GPS_PosIdx].mSec;
|
int32_t msDiff = GPS_Pos[GPS_PosIdx].mSec;
|
||||||
if(msDiff>=500) { msDiff-=1000; UnixTime++; } // [ms]
|
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
|
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
|
LED_PCB_Flash(10); // Flash the LED for 2 ms
|
||||||
if(NMEA.isGxGSV()) ProcessGSV(NMEA); // process satellite data
|
if(NMEA.isGxGSV()) ProcessGSV(NMEA); // process satellite data
|
||||||
else if(NMEA.isGxRMC())
|
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; }
|
GPS_Burst.GxRMC=1; }
|
||||||
else if(NMEA.isGxGGA())
|
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; }
|
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
|
GPS_Pos[GPS_PosIdx].ReadNMEA(NMEA); // read position elements from NMEA
|
||||||
#ifdef DEBUG_PRINT
|
#ifdef DEBUG_PRINT
|
||||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||||
|
@ -618,10 +633,11 @@ static void GPS_NMEA(void) // wh
|
||||||
#endif
|
#endif
|
||||||
#ifndef WITH_GPS_NMEA_PASS
|
#ifndef WITH_GPS_NMEA_PASS
|
||||||
// these NMEA from GPS we want to pass to the console
|
// these NMEA from GPS we want to pass to the console
|
||||||
static uint8_t Count=0;
|
// static uint8_t Count=0;
|
||||||
bool RatePass=0;
|
// bool RatePass=0;
|
||||||
Count++; if(Count>=5) { Count=0; RatePass=1; }
|
// Count++; if(Count>=5) { Count=0; RatePass=1; }
|
||||||
if( NMEA.isP() || NMEA.isGxRMC() || NMEA.isGxGGA() || NMEA.isGxGSA() || NMEA.isP() || NMEA.isGxGSV() || (RatePass && (NMEA.isGPTXT())) )
|
// 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
|
// we would need to patch the GGA here for the GPS which does not calc. nor correct for GeoidSepar
|
||||||
#endif
|
#endif
|
||||||
{ if(Parameters.Verbose)
|
{ if(Parameters.Verbose)
|
||||||
|
@ -1041,17 +1057,17 @@ void vTaskGPS(void* pvParameters)
|
||||||
*/
|
*/
|
||||||
if(LineIdle==0) // if any bytes were received ?
|
if(LineIdle==0) // if any bytes were received ?
|
||||||
{ if(!GPS_Burst.Active) GPS_BurstStart(); // if not already started then declare burst started
|
{ 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
|
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
|
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.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
|
{ if(!GPS_Burst.Complete && GPS_Burst.GxGGA && GPS_Burst.GxRMC)
|
||||||
GPS_BurstEnd(); } // declare burst ended
|
{ GPS_BurstComplete(); } // if not complete yet, then declare burst complete
|
||||||
|
}
|
||||||
else if(LineIdle>=1500) // if idle for more than 1.5 sec
|
else if(LineIdle>=1500) // if idle for more than 1.5 sec
|
||||||
{ GPS_Status.Flags=0; }
|
{ 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
|
if(NoValidData>=2000) // if no valid data from GPS for 2sec
|
||||||
|
|
|
@ -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 uint8_t GPS_PosIdx; // Pipe index, increments with every GPS position received
|
||||||
extern GPS_Position GPS_Pos[GPS_PosPipeSize]; // GPS position pipe
|
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_Altitude; // [0.1m] altitude (height above Geoid)
|
||||||
extern int32_t GPS_Latitude; // [0.0001/60 deg]
|
extern int32_t GPS_Latitude; // [0.0001/60 deg]
|
||||||
extern int32_t GPS_Longitude; // [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 RateConfig:1; // navigation rate is configured
|
||||||
bool :1; //
|
bool :1; //
|
||||||
} ;
|
} ;
|
||||||
} Status;
|
} Status; //
|
||||||
|
|
||||||
extern Status GPS_Status;
|
extern Status GPS_Status; // GPS status bits
|
||||||
|
|
||||||
uint32_t GPS_getBaudRate(void); // [bps]
|
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_PPS = 0x01;
|
||||||
const EventBits_t GPSevt_NewPos = 0x02;
|
const EventBits_t GPSevt_NewPos = 0x02;
|
||||||
|
|
||||||
extern FlightMonitor Flight;
|
extern FlightMonitor Flight; // detect/monitor takeoff/flight/landing
|
||||||
|
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
extern "C"
|
extern "C"
|
||||||
|
|
18
main/nmea.h
18
main/nmea.h
|
@ -102,6 +102,10 @@ inline uint8_t NMEA_AppendCheckCRNL(char *NMEA, uint8_t Len) { return NMEA_Appen
|
||||||
{ if(Data[1]!='G') return 0;
|
{ if(Data[1]!='G') return 0;
|
||||||
return Data[2]=='N'; }
|
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 ?
|
uint8_t isGx(void) const // GPS or GLONASS sentence ?
|
||||||
{ return Data[1]=='G'; }
|
{ 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;
|
if(Data[4]!='G') return 0;
|
||||||
return Data[5]=='A'; }
|
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
|
uint8_t isGPGSA(void) const // GPS satellite data
|
||||||
{ if(!isGP()) return 0;
|
{ if(!isGP()) return 0;
|
||||||
if(Data[3]!='G') 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'; }
|
return Data[5]=='A'; }
|
||||||
|
|
||||||
uint8_t isGxGSV(void) const // Satellite data
|
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[3]!='G') return 0;
|
||||||
if(Data[4]!='S') return 0;
|
if(Data[4]!='S') return 0;
|
||||||
return Data[5]=='V'; }
|
return Data[5]=='V'; }
|
||||||
|
|
78
main/ogn.h
78
main/ogn.h
|
@ -653,8 +653,6 @@ class GPS_Time
|
||||||
int8_t Year, Month, Day; // Date (UTC) from GPS
|
int8_t Year, Month, Day; // Date (UTC) from GPS
|
||||||
int8_t Hour, Min, Sec; // Time-of-day (UTC) from GPS
|
int8_t Hour, Min, Sec; // Time-of-day (UTC) from GPS
|
||||||
int16_t mSec; // [ms]
|
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:
|
public:
|
||||||
|
|
||||||
|
@ -742,10 +740,14 @@ class GPS_Time
|
||||||
{ uint16_t Days = DaysSinceYear2000() + DaysSimce1jan();
|
{ uint16_t Days = DaysSinceYear2000() + DaysSimce1jan();
|
||||||
return Times60(Times60(Times24((uint32_t)(Days+10957)))) + getDayTime(); }
|
return Times60(Times60(Times24((uint32_t)(Days+10957)))) + getDayTime(); }
|
||||||
|
|
||||||
uint32_t getFatTime(void) const // return timestamp in FAT format
|
uint16_t getFatDate(void) const // return date in FAT format
|
||||||
{ uint16_t Date = ((uint16_t)(Year+20)<<9) | ((uint16_t)Month<<5) | Day;
|
{ return ((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 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
|
void setUnixTime(uint32_t Time) // works except for the 1.1.2000
|
||||||
{ uint32_t Days = Time/SecsPerDay; // [day] since 1970
|
{ uint32_t Days = Time/SecsPerDay; // [day] since 1970
|
||||||
|
@ -770,6 +772,34 @@ class GPS_Time
|
||||||
uint64_t getUnixTime_ms(void) const
|
uint64_t getUnixTime_ms(void) const
|
||||||
{ return (uint64_t)getUnixTime()*1000 + mSec; }
|
{ 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:
|
private:
|
||||||
|
|
||||||
static const uint32_t SecsPerMin = 60;
|
static const uint32_t SecsPerMin = 60;
|
||||||
|
@ -1398,15 +1428,21 @@ class GPS_Position: public GPS_Time
|
||||||
int32_t calcAltitudeExtrapolation(int32_t Time) const // [ms]
|
int32_t calcAltitudeExtrapolation(int32_t Time) const // [ms]
|
||||||
{ return Time*ClimbRate/1000; } // [0.1m]
|
{ 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]
|
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]
|
int32_t calcLongitudeExtrapolation(int32_t Time, int32_t LonSpeed) const // [ms]
|
||||||
{ int16_t LatCosine = calcLatCosine(calcLatAngle16(Latitude));
|
{ int16_t LatCosine = calcLatCosine(calcLatAngle16(Latitude));
|
||||||
return calcLongitudeExtrapolation(Time, LonSpeed, LatCosine); }
|
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]
|
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
|
// static int32_t calcLatDistance(int32_t Lat1, int32_t Lat2) // [m] distance along latitude
|
||||||
// { return ((int64_t)(Lat2-Lat1)*0x2f684bda+0x80000000)>>32; }
|
// { return ((int64_t)(Lat2-Lat1)*0x2f684bda+0x80000000)>>32; }
|
||||||
|
@ -1581,32 +1617,6 @@ class GPS_Position: public GPS_Time
|
||||||
else if(DOP>255) DOP=255;
|
else if(DOP>255) DOP=255;
|
||||||
VDOP=DOP; return 0; }
|
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:
|
public:
|
||||||
|
|
||||||
int8_t static IndexNMEA(uint8_t Index[20], const char *Seq) // index parameters and verify the NMEA checksum
|
int8_t static IndexNMEA(uint8_t Index[20], const char *Seq) // index parameters and verify the NMEA checksum
|
||||||
|
|
|
@ -7,6 +7,14 @@
|
||||||
|
|
||||||
#include "mbedtls/md5.h"
|
#include "mbedtls/md5.h"
|
||||||
#include "mbedtls/rsa.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 "hal.h"
|
||||||
#include "gps.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
|
int Log_Free(void) { return Log_FIFO.Free(); } // how much space left in the buffer
|
||||||
|
|
||||||
static int Log_Open(void)
|
static int Log_Open(void)
|
||||||
{ LogDate=GPS_FatTime>>16; // get the FAT-time date part
|
{ // LogDate=GPS_DateTime.getFatDate(); // get the FAT-time date part
|
||||||
int32_t Day = LogDate &0x1F; // get day, month, year
|
// int32_t Day = LogDate &0x1F; // get day, month, year
|
||||||
int32_t Month = (LogDate>>5)&0x0F;
|
// int32_t Month = (LogDate>>5)&0x0F;
|
||||||
int32_t Year = (LogDate>>9)-20;
|
// 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;
|
uint32_t Date = 0;
|
||||||
if(Year>=0) Date = Day*10000 + Month*100 + Year; // create DDMMYY number for easy printout
|
if(Year>=0) Date = Day*10000 + Month*100 + Year; // create DDMMYY number for easy printout
|
||||||
strcpy(LogFileName, "/sdcard/CONS/TR000000.LOG");
|
strcpy(LogFileName, "/sdcard/CONS/TR000000.LOG");
|
||||||
|
@ -284,9 +295,54 @@ static void IGC_Check(void) // check if
|
||||||
|
|
||||||
#ifdef WITH_SDLOG
|
#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"
|
extern "C"
|
||||||
void vTaskSDLOG(void* pvParameters)
|
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[2] = Flight.Code36(ID%36); ID/=36;
|
||||||
IGC_Serial[1] = Flight.Code36(ID%36); ID/=36;
|
IGC_Serial[1] = Flight.Code36(ID%36); ID/=36;
|
||||||
IGC_Serial[0] = Flight.Code36(ID%36);
|
IGC_Serial[0] = Flight.Code36(ID%36);
|
||||||
|
|
Ładowanie…
Reference in New Issue