ekf2: Add parameters and support for GPS quality checks

src/modules/ekf2/ekf2_main.cpp

@@ -74,6 +74,7 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/ekf2_innovations.h>
+#include <uORB/topics/vehicle_control_mode.h>
 
 #include <ecl/EKF/ekf.h>
 
@@ -128,6 +129,7 @@ private:
 	int		_gps_sub = -1;
 	int		_airspeed_sub = -1;
 	int		_params_sub = -1;
+        int		_control_mode_sub = -1;
 
 	orb_advert_t _att_pub;
 	orb_advert_t _lpos_pub;
@@ -146,8 +148,6 @@ private:
 	control::BlockParamFloat *_baro_delay_ms;
 	control::BlockParamFloat *_gps_delay_ms;
 	control::BlockParamFloat *_airspeed_delay_ms;
-	control::BlockParamFloat 	*_requiredEph;
-	control::BlockParamFloat 	*_requiredEpv;
 
 	control::BlockParamFloat *_gyro_noise;
 	control::BlockParamFloat *_accel_noise;
@@ -167,6 +167,15 @@ private:
 	control::BlockParamFloat *_mag_declination_deg;	// magnetic declination in degrees
 	control::BlockParamFloat *_heading_innov_gate;	// innovation gate for heading innovation test
 
+        control::BlockParamInt *_gps_check_mask;        // bitmasked integer used to activate the different GPS quality checks
+        control::BlockParamFloat *_requiredEph;         // maximum acceptable horiz position error (m)
+        control::BlockParamFloat *_requiredEpv;         // maximum acceptable vert position error (m)
+        control::BlockParamFloat *_requiredSacc;        // maximum acceptable speed error (m/s)
+        control::BlockParamInt *_requiredNsats;         // minimum acceptable satellite count
+        control::BlockParamFloat *_requiredGDoP;        // maximum acceptable geometric dilution of precision
+        control::BlockParamFloat *_requiredHdrift;      // maximum acceptable horizontal drift speed (m/s)
+        control::BlockParamFloat *_requiredVdrift;      // maximum acceptable vertical drift speed (m/s)
+
 	EstimatorBase *_ekf;
 
 	int update_subscriptions();
@@ -191,8 +200,6 @@ Ekf2::Ekf2():
 	_baro_delay_ms = new control::BlockParamFloat(this, "EKF2_BARO_DELAY", false, &params->baro_delay_ms);
 	_gps_delay_ms = new control::BlockParamFloat(this, "EKF2_GPS_DELAY", false, &params->gps_delay_ms);
 	_airspeed_delay_ms = new control::BlockParamFloat(this, "EKF2_ASP_DELAY", false, &params->airspeed_delay_ms);
-	_requiredEph = new control::BlockParamFloat(this, "EKF2_REQ_EPH", false, &params->requiredEph);
-	_requiredEpv = new control::BlockParamFloat(this, "EKF2_REQ_EPV", false, &params->requiredEpv);
 
 	_gyro_noise = new control::BlockParamFloat(this, "EKF2_G_NOISE", false, &params->gyro_noise);
 	_accel_noise = new control::BlockParamFloat(this, "EKF2_ACC_NOISE", false, &params->accel_noise);
@@ -210,6 +217,16 @@ Ekf2::Ekf2():
 	_mag_heading_noise = new control::BlockParamFloat(this, "EKF2_HEAD_NOISE", false, &params->mag_heading_noise);
 	_mag_declination_deg = new control::BlockParamFloat(this, "EKF2_MAG_DECL", false, &params->mag_declination_deg);
 	_heading_innov_gate = new control::BlockParamFloat(this, "EKF2_H_INOV_GATE", false, &params->heading_innov_gate);
+
+        _gps_check_mask = new control::BlockParamInt(this, "EKF2_GPS_CHECK", false, &params->gps_check_mask);
+        _requiredEph = new control::BlockParamFloat(this, "EKF2_REQ_EPH", false, &params->req_hacc);
+        _requiredEpv = new control::BlockParamFloat(this, "EKF2_REQ_EPV", false, &params->req_vacc);
+        _requiredSacc = new control::BlockParamFloat(this, "EKF2_REQ_SACC", false, &params->req_sacc);
+        _requiredNsats = new control::BlockParamInt(this, "EKF2_REQ_NSATS", false, &params->req_nsats);
+        _requiredGDoP = new control::BlockParamFloat(this, "EKF2_REQ_GDOP", false, &params->req_gdop);
+        _requiredHdrift = new control::BlockParamFloat(this, "EKF2_REQ_HDRIFT", false, &params->req_hdrift);
+        _requiredVdrift = new control::BlockParamFloat(this, "EKF2_REQ_VDRIFT", false, &params->req_vdrift);
+
 }
 
 Ekf2::~Ekf2()
