AP_NavEKF2: Add fix status to GPS check report message

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@@ -343,89 +343,95 @@ bool NavEKF2_core::readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &d
 void NavEKF2_core::readGpsData()
 {
     // check for new GPS data
-    if ((_ahrs->get_gps().last_message_time_ms() != lastTimeGpsReceived_ms) &&
+    if (_ahrs->get_gps().last_message_time_ms() != lastTimeGpsReceived_ms) {
-            (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D))
+        if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
-    {
+            // report GPS fix status
-        // store fix time from previous read
+            gpsCheckStatus.bad_fix = false;
-        secondLastGpsTime_ms = lastTimeGpsReceived_ms;
 
-        // get current fix time
+            // store fix time from previous read
-        lastTimeGpsReceived_ms = _ahrs->get_gps().last_message_time_ms();
+            secondLastGpsTime_ms = lastTimeGpsReceived_ms;
 
-        // estimate when the GPS fix was valid, allowing for GPS processing and other delays
+            // get current fix time
-        // ideally we should be using a timing signal from the GPS receiver to set this time
+            lastTimeGpsReceived_ms = _ahrs->get_gps().last_message_time_ms();
-        gpsDataNew.time_ms = lastTimeGpsReceived_ms - frontend._gpsDelay_ms;
 
-        // read the NED velocity from the GPS
+            // estimate when the GPS fix was valid, allowing for GPS processing and other delays
-        gpsDataNew.vel = _ahrs->get_gps().velocity();
+            // ideally we should be using a timing signal from the GPS receiver to set this time
+            gpsDataNew.time_ms = lastTimeGpsReceived_ms - frontend._gpsDelay_ms;
 
-        // Use the speed accuracy from the GPS if available, otherwise set it to zero.
+            // read the NED velocity from the GPS
-        // Apply a decaying envelope filter with a 5 second time constant to the raw speed accuracy data
+            gpsDataNew.vel = _ahrs->get_gps().velocity();
-        float alpha = constrain_float(0.0002f * (lastTimeGpsReceived_ms - secondLastGpsTime_ms),0.0f,1.0f);
-        gpsSpdAccuracy *= (1.0f - alpha);
-        float gpsSpdAccRaw;
-        if (!_ahrs->get_gps().speed_accuracy(gpsSpdAccRaw)) {
-            gpsSpdAccuracy = 0.0f;
-        } else {
-            gpsSpdAccuracy = max(gpsSpdAccuracy,gpsSpdAccRaw);
-        }
 
-        // check if we have enough GPS satellites and increase the gps noise scaler if we don't
+            // Use the speed accuracy from the GPS if available, otherwise set it to zero.
-        if (_ahrs->get_gps().num_sats() >= 6 && (PV_AidingMode == AID_ABSOLUTE)) {
+            // Apply a decaying envelope filter with a 5 second time constant to the raw speed accuracy data
-            gpsNoiseScaler = 1.0f;
+            float alpha = constrain_float(0.0002f * (lastTimeGpsReceived_ms - secondLastGpsTime_ms),0.0f,1.0f);
-        } else if (_ahrs->get_gps().num_sats() == 5 && (PV_AidingMode == AID_ABSOLUTE)) {
+            gpsSpdAccuracy *= (1.0f - alpha);
-            gpsNoiseScaler = 1.4f;
+            float gpsSpdAccRaw;
-        } else { // <= 4 satellites or in constant position mode
+            if (!_ahrs->get_gps().speed_accuracy(gpsSpdAccRaw)) {
-            gpsNoiseScaler = 2.0f;
+                gpsSpdAccuracy = 0.0f;
-        }
+            } else {
-
+                gpsSpdAccuracy = max(gpsSpdAccuracy,gpsSpdAccRaw);
-        // Check if GPS can output vertical velocity and set GPS fusion mode accordingly
-        if (_ahrs->get_gps().have_vertical_velocity() && frontend._fusionModeGPS == 0) {
-            useGpsVertVel = true;
-        } else {
-            useGpsVertVel = false;
-        }
-
-        // Monitor quality of the GPS velocity data before and after alignment using separate checks
-        if (PV_AidingMode != AID_ABSOLUTE) {
-            // Pre-alignment checks
-            gpsQualGood = calcGpsGoodToAlign();
-        } else {
-            // Post-alignment checks
-            calcGpsGoodForFlight();
-        }
-
-        // read latitutde and longitude from GPS and convert to local NE position relative to the stored origin
-        // If we don't have an origin, then set it to the current GPS coordinates
-        const struct Location &gpsloc = _ahrs->get_gps().location();
-        if (validOrigin) {
-            gpsDataNew.pos = location_diff(EKF_origin, gpsloc);
-        } else if (gpsQualGood) {
-            // Set the NE origin to the current GPS position
-            setOrigin();
-            // Now we know the location we have an estimate for the magnetic field declination and adjust the earth field accordingly
-            alignMagStateDeclination();
-            // Set the height of the NED origin to ‘height of baro height datum relative to GPS height datum'
-            EKF_origin.alt = gpsloc.alt - baroDataNew.hgt;
-            // We are by definition at the origin at the instant of alignment so set NE position to zero
-            gpsDataNew.pos.zero();
-            // If GPS useage isn't explicitly prohibited, we switch to absolute position mode
-            if (isAiding && frontend._fusionModeGPS != 3) {
-                PV_AidingMode = AID_ABSOLUTE;
-                // Initialise EKF position and velocity states
-                ResetPosition();
-                ResetVelocity();
             }
+
+            // check if we have enough GPS satellites and increase the gps noise scaler if we don't
+            if (_ahrs->get_gps().num_sats() >= 6 && (PV_AidingMode == AID_ABSOLUTE)) {
+                gpsNoiseScaler = 1.0f;
+            } else if (_ahrs->get_gps().num_sats() == 5 && (PV_AidingMode == AID_ABSOLUTE)) {
+                gpsNoiseScaler = 1.4f;
+            } else { // <= 4 satellites or in constant position mode
+                gpsNoiseScaler = 2.0f;
+            }
+
+            // Check if GPS can output vertical velocity and set GPS fusion mode accordingly
+            if (_ahrs->get_gps().have_vertical_velocity() && frontend._fusionModeGPS == 0) {
+                useGpsVertVel = true;
+            } else {
+                useGpsVertVel = false;
+            }
+
+            // Monitor quality of the GPS velocity data before and after alignment using separate checks
+            if (PV_AidingMode != AID_ABSOLUTE) {
+                // Pre-alignment checks
+                gpsQualGood = calcGpsGoodToAlign();
+            } else {
+                // Post-alignment checks
+                calcGpsGoodForFlight();
+            }
+
+            // read latitutde and longitude from GPS and convert to local NE position relative to the stored origin
+            // If we don't have an origin, then set it to the current GPS coordinates
+            const struct Location &gpsloc = _ahrs->get_gps().location();
+            if (validOrigin) {
+                gpsDataNew.pos = location_diff(EKF_origin, gpsloc);
+            } else if (gpsQualGood) {
+                // Set the NE origin to the current GPS position
+                setOrigin();
+                // Now we know the location we have an estimate for the magnetic field declination and adjust the earth field accordingly
+                alignMagStateDeclination();
+                // Set the height of the NED origin to ‘height of baro height datum relative to GPS height datum'
+                EKF_origin.alt = gpsloc.alt - baroDataNew.hgt;
+                // We are by definition at the origin at the instant of alignment so set NE position to zero
+                gpsDataNew.pos.zero();
+                // If GPS useage isn't explicitly prohibited, we switch to absolute position mode
+                if (isAiding && frontend._fusionModeGPS != 3) {
+                    PV_AidingMode = AID_ABSOLUTE;
+                    // Initialise EKF position and velocity states
+                    ResetPosition();
+                    ResetVelocity();
+                }
+            }
+
+            // calculate a position offset which is applied to NE position and velocity wherever it is used throughout code to allow GPS position jumps to be accommodated gradually
+            decayGpsOffset();
+
+            // save measurement to buffer to be fused later
+            StoreGPS();
+
+            // declare GPS available for use
+            gpsNotAvailable = false;
+        } else {
+            // report GPS fix status
+            gpsCheckStatus.bad_fix = true;
         }
-
-        // calculate a position offset which is applied to NE position and velocity wherever it is used throughout code to allow GPS position jumps to be accommodated gradually
-        decayGpsOffset();
-
-        // save measurement to buffer to be fused later
-        StoreGPS();
-
-        // declare GPS available for use
-        gpsNotAvailable = false;
     }
 
     // We need to handle the case where GPS is lost for a period of time that is too long to dead-reckon

libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp

@@ -429,6 +429,7 @@ void  NavEKF2_core::getFilterGpsStatus(nav_gps_status &faults) const
     faults.flags.bad_hAcc   = gpsCheckStatus.bad_hAcc; // reported horizontal position accuracy is insufficient
     faults.flags.bad_sats   = gpsCheckStatus.bad_sats; // reported number of satellites is insufficient
     faults.flags.bad_yaw    = gpsCheckStatus.bad_yaw; // EKF heading accuracy is too large for GPS use
+    faults.flags.bad_fix    = gpsCheckStatus.bad_fix; // The GPS cannot provide the 3D fix required
 }
 
 // send an EKF_STATUS message to GCS

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -824,6 +824,7 @@ private:
         bool bad_hAcc:1;
         bool bad_sats:1;
         bool bad_yaw:1;
+        bool bad_fix:1;
     } gpsCheckStatus;
 
     // states held by magnetomter fusion across time steps