feat(ekf2): gnss altitude correction-drift detector

msg/CMakeLists.txt

@@ -114,6 +114,7 @@ set(msg_files
 	GpioIn.msg
 	GpioOut.msg
 	GpioRequest.msg
+	GpsAltitudeDriftCorrection.msg
 	GpsDump.msg
 	GpsInjectData.msg
 	Gripper.msg

msg/GpsAltitudeDriftCorrection.msg

@@ -0,0 +1,4 @@
+uint64 timestamp		# [us] time since system start
+
+float32 altitude_offset		# [m] detected altitude offset to apply
+

msg/versioned/VehicleLocalPosition.msg

@@ -45,6 +45,7 @@ float32 unaided_heading                 # Same as heading but generated by integ
 float32 delta_heading			# Heading delta caused by latest heading reset [rad]
 uint8 heading_reset_counter		# Index of latest heading reset
 bool heading_good_for_control
+bool altitude_good_for_local_control
 
 float32 tilt_var
 

src/modules/commander/Commander.cpp

@@ -2210,7 +2210,6 @@ void Commander::landDetectorUpdate()
 				events::send(events::ID("commander_takeoff_detected"), events::Log::Info, "Takeoff detected");
 				_vehicle_status.takeoff_time = hrt_absolute_time();
 				_have_taken_off_since_arming = true;
-				_home_position.setTakeoffTime(_vehicle_status.takeoff_time);
 			}
 
 			// automatically set or update home position

src/modules/commander/HomePosition.cpp

@@ -330,22 +330,6 @@ void HomePosition::update(bool set_automatically, bool check_if_changed)
 	_global_position_sub.update();
 	_attitude_sub.update();
 
-	if (_vehicle_air_data_sub.updated()) {
-		vehicle_air_data_s baro_data;
-		_vehicle_air_data_sub.copy(&baro_data);
-		const float baro_alt = baro_data.baro_alt_meter;
-
-		if (_last_baro_timestamp != 0) {
-			const float dt = baro_data.timestamp - _last_baro_timestamp;
-			_lpf_baro.update(baro_alt, dt);
-
-		} else {
-			_lpf_baro.reset(baro_alt);
-		}
-
-		_last_baro_timestamp = baro_data.timestamp;
-	}
-
 	if (_vehicle_gps_position_sub.updated()) {
 		sensor_gps_s vehicle_gps_position;
 		_vehicle_gps_position_sub.copy(&vehicle_gps_position);
@@ -365,49 +349,21 @@ void HomePosition::update(bool set_automatically, bool check_if_changed)
 
 		_gps_position_for_home_valid = time_valid && fix_valid && eph_valid && epv_valid && evh_valid
 					       && isGpsHorizontalFusionEnabled();
+	}
 
