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

Improve merging pressure and GPS data at higher update rates, but still needs work

pull/30/head
Pawel Jalocha 2020-12-24 15:04:08 +00:00
rodzic bee6b181a6
commit a13d670db3
4 zmienionych plików z 47 dodań i 30 usunięć
Pokaż statystyki Ściągnij plik aktualizacji Ściągnij plik porównania Expand all files Collapse all files

56
main/gps.cpp
Wyświetl plik

@ -76,7 +76,7 @@ static union
} ; } ;
} GPS_Burst; } GPS_Burst;
// for the autobaud on the GPS port // for the autobaud on the GPS port
const int GPS_BurstTimeout = 50; // [ms] const int GPS_BurstTimeout = 100; // [ms]
// static const uint8_t BaudRates=7; // number of possible baudrates choices // static const uint8_t BaudRates=7; // number of possible baudrates choices
// static uint8_t BaudRateIdx=0; // actual choice // static uint8_t BaudRateIdx=0; // actual choice
@ -307,8 +307,7 @@ static void GPS_BurstStart(void) // wh
#endif #endif
#ifdef WITH_GPS_MTK #ifdef WITH_GPS_MTK
if(Parameters.NavRate) if(Parameters.NavRate)
{ strcpy(GPS_Cmd, "$PMTK220,"); // MTK command to change the navigation rate { uint8_t Len = Format_String(GPS_Cmd, "$PMTK220,"); // navigation rate
uint8_t Len = strlen(GPS_Cmd);
uint16_t OneSec = 1000; uint16_t OneSec = 1000;
Len += Format_UnsDec(GPS_Cmd+Len, OneSec/Parameters.NavRate); Len += Format_UnsDec(GPS_Cmd+Len, OneSec/Parameters.NavRate);
Len += NMEA_AppendCheck(GPS_Cmd, Len); Len += NMEA_AppendCheck(GPS_Cmd, Len);
@ -316,7 +315,18 @@ static void GPS_BurstStart(void) // wh
GPS_Cmd[Len++]='\n'; GPS_Cmd[Len++]='\n';
GPS_Cmd[Len]=0; GPS_Cmd[Len]=0;
Format_String(GPS_UART_Write, GPS_Cmd, Len, 0); Format_String(GPS_UART_Write, GPS_Cmd, Len, 0);
strcpy(GPS_Cmd, "$PMTK353,"); GPS_Status.ModeConfig=1; }
if(Parameters.NavMode)
{ uint8_t Len = Format_String(GPS_Cmd, "$PMTK886,"); // MTK command to change the navigation mode
GPS_Cmd[Len++]='0'+Parameters.NavMode;
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.GNSS)
{ uint8_t Len = Format_String(GPS_Cmd, "$PMTK353,"); // GNSS configuration
GPS_Cmd[Len++]='0'+Parameters.EnableGPS; // search (or not) for GPS satellites GPS_Cmd[Len++]='0'+Parameters.EnableGPS; // search (or not) for GPS satellites
GPS_Cmd[Len++]=','; GPS_Cmd[Len++]=',';
GPS_Cmd[Len++]='0'+Parameters.EnableGLO; // search (or not) for GLONASS satellites (L86 supports) GPS_Cmd[Len++]='0'+Parameters.EnableGLO; // search (or not) for GLONASS satellites (L86 supports)
@ -330,20 +340,7 @@ static void GPS_BurstStart(void) // wh
GPS_Cmd[Len++]='\r'; GPS_Cmd[Len++]='\r';
GPS_Cmd[Len++]='\n'; GPS_Cmd[Len++]='\n';
GPS_Cmd[Len]=0; GPS_Cmd[Len]=0;
Format_String(GPS_UART_Write, GPS_Cmd, Len, 0); Format_String(GPS_UART_Write, GPS_Cmd, Len, 0); }
GPS_Status.ModeConfig=1;
}
if(Parameters.NavMode)
{ strcpy(GPS_Cmd, "$PMTK886,"); // MTK command to change the navigation mode
uint8_t Len = strlen(GPS_Cmd);
GPS_Cmd[Len++]='0'+Parameters.NavMode;
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;
}
#endif #endif
} }
if(!GPS_Status.BaudConfig) // if GPS baud config is not done yet if(!GPS_Status.BaudConfig) // if GPS baud config is not done yet
@ -472,7 +469,7 @@ static void GPS_BurstComplete(void) // wh
} }
} }
if(GPS_Pos[GPS_PosIdx].isValid()) // position is complete and locked if(GPS_Pos[GPS_PosIdx].isValid()) // position is complete and locked
{ if(Parameters.manGeoidSepar) // if GeoidSepar is "manual" - this implies the GPS does not correct for it { if(Parameters.manGeoidSepar) // if GeoidSepar is "manual" - this implies the GPS does not correct for it
{ GPS_Pos[GPS_PosIdx].GeoidSeparation = Parameters.GeoidSepar; // copy the manually set GeoidSepar { GPS_Pos[GPS_PosIdx].GeoidSeparation = Parameters.GeoidSepar; // copy the manually set GeoidSepar
GPS_Pos[GPS_PosIdx].Altitude -= Parameters.GeoidSepar; } // correct the Altitude - we likely need a separate flag for this GPS_Pos[GPS_PosIdx].Altitude -= Parameters.GeoidSepar; } // correct the Altitude - we likely need a separate flag for this
GPS_Pos[GPS_PosIdx].calcLatitudeCosine(); GPS_Pos[GPS_PosIdx].calcLatitudeCosine();
@ -525,7 +522,7 @@ static void GPS_BurstComplete(void) // wh
} }
uint8_t NextPosIdx = (GPS_PosIdx+1)&PosPipeIdxMask; // next position to be recorded uint8_t NextPosIdx = (GPS_PosIdx+1)&PosPipeIdxMask; // next position to be recorded
if( GPS_Pos[GPS_PosIdx].isTimeValid() && GPS_Pos[NextPosIdx].isTimeValid() ) if( GPS_Pos[GPS_PosIdx].isTimeValid() && GPS_Pos[NextPosIdx].isTimeValid() )
{ int32_t Period = GPS_Pos[GPS_PosIdx].calcTimeDiff(GPS_Pos[NextPosIdx]); { int32_t Period = GPS_Pos[GPS_PosIdx].calcTimeDiff(GPS_Pos[NextPosIdx]); // [1/100sec]
if(Period>0) GPS_PosPeriod = (Period+GPS_PosPipeSize/2)/(GPS_PosPipeSize-1); if(Period>0) GPS_PosPeriod = (Period+GPS_PosPipeSize/2)/(GPS_PosPipeSize-1);
#ifdef DEBUG_PRINT #ifdef DEBUG_PRINT
xSemaphoreTake(CONS_Mutex, portMAX_DELAY); xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
@ -545,9 +542,20 @@ static void GPS_BurstComplete(void) // wh
// Sec++; if(Sec>=60) Sec=0; // Sec++; if(Sec>=60) Sec=0;
// GPS_Pos[NextPosIdx].Sec=Sec; // set the correct time for the next position // GPS_Pos[NextPosIdx].Sec=Sec; // set the correct time for the next position
GPS_Pos[NextPosIdx].copyTime(GPS_Pos[GPS_PosIdx]); // copy time from current position GPS_Pos[NextPosIdx].copyTime(GPS_Pos[GPS_PosIdx]); // copy time from current position
GPS_Pos[NextPosIdx].incrTime(); // increment time by 1 sec GPS_Pos[NextPosIdx].incrTimeFrac(GPS_PosPeriod); // increment time by the expected period
Flight.Process(GPS_Pos[GPS_PosIdx]); Flight.Process(GPS_Pos[GPS_PosIdx]);
// GPS_Pos[NextPosIdx].copyDate(GPS_Pos[GPS_PosIdx]); // GPS_Pos[NextPosIdx].copyDate(GPS_Pos[GPS_PosIdx]);
#ifdef DEBUG_PRINT
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
Format_String(CONS_UART_Write, "GPS -> ");
Format_UnsDec(CONS_UART_Write, (uint16_t)GPS_Pos[NextPosIdx].Sec, 2);
CONS_UART_Write('.');
Format_UnsDec(CONS_UART_Write, (uint16_t)GPS_Pos[NextPosIdx].FracSec, 2);
Format_String(CONS_UART_Write, "s ");
Format_UnsDec(CONS_UART_Write, GPS_PosPeriod, 3, 2);
Format_String(CONS_UART_Write, "s\n");
xSemaphoreGive(CONS_Mutex);
#endif
GPS_PosIdx=NextPosIdx; // advance the index GPS_PosIdx=NextPosIdx; // advance the index
xEventGroupSetBits(GPS_Event, GPSevt_NewPos); xEventGroupSetBits(GPS_Event, GPSevt_NewPos);
} }
@ -557,16 +565,16 @@ static void GPS_BurstEnd(void) // wh
// ---------------------------------------------------------------------------- // ----------------------------------------------------------------------------
GPS_Position *GPS_getPosition(uint8_t &BestIdx, int16_t &BestRes, int8_t Sec, int8_t Frac) // return GPS position closest to the given Sec.Frac GPS_Position *GPS_getPosition(uint8_t &BestIdx, int16_t &BestRes, int8_t Sec, int8_t Frac, bool Ready) // return GPS position closest to the given Sec.Frac
{ int16_t TargetTime = Frac+(int16_t)Sec*100; // target time including the seconds { int16_t TargetTime = Frac+(int16_t)Sec*100; // target time including the seconds
BestIdx=0; BestRes=0x7FFF; BestIdx=0; BestRes=0x7FFF;
for(uint8_t Idx=0; Idx<GPS_PosPipeSize; Idx++) // run through the GPS positions stored in the pipe for(uint8_t Idx=0; Idx<GPS_PosPipeSize; Idx++) // run through the GPS positions stored in the pipe
{ GPS_Position *Pos=GPS_Pos+Idx; { GPS_Position *Pos=GPS_Pos+Idx;
if(!Pos->isReady) continue; // skip those not-ready yet if(Ready) if(!Pos->isReady) continue; // if only Ready positions requested: skip those not-ready yet
int16_t Diff = TargetTime - (Pos->FracSec + (int16_t)Pos->Sec*100); // difference from the target time int16_t Diff = TargetTime - (Pos->FracSec + (int16_t)Pos->Sec*100); // difference from the target time
if(Diff<(-3000)) Diff+=6000; // wrap-around 60 sec if(Diff<(-3000)) Diff+=6000; // wrap-around 60 sec
else if(Diff>=3000) Diff-=6000; else if(Diff>=3000) Diff-=6000;
if(abs(Diff)<abs(BestRes)) { BestRes=Diff; BestIdx=Idx; } // store the smallest difference from target if(abs(Diff)<abs(BestRes)) { BestRes=Diff; BestIdx=Idx; } // store the smallest difference from target
} }
return BestRes==0x7FFF ? 0:GPS_Pos+BestIdx; } return BestRes==0x7FFF ? 0:GPS_Pos+BestIdx; }