@@ -237,6 +254,7 @@ void Ekf2::task_main()
 	_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
 	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
+        _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 
 	px4_pollfd_struct_t fds[2] = {};
 	fds[0].fd = _sensors_sub;
@@ -277,9 +295,11 @@ void Ekf2::task_main()
 
 		bool gps_updated = false;
 		bool airspeed_updated = false;
+                bool control_mode_updated = false;
 
 		sensor_combined_s sensors = {};
 		airspeed_s airspeed = {};
+                vehicle_control_mode_s vehicle_control_mode = {};
 
 		orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors);
 
@@ -296,7 +316,15 @@ void Ekf2::task_main()
 			orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed);
 		}
 
-		hrt_abstime now = hrt_absolute_time();
+                // Use the control model data to determine if the motors are armed as a surrogate for an on-ground vs in-air status
+                // TODO implement a global vehicle on-ground/in-air check
+                orb_check(_control_mode_sub, &control_mode_updated);
+                if (control_mode_updated) {
+                        orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &vehicle_control_mode);
+                        _ekf->_vehicle_armed = vehicle_control_mode.flag_armed;
+                }
+
+                hrt_abstime now = hrt_absolute_time();
 		// push imu data into estimator
 		_ekf->setIMUData(now, sensors.gyro_integral_dt[0], sensors.accelerometer_integral_dt[0],
 				 &sensors.gyro_integral_rad[0], &sensors.accelerometer_integral_m_s[0]);
@@ -317,12 +345,16 @@ void Ekf2::task_main()
 			gps_msg.fix_type = gps.fix_type;
 			gps_msg.eph = gps.eph;
 			gps_msg.epv = gps.epv;
+                        gps_msg.sacc = gps.s_variance_m_s;
 			gps_msg.time_usec_vel = gps.timestamp_velocity;
 			gps_msg.vel_m_s = gps.vel_m_s;
 			gps_msg.vel_ned[0] = gps.vel_n_m_s;
 			gps_msg.vel_ned[1] = gps.vel_e_m_s;
 			gps_msg.vel_ned[2] = gps.vel_d_m_s;
 			gps_msg.vel_ned_valid = gps.vel_ned_valid;
+                        gps_msg.nsats = gps.satellites_used;
+                        //TODO add gdop to gps topic
+                        gps_msg.gdop = 0.0f;
 
 			_ekf->setGpsData(gps.timestamp_position, &gps_msg);
 		}
@@ -376,8 +408,8 @@ void Ekf2::task_main()
 		lpos.xy_global =
 			_ekf->position_is_valid();// true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon)
 		lpos.z_global = true;// true if z is valid and has valid global reference (ref_alt)
-		lpos.ref_lat = _ekf->_posRef.lat_rad * 180.0 / M_PI; // Reference point latitude in degrees
-		lpos.ref_lon = _ekf->_posRef.lon_rad * 180.0 / M_PI; // Reference point longitude in degrees
+                lpos.ref_lat = _ekf->_pos_ref.lat_rad * 180.0 / M_PI; // Reference point latitude in degrees
+                lpos.ref_lon = _ekf->_pos_ref.lon_rad * 180.0 / M_PI; // Reference point longitude in degrees
 		lpos.ref_alt =
 			_ekf->_gps_alt_ref; // Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level
 
@@ -451,7 +483,7 @@ void Ekf2::task_main()
 			global_pos.time_utc_usec = gps.time_utc_usec; // GPS UTC timestamp in microseconds
 
 			double est_lat, est_lon;
-			map_projection_reproject(&_ekf->_posRef, lpos.x, lpos.y, &est_lat, &est_lon);
+                        map_projection_reproject(&_ekf->_pos_ref, lpos.x, lpos.y, &est_lat, &est_lon);
 			global_pos.lat = est_lat; // Latitude in degrees
 			global_pos.lon = est_lon; // Longitude in degrees
 
