From c9c4f0825d5b463a5d4831bc1d9143e09852adf4 Mon Sep 17 00:00:00 2001 From: Pawel Jalocha Date: Wed, 16 Dec 2020 02:18:31 +0000 Subject: [PATCH] Fix timing issues, in particular at GPS rates above 1Hz, other minor fixes --- main/format.h | 8 ++++++++ main/gps.cpp | 30 ++++++++++++++++++++++------- main/http.cpp | 15 +++++++++++++++ main/proc.cpp | 25 +++++++++++++++++------- main/timesync.cpp | 49 +++++++++++++++++++++++++++++++++++++++-------- main/timesync.h | 3 +++ main/ubx.h | 24 +++++++++++++++++++++++ 7 files changed, 132 insertions(+), 22 deletions(-) diff --git a/main/format.h b/main/format.h index 69a55ed..3679a00 100644 --- a/main/format.h +++ b/main/format.h @@ -54,6 +54,14 @@ template Word>>=4; } return Digits; } +template + void Format_Bin( void (*Output)(char), Type Word) +{ const uint8_t Digits = sizeof(Type)<<3; + for( Type Mask = (Type)1<<(Digits-1); Mask; Mask>>=1) + { bool Bit = Word&Mask; + (*Output)('0'+Bit); } +} + uint8_t Format_HHcMMcSS(char *Out, uint32_t Time); uint8_t Format_HHMMSS(char *Out, uint32_t Time); void Format_HHMMSS(void (*Output)(char), uint32_t Time); diff --git a/main/gps.cpp b/main/gps.cpp index 91fb38f..81835dd 100644 --- a/main/gps.cpp +++ b/main/gps.cpp @@ -192,7 +192,7 @@ static void GPS_PPS_On(void) // called on rising edge o xSemaphoreTake(CONS_Mutex, portMAX_DELAY); Format_UnsDec(CONS_UART_Write, TimeSync_Time()%60, 2); CONS_UART_Write('.'); - Format_UnsDec(CONS_UART_Write, TimeSync_msTime(),3); + Format_UnsDec(CONS_UART_Write, TimeSync_msTime(), 3); Format_String(CONS_UART_Write, " -> PPS\n"); xSemaphoreGive(CONS_Mutex); #endif @@ -251,7 +251,7 @@ static void GPS_BurstStart(void) // wh CONS_UART_Write('.'); Format_UnsDec(CONS_UART_Write, TimeSync_msTime(Burst_Tick), 3); Format_String(CONS_UART_Write, " -> GPS_BurstStart() GPS:"); - Format_Hex(CONS_UART_Write, GPS_Status.Flags); + Format_Bin(CONS_UART_Write, GPS_Status.Flags); Format_String(CONS_UART_Write, "\n"); xSemaphoreGive(CONS_Mutex); #endif @@ -316,6 +316,21 @@ static void GPS_BurstStart(void) // wh GPS_Cmd[Len++]='\n'; GPS_Cmd[Len]=0; Format_String(GPS_UART_Write, GPS_Cmd, Len, 0); + strcpy(GPS_Cmd, "$PMTK353,"); + GPS_Cmd[Len++]='0'+Parameters.EnableGPS; // search (or not) for GPS satellites + GPS_Cmd[Len++]=','; + GPS_Cmd[Len++]='0'+Parameters.EnableGLO; // search (or not) for GLONASS satellites (L86 supports) + GPS_Cmd[Len++]=','; + GPS_Cmd[Len++]='0'+Parameters.EnableGAL; // search (or not) for GALILEO satellites (not clear if already supported) + GPS_Cmd[Len++]=','; + GPS_Cmd[Len++]='0'; // GALILEO full mode (whatever it is ?) (not supported yet) + GPS_Cmd[Len++]=','; + GPS_Cmd[Len++]='0'; // search (or not) for BAIDOU satellites (not supported yet) + Len += NMEA_AppendCheck(GPS_Cmd, Len); + GPS_Cmd[Len++]='\r'; + GPS_Cmd[Len++]='\n'; + GPS_Cmd[Len]=0; + Format_String(GPS_UART_Write, GPS_Cmd, Len, 0); GPS_Status.ModeConfig=1; } if(Parameters.NavMode) @@ -429,7 +444,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_Hex(CONS_UART_Write, GPS_Status.Flags); + Format_Bin(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); @@ -446,12 +461,13 @@ static void GPS_BurstComplete(void) // wh int32_t msDiff = GPS_Pos[GPS_PosIdx].FracSec; if(msDiff>=50) { msDiff-=100; UnixTime++; } // [0.01s] msDiff*=10; // [ms] + TimeSync_SoftPPS(Burst_Tick, UnixTime, msDiff+Parameters.PPSdelay); // if(abs(msDiff)<=200) // if (almost) full-second burst - { // TickType_t PPS_Age = Burst_Tick-PPS_Tick; + // { // TickType_t PPS_Age = Burst_Tick-PPS_Tick; // if(PPS_Age>10000) TimeSync_SoftPPS(Burst_Tick, UnixTime, Parameters.PPSdelay); // else TimeSync_SetTime(Burst_Tick-Parameters.PPSdelay, UnixTime); - TimeSync_SoftPPS(Burst_Tick, UnixTime, msDiff+Parameters.PPSdelay); - } + // TimeSync_SoftPPS(Burst_Tick, UnixTime, msDiff+Parameters.PPSdelay); + // } #endif } } @@ -596,7 +612,7 @@ static void GPS_NMEA(void) // wh 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() || (RatePass && (NMEA.isGPTXT() || NMEA.isGxGSV())) ) + if( NMEA.isP() || NMEA.isGxRMC() || NMEA.isGxGGA() || NMEA.isGxGSA() || NMEA.isP() || NMEA.isGxGSV() || (RatePass && (NMEA.isGPTXT())) ) // we would need to patch the GGA here for the GPS which does not calc. nor correct for GeoidSepar #endif { if(Parameters.Verbose) diff --git a/main/http.cpp b/main/http.cpp index c52b92b..3643b70 100644 --- a/main/http.cpp +++ b/main/http.cpp @@ -138,6 +138,16 @@ static void ParmForm_GPS(httpd_req_t *Req) // produce HTML form for GPS paramet httpd_resp_sendstr_chunk(Req, "\n"); + httpd_resp_sendstr_chunk(Req, "GNSS mode\n"); + + httpd_resp_sendstr_chunk(Req, "PPS delay\n"); + httpd_resp_sendstr_chunk(Req, "\n"); } static void ParmForm_Other(httpd_req_t *Req) // produce HTML form for aircraft parameters @@ -240,6 +250,11 @@ static void ParmForm_AP(httpd_req_t *Req) // Wi-Fi access point parameters { cha httpd_resp_send_chunk(Req, Line, Len); httpd_resp_sendstr_chunk(Req, "\">\n"); + httpd_resp_sendstr_chunk(Req, "Min. RSSI [dBm]\n"); + httpd_resp_sendstr_chunk(Req, "\n"); } #endif diff --git a/main/proc.cpp b/main/proc.cpp index 2453d3f..399f8ed 100644 --- a/main/proc.cpp +++ b/main/proc.cpp @@ -483,8 +483,11 @@ void vTaskPROC(void* pvParameters) RF_RxFIFO.Read(); } // remove this packet from the queue static uint32_t PrevSlotTime=0; // remember previous time slot to detect a change - uint32_t SlotTime = TimeSync_Time(); // time slot - if(TimeSync_msTime()<340) SlotTime--; // lasts up to 0.300sec after the PPS + uint32_t Time; // [sec] time slot + TickType_t msTime; // [msec] + TimeSync_Time(Time, msTime); + uint32_t SlotTime=Time; + if(msTime<340) SlotTime--; // lasts up to 0.300sec after the PPS if(SlotTime==PrevSlotTime) continue; // stil same time slot, go back to RX processing PrevSlotTime=SlotTime; // new slot started @@ -498,13 +501,21 @@ void vTaskPROC(void* pvParameters) // GPS_Position *Position = GPS_getPosition(); #ifdef DEBUG_PRINT xSemaphoreTake(CONS_Mutex, portMAX_DELAY); - Format_String(CONS_UART_Write, "getPos("); + // Format_UnsDec(CONS_UART_Write, TimeSync_Time()%60, 2); + // Format_UnsDec(CONS_UART_Write, Time%60, 2); + Format_UnsDec(CONS_UART_Write, Time, 10); + CONS_UART_Write('.'); + // Format_UnsDec(CONS_UART_Write, TimeSync_msTime(), 3); + Format_UnsDec(CONS_UART_Write, msTime, 3); + Format_String(CONS_UART_Write, " -> getPos("); Format_UnsDec(CONS_UART_Write, SlotTime%60, 2); Format_String(CONS_UART_Write, ") => "); - Format_UnsDec(CONS_UART_Write, (uint16_t)BestIdx); - CONS_UART_Write(':'); - Format_SignDec(CONS_UART_Write, BestResid, 3, 2); - Format_String(CONS_UART_Write, "s\n"); + if(Position) + { Format_UnsDec(CONS_UART_Write, (uint16_t)BestIdx); + CONS_UART_Write(':'); + Format_SignDec(CONS_UART_Write, BestResid, 3, 2); + Format_String(CONS_UART_Write, "s"); } + Format_String(CONS_UART_Write, "\n"); xSemaphoreGive(CONS_Mutex); #endif diff --git a/main/timesync.cpp b/main/timesync.cpp index a6d3a01..4a7eeaf 100644 --- a/main/timesync.cpp +++ b/main/timesync.cpp @@ -11,34 +11,67 @@ void TimeSync_HardPPS(TickType_t Tick) // [m void TimeSync_HardPPS(void) { TimeSync_HardPPS(xTaskGetTickCount()); } // void TimeSync_SoftPPS(TickType_t Tick, uint32_t Time, int32_t msOfs) // [ms], [sec], [ms] software PPS: from GPS burst start or from MAV -{ Tick-=msOfs; // [ms] +{ +#ifdef DEBUG_PRINT + xSemaphoreTake(CONS_Mutex, portMAX_DELAY); + Format_String(CONS_UART_Write, "TimeSync_SoftPPS("); + Format_UnsDec(CONS_UART_Write, Tick); + Format_String(CONS_UART_Write, ", "); + Format_UnsDec(CONS_UART_Write, Time); + Format_String(CONS_UART_Write, ", "); + Format_SignDec(CONS_UART_Write, msOfs); + Format_String(CONS_UART_Write, ") => "); + xSemaphoreGive(CONS_Mutex); +#endif + Tick-=msOfs; // [ms] TickType_t Incr=(Tick-TimeSync_RefTick+500)/1000; // [sec] TimeSync_RefTime = Time; // [sec] TimeSync_RefTick += Incr*1000; // [ms] // if(Tick>TimeSync_RefTick) TimeSync_RefTick++; // else if(Tick>4; } + TimeSync_RefTick += (Diff+8)>>4; +#ifdef DEBUG_PRINT + xSemaphoreTake(CONS_Mutex, portMAX_DELAY); + Format_UnsDec(CONS_UART_Write, TimeSync_RefTick); + Format_String(CONS_UART_Write, ", "); + Format_UnsDec(CONS_UART_Write, TimeSync_RefTime); + Format_String(CONS_UART_Write, "\n"); + xSemaphoreGive(CONS_Mutex); +#endif +} void TimeSync_CorrRef(int16_t Corr) { TimeSync_RefTick += Corr; } TickType_t TimeSync_msTime(TickType_t Tick) // [ms] get fractional time which corresponds to given system tick -{ TickType_t msTime=Tick-TimeSync_RefTick; +{ TickType_t msTime = Tick+1000-TimeSync_RefTick; if(msTime<1000) return msTime; if(msTime<2000) return msTime-1000; + if(msTime<3000) return msTime-2000; return msTime%1000; } TickType_t TimeSync_msTime(void) // [msec] get fractional time now { return TimeSync_msTime(xTaskGetTickCount()); } uint32_t TimeSync_Time(TickType_t Tick) // [sec] get Time which corresponds to given system tick -{ int32_t Time=Tick-TimeSync_RefTick; - // if(Time<0) return TimeSync_RefTime - (); - if(Time<1000) return TimeSync_RefTime; - if(Time<2000) return TimeSync_RefTime+1; - return TimeSync_RefTime + (Time/1000); } +{ TickType_t msTime = Tick+1000-TimeSync_RefTick; + if(msTime<1000) return TimeSync_RefTime-1; + if(msTime<2000) return TimeSync_RefTime; + if(msTime<3000) return TimeSync_RefTime+1; + return TimeSync_RefTime-1 + (msTime/1000); } uint32_t TimeSync_Time(void) // [sec] get Time now { return TimeSync_Time(xTaskGetTickCount()); } +void TimeSync_Time(uint32_t &Time, TickType_t &msTime, TickType_t Tick) +{ TickType_t diffTime=Tick+1000-TimeSync_RefTick; + // if(diffTime< 0) { Time = TimeSync_RefTime-1; msTime=diffTime+1000; return; } + if(diffTime<1000) { Time = TimeSync_RefTime-1; msTime=diffTime ; return; } + if(diffTime<2000) { Time = TimeSync_RefTime ; msTime=diffTime-1000; return; } + if(diffTime<3000) { Time = TimeSync_RefTime+1; msTime=diffTime-2000; return; } + TickType_t Diff = diffTime/1000; diffTime -= Diff*1000; + Time = TimeSync_RefTime+Diff-1; msTime=diffTime; } + +void TimeSync_Time(uint32_t &Time, TickType_t &msTime) +{ TimeSync_Time(Time, msTime, xTaskGetTickCount()); } diff --git a/main/timesync.h b/main/timesync.h index d87c146..858ff2e 100644 --- a/main/timesync.h +++ b/main/timesync.h @@ -16,6 +16,9 @@ TickType_t TimeSync_msTime(void); uint32_t TimeSync_Time(TickType_t Tick); // [sec] get Time which corresponds to given system tick uint32_t TimeSync_Time(void); +void TimeSync_Time(uint32_t &Time, TickType_t &msTime, TickType_t Tick); +void TimeSync_Time(uint32_t &Time, TickType_t &msTime); + void TimeSync_CorrRef(int16_t Corr); // [ms] correct the time reference [RTOS tick] #endif // __TIMESYNC_H__ diff --git a/main/ubx.h b/main/ubx.h index fe0b352..8318fdb 100644 --- a/main/ubx.h +++ b/main/ubx.h @@ -189,6 +189,30 @@ class UBX_CFG_RATE // 0x06 0x08 uint16_t timeRef; // 0=UTC, 1=GPS } ; +// for GPS: 0x00, 0x08, 0x10, 0x00, 0x01, 0x00, 0x01, 0x01 (enable) +// for SBAS: 0x01, 0x02, 0x03, 0x00, 0x01, 0x00, 0x01, 0x01 (enable) +// for BeiDou: 0x03, 0x00, 0x10, 0x00, 0x00, 0x00, 0x01, 0x01 (disable) +// for QZSS: 0x05, 0x00, 0x03, 0x00, 0x00, 0x00, 0x01, 0x01 (disable) +// for Glonass: 0x06, 0x04, 0x0E, 0x00, 0x00, 0x00, 0x01, 0x01 (disable) +// for Galileo: 0x02, 0x04, 0x08, 0x00, 0x00, 0x00, 0x01, 0x01 (disable) +class UBX_CFG_GNSS_Block +{ public: + uint8_t gnssId; // system identifier: 0=GPS, 1=SBAS, 2=Galileo, 3=BeiDou, 4=IMES, 5=QZSS, 6=Glonass + uint8_t resTrkCh; // number of reserved tracking channels + uint8_t maxTrkCh; // + uint8_t reserved; + uint32_t flags; // bit #0 = enable, bit #16..23 = sigCfgMask +} ; + +class UBX_CFG_GNSS // 0x06 0x3E +{ public: + uint8_t msgVer; // version = 0 + uint8_t numTrkChHw; // number of hardware tracking channels (read-only) + uint8_t numTrkChUse; // number of tracker channels to use (0xFF = use max. allowed by hardware) + uint8_t numConfigBlocks; // number of config. blocks which follows: 8 bytes per block + UBX_CFG_GNSS_Block Block[6]; // 5 or 6 blocks +} ; + class UBX_CFG_NAV5 // 0x06 0x24 { public: uint16_t mask; // bit #0 = apply dynamic mode settings, #1 = apply min. elev. settings, #2 = apply fix mode settings