2
main/gps.h
Wyświetl plik

@ -48,7 +48,7 @@ uint32_t GPS_getBaudRate(void); // [bps]
GPS_Position *GPS_getPosition(void); GPS_Position *GPS_getPosition(void);
GPS_Position *GPS_getPosition(int8_t Sec); // return GPS position for given Sec GPS_Position *GPS_getPosition(int8_t Sec); // return GPS position for given Sec
GPS_Position *GPS_getPosition(uint8_t &BestIdx, int16_t &BestRes, int8_t Sec, int8_t Frac); // return GPS position closest to the given Sec.Frac GPS_Position *GPS_getPosition(uint8_t &BestIdx, int16_t &BestRes, int8_t Sec, int8_t Frac, bool Ready=1); // return GPS position closest to the given Sec.Frac
int16_t GPS_AverageSpeed(void); // [0.1m/s] calc. average speed based on most recent GPS positions int16_t GPS_AverageSpeed(void); // [0.1m/s] calc. average speed based on most recent GPS positions

5
main/ogn.h
Wyświetl plik

@ -666,6 +666,11 @@ class GPS_Time
bool isDateValid(void) const // is the GPS date valid ? bool isDateValid(void) const // is the GPS date valid ?
{ return (Year>=0) && (Month>=0) && (Day>=0); } { return (Year>=0) && (Month>=0) && (Day>=0); }
void incrTimeFrac(int8_t Frac)
{ if(Frac>=100) { incrTime(); Frac-=100; }
if(Frac>=100-FracSec) { incrTime(); Frac-=100; }
FracSec+=Frac; }
uint8_t incrTime(void) // increment HH:MM:SS by one second uint8_t incrTime(void) // increment HH:MM:SS by one second
{ Sec++; if(Sec<60) return 0; { Sec++; if(Sec<60) return 0;
Sec=0; Sec=0;

14
main/sens.cpp
Wyświetl plik

@ -201,9 +201,11 @@ static void ProcBaro(void)
uint16_t msTime = TimeSync_msTime(MeasTick); uint16_t msTime = TimeSync_msTime(MeasTick);
uint8_t Frac = Sec%10; // [0.1s] uint8_t Frac = Sec%10; // [0.1s]
if(Frac==0) // if(Frac==0)
{ GPS_Position *PosPtr = GPS_getPosition(Sec/10); // get GPS position record for this second { // GPS_Position *PosPtr = GPS_getPosition(Sec/10); // get GPS position record for this second
#ifdef DEBUG_PRINT uint8_t BestIdx; int16_t BestRes;
GPS_Position *PosPtr = GPS_getPosition(BestIdx, BestRes, Sec/10, Frac*10, 0);
// #ifdef DEBUG_PRINT
xSemaphoreTake(CONS_Mutex, portMAX_DELAY); xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
Format_String(CONS_UART_Write, "ProcBaro: "); Format_String(CONS_UART_Write, "ProcBaro: ");
Format_UnsDec(CONS_UART_Write, (uint16_t)Sec, 3, 1); Format_UnsDec(CONS_UART_Write, (uint16_t)Sec, 3, 1);
@ -213,10 +215,12 @@ static void ProcBaro(void)
CONS_UART_Write('.'); CONS_UART_Write('.');
Format_UnsDec(CONS_UART_Write, (uint16_t)PosPtr->FracSec, 2); Format_UnsDec(CONS_UART_Write, (uint16_t)PosPtr->FracSec, 2);
CONS_UART_Write('s'); } CONS_UART_Write('s'); }
Format_String(CONS_UART_Write, " => ");
Format_SignDec(CONS_UART_Write, BestRes, 3, 2);
Format_String(CONS_UART_Write, "\n"); Format_String(CONS_UART_Write, "\n");
xSemaphoreGive(CONS_Mutex); xSemaphoreGive(CONS_Mutex);
#endif // #endif
if(PosPtr) // if found: if(PosPtr && abs(BestRes)<=20) // if found and small time error
{ PosPtr->Pressure = Pressure; // [0.25Pa] { PosPtr->Pressure = Pressure; // [0.25Pa]
PosPtr->StdAltitude = StdAltitude; // store standard pressure altitude PosPtr->StdAltitude = StdAltitude; // store standard pressure altitude
PosPtr->ClimbRate = ClimbRate/10; // [0.1m/s] PosPtr->ClimbRate = ClimbRate/10; // [0.1m/s]