1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 01:05:21 +03:00

fixed gps fix estimation on gps timeout

This commit is contained in:
Roman Lut 2023-05-19 23:40:06 +02:00
parent 7d007a5c19
commit cdd299f045
7 changed files with 24 additions and 35 deletions

View file

@ -3515,7 +3515,6 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
if (feature(FEATURE_GPS) && SIMULATOR_HAS_OPTION(HITL_HAS_NEW_GPS_DATA)) {
gpsSolDRV.fixType = sbufReadU8(src);
gpsSolDRV.hdop = gpsSolDRV.fixType == GPS_NO_FIX ? 9999 : 100;
gpsSolDRV.flags.hasNewData = true;
gpsSolDRV.numSat = sbufReadU8(src);
if (gpsSolDRV.fixType != GPS_NO_FIX) {
@ -3536,14 +3535,12 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
gpsSolDRV.eph = 100;
gpsSolDRV.epv = 100;
ENABLE_STATE(GPS_FIX);
} else {
sbufAdvance(src, sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3);
}
// Feed data to navigation
gpsProcessNewDriverData();
gpsProcessNewSolutionData();
gpsProcessNewSolutionData(false);
} else {
sbufAdvance(src, sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3);
}

View file

@ -168,7 +168,7 @@ bool canEstimateGPSFix(void)
#if defined(USE_GPS) && defined(USE_MAG) && defined(USE_BARO)
//we do not check neither sensors(SENSOR_GPS) nor FEATURE(FEATURE_GPS) because:
//1) checking STATE(GPS_FIX_HOME) is enought to ensure that GPS sensor was initialized once
//1) checking STATE(GPS_FIX_HOME) is enough to ensure that GPS sensor was initialized once
//2) sensors(SENSOR_GPS) is false on GPS timeout. We also want to support GPS timeouts, not just lost fix
return positionEstimationConfig()->allow_gps_fix_estimation && STATE(AIRPLANE) &&
sensors(SENSOR_BARO) && baroIsHealthy() &&
@ -231,7 +231,6 @@ void updateEstimatedGPSFix(void)
gpsSol.fixType = GPS_FIX_3D;
gpsSol.hdop = 99;
gpsSol.flags.hasNewData = true;
gpsSol.numSat = 99;
gpsSol.eph = 100;
@ -298,8 +297,9 @@ void gpsProcessNewDriverData(void)
//called after:
//1)driver copies gpsSolDRV to gpsSol
//2)gpsSol is processed by "Disable GPS logical switch"
//3)gpsSol is processed by GPS Fix estimator
void gpsProcessNewSolutionData(void)
//3)gpsSol is processed by GPS Fix estimator - updateEstimatedGPSFix()
//On GPS sensor timeout - called after updateEstimatedGPSFix()
void gpsProcessNewSolutionData(bool timeout)
{
if ( gpsSol.numSat == 99 ) {
ENABLE_STATE(GPS_ESTIMATED_FIX);
@ -320,8 +320,10 @@ void gpsProcessNewSolutionData(void)
}
}
// Set sensor as ready and available
sensorsSet(SENSOR_GPS);
if (!timeout) {
// Data came from GPS sensor - set sensor as ready and available (it may still not have GPS fix)
sensorsSet(SENSOR_GPS);
}
// Pass on GPS update to NAV and IMU
onNewGPSData();
@ -357,15 +359,13 @@ static void gpsResetSolution(gpsSolutionData_t* gpsSol)
void gpsTryEstimateOnTimeout(void)
{
gpsResetSolution(&gpsSolDRV);
gpsResetSolution(&gpsSol);
DISABLE_STATE(GPS_FIX);
updateEstimatedGPSFix();
if (STATE(GPS_ESTIMATED_FIX)) {
onNewGPSData();
gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat;
if (gpsSol.fixType == GPS_FIX_3D) { //estimation kicked in
gpsProcessNewSolutionData(true);
}
}
@ -462,28 +462,21 @@ bool gpsUpdate(void)
#ifdef USE_SIMULATOR
if (ARMING_FLAG(SIMULATOR_MODE_HITL)) {
if ( SIMULATOR_HAS_OPTION(HITL_GPS_TIMEOUT))
{
if ( SIMULATOR_HAS_OPTION(HITL_GPS_TIMEOUT)) {
gpsSetState(GPS_LOST_COMMUNICATION);
sensorsClear(SENSOR_GPS);
gpsStats.timeouts = 5;
gpsTryEstimateOnTimeout();
return false;
}
else
{
} else {
gpsSetState(GPS_RUNNING);
sensorsSet(SENSOR_GPS);
bool res = gpsSol.flags.hasNewData;
gpsSol.flags.hasNewData = false;
return res;
}
bool res = gpsSol.flags.hasNewData;
gpsSol.flags.hasNewData = false;
return res;
}
#endif
// Assume that we don't have new data this run
gpsSol.flags.hasNewData = false;
switch (gpsState.state) {
default:
case GPS_INITIALIZING:
@ -491,11 +484,9 @@ bool gpsUpdate(void)
if ((millis() - gpsState.lastStateSwitchMs) >= GPS_INIT_DELAY) {
// Reset internals
DISABLE_STATE(GPS_FIX);
gpsSol.fixType = GPS_NO_FIX;
// Reset solution
gpsResetSolution(&gpsSolDRV);
gpsResetSolution(&gpsSol);
// Call GPS protocol reset handler
gpsProviders[gpsState.gpsConfig->provider].restart();
@ -529,7 +520,9 @@ bool gpsUpdate(void)
gpsTryEstimateOnTimeout();
}
return gpsSol.flags.hasNewData;
bool res = gpsSol.flags.hasNewData;
gpsSol.flags.hasNewData = false;
return res;
}
void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort)

View file

@ -46,7 +46,7 @@ void gpsFakeRestart(void)
void gpsFakeHandle(void)
{
gpsProcessNewSolutionData();
gpsProcessNewSolutionData(false);
}
void gpsFakeSet(
@ -79,7 +79,6 @@ void gpsFakeSet(
gpsSolDRV.flags.validVelNE = true;
gpsSolDRV.flags.validVelD = true;
gpsSolDRV.flags.validEPE = true;
gpsSolDRV.flags.hasNewData = true;
if (time) {
struct tm* gTime = gmtime(&time);

View file

@ -63,7 +63,7 @@ void gpsRestartMSP(void)
void gpsHandleMSP(void)
{
if (newDataReady) {
gpsProcessNewSolutionData();
gpsProcessNewSolutionData(false);
newDataReady = false;
}
}

View file

@ -313,7 +313,7 @@ STATIC_PROTOTHREAD(gpsProtocolStateThreadNMEA)
// GPS is ready - execute the gpsProcessNewSolutionData() based on gpsProtocolReceiverThread semaphore
while (1) {
ptSemaphoreWait(semNewDataReady);
gpsProcessNewSolutionData();
gpsProcessNewSolutionData(false);
}
ptEnd(0);

View file

@ -68,7 +68,7 @@ extern uint16_t gpsConstrainEPE(uint32_t epe);
extern uint16_t gpsConstrainHDOP(uint32_t hdop);
void gpsProcessNewDriverData(void);
void gpsProcessNewSolutionData(void);
void gpsProcessNewSolutionData(bool);
void gpsSetProtocolTimeout(timeMs_t timeoutMs);
extern void gpsRestartUBLOX(void);

View file

@ -1053,7 +1053,7 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread)
// GPS is ready - execute the gpsProcessNewSolutionData() based on gpsProtocolReceiverThread semaphore
while (1) {
ptSemaphoreWait(semNewDataReady);
gpsProcessNewSolutionData();
gpsProcessNewSolutionData(false);
}
ptEnd(0);