@@ -487,6 +519,7 @@ void Ekf2::task_main()
 		status.timestamp = hrt_absolute_time();
 		_ekf->get_state_delayed(status.states);
 		_ekf->get_covariances(status.covariances);
+                status.gps_check_fail_flags = _ekf->_gps_check_fail_status.value;
 
 		if (_estimator_status_pub == nullptr) {
 			_estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &status);
@@ -614,4 +647,4 @@ int ekf2_main(int argc, char *argv[])
 
 	PX4_WARN("unrecognized command");
 	return 1;
-}
+}

src/modules/ekf2/ekf2_params.c

@@ -81,6 +81,25 @@ PARAM_DEFINE_FLOAT(EKF2_GPS_DELAY, 200);
  */
 PARAM_DEFINE_FLOAT(EKF2_ASP_DELAY, 200);
 
+/**
+ * Integer bitmask controlling GPS checks. Set bits to 1 to enable checks. Checks enabled by the following bit positions
+ * 0 : Minimum required sat count set by EKF2_REQ_NSATS
+ * 1 : Minimum required GDoP set by EKF2_REQ_GDOP
+ * 2 : Maximum allowed horizontal position error set by EKF2_REQ_EPH
+ * 3 : Maximum allowed vertical position error set by EKF2_REQ_EPV
+ * 4 : Maximum allowed speed error set by EKF2_REQ_SACC
+ * 5 : Maximum allowed horizontal position rate set by EKF2_REQ_HDRIFT. This check can only be used if the vehciel is stationary during alignment.
+ * 6 : Maximum allowed vertical position rate set by EKF2_REQ_VDRIFT. This check can only be used if the vehciel is stationary during alignment.
+ * 7 : Maximum allowed horizontal speed set by EKF2_REQ_HDRIFT. This check can only be used if the vehciel is stationary during alignment.
+ * 8 : Maximum allowed vertical velocity discrepancy set by EKF2_REQ_VDRIFT
+ *
+ * @group EKF2
+ * @min 0
+ * @max 511
+ * @unit
+ */
+PARAM_DEFINE_INT32(EKF2_GPS_CHECK, 21);
+
 /**
  * Required EPH to use GPS.
  *
@@ -89,7 +108,7 @@ PARAM_DEFINE_FLOAT(EKF2_ASP_DELAY, 200);
  * @max 100
  * @unit m
  */
-PARAM_DEFINE_FLOAT(EKF2_REQ_EPH, 10);
+PARAM_DEFINE_FLOAT(EKF2_REQ_EPH, 5.0f);
 
 /**
  * Required EPV to use GPS.
@@ -99,7 +118,57 @@ PARAM_DEFINE_FLOAT(EKF2_REQ_EPH, 10);
  * @max 100
  * @unit m
  */
-PARAM_DEFINE_FLOAT(EKF2_REQ_EPV, 20);
+PARAM_DEFINE_FLOAT(EKF2_REQ_EPV, 8.0f);
+
+/**
+ * Required speed accuracy to use GPS.
+ *
+ * @group EKF2
+ * @min 0.5
+ * @max 5.0
+ * @unit m/s
+ */
+PARAM_DEFINE_FLOAT(EKF2_REQ_SACC, 1.0f);
+
+/**
+ * Required satellite count to use GPS.
+ *
+ * @group EKF2
+ * @min 4
+ * @max 12
+ * @unit
+ */
+PARAM_DEFINE_INT32(EKF2_REQ_NSATS, 6);
+
+/**
+ * Required GDoP to use GPS.
+ *
+ * @group EKF2
+ * @min 1.5
+ * @max 5.0
+ * @unit
+ */
+PARAM_DEFINE_FLOAT(EKF2_REQ_GDOP, 2.5f);
+
+/**
+ * Maximum horizontal drift speed to use GPS.
+ *
+ * @group EKF2
+ * @min 0.1
+ * @max 1.0
+ * @unit m/s
+ */
+PARAM_DEFINE_FLOAT(EKF2_REQ_HDRIFT, 0.3f);
+
+/**
+ * Maximum vertical drift speed to use GPS.
+ *
+ * @group EKF2
+ * @min 0.1
+ * @max 1.5
+ * @unit m/s
+ */
+PARAM_DEFINE_FLOAT(EKF2_REQ_VDRIFT, 0.5f);
 
 /**
  * Gyro noise.