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

Fix timing issues, in particular at GPS rates above 1Hz, other minor fixes

pull/30/head
Pawel Jalocha 2020-12-16 02:18:31 +00:00
rodzic d51f1b0c42
commit c9c4f0825d
7 zmienionych plików z 132 dodań i 22 usunięć
Pokaż statystyki Ściągnij plik aktualizacji Ściągnij plik porównania Expand all files Collapse all files

8
main/format.h
Wyświetl plik

@ -54,6 +54,14 @@ template <class Type>
Word>>=4; }
return Digits; }
template <class Type>
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);

30
main/gps.cpp
Wyświetl plik

@ -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)

15
main/http.cpp
Wyświetl plik

@ -138,6 +138,16 @@ static void ParmForm_GPS(httpd_req_t *Req) // produce HTML form for GPS paramet
httpd_resp_sendstr_chunk(Req, "</td></tr>\n");
httpd_resp_sendstr_chunk(Req, "<tr><td>GNSS mode</td><td><input type=\"text\" name=\"GNSS\" size=\"10\" value=\"0x");
Len=Format_Hex(Line, Parameters.GNSS);
httpd_resp_send_chunk(Req, Line, Len);
httpd_resp_sendstr_chunk(Req, "\"></td></tr>\n");
httpd_resp_sendstr_chunk(Req, "<tr><td>PPS delay</td><td><input type=\"text\" name=\"PPSdelay\" size=\"10\" value=\"0x");
Len=Format_UnsDec(Line, Parameters.PPSdelay);
httpd_resp_send_chunk(Req, Line, Len);
httpd_resp_sendstr_chunk(Req, "\"></td></tr>\n");
httpd_resp_sendstr_chunk(Req, "</table></form>\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, "\"></td></tr>\n");
httpd_resp_sendstr_chunk(Req, "<tr><td>Min. RSSI [dBm]</td><td><input type=\"text\" name=\"APminSig\" size=\"10\" value=\"");
Len=Format_SignDec(Line, Parameters.APminSig);
httpd_resp_send_chunk(Req, Line, Len);
httpd_resp_sendstr_chunk(Req, "\"></td></tr>\n");
httpd_resp_sendstr_chunk(Req, "</table></form>\n"); }
#endif

25
main/proc.cpp
Wyświetl plik

@ -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

49
main/timesync.cpp
Wyświetl plik

@ -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<TimeSync_RefTick) TimeSync_RefTick--;
int32_t Diff = Tick-TimeSync_RefTick;
TimeSync_RefTick += (Diff+8)>>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()); }

3
main/timesync.h
Wyświetl plik

@ -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__

24
main/ubx.h
Wyświetl plik

@ -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