WIP: move GPS status to sensors

.ci/Jenkinsfile-hardware

@@ -943,6 +943,7 @@ void printTopics() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_mag"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_preflight_mag"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_selection"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensors_status_gps"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensors_status_imu"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener system_power"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener task_stack_info"'

msg/CMakeLists.txt

@@ -140,6 +140,7 @@ set(msg_files
 	sensor_preflight_mag.msg
 	sensor_selection.msg
 	sensors_status_imu.msg
+	sensors_status_gps.msg
 	system_power.msg
 	takeoff_status.msg
 	task_stack_info.msg

msg/sensors_status_gps.msg

@@ -0,0 +1,28 @@
+#
+# Sensor check metrics. This will be zero for a sensor that's primary or unpopulated.
+#
+uint64 timestamp # time since system start (microseconds)
+
+uint32[2] device_ids
+float32[2] inconsistency
+bool[2] healthy
+uint8[2] priority
+
+
+# drift metrics
+float32[2] drift_rate_hpos # Horizontal position rate magnitude checked using EKF2_REQ_HDRIFT (m/s)
+float32[2] drift_rate_vpos # Vertical position rate magnitude checked using EKF2_REQ_VDRIFT (m/s)
+float32[2] drift_hspd      # Filtered horizontal velocity magnitude checked using EKF2_REQ_HDRIFT (m/s)
+
+float32[2] error_norm      # normalised gps error
+
+bool[2] fail_fix    # true if the fix type is insufficient (no 3D solution)
+bool[2] fail_nsats  # true if number of satellites used is insufficient
+bool[2] fail_pdop   # true if position dilution of precision is insufficient
+bool[2] fail_hacc   # true if reported horizontal accuracy is insufficient
+bool[2] fail_vacc   # true if reported vertical accuracy is insufficient
+bool[2] fail_sacc   # true if reported speed accuracy is insufficient
+bool[2] fail_hdrift # true if horizontal drift is excessive (can only be used when stationary on ground)
+bool[2] fail_vdrift # true if vertical drift is excessive (can only be used when stationary on ground)
+bool[2] fail_hspeed # true if horizontal speed is excessive (can only be used when stationary on ground)
+bool[2] fail_vspeed # true if vertical speed error is excessive

src/modules/logger/logged_topics.cpp

@@ -86,6 +86,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("sensor_correction");
 	add_topic("sensor_gyro_fft", 50);
 	add_topic("sensor_selection");
+	add_topic("sensors_status_gps", 500);
 	add_topic("sensors_status_imu", 200);
 	add_topic("system_power", 500);
 	add_topic("takeoff_status", 1000);

src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp

@@ -98,6 +98,14 @@ void VehicleGPSPosition::ParametersUpdate(bool force)
 	}
 }
 
