diff --git a/main/gps.cpp b/main/gps.cpp index 4ea0053..8560c25 100644 --- a/main/gps.cpp +++ b/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 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 diff --git a/main/gps.h b/main/gps.h index 54a31a5..8129a26 100644 --- a/main/gps.h +++ b/main/gps.h @@ -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" diff --git a/main/nmea.h b/main/nmea.h index b581c54..72f33be 100644 --- a/main/nmea.h +++ b/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; 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'; } diff --git a/main/ogn.h b/main/ogn.h index a92d53e..98f417b 100644 --- a/main/ogn.h +++ b/main/ogn.h @@ -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 diff --git a/main/sdlog.cpp b/main/sdlog.cpp index 47a333a..3157bd0 100644 --- a/main/sdlog.cpp +++ b/main/sdlog.cpp @@ -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);