fixed gps fix estimation on gps timeout

src/main/fc/fc_msp.c

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

src/main/io/gps.c

@@ -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
+//3)gpsSol is processed by GPS Fix estimator - updateEstimatedGPSFix()
-void gpsProcessNewSolutionData(void)
+//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
+    if (!timeout) {
-    sensorsSet(SENSOR_GPS);
+        // 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)) {
+    if (gpsSol.fixType == GPS_FIX_3D) {  //estimation kicked in
-        onNewGPSData();
+        gpsProcessNewSolutionData(true);
-        gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat;
     }
 }
 
@@ -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)

src/main/io/gps_fake.c

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

src/main/io/gps_msp.c

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

src/main/io/gps_nmea.c

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

src/main/io/gps_private.h

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

src/main/io/gps_ublox.c

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