+void VehicleGPSPosition::resetGpsDriftCheckFilters()
+{
+	for (int i = 0; i < GPS_MAX_RECEIVERS; i++) {
+		_gps_velNE_filt[i].setZero();
+		_gps_pos_deriv_filt[i].setZero();
+	}
+}
+
 void VehicleGPSPosition::Run()
 {
 	perf_begin(_cycle_perf);
@@ -113,21 +121,134 @@ void VehicleGPSPosition::Run()
 	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
 		gps_updated = _sensor_gps_sub[i].updated();
 
-		sensor_gps_s gps_data;
+		sensor_gps_s gps;
 
 		if (gps_updated) {
 			any_gps_updated = true;
 
-			_sensor_gps_sub[i].copy(&gps_data);
-			_gps_blending.setGpsData(gps_data, i);
+			_sensor_gps_sub[i].copy(&gps);
+			_gps_blending.setGpsData(gps, i);
 
 			if (!_sensor_gps_sub[i].registered()) {
 				_sensor_gps_sub[i].registerCallback();
 			}
+
+			// update drift metrics
+			if (gps.timestamp > _timestamp_prev[i]) {
+
+				// check if GPS quality is degraded
+				_sensors_status_gps.error_norm[i] = fmaxf((gps.eph / _param_sens_gps_rq_eph.get()),
+								    (gps.epv / _param_sens_gps_rq_epv.get()));
+				_sensors_status_gps.error_norm[i] = fmaxf(_sensors_status_gps.error_norm[i],
+								    (gps.s_variance_m_s / _param_sens_gps_rq_sacc.get()));
+
+				_sensors_status_gps.device_ids[i] = gps.device_id;
+
+				// Check the fix type
+				_sensors_status_gps.fail_fix[i] = (gps.fix_type < 3);
+
+				// Check the number of satellites
+				_sensors_status_gps.fail_nsats[i] = (gps.satellites_used < _param_sens_gps_rq_nsat.get());
+
+				// Check the position dilution of precision
+				float pdop = sqrtf(gps.hdop * gps.hdop + gps.vdop * gps.vdop);
+				_sensors_status_gps.fail_pdop[i] = (pdop > _param_sens_gps_rq_pdop.get());
+
+				// Check the reported horizontal and vertical position accuracy
+				_sensors_status_gps.fail_hacc[i] = (gps.eph > _param_sens_gps_rq_eph.get());
+				_sensors_status_gps.fail_vacc[i] = (gps.epv > _param_sens_gps_rq_epv.get());
+
+				// Check the reported speed accuracy
+				_sensors_status_gps.fail_sacc[i] = (gps.s_variance_m_s > _param_sens_gps_rq_sacc.get());
+
+
+				if (_landed && _at_rest) {
+
+					// The following checks are only valid when the vehicle is at rest
+					const double lat = gps.lat * 1.0e-7;
+					const double lon = gps.lon * 1.0e-7;
+
+					// Calculate position movement since last measurement
+					float delta_pos_n = 0.0f;
+					float delta_pos_e = 0.0f;
+
+					// calculate position movement since last GPS fix
+					if (_gps_pos_prev[i].timestamp > 0) {
+						map_projection_project(&_gps_pos_prev[i], lat, lon, &delta_pos_n, &delta_pos_e);
+
+					} else {
+						// no previous position has been set
+						map_projection_init_timestamped(&_gps_pos_prev[i], lat, lon, gps.timestamp);
+						_gps_alt_prev[i] = 1e-3f * (float)gps.alt;
+					}
+
+					if (_timestamp_prev[i] != 0) {
+						static constexpr float filt_time_const = 10.0f;
+						const float dt = math::constrain((gps.timestamp - _timestamp_prev[i]) * 1e-6f, 0.001f, filt_time_const);
+						const float filter_coef = dt / filt_time_const;
+
+						// Calculate the horizontal and vertical drift velocity components and limit to 10x the threshold
+						const matrix::Vector3f vel_limit{_param_sens_gps_rq_hdrf.get(), _param_sens_gps_rq_hdrf.get(), _param_sens_gps_rq_vdrf.get()};
+						matrix::Vector3f pos_derived{delta_pos_n, delta_pos_e, (_gps_alt_prev[i] - 1e-3f * (float)gps.alt)};
+						pos_derived = matrix::constrain(pos_derived / dt, -10.f * vel_limit, 10.f * vel_limit);
+
+						// Apply a low pass filter
+						_gps_pos_deriv_filt[i] = pos_derived * filter_coef + _gps_pos_deriv_filt[i] * (1.f - filter_coef);
+
+						// Calculate the horizontal drift speed and fail if too high
+						_sensors_status_gps.drift_rate_hpos[i] = Vector2f(_gps_pos_deriv_filt[i].xy()).norm();
+						_sensors_status_gps.fail_hdrift[i] = (_gps_drift_metrics[i][0] > _param_sens_gps_rq_hdrf.get());
+
+						// Fail if the vertical drift speed is too high
+						_sensors_status_gps.drift_rate_vpos[i] = fabsf(_gps_pos_deriv_filt[i](2));
+						_sensors_status_gps.fail_vdrift[i] = (_gps_drift_metrics[i][1] > _param_sens_gps_rq_vdrf.get());
+
+						// Check the magnitude of the filtered horizontal GPS velocity
+
+						// Calculate time lapsed since last update, limit to prevent numerical errors and calculate a lowpass filter coefficient
+						const Vector2f gps_velNE = matrix::constrain(Vector2f{gps.vel_n_m_s, gps.vel_e_m_s},
+									   -10.f * _param_sens_gps_rq_hdrf.get(), 10.f * _param_sens_gps_rq_hdrf.get());
+						_gps_velNE_filt[i] = gps_velNE * filter_coef + _gps_velNE_filt[i] * (1.f - filter_coef);
+						_sensors_status_gps.drift_hspd[i] = _gps_velNE_filt[i].norm();
+						_sensors_status_gps.fail_hspeed[i] = (_gps_drift_metrics[i][2] > _param_sens_gps_rq_hdrf.get());
+					}
+
+				} else if (!_landed) {
+					// These checks are always declared as passed when flying
+					// If on ground and moving, the last result before movement commenced is kept
+					// _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();
+				}
+
+				_sensors_status_gps.healthy[i] = (_sensors_status_gps.fail_fix[i] && (_param_sens_gps_checks.get() & MASK_GPS_NSATS))
+								 && (_sensors_status_gps.fail_fix[i] && (_param_sens_gps_checks.get() & MASK_GPS_PDOP))
+								 && (_sensors_status_gps.fail_pdop[i] && (_param_sens_gps_checks.get() & MASK_GPS_HACC))
+								 && (_sensors_status_gps.fail_vacc[i] && (_param_sens_gps_checks.get() & MASK_GPS_VACC))
+								 && (_sensors_status_gps.fail_sacc[i] && (_param_sens_gps_checks.get() & MASK_GPS_SACC))
+								 && (_sensors_status_gps.fail_hdrift[i] && (_param_sens_gps_checks.get() & MASK_GPS_HDRIFT))
+								 && (_sensors_status_gps.fail_vdrift[i] && (_param_sens_gps_checks.get() & MASK_GPS_VDRIFT))
+								 && (_sensors_status_gps.fail_hspeed[i] && (_param_sens_gps_checks.get() & MASK_GPS_HSPD))
+								 && (_sensors_status_gps.fail_vspeed[i] && (_param_sens_gps_checks.get() & MASK_GPS_VSPD));
+			}
+
+			_timestamp_prev[i] = gps.timestamp;
 		}
 	}
 
 	if (any_gps_updated) {
+		_sensors_status_gps.timestamp = hrt_absolute_time();
+		_sensors_status_gps_pub.publish(_sensors_status_gps);
+
 		_gps_blending.update(hrt_absolute_time());
 
 		if (_gps_blending.isNewOutputDataAvailable()) {
@@ -177,6 +298,11 @@ void VehicleGPSPosition::Publish(const sensor_gps_s &gps, uint8_t selected)
 void VehicleGPSPosition::PrintStatus()
 {
 	//PX4_INFO("selected GPS: %d", _gps_select_index);
+	for (int i = 0; i < GPS_MAX_RECEIVERS; i++) {
+		if (_timestamp_prev[i] != 0) {
+			PX4_INFO_RAW("GPS %d healthy: %d", i, _sensors_status_gps.healthy[i]);
+		}
+	}
 }
 
 }; // namespace sensors

src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp

@@ -45,6 +45,7 @@
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/sensors_status_gps.h>
 #include <uORB/topics/vehicle_gps_position.h>
 
 #include "gps_blending.hpp"
@@ -71,6 +72,8 @@ private:
 	void ParametersUpdate(bool force = false);
 	void Publish(const sensor_gps_s &gps, uint8_t selected);
 
+	void resetGpsDriftCheckFilters();
+
 	// defines used to specify the mask position for use of different accuracy metrics in the GPS blending algorithm
 	static constexpr uint8_t BLEND_MASK_USE_SPD_ACC  = 1;
 	static constexpr uint8_t BLEND_MASK_USE_HPOS_ACC = 2;
@@ -81,6 +84,7 @@ private:
 	static_assert(GPS_MAX_RECEIVERS == GpsBlending::GPS_MAX_RECEIVERS_BLEND,
 		      "GPS_MAX_RECEIVERS must match to GPS_MAX_RECEIVERS_BLEND");
 
+	uORB::Publication<sensors_status_gps_s> _sensors_status_gps_pub{ORB_ID(sensors_status_gps)};
 	uORB::Publication<vehicle_gps_position_s> _vehicle_gps_position_pub{ORB_ID(vehicle_gps_position)};
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
@@ -90,6 +94,34 @@ private:
 		{this, ORB_ID(sensor_gps), 1},
 	};
 
