EKF: Add interface to enable logging of GPS drift metrics (#490)

* EKF: Add interface to enable logging of GPS drift metrics

* EKF: Fix bug affecting rate of GPS drift publication

Also fix variable name.

EKF/ekf.h

@@ -167,6 +167,16 @@ public:
 	*/
 	void get_imu_vibe_metrics(float vibe[3]);
 
+	/*
+	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 get_gps_drift_metrics(float drift[3], bool *blocked);
+
 	// return true if the global position estimate is valid
 	bool global_position_is_valid();
 

EKF/ekf_helper.cpp

@@ -987,6 +987,25 @@ void Ekf::get_imu_vibe_metrics(float vibe[3])
 	memcpy(vibe, _vibe_metrics, 3 * sizeof(float));
 }
 
+/*
+	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 = !_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)
 {

EKF/estimator_interface.h

@@ -140,6 +140,16 @@ public:
 	*/
 	virtual void get_imu_vibe_metrics(float vibe[3]) = 0;
 
+	/*
+	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
+	*/
+	virtual bool get_gps_drift_metrics(float drift[3], bool *blocked) = 0;
+
 	// get the ekf WGS-84 origin position and height and the system time it was last set
 	// return true if the origin is valid
 	virtual bool get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) = 0;
@@ -480,8 +490,13 @@ protected:
 					// [0] Level of coning vibration in the IMU delta angles (rad^2)
 					// [1] high frequency vibraton level in the IMU delta angle data (rad)
 					// [2] high frequency vibration level in the IMU delta velocity data (m/s)
+	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)
 	bool _vehicle_at_rest{false};	// true when the vehicle is at rest
 	uint64_t _time_last_move_detect_us{0};	// timestamp of last movement detection event in microseconds
+	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;

EKF/gps_checks.cpp

@@ -163,8 +163,8 @@ bool Ekf::gps_is_good(struct gps_message *gps)
 		_gpsDriftVelE = velE * filter_coef + _gpsDriftVelE * (1.0f - filter_coef);
 
 		// Calculate the horizontal drift speed and fail if too high
-		float drift_speed = sqrtf(_gpsDriftVelN * _gpsDriftVelN + _gpsDriftVelE * _gpsDriftVelE);
-		_gps_check_fail_status.flags.hdrift = (drift_speed > _params.req_hdrift);
+		_gps_drift_metrics[0] = sqrtf(_gpsDriftVelN * _gpsDriftVelN + _gpsDriftVelE * _gpsDriftVelE);
+		_gps_check_fail_status.flags.hdrift = (_gps_drift_metrics[0] > _params.req_hdrift);
 
 		// Calculate the vertical drift velocity and limit to 10x the threshold
 		float vz_drift_limit = 10.0f * _params.req_vdrift;
@@ -175,7 +175,8 @@ bool Ekf::gps_is_good(struct gps_message *gps)
 		_gps_drift_velD = velD * filter_coef + _gps_drift_velD * (1.0f - filter_coef);
 
 		// Fail if the vertical drift speed is too high
-		_gps_check_fail_status.flags.vdrift = (fabsf(_gps_drift_velD) > _params.req_vdrift);
+		_gps_drift_metrics[1] = fabsf(_gps_drift_velD);
+		_gps_check_fail_status.flags.vdrift = (_gps_drift_metrics[1] > _params.req_vdrift);
 
 		// Check the magnitude of the filtered horizontal GPS velocity
 		float vxy_drift_limit = 10.0f * _params.req_hdrift;
@@ -183,8 +184,10 @@ bool Ekf::gps_is_good(struct gps_message *gps)
 		float gps_velE = fminf(fmaxf(gps->vel_ned[1], -vxy_drift_limit), vxy_drift_limit);
 		_gps_velN_filt = gps_velN * filter_coef + _gps_velN_filt * (1.0f - filter_coef);
 		_gps_velE_filt  = gps_velE * filter_coef + _gps_velE_filt  * (1.0f - filter_coef);
-		float horiz_speed = sqrtf(_gps_velN_filt * _gps_velN_filt + _gps_velE_filt * _gps_velE_filt);
-		_gps_check_fail_status.flags.hspeed = (horiz_speed > _params.req_hdrift);
+		_gps_drift_metrics[2] = sqrtf(_gps_velN_filt * _gps_velN_filt + _gps_velE_filt * _gps_velE_filt);
+		_gps_check_fail_status.flags.hspeed = (_gps_drift_metrics[2] > _params.req_hdrift);
+
+		_gps_drift_updated = true;
 
 	} else if (_control_status.flags.in_air) {
 		// These checks are always declared as passed when flying
@@ -192,6 +195,11 @@ bool Ekf::gps_is_good(struct 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;
+
+	} else {
+		// This is the case where the vehicle is on ground and IMU movement is blocking the drift calculation
+		_gps_drift_updated = true;
 
 	}