EKF: additional GPS check logic

Improve naming of NED origin initialisation status
Add check for GPS solution type
Use GPS checks when regaining GPs in-flight

EKF/control.cpp

@@ -52,7 +52,7 @@ void Ekf::controlFusionModes()
     // GPS fusion mode selection logic
     // To start use GPS we need angular alignment completed, the local NED origin set and fresh GPS data
     if (!_control_status.flags.gps) {
-        if (_control_status.flags.angle_align && (_time_last_imu - _time_last_gps) < 5e5 && _gps_initialised) {
+        if (_control_status.flags.angle_align && (_time_last_imu - _time_last_gps) < 5e5 && _NED_origin_initialised && (_time_last_imu - _last_gps_fail_us > 5e6)) {
             _control_status.flags.gps = true;
             resetPosition();
             resetVelocity();

EKF/ekf.cpp

@@ -224,7 +224,7 @@ bool Ekf::initialiseFilter(void)
 void Ekf::predictState()
 {
 	if (!_earth_rate_initialised) {
-		if (_gps_initialised) {
+        if (_NED_origin_initialised) {
             calcEarthRateNED(_earth_rate_NED, _pos_ref.lat_rad);
 			_earth_rate_initialised = true;
 		}

EKF/ekf.h

@@ -159,15 +159,16 @@ private:
     // publish the status of various GPS quality checks
     union gps_check_fail_status_u {
         struct {
-            uint16_t nsats  : 1; // 0 - true if number of satellites used is insufficient
+            uint16_t fix    : 1; // 0 - true if the fix type is insufficient (no 3D solution)
-            uint16_t gdop   : 1; // 1 - true if geometric dilution of precision is insufficient
+            uint16_t nsats  : 1; // 1 - true if number of satellites used is insufficient
-            uint16_t hacc   : 1; // 2 - true if reported horizontal accuracy is insufficient
+            uint16_t gdop   : 1; // 2 - true if geometric dilution of precision is insufficient
-            uint16_t vacc   : 1; // 3 - true if reported vertical accuracy is insufficient
+            uint16_t hacc   : 1; // 3 - true if reported horizontal accuracy is insufficient
-            uint16_t sacc   : 1; // 4 - true if reported speed accuracy is insufficient
+            uint16_t vacc   : 1; // 4 - true if reported vertical accuracy is insufficient
-            uint16_t hdrift : 1; // 5 - true if horizontal drift is excessive (can only be used when stationary on ground)
+            uint16_t sacc   : 1; // 5 - true if reported speed accuracy is insufficient
-            uint16_t vdrift : 1; // 6 - true if vertical drift is excessive (can only be used when stationary on ground)
+            uint16_t hdrift : 1; // 6 - true if horizontal drift is excessive (can only be used when stationary on ground)
-            uint16_t hspeed : 1; // 7 - true if horizontal speed is excessive (can only be used when stationary on ground)
+            uint16_t vdrift : 1; // 7 - true if vertical drift is excessive (can only be used when stationary on ground)
-            uint16_t vspeed : 1; // 8 - true if vertical speed error is excessive
+            uint16_t hspeed : 1; // 8 - true if horizontal speed is excessive (can only be used when stationary on ground)
+            uint16_t vspeed : 1; // 9 - true if vertical speed error is excessive
         } flags;
         uint16_t value;
     }_gps_check_fail_status;
@@ -216,8 +217,6 @@ private:
 
 	void calcEarthRateNED(Vector3f &omega, double lat_rad) const;
 
-    void initialiseGPS(struct gps_message *gps);
-
     // return true id the GPS quality is good enough to set an origin and start aiding
     bool gps_is_good(struct gps_message *gps);
 

EKF/estimator_base.cpp

@@ -247,7 +247,7 @@ void EstimatorBase::initialiseVariables(uint64_t time_usec)
 	_imu_updated = false;
 	_start_predict_enabled = false;
 	_initialised = false;
-	_gps_initialised = false;
+    _NED_origin_initialised = false;
 	_gps_speed_valid = false;
 
 	_mag_healthy = false;
@@ -266,8 +266,8 @@ void EstimatorBase::initialiseVariables(uint64_t time_usec)
 bool EstimatorBase::position_is_valid()
 {
 	// return true if the position estimate is valid
-	// TODO implement proper check based on published GPS accuracy, innovaton consistency checks and timeout status
+	// TOTO implement proper check based on published GPS accuracy, innovaton consistency checks and timeout status
-    return _gps_initialised &&  (_time_last_imu - _time_last_gps) < 5e6;
+    return _NED_origin_initialised &&  (_time_last_imu - _time_last_gps) < 5e6;
 }
 
 void EstimatorBase::printStoredIMU()

EKF/estimator_base.h

@@ -314,14 +314,14 @@ protected:
 	outputSample _output_new;
 	imuSample _imu_sample_new;
 
-
+    uint64_t _imu_ticks;
-	uint64_t _imu_ticks;
 
 	bool _imu_updated = false;
 	bool _start_predict_enabled = false;
 	bool _initialised = false;
+    bool _vehicle_armed = false;     // vehicle arm status used to turn off functionality used on the ground
 
-    bool _gps_initialised = false;
+    bool _NED_origin_initialised = false;
     bool _gps_speed_valid = false;
     struct map_projection_reference_s _pos_ref = {};    // Contains WGS-84 position latitude and longitude (radians)
 
@@ -356,8 +356,7 @@ protected:
 		bool bad_sideslip: 1;
 	} _fault_status;
 
+    // initialise variables to default startup values and set time stamps to specified value
 	void initialiseVariables(uint64_t timestamp);
-    bool _vehicle_armed = false;     // vehicle arm status used to turn off funtionality used on the ground
-
 
 };

EKF/gps_checks.cpp

@@ -54,29 +54,30 @@
 
 bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
 {
-    if(_gps_initialised) {
+    // run gps checks if we have not yet set the NED origin or have not started using GPS
+    if(!_NED_origin_initialised || !_control_status.flags.gps) {
+        // if we have good GPS data and the NED origin is not set, set to the last GPS fix
+        if (gps_is_good(gps) && !_NED_origin_initialised) {
+            printf("gps is good -  setting EKF origin\n");
+            // Initialise projection
+            double lat = gps->lat / 1.0e7;
+            double lon = gps->lon / 1.0e7;
+            map_projection_init(&_pos_ref, lat, lon);
+            _gps_alt_ref = gps->alt / 1e3f;
+            _NED_origin_initialised = true;
+            _last_gps_origin_time_us = _time_last_imu;
+        }
+    }
+
+    // start collecting GPS if there is a 3D fix and the NED origin has been set
+    if (_NED_origin_initialised && gps->fix_type >= 3) {
         return true;
     } else {
-        initialiseGPS(gps);
+        return false;
     }
     return false;
 }
 
-void Ekf::initialiseGPS(struct gps_message *gps)
-{
-    //Check if the GPS fix is good enough for us to use
-    if (gps_is_good(gps)) {
-        printf("gps is good\n");
-        // Initialise projection
-        double lat = gps->lat / 1.0e7;
-        double lon = gps->lon / 1.0e7;
-        map_projection_init(&_pos_ref, lat, lon);
-        _gps_alt_ref = gps->alt / 1e3f;
-        _gps_initialised = true;
-        _last_gps_origin_time_us = _time_last_imu;
-    }
-}
-
 /*
  * Return true if the GPS solution quality is adequate to set an origin for the EKF
  * and start GPS aiding.
@@ -86,6 +87,9 @@ void Ekf::initialiseGPS(struct gps_message *gps)
 */
 bool Ekf::gps_is_good(struct gps_message *gps)
 {
+    // Check the fix type
+    _gps_check_fail_status.flags.fix = (gps->fix_type < 3);
+
     // Check the number of satellites
     _gps_check_fail_status.flags.nsats = (gps->nsats < _params.req_nsats);
 
@@ -185,6 +189,7 @@ bool Ekf::gps_is_good(struct gps_message *gps)
 
     // if any user selected checks have failed, record the fail time
     if (
+     _gps_check_fail_status.flags.fix ||
     (_gps_check_fail_status.flags.nsats   && (_params.gps_check_mask & MASK_GPS_NSATS)) ||
     (_gps_check_fail_status.flags.gdop    && (_params.gps_check_mask & MASK_GPS_GDOP)) ||
     (_gps_check_fail_status.flags.hacc    && (_params.gps_check_mask & MASK_GPS_HACC)) ||