-		if (_param_com_home_en.get() && _gps_position_for_home_valid && _last_gps_timestamp != 0 && _last_baro_timestamp != 0
-		    && _takeoff_time != 0 && now < _takeoff_time + kHomePositionCorrectionTimeWindow) {
+	// Apply home altitude correction from GPS altitude drift detection
+	if (_param_com_home_en.get() && _gps_alt_drift_sub.updated()) {
+		gps_altitude_drift_correction_s correction;
+		_gps_alt_drift_sub.copy(&correction);
 
-			const float gps_alt = static_cast<float>(_gps_alt);
+		home_position_s home = _home_position_pub.get();
 
-			if (!PX4_ISFINITE(_gps_vel_integral)) {
-				_gps_vel_integral = gps_alt; // initialize the gps-vel-integral at same altitude as gps-pos
-				_baro_gps_static_offset = gps_alt - _lpf_baro.getState();
-			}
-
-			_gps_vel_integral += 1e-6f * (vehicle_gps_position.timestamp - _last_gps_timestamp) * (-vehicle_gps_position.vel_d_m_s);
-
-			// correct baro_alt with offset from GPS alt from when the drift integral was initialized
-			const float baro_alt_corrected = _lpf_baro.getState() + _baro_gps_static_offset;
-			const float gps_alt_with_home_offset = gps_alt + _home_altitude_offset_applied;
-
-			// Apply home altitude correction only if the GPS velocity-integrated altitude and baro altitude
-			// are more consistent with each other than either is with the GPS altitude (with home offset).
-			if (fabsf(baro_alt_corrected - _gps_vel_integral) < fabsf(baro_alt_corrected - gps_alt_with_home_offset) &&
-			    fabsf(baro_alt_corrected - _gps_vel_integral) < fabsf(_gps_vel_integral - gps_alt_with_home_offset)) {
-
-				const float offset_new = baro_alt_corrected - gps_alt - _home_altitude_offset_applied;
-
-				if (fabsf(offset_new) > kAltitudeDifferenceThreshold) {
-
-					home_position_s home = _home_position_pub.get();
-					home.alt -= offset_new;
-					home.z += offset_new;
-					home.timestamp = now;
-					home.manual_home = false;
-					home.update_count = _home_position_pub.get().update_count + 1U;
-
-					_home_position_pub.update(home);
-					_home_altitude_offset_applied = baro_alt_corrected - gps_alt; // offset present when home position was last corrected
-				}
-			}
-
-		} else {
-			_gps_vel_integral = NAN;
-		}
-
-		_last_gps_timestamp = vehicle_gps_position.timestamp;
+		home.alt += correction.altitude_offset;
+		home.z -= correction.altitude_offset;
+		home.timestamp = hrt_absolute_time();
+		home.manual_home = false;
+		home.update_count = _home_position_pub.get().update_count + 1U;
+		_home_position_pub.update(home);
 	}
 
 	const vehicle_local_position_s &lpos = _local_position_sub.get();

src/modules/commander/HomePosition.hpp

@@ -40,9 +40,8 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/gps_altitude_drift_correction.h>
 #include <uORB/topics/failsafe_flags.h>
-#include <uORB/topics/vehicle_air_data.h>
-#include <lib/mathlib/math/filter/AlphaFilter.hpp>
 #include <px4_platform_common/module_params.h>
 
 using namespace time_literals;
@@ -53,9 +52,6 @@ static constexpr float kHomePositionGPSRequiredEPV = 10.f;
 static constexpr float kHomePositionGPSRequiredEVH = 1.f;
 static constexpr float kMinHomePositionChangeEPH = 1.f;
 static constexpr float kMinHomePositionChangeEPV = 1.5f;
-static constexpr float kLpfBaroTimeConst = 5.f;
-static constexpr float kAltitudeDifferenceThreshold = 1.f; // altitude difference after which home position gets updated
-static constexpr uint64_t kHomePositionCorrectionTimeWindow = 120_s;
 
 class HomePosition: public ModuleParams
 {
@@ -66,7 +62,6 @@ public:
 	bool setHomePosition(bool force = false);
 	void setInAirHomePosition();
 	bool setManually(double lat, double lon, float alt, float roll, float pitch, float yaw);
-	void setTakeoffTime(uint64_t takeoff_time) { _takeoff_time = takeoff_time; }
 
 	void update(bool set_automatically, bool check_if_changed);
 
@@ -85,20 +80,11 @@ private:
 	static void fillGlobalHomePos(home_position_s &home, double lat, double lon, double alt);
 
 	uORB::Subscription					_vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
+	uORB::Subscription					_gps_alt_drift_sub{ORB_ID(gps_altitude_drift_correction)};
 
 	uORB::SubscriptionData<vehicle_global_position_s>	_global_position_sub{ORB_ID(vehicle_global_position)};
 	uORB::SubscriptionData<vehicle_local_position_s>	_local_position_sub{ORB_ID(vehicle_local_position)};
-	uORB::SubscriptionData<vehicle_attitude_s>	_attitude_sub{ORB_ID(vehicle_attitude)};
-	uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
-
-	uint64_t _last_gps_timestamp{0};
-	uint64_t _last_baro_timestamp{0};
-	AlphaFilter<float> _lpf_baro{kLpfBaroTimeConst};
-	float _gps_vel_integral{NAN};
-	float _home_altitude_offset_applied{0.f};
-	float _baro_gps_static_offset{0.f};
-	uint64_t _takeoff_time{0};
-
+	uORB::SubscriptionData<vehicle_attitude_s>		_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::PublicationData<home_position_s>			_home_position_pub{ORB_ID(home_position)};
 
 	uint8_t							_heading_reset_counter{0};

src/modules/ekf2/EKF2.cpp

@@ -1618,6 +1618,7 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 	lpos.heading_var = _ekf.getYawVar();
 	lpos.delta_heading = Eulerf(delta_q_reset).psi();
 	lpos.heading_good_for_control = _ekf.isYawFinalAlignComplete();
+	lpos.altitude_good_for_local_control = _gps_alt_drift.altitude_good_for_local_control;
 	lpos.tilt_var = _ekf.getTiltVariance();
 
 #if defined(CONFIG_EKF2_TERRAIN)
@@ -2181,6 +2182,10 @@ void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps)
 
 		_ekf.setBaroData(baroSample{airdata.timestamp_sample, airdata.baro_alt_meter, reset});
 
+#if defined(CONFIG_EKF2_GNSS)
+		_gps_alt_drift.updateBaroLpf(airdata.baro_alt_meter, airdata.timestamp_sample);
+#endif // CONFIG_EKF2_GNSS
+
 		ekf2_timestamps.vehicle_air_data_timestamp_rel = (int16_t)((int64_t)airdata.timestamp / 100 -
 				(int64_t)ekf2_timestamps.timestamp / 100);
 	}
@@ -2408,6 +2413,112 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
 #if defined(CONFIG_EKF2_GNSS)
+void EKF2::GpsAltDriftDetector::updateBaroLpf(float baro_alt, uint64_t timestamp)
+{
+	const float dt = 1e-6f * (timestamp - last_baro_ts);
+
+	if (last_baro_ts != 0 && dt < 1.f) {
+		baro_lpf.update(baro_alt, dt);
+
+	} else {
+		baro_lpf.reset(baro_alt);
+	}
+
+	last_baro_ts = timestamp;
+}
+
+void EKF2::GpsAltDriftDetector::update(const sensor_gps_s &gps, uORB::PublicationMulti<gps_altitude_drift_correction_s> &pub)
+{
+	const bool gps_timeout = (last_gps_ts != 0) && (gps.timestamp - last_gps_ts > 500000);
+
+	if (!gps_timeout && (last_gps_ts != 0) && (last_baro_ts != 0)) {
+
+		vel_integral += 1e-6f * (gps.timestamp - last_gps_ts) * (-gps.vel_d_m_s);
+
+		// sample at 1Hz normally, or immediately on pending hit
+		const bool sample_due = (last_sample_ts == 0)
+					|| hit_pending
+					|| (gps.timestamp >= last_sample_ts + 1000000);
+
+		if (sample_due) {
+			const float gps_alt = static_cast<float>(gps.altitude_msl_m);
+			d1[widx] = gps_alt - baro_lpf.getState();
+			d2[widx] = gps_alt - vel_integral;
+
+			widx = (widx + 1) % kWindowSize;
+
+			if (wcount < kWindowSize) {
+				wcount++;
+			}
+
+			last_sample_ts = gps.timestamp;
+
+			if (wcount > 1) {
+				const int newest = (widx - 1 + kWindowSize) % kWindowSize;
+				const int oldest = (widx - wcount + kWindowSize) % kWindowSize;
+
+				const float a = fabsf(d1[newest] - d1[oldest]); // change in (gps_alt - baro_alt) over window
+				const float b = fabsf(d2[newest] - d2[oldest]); // change in (gps_alt - vel_integral) over window
+				const float c = fabsf((d1[newest] - d2[newest]) - (d1[oldest] - d2[oldest])); // change in (vel_integral - baro_alt) over window
+
+				// gps_alt drift has to have relevant magnitude and larger than vel-baro drift
+				const bool hit = (a > kDriftThreshold) && (b > kDriftThreshold) && (a > c) && (b > c);
+
+				// hit pending to filter out single outliers
+				if (hit && hit_pending) {
+					gps_altitude_drift_correction_s correction{};
+					correction.timestamp = hrt_absolute_time();
+					correction.altitude_offset = d1[newest] - d1[oldest];
+					pub.publish(correction);
+					hit_pending = false;
+					altitude_good_for_local_control = false;
+					wcount = 1;
+
+				} else {
+					hit_pending = hit;
+				}
+
+				// Re-enable when recent samples show stability
+				if (!altitude_good_for_local_control && wcount >= kStabilityWindow) {
+					const int recent = (widx - kStabilityWindow + kWindowSize) % kWindowSize;
+					const float a_recent = fabsf(d1[newest] - d1[recent]);
+					const float b_recent = fabsf(d2[newest] - d2[recent]);
+
+					if ((a_recent < 0.2f * kDriftThreshold) && (b_recent < 0.2f * kDriftThreshold)) {
+						altitude_good_for_local_control = true;
+
+						const float residual = d1[newest] - d1[oldest];
+
+						if (fabsf(residual) > 0.01f) {
+							gps_altitude_drift_correction_s correction{};
+							correction.timestamp = hrt_absolute_time();
+							correction.altitude_offset = residual;
+							pub.publish(correction);
+						}
+
+						wcount = 1;
+					}
+				}
+			}
+		}
+
+	} else {
+		reset();
+	}
+
+	last_gps_ts = gps.timestamp;
+}
+
+void EKF2::GpsAltDriftDetector::reset()
+{
+	vel_integral = 0.f;
+	wcount = 0;
+	widx = 0;
+	last_sample_ts = 0;
+	hit_pending = false;
+	altitude_good_for_local_control = true;
+}
+
 void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 {
 	// EKF GPS message
@@ -2480,6 +2591,12 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 			_last_geoid_height_update_us = gnss_sample.time_us;
 		}
 
+		if (_ekf.control_status_flags().in_air &&  _ekf.getHeightSensorRef() == HeightSensor::GNSS) {
+			_gps_alt_drift.update(vehicle_gps_position, _gps_alt_drift_pub);
+
+		} else {
+			_gps_alt_drift.reset();
+		}
 	}
 }
 

src/modules/ekf2/EKF2.hpp

@@ -103,6 +103,7 @@
 
 #if defined(CONFIG_EKF2_GNSS)
 # include <uORB/topics/estimator_gps_status.h>
+# include <uORB/topics/gps_altitude_drift_correction.h>
 # include <uORB/topics/sensor_gps.h>
 #endif // CONFIG_EKF2_GNSS
 
@@ -470,6 +471,33 @@ private:
 	hrt_abstime _status_gnss_yaw_pub_last {0};
 	uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_gnss_yaw_pub {ORB_ID(estimator_aid_src_gnss_yaw)};
 # endif // CONFIG_EKF2_GNSS_YAW
+
+	struct GpsAltDriftDetector {
+		static constexpr int kWindowSize = 20; // roughly 20 seconds (at 1 Hz sample rate)
+		static constexpr int kStabilityWindow = 5; // samples to check for re-enable
+		static constexpr float kBaroLpfTimeConst = 3.f;
+		static constexpr float kDriftThreshold = 1.f; // [m]
+
+		AlphaFilter<float> baro_lpf;
+		uint64_t last_baro_ts{0};
+		uint64_t last_gps_ts{0};
+		float vel_integral{0.f};
+
+		float d1[kWindowSize] {}; // gps_alt - baro_alt
+		float d2[kWindowSize] {}; // gps_alt - vel_integral
+		int widx{0};
+		int wcount{0};
+		uint64_t last_sample_ts{0};
+		bool hit_pending{false};
+		bool altitude_good_for_local_control{true};
+
+		void updateBaroLpf(float baro_alt, uint64_t timestamp);
+		void update(const sensor_gps_s &gps, uORB::PublicationMulti<gps_altitude_drift_correction_s> &pub);
+		void reset();
+	};
+
+	GpsAltDriftDetector _gps_alt_drift{};
+	uORB::PublicationMulti<gps_altitude_drift_correction_s> _gps_alt_drift_pub{ORB_ID(gps_altitude_drift_correction)};
 #endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_GRAVITY_FUSION)