+	bool _landed{true};
+	bool _at_rest{true};
+
+	sensors_status_gps_s _sensors_status_gps{};
+
+	// variables used for the GPS quality checks
+	hrt_abstime _timestamp_prev[GPS_MAX_RECEIVERS] {};
+	map_projection_reference_s _gps_pos_prev[GPS_MAX_RECEIVERS] {};  // Contains WGS-84 position latitude and longitude (radians) of the previous GPS message
+	float _gps_alt_prev[GPS_MAX_RECEIVERS] {};	// height from the previous GPS message (m)
+	matrix::Vector3f _gps_pos_deriv_filt[GPS_MAX_RECEIVERS] {};	///< GPS NED position derivative (m/sec)
+	matrix::Vector2f _gps_velNE_filt[GPS_MAX_RECEIVERS] {};	///< filtered GPS North and East velocity (m/sec)
+
+	float _gps_drift_metrics[GPS_MAX_RECEIVERS][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)
+
+	// GPS pre-flight check bit locations
+	static constexpr uint32_t MASK_GPS_NSATS = (1 << 0);
+	static constexpr uint32_t MASK_GPS_PDOP = (1 << 1);
+	static constexpr uint32_t MASK_GPS_HACC = (1 << 2);
+	static constexpr uint32_t MASK_GPS_VACC = (1 << 3);
+	static constexpr uint32_t MASK_GPS_SACC = (1 << 4);
+	static constexpr uint32_t MASK_GPS_HDRIFT = (1 << 5);
+	static constexpr uint32_t MASK_GPS_VDRIFT = (1 << 6);
+	static constexpr uint32_t MASK_GPS_HSPD = (1 << 7);
+	static constexpr uint32_t MASK_GPS_VSPD = (1 << 8);
+
 	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
 
 	GpsBlending _gps_blending;
@@ -97,7 +129,17 @@ private:
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::SENS_GPS_MASK>) _param_sens_gps_mask,
 		(ParamFloat<px4::params::SENS_GPS_TAU>) _param_sens_gps_tau,
-		(ParamInt<px4::params::SENS_GPS_PRIME>) _param_sens_gps_prime
+		(ParamInt<px4::params::SENS_GPS_PRIME>) _param_sens_gps_prime,
+
+		(ParamFloat<px4::params::SENS_GPS_RQ_EPH>) _param_sens_gps_rq_eph,
+		(ParamFloat<px4::params::SENS_GPS_RQ_EPV>) _param_sens_gps_rq_epv,
+		(ParamFloat<px4::params::SENS_GPS_RQ_SACC>) _param_sens_gps_rq_sacc,
+		(ParamInt<px4::params::SENS_GPS_RQ_NSAT>) _param_sens_gps_rq_nsat,
+		(ParamFloat<px4::params::SENS_GPS_RQ_PDOP>) _param_sens_gps_rq_pdop,
+		(ParamFloat<px4::params::SENS_GPS_RQ_HDRF>) _param_sens_gps_rq_hdrf,
+		(ParamFloat<px4::params::SENS_GPS_RQ_VDRF>) _param_sens_gps_rq_vdrf,
+
+		(ParamInt<px4::params::SENS_GPS_CHECKS>) _param_sens_gps_checks
 	)
 };
 }; // namespace sensors

src/modules/sensors/vehicle_gps_position/params.c

@@ -80,3 +80,110 @@ PARAM_DEFINE_FLOAT(SENS_GPS_TAU, 10.0f);
  * @max 1
  */
 PARAM_DEFINE_INT32(SENS_GPS_PRIME, 0);
+
+
+
+
+
+/**
+ * Required EPH to use GPS.
+ *
+ * @group Sensors
+ * @min 2
+ * @max 100
+ * @unit m
+ * @decimal 1
+ */
+PARAM_DEFINE_FLOAT(SENS_GPS_RQ_EPH, 3.0f);
+
+/**
+ * Required EPV to use GPS.
+ *
+ * @group Sensors
+ * @min 2
+ * @max 100
+ * @unit m
+ * @decimal 1
+ */
+PARAM_DEFINE_FLOAT(SENS_GPS_RQ_EPV, 5.0f);
+
+/**
+ * Required speed accuracy to use GPS.
+ *
+ * @group Sensors
+ * @min 0.5
+ * @max 5.0
+ * @unit m/s
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(SENS_GPS_RQ_SACC, 0.5f);
+
+/**
+ * Required satellite count to use GPS.
+ *
+ * @group Sensors
+ * @min 4
+ * @max 12
+ */
+PARAM_DEFINE_INT32(SENS_GPS_RQ_NSAT, 6);
+
+/**
+ * Maximum PDOP to use GPS.
+ *
+ * @group Sensors
+ * @min 1.5
+ * @max 5.0
+ * @decimal 1
+ */
+PARAM_DEFINE_FLOAT(SENS_GPS_RQ_PDOP, 2.5f);
+
+/**
+ * Maximum horizontal drift speed to use GPS.
+ *
+ * @group Sensors
+ * @min 0.1
+ * @max 1.0
+ * @unit m/s
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(SENS_GPS_RQ_HDRF, 0.1f);
+
+/**
+ * Maximum vertical drift speed to use GPS.
+ *
+ * @group Sensors
+ * @min 0.1
+ * @max 1.5
+ * @decimal 2
+ * @unit m/s
+ */
+PARAM_DEFINE_FLOAT(SENS_GPS_RQ_VDRF, 0.2f);
+
+/**
+ * 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 SENS_GPS_RQ_NSATS
+ * 1 : Maximum allowed PDOP set by SENS_GPS_RQ_PDOP
+ * 2 : Maximum allowed horizontal position error set by SENS_GPS_RQ_EPH
+ * 3 : Maximum allowed vertical position error set by SENS_GPS_RQ_EPV
+ * 4 : Maximum allowed speed error set by SENS_GPS_RQ_SACC
+ * 5 : Maximum allowed horizontal position rate set by SENS_GPS_RQ_HDRIFT. This check will only run when the vehicle is on ground and stationary. Detection of the stationary condition is controlled by the EKF2_MOVE_TEST parameter.
+ * 6 : Maximum allowed vertical position rate set by SENS_GPS_RQ_VDRIFT. This check will only run when the vehicle is on ground and stationary. Detection of the stationary condition is controlled by the EKF2_MOVE_TEST parameter.
+ * 7 : Maximum allowed horizontal speed set by SENS_GPS_RQ_HDRIFT. This check will only run when the vehicle is on ground and stationary. Detection of the stationary condition is controlled by the EKF2_MOVE_TEST parameter.
+ * 8 : Maximum allowed vertical velocity discrepancy set by SENS_GPS_RQ_VDRIFT
+ *
+ * @group Sensors
+ * @min 0
+ * @max 511
+ * @bit 0 Min sat count (SENS_GPS_RQ_NSAT)
+ * @bit 1 Max PDOP (SENS_GPS_RQ_PDOP)
+ * @bit 2 Max horizontal position error (SENS_GPS_RQ_EPH)
+ * @bit 3 Max vertical position error (SENS_GPS_RQ_EPV)
+ * @bit 4 Max speed error (SENS_GPS_RQ_SACC)
+ * @bit 5 Max horizontal position rate (SENS_GPS_RQ_HDRF)
+ * @bit 6 Max vertical position rate (SENS_GPS_RQ_VDRF)
+ * @bit 7 Max horizontal speed (SENS_GPS_RQ_HDRIFT)
+ * @bit 8 Max vertical velocity discrepancy (SENS_GPS_RQ_VDRF)
+ */
+PARAM_DEFINE_INT32(SENS_GPS_CHECKS, 245);