ekf2: add new estimator_gps_status.msg

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

.ci/Jenkinsfile-hardware

@@ -861,7 +861,7 @@ void printTopics() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener cpuload" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener distance_sensor" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener ekf2_timestamps" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener ekf_gps_drift" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_gps_status" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener esc_status" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_attitude" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_baro_bias" || true'

msg/CMakeLists.txt

@@ -64,7 +64,7 @@ set(msg_files
 	differential_pressure.msg
 	distance_sensor.msg
 	ekf2_timestamps.msg
-	ekf_gps_drift.msg
+	estimator_gps_status.msg
 	esc_report.msg
 	esc_status.msg
 	estimator_baro_bias.msg

msg/ekf_gps_drift.msg

@@ -1,6 +0,0 @@
-uint64 timestamp		# time since system start (microseconds)
-float32 hpos_drift_rate		# Horizontal position rate magnitude checked using EKF2_REQ_HDRIFT (m/s)
-float32 vpos_drift_rate		# Vertical position rate magnitude checked using EKF2_REQ_VDRIFT (m/s)
-float32 hspd			# Filtered horizontal velocity magnitude checked using EKF2_REQ_HDRIFT (m/s)
-
-bool blocked			# true when drift calculation is blocked due to IMU movement check

msg/estimator_gps_status.msg

@@ -0,0 +1,19 @@
+uint64 timestamp                          # time since system start (microseconds)
+uint64 timestamp_sample                   # the timestamp of the raw data (microseconds)
+
+bool checks_passed
+
+bool check_fail_gps_fix          # 0 : insufficient fix type (no 3D solution)
+bool check_fail_min_sat_count    # 1 : minimum required sat count fail
+bool check_fail_max_pdop         # 2 : maximum allowed PDOP fail
+bool check_fail_max_horz_err     # 3 : maximum allowed horizontal position error fail
+bool check_fail_max_vert_err     # 4 : maximum allowed vertical position error fail
+bool check_fail_max_spd_err      # 5 : maximum allowed speed error fail
+bool check_fail_max_horz_drift   # 6 : maximum allowed horizontal position drift fail - requires stationary vehicle
+bool check_fail_max_vert_drift   # 7 : maximum allowed vertical position drift fail - requires stationary vehicle
+bool check_fail_max_horz_spd_err # 8 : maximum allowed horizontal speed fail - requires stationary vehicle
+bool check_fail_max_vert_spd_err # 9 : maximum allowed vertical velocity discrepancy fail
+
+float32 position_drift_rate_horizontal_m_s # Horizontal position rate magnitude (m/s)
+float32 position_drift_rate_vertical_m_s   # Vertical position rate magnitude (m/s)
+float32 filtered_horizontal_speed_m_s      # Filtered horizontal velocity magnitude (m/s)

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();
 	}
 

src/modules/ekf2/EKF2.cpp

@@ -191,17 +191,17 @@ bool EKF2::multi_init(int imu, int mag)
 	_wind_pub.advertise();
 
 	_ekf2_timestamps_pub.advertise();
-	_ekf_gps_drift_pub.advertise();
 	_estimator_baro_bias_pub.advertise();
 	_estimator_event_flags_pub.advertise();
+	_estimator_gps_status_pub.advertise();
 	_estimator_innovation_test_ratios_pub.advertise();
 	_estimator_innovation_variances_pub.advertise();
 	_estimator_innovations_pub.advertise();
 	_estimator_optical_flow_vel_pub.advertise();
 	_estimator_sensor_bias_pub.advertise();
 	_estimator_states_pub.advertise();
-	_estimator_status_pub.advertise();
 	_estimator_status_flags_pub.advertise();
+	_estimator_status_pub.advertise();
 	_estimator_visual_odometry_aligned_pub.advertised();
 	_yaw_est_pub.advertise();
 
@@ -573,8 +573,8 @@ void EKF2::Run()
 
 			// publish status/logging messages
 			PublishBaroBias(now);
-			PublishEkfDriftMetrics(now);
 			PublishEventFlags(now);
+			PublishGpsStatus(now);
 			PublishInnovations(now);
 			PublishInnovationTestRatios(now);
 			PublishInnovationVariances(now);
@@ -647,24 +647,6 @@ void EKF2::PublishBaroBias(const hrt_abstime &timestamp)
 	}
 }
 
-void EKF2::PublishEkfDriftMetrics(const hrt_abstime &timestamp)
-{
-	// publish GPS drift data only when updated to minimise overhead
-	float gps_drift[3];
-	bool blocked;
-
-	if (_ekf.get_gps_drift_metrics(gps_drift, &blocked)) {
-		ekf_gps_drift_s drift_data;
-		drift_data.hpos_drift_rate = gps_drift[0];
-		drift_data.vpos_drift_rate = gps_drift[1];
-		drift_data.hspd = gps_drift[2];
-		drift_data.blocked = blocked;
-		drift_data.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
-
-		_ekf_gps_drift_pub.publish(drift_data);
-	}
-}
-
 void EKF2::PublishEventFlags(const hrt_abstime &timestamp)
 {
 	// information events
@@ -768,6 +750,41 @@ void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp)
 	}
 }
 
+void EKF2::PublishGpsStatus(const hrt_abstime &timestamp)
+{
+	const hrt_abstime timestamp_sample = _ekf.get_gps_sample_delayed().time_us;
+
+	if (timestamp_sample == _last_gps_status_published) {
+		return;
+	}
+
+	estimator_gps_status_s estimator_gps_status{};
+	estimator_gps_status.timestamp_sample = timestamp_sample;
+
+	estimator_gps_status.position_drift_rate_horizontal_m_s = _ekf.gps_horizontal_position_drift_rate_m_s();
+	estimator_gps_status.position_drift_rate_vertical_m_s   = _ekf.gps_vertical_position_drift_rate_m_s();
+	estimator_gps_status.filtered_horizontal_speed_m_s      = _ekf.gps_filtered_horizontal_velocity_m_s();
+
+	estimator_gps_status.checks_passed = _ekf.gps_checks_passed();
+
+	estimator_gps_status.check_fail_gps_fix          = _ekf.gps_check_fail_status_flags().fix;
+	estimator_gps_status.check_fail_min_sat_count    = _ekf.gps_check_fail_status_flags().nsats;
+	estimator_gps_status.check_fail_max_pdop         = _ekf.gps_check_fail_status_flags().pdop;
+	estimator_gps_status.check_fail_max_horz_err     = _ekf.gps_check_fail_status_flags().hacc;
+	estimator_gps_status.check_fail_max_vert_err     = _ekf.gps_check_fail_status_flags().vacc;
+	estimator_gps_status.check_fail_max_spd_err      = _ekf.gps_check_fail_status_flags().sacc;
+	estimator_gps_status.check_fail_max_horz_drift   = _ekf.gps_check_fail_status_flags().hdrift;
+	estimator_gps_status.check_fail_max_vert_drift   = _ekf.gps_check_fail_status_flags().vdrift;
+	estimator_gps_status.check_fail_max_horz_spd_err = _ekf.gps_check_fail_status_flags().hspeed;
+	estimator_gps_status.check_fail_max_vert_spd_err = _ekf.gps_check_fail_status_flags().vspeed;
+
+	estimator_gps_status.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+	_estimator_gps_status_pub.publish(estimator_gps_status);
+
+
+	_last_gps_status_published = timestamp_sample;
+}
+
 void EKF2::PublishInnovations(const hrt_abstime &timestamp)
 {
 	// publish estimator innovation data
@@ -1152,11 +1169,9 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
 
 	_ekf.getOutputTrackingError().copyTo(status.output_tracking_error);
 
-	_ekf.get_gps_check_status(&status.gps_check_fail_flags);
-
 	// only report enabled GPS check failures (the param indexes are shifted by 1 bit, because they don't include
 	// the GPS Fix bit, which is always checked)
-	status.gps_check_fail_flags &= ((uint16_t)_params->gps_check_mask << 1) | 1;
+	status.gps_check_fail_flags = _ekf.gps_check_fail_status().value & (((uint16_t)_params->gps_check_mask << 1) | 1);
 
 	status.control_mode_flags = _ekf.control_status().value;
 	status.filter_fault_flags = _ekf.fault_status().value;

src/modules/ekf2/EKF2.hpp

@@ -67,9 +67,9 @@
 #include <uORB/topics/airspeed_validated.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/ekf2_timestamps.h>
-#include <uORB/topics/ekf_gps_drift.h>
 #include <uORB/topics/estimator_baro_bias.h>
 #include <uORB/topics/estimator_event_flags.h>
+#include <uORB/topics/estimator_gps_status.h>
 #include <uORB/topics/estimator_innovations.h>
 #include <uORB/topics/estimator_optical_flow_vel.h>
 #include <uORB/topics/estimator_sensor_bias.h>
@@ -137,9 +137,9 @@ private:
 
 	void PublishAttitude(const hrt_abstime &timestamp);
 	void PublishBaroBias(const hrt_abstime &timestamp);
-	void PublishEkfDriftMetrics(const hrt_abstime &timestamp);
 	void PublishEventFlags(const hrt_abstime &timestamp);
 	void PublishGlobalPosition(const hrt_abstime &timestamp);
+	void PublishGpsStatus(const hrt_abstime &timestamp);
 	void PublishInnovations(const hrt_abstime &timestamp);
 	void PublishInnovationTestRatios(const hrt_abstime &timestamp);
 	void PublishInnovationVariances(const hrt_abstime &timestamp);
@@ -238,6 +238,7 @@ private:
 	Vector3f _last_mag_calibration_published{};
 
 	hrt_abstime _last_sensor_bias_published{0};
+	hrt_abstime _last_gps_status_published{0};
 
 	float _last_baro_bias_published{};
 
@@ -285,17 +286,17 @@ private:
 	uint32_t _filter_information_event_changes{0};
 
 	uORB::PublicationMulti<ekf2_timestamps_s>            _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
-	uORB::PublicationMulti<ekf_gps_drift_s>              _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)};
 	uORB::PublicationMulti<estimator_baro_bias_s>        _estimator_baro_bias_pub{ORB_ID(estimator_baro_bias)};
+	uORB::PublicationMulti<estimator_event_flags_s>      _estimator_event_flags_pub{ORB_ID(estimator_event_flags)};
+	uORB::PublicationMulti<estimator_gps_status_s>       _estimator_gps_status_pub{ORB_ID(estimator_gps_status)};
 	uORB::PublicationMulti<estimator_innovations_s>      _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
 	uORB::PublicationMulti<estimator_innovations_s>      _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};
 	uORB::PublicationMulti<estimator_innovations_s>      _estimator_innovations_pub{ORB_ID(estimator_innovations)};
 	uORB::PublicationMulti<estimator_optical_flow_vel_s> _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
 	uORB::PublicationMulti<estimator_sensor_bias_s>      _estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)};
 	uORB::PublicationMulti<estimator_states_s>           _estimator_states_pub{ORB_ID(estimator_states)};
-	uORB::PublicationMulti<estimator_status_s>           _estimator_status_pub{ORB_ID(estimator_status)};
 	uORB::PublicationMulti<estimator_status_flags_s>     _estimator_status_flags_pub{ORB_ID(estimator_status_flags)};
-	uORB::PublicationMulti<estimator_event_flags_s>      _estimator_event_flags_pub{ORB_ID(estimator_event_flags)};
+	uORB::PublicationMulti<estimator_status_s>           _estimator_status_pub{ORB_ID(estimator_status)};
 	uORB::PublicationMulti<vehicle_odometry_s>           _estimator_visual_odometry_aligned_pub{ORB_ID(estimator_visual_odometry_aligned)};
 	uORB::PublicationMulti<yaw_estimator_status_s>       _yaw_est_pub{ORB_ID(yaw_estimator_status)};
 

src/modules/logger/logged_topics.cpp

@@ -138,9 +138,9 @@ void LoggedTopics::add_default_topics()
 #endif
 
 	// always add the first instance
-	add_topic("ekf_gps_drift", 1000);
 	add_topic("estimator_baro_bias", 500);
 	add_topic("estimator_event_flags", 0);
+	add_topic("estimator_gps_status", 1000);
 	add_topic("estimator_innovation_test_ratios", 500);
 	add_topic("estimator_innovation_variances", 500);
 	add_topic("estimator_innovations", 500);
@@ -152,9 +152,9 @@ void LoggedTopics::add_default_topics()
 	add_topic("estimator_visual_odometry_aligned", 200);
 	add_topic("yaw_estimator_status", 1000);
 
-	add_optional_topic_multi("ekf_gps_drift", 1000, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_baro_bias", 500, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_event_flags", 0, MAX_ESTIMATOR_INSTANCES);
+	add_optional_topic_multi("estimator_gps_status", 1000, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_innovation_test_ratios", 500, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_innovation_variances", 500, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_innovations", 500, MAX_ESTIMATOR_INSTANCES);
@@ -194,9 +194,9 @@ void LoggedTopics::add_default_topics()
 	add_topic("vehicle_local_position_groundtruth", 100);
 
 	// EKF replay
-	add_topic("ekf_gps_drift");
 	add_topic("estimator_baro_bias");
 	add_topic("estimator_event_flags");
+	add_topic("estimator_gps_status");
 	add_topic("estimator_innovation_test_ratios");
 	add_topic("estimator_innovation_variances");
 	add_topic("estimator_innovations");