ekf2: add new estimator_gps_status.msg

- includes the estimator status check fail bits broken out as descriptive booleans
 - absorbs ekf_gps_drift.msg

src/modules/ekf2/EKF/ekf.h

@@ -246,9 +246,6 @@ public:
 
 	bool accel_bias_inhibited() const { return _accel_bias_inhibit[0] || _accel_bias_inhibit[1] || _accel_bias_inhibit[2]; }
 
-	// get GPS check status
-	void get_gps_check_status(uint16_t *val) const { *val = _gps_check_fail_status.value; }
-
 	const auto &state_reset_status() const { return _state_reset_status; }
 
 	// return the amount the local vertical position changed in the last reset and the number of reset events
@@ -311,6 +308,11 @@ public:
 	// set minimum continuous period without GPS fail required to mark a healthy GPS status
 	void set_min_required_gps_health_time(uint32_t time_us) { _min_gps_health_time_us = time_us; }
 
+	const gps_check_fail_status_u &gps_check_fail_status() const { return _gps_check_fail_status; }
+	const decltype(gps_check_fail_status_u::flags) &gps_check_fail_status_flags() const { return _gps_check_fail_status.flags; }
+
+	bool gps_checks_passed() const { return _gps_checks_passed; };
+
 	// get solution data from the EKF-GSF emergency yaw estimator
 	// returns false when data is not available
 	bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -750,27 +750,6 @@ void Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
 	}
 }
 
-/*
-	First argument returns GPS drift  metrics in the following array locations
-	0 : Horizontal position drift rate (m/s)
-	1 : Vertical position drift rate (m/s)
-	2 : Filtered horizontal velocity (m/s)
-	Second argument returns true when IMU movement is blocking the drift calculation
-	Function returns true if the metrics have been updated and not returned previously by this function
-*/
-bool Ekf::get_gps_drift_metrics(float drift[3], bool *blocked)
-{
-	memcpy(drift, _gps_drift_metrics, 3 * sizeof(float));
-	*blocked = !_control_status.flags.vehicle_at_rest;
-
-	if (_gps_drift_updated) {
-		_gps_drift_updated = false;
-		return true;
-	}
-
-	return false;
-}
-
 // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
 void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const
 {
@@ -1823,4 +1802,8 @@ void Ekf::resetGpsDriftCheckFilters()
 {
 	_gps_velNE_filt.setZero();
 	_gps_pos_deriv_filt.setZero();
+
+	_gps_horizontal_position_drift_rate_m_s = NAN;
+	_gps_vertical_position_drift_rate_m_s = NAN;
+	_gps_filtered_horizontal_velocity_m_s = NAN;
 }

src/modules/ekf2/EKF/estimator_interface.h

@@ -245,17 +245,20 @@ public:
 	// Getter for the average EKF update period in s
 	float get_dt_ekf_avg() const { return _dt_ekf_avg; }
 
-	// Getter for the imu sample on the delayed time horizon
+	// Getters for samples on the delayed time horizon
 	const imuSample &get_imu_sample_delayed() const { return _imu_sample_delayed; }
-
-	// Getter for the baro sample on the delayed time horizon
 	const baroSample &get_baro_sample_delayed() const { return _baro_sample_delayed; }
+	const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; }
 
 	const bool &global_origin_valid() const { return _NED_origin_initialised; }
 	const MapProjection &global_origin() const { return _pos_ref; }
 
 	void print_status();
 
+	float gps_horizontal_position_drift_rate_m_s() const { return _gps_horizontal_position_drift_rate_m_s; }
+	float gps_vertical_position_drift_rate_m_s() const { return _gps_vertical_position_drift_rate_m_s; }
+	float gps_filtered_horizontal_velocity_m_s() const { return _gps_filtered_horizontal_velocity_m_s; }
+
 protected:
 
 	EstimatorInterface() = default;
@@ -346,14 +349,12 @@ protected:
 	bool _deadreckon_time_exceeded{true};	// true if the horizontal nav solution has been deadreckoning for too long and is invalid
 	bool _is_wind_dead_reckoning{false};	// true if we are navigationg reliant on wind relative measurements
 
-	float _gps_drift_metrics[3] {};	// Array containing GPS drift metrics
-					// [0] Horizontal position drift rate (m/s)
-					// [1] Vertical position drift rate (m/s)
-					// [2] Filtered horizontal velocity (m/s)
+	float _gps_horizontal_position_drift_rate_m_s{0}; // Horizontal position drift rate (m/s)
+	float _gps_vertical_position_drift_rate_m_s{0};   // Vertical position drift rate (m/s)
+	float _gps_filtered_horizontal_velocity_m_s{0};   // Filtered horizontal velocity (m/s)
 
 	uint64_t _time_last_on_ground_us{0};	///< last time we were on the ground (uSec)
 	uint64_t _time_last_in_air{0};		///< last time we were in air (uSec)
-	bool _gps_drift_updated{false};	// true when _gps_drift_metrics has been updated and is ready for retrieval
 
 	// data buffer instances
 	RingBuffer<imuSample> _imu_buffer{12};           // buffer length 12 with default parameters

src/modules/ekf2/EKF/gps_checks.cpp

@@ -201,22 +201,20 @@ bool Ekf::gps_is_good(const gps_message &gps)
 		_gps_pos_deriv_filt = pos_derived * filter_coef + _gps_pos_deriv_filt * (1.0f - filter_coef);
 
 		// Calculate the horizontal drift speed and fail if too high
-		_gps_drift_metrics[0] = Vector2f(_gps_pos_deriv_filt.xy()).norm();
-		_gps_check_fail_status.flags.hdrift = (_gps_drift_metrics[0] > _params.req_hdrift);
+		_gps_horizontal_position_drift_rate_m_s = Vector2f(_gps_pos_deriv_filt.xy()).norm();
+		_gps_check_fail_status.flags.hdrift = (_gps_horizontal_position_drift_rate_m_s > _params.req_hdrift);
 
 		// Fail if the vertical drift speed is too high
-		_gps_drift_metrics[1] = fabsf(_gps_pos_deriv_filt(2));
-		_gps_check_fail_status.flags.vdrift = (_gps_drift_metrics[1] > _params.req_vdrift);
+		_gps_vertical_position_drift_rate_m_s = fabsf(_gps_pos_deriv_filt(2));
+		_gps_check_fail_status.flags.vdrift = (_gps_vertical_position_drift_rate_m_s > _params.req_vdrift);
 
 		// Check the magnitude of the filtered horizontal GPS velocity
 		const Vector2f gps_velNE = matrix::constrain(Vector2f(gps.vel_ned.xy()),
 					   -10.0f * _params.req_hdrift,
 					   10.0f * _params.req_hdrift);
 		_gps_velNE_filt = gps_velNE * filter_coef + _gps_velNE_filt * (1.0f - filter_coef);
-		_gps_drift_metrics[2] = _gps_velNE_filt.norm();
-		_gps_check_fail_status.flags.hspeed = (_gps_drift_metrics[2] > _params.req_hdrift);
-
-		_gps_drift_updated = true;
+		_gps_filtered_horizontal_velocity_m_s = _gps_velNE_filt.norm();
+		_gps_check_fail_status.flags.hspeed = (_gps_filtered_horizontal_velocity_m_s > _params.req_hdrift);
 
 	} else if (_control_status.flags.in_air) {
 		// These checks are always declared as passed when flying
@@ -224,14 +222,11 @@ bool Ekf::gps_is_good(const gps_message &gps)
 		_gps_check_fail_status.flags.hdrift = false;
 		_gps_check_fail_status.flags.vdrift = false;
 		_gps_check_fail_status.flags.hspeed = false;
-		_gps_drift_updated = false;
 
 		resetGpsDriftCheckFilters();
 
 	} else {
 		// This is the case where the vehicle is on ground and IMU movement is blocking the drift calculation
-		_gps_drift_updated = true;
-
 		resetGpsDriftCheckFilters();
 	}