Merge remote-tracking branch 'px4/main' into pr-ekf2_astyle_follow_up

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp

@@ -623,7 +623,7 @@ void EstimatorChecks::checkEstimatorStatusFlags(const Context &context, Report &
 			}
 		}
 
-		if (estimator_status_flags.cs_gps_yaw_fault) {
+		if (estimator_status_flags.cs_gnss_yaw_fault) {
 			/* EVENT
 			 * @description
 			 * Land now

src/modules/ekf2/CMakeLists.txt

@@ -175,7 +175,7 @@ if(CONFIG_EKF2_GNSS)
 	)
 
 	if(CONFIG_EKF2_GNSS_YAW)
-		list(APPEND EKF_SRCS EKF/aid_sources/gnss/gps_yaw_fusion.cpp)
+		list(APPEND EKF_SRCS EKF/aid_sources/gnss/gnss_yaw_control.cpp)
 	endif()
 
 	list(APPEND EKF_LIBS yaw_estimator)

src/modules/ekf2/EKF/CMakeLists.txt

@@ -96,7 +96,7 @@ if(CONFIG_EKF2_GNSS)
 	)
 
 	if(CONFIG_EKF2_GNSS_YAW)
-		list(APPEND EKF_SRCS aid_sources/gnss/gps_yaw_fusion.cpp)
+		list(APPEND EKF_SRCS aid_sources/gnss/gnss_yaw_control.cpp)
 	endif()
 
 	add_subdirectory(yaw_estimator)

src/modules/ekf2/EKF/aid_sources/auxvel/auxvel_fusion.cpp

@@ -33,12 +33,12 @@
 
 #include "ekf.h"
 
-void Ekf::controlAuxVelFusion()
+void Ekf::controlAuxVelFusion(const imuSample &imu_sample)
 {
 	if (_auxvel_buffer) {
 		auxVelSample sample;
 
-		if (_auxvel_buffer->pop_first_older_than(_time_delayed_us, &sample)) {
+		if (_auxvel_buffer->pop_first_older_than(imu_sample.time_us, &sample)) {
 
 			updateAidSourceStatus(_aid_src_aux_vel,
 					      sample.time_us,                                           // sample timestamp

src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp

@@ -41,7 +41,7 @@
 // Maximum allowable time interval between pressure altitude measurements (uSec)
 static constexpr uint64_t BARO_MAX_INTERVAL{200'000};
 
-void Ekf::controlBaroHeightFusion()
+void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
 {
 	static constexpr const char *HGT_SRC_NAME = "baro";
 
@@ -52,10 +52,10 @@ void Ekf::controlBaroHeightFusion()
 
 	baroSample baro_sample;
 
-	if (_baro_buffer && _baro_buffer->pop_first_older_than(_time_delayed_us, &baro_sample)) {
+	if (_baro_buffer && _baro_buffer->pop_first_older_than(imu_sample.time_us, &baro_sample)) {
 
 #if defined(CONFIG_EKF2_BARO_COMPENSATION)
-		const float measurement = compensateBaroForDynamicPressure(baro_sample.hgt);
+		const float measurement = compensateBaroForDynamicPressure(imu_sample, baro_sample.hgt);
 #else
 		const float measurement = baro_sample.hgt;
 #endif
@@ -137,10 +137,12 @@ void Ekf::controlBaroHeightFusion()
 					resetVerticalPositionTo(-(_baro_lpf.getState() - bias_est.getBias()), measurement_var);
 					bias_est.setBias(_state.pos(2) + _baro_lpf.getState());
 
-					// reset vertical velocity
-					resetVerticalVelocityToZero();
+					// reset vertical velocity if no valid sources available
+					if (!isVerticalVelocityAidingActive()) {
+						resetVerticalVelocityToZero();
+					}
 
-					aid_src.time_last_fuse = _time_delayed_us;
+					aid_src.time_last_fuse = imu_sample.time_us;
 
 				} else if (is_fusion_failing) {
 					// Some other height source is still working
@@ -169,7 +171,7 @@ void Ekf::controlBaroHeightFusion()
 					bias_est.setBias(_state.pos(2) + _baro_lpf.getState());
 				}
 
-				aid_src.time_last_fuse = _time_delayed_us;
+				aid_src.time_last_fuse = imu_sample.time_us;
 				bias_est.setFusionActive();
 				_control_status.flags.baro_hgt = true;
 			}
@@ -198,7 +200,7 @@ void Ekf::stopBaroHgtFusion()
 }
 
 #if defined(CONFIG_EKF2_BARO_COMPENSATION)
-float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const
+float Ekf::compensateBaroForDynamicPressure(const imuSample &imu_sample, const float baro_alt_uncompensated) const
 {
 	if (_control_status.flags.wind && local_position_is_valid()) {
 		// calculate static pressure error = Pmeas - Ptruth
@@ -206,7 +208,8 @@ float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated)
 		// negative X and Y directions. Used to correct baro data for positional errors
 
 		// Calculate airspeed in body frame
-		const Vector3f vel_imu_rel_body_ned = _R_to_earth * (_ang_rate_delayed_raw % _params.imu_pos_body);
+		const Vector3f angular_velocity = (imu_sample.delta_ang / imu_sample.delta_ang_dt) - _state.gyro_bias;
+		const Vector3f vel_imu_rel_body_ned = _R_to_earth * (angular_velocity % _params.imu_pos_body);
 		const Vector3f velocity_earth = _state.vel - vel_imu_rel_body_ned;
 
 		const Vector3f wind_velocity_earth(_state.wind_vel(0), _state.wind_vel(1), 0.0f);

src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp

@@ -41,7 +41,7 @@
 // Maximum allowable time interval between external vision system measurements (uSec)
 static constexpr uint64_t EV_MAX_INTERVAL{200'000};
 
-void Ekf::controlExternalVisionFusion()
+void Ekf::controlExternalVisionFusion(const imuSample &imu_sample)
 {
 	_ev_pos_b_est.predict(_dt_ekf_avg);
 	_ev_hgt_b_est.predict(_dt_ekf_avg);
@@ -49,7 +49,7 @@ void Ekf::controlExternalVisionFusion()
 	// Check for new external vision data
 	extVisionSample ev_sample;
 
-	if (_ext_vision_buffer && _ext_vision_buffer->pop_first_older_than(_time_delayed_us, &ev_sample)) {
+	if (_ext_vision_buffer && _ext_vision_buffer->pop_first_older_than(imu_sample.time_us, &ev_sample)) {
 
 		bool ev_reset = (ev_sample.reset_counter != _ev_sample_prev.reset_counter);
 
@@ -63,10 +63,11 @@ void Ekf::controlExternalVisionFusion()
 				&& isNewestSampleRecent(_time_last_ext_vision_buffer_push, EV_MAX_INTERVAL);
 
 		updateEvAttitudeErrorFilter(ev_sample, ev_reset);
-		controlEvYawFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_yaw);
-		controlEvVelFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel);
-		controlEvPosFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_pos);
-		controlEvHeightFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_hgt);
+		controlEvYawFusion(imu_sample, ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_yaw);
+		controlEvVelFusion(imu_sample, ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel);
+		controlEvPosFusion(imu_sample, ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_pos);
+		controlEvHeightFusion(imu_sample, ev_sample, starting_conditions_passing, ev_reset, quality_sufficient,
+				      _aid_src_ev_hgt);
 
 		if (quality_sufficient) {
 			_ev_sample_prev = ev_sample;

src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp

@@ -38,8 +38,9 @@
 
 #include "ekf.h"
 
-void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
-				const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src)
+void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSample &ev_sample,
+				const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient,
+				estimator_aid_source1d_s &aid_src)
 {
 	static constexpr const char *AID_SRC_NAME = "EV height";
 
@@ -148,33 +149,6 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
 				resetVerticalPositionTo(measurement - bias_est.getBias(), measurement_var);
 				bias_est.setBias(-_state.pos(2) + measurement);
 
-				// reset vertical velocity
-				if (ev_sample.vel.isAllFinite() && (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))) {
-
-					// correct velocity for offset relative to IMU
-					const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
-					const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
-
-					switch (ev_sample.vel_frame) {
-					case VelocityFrame::LOCAL_FRAME_NED:
-					case VelocityFrame::LOCAL_FRAME_FRD: {
-							const Vector3f reset_vel = ev_sample.vel - vel_offset_earth;
-							resetVerticalVelocityTo(reset_vel(2), math::max(ev_sample.velocity_var(2), sq(_params.ev_vel_noise)));
-						}
-						break;
-
-					case VelocityFrame::BODY_FRAME_FRD: {
-							const Vector3f reset_vel = _R_to_earth * (ev_sample.vel - vel_offset_body);
-							const Matrix3f reset_vel_cov = _R_to_earth * matrix::diag(ev_sample.velocity_var) * _R_to_earth.transpose();
-							resetVerticalVelocityTo(reset_vel(2), math::max(reset_vel_cov(2, 2), sq(_params.ev_vel_noise)));
-						}
-						break;
-					}
-
-				} else {
-					resetVerticalVelocityToZero();
-				}
-
 				aid_src.time_last_fuse = _time_delayed_us;
 
 			} else if (is_fusion_failing) {

src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp

@@ -41,8 +41,9 @@
 static constexpr const char *EV_AID_SRC_NAME = "EV position";
 
 
-void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
-			     const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src)
+void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample &ev_sample,
+			     const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient,
+			     estimator_aid_source2d_s &aid_src)
 {
 	const bool yaw_alignment_changed = (!_control_status_prev.flags.ev_yaw && _control_status.flags.ev_yaw)
 					   || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);

src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp

@@ -41,8 +41,9 @@
 #include <ekf_derivation/generated/compute_ev_body_vel_hy.h>
 #include <ekf_derivation/generated/compute_ev_body_vel_hz.h>
 
-void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
-			     const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src)
+void Ekf::controlEvVelFusion(const imuSample &imu_sample, const extVisionSample &ev_sample,
+			     const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient,
+			     estimator_aid_source3d_s &aid_src)
 {
 	static constexpr const char *AID_SRC_NAME = "EV velocity";
 
@@ -55,8 +56,9 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 					     && ev_sample.vel.isAllFinite();
 
 	// correct velocity for offset relative to IMU
+	const Vector3f angular_velocity = imu_sample.delta_ang / imu_sample.delta_ang_dt - _state.gyro_bias;
 	const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
-	const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
+	const Vector3f vel_offset_body = angular_velocity % pos_offset_body;
 	const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
 
 	// rotate measurement into correct earth frame if required
@@ -210,6 +212,17 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 
 					stopEvVelFusion();
 				}
+
+			} else if (isHeightResetRequired()) {
+				// reset vertical velocity if height is failing
+				if (ev_sample.vel_frame == VelocityFrame::BODY_FRAME_FRD) {
+					const Vector3f measurement_ekf_frame = _R_to_earth * measurement;
+					const Vector3f measurement_var_ekf_frame = rotateVarianceToEkf(measurement_var);
+					resetVerticalVelocityTo(measurement_ekf_frame(2), measurement_var_ekf_frame(2));
+
+				} else {
+					resetVerticalVelocityTo(measurement(2), measurement_var(2));
+				}
 			}
 
 		} else {

src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp

@@ -38,8 +38,9 @@
 
 #include "ekf.h"
 
-void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
-			     const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src)
+void Ekf::controlEvYawFusion(const imuSample &imu_sample, const extVisionSample &ev_sample,
+			     const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient,
+			     estimator_aid_source1d_s &aid_src)
 {
 	static constexpr const char *AID_SRC_NAME = "EV yaw";
 

src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp

@@ -111,15 +111,6 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
 					resetVerticalPositionTo(-(measurement - bias_est.getBias()), measurement_var);
 					bias_est.setBias(_state.pos(2) + measurement);
 
-					// reset vertical velocity
-					if (PX4_ISFINITE(gps_sample.vel(2)) && (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VEL))) {
-						// use 1.5 as a typical ratio of vacc/hacc
-						resetVerticalVelocityTo(gps_sample.vel(2), sq(math::max(1.5f * gps_sample.sacc, _params.gps_vel_noise)));
-
-					} else {
-						resetVerticalVelocityToZero();
-					}
-
 					aid_src.time_last_fuse = _time_delayed_us;
 
 				} else if (is_fusion_failing) {

src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp

@@ -32,37 +32,122 @@
  ****************************************************************************/
 
 /**
- * @file gps_yaw_fusion.cpp
- * Definition of functions required to use yaw obtained from GPS dual antenna measurements.
- * Equations generated using EKF/python/ekf_derivation/main.py
- *
- * @author Paul Riseborough <p_riseborough@live.com.au>
+ * @file gnss_yaw_control.cpp
+ * Definition of functions required to use yaw obtained from GNSS dual antenna measurements.
+ * Equations generated using src/modules/ekf2/EKF/python/ekf_derivation/derivation.py
  *
  */
 
 #include "ekf.h"
-#include <ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h>
 
 #include <mathlib/mathlib.h>
 #include <cstdlib>
 
-void Ekf::updateGpsYaw(const gnssSample &gps_sample)
+#include <ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h>
+
+void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample)
+{
+	if (!(_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::YAW))
+	    || _control_status.flags.gnss_yaw_fault) {
+
+		stopGnssYawFusion();
+		return;
+	}
+
+	const bool is_new_data_available = PX4_ISFINITE(gnss_sample.yaw);
+
+	if (is_new_data_available) {
+
+		updateGnssYaw(gnss_sample);
+
+		const bool continuing_conditions_passing = _control_status.flags.tilt_align;
+
+		// Maximum allowable time interval between GNSS yaw measurements (uSec)
+		static constexpr uint64_t GNSS_YAW_MAX_INTERVAL{1'500'000};
+		const bool data_intermittent = !isNewestSampleRecent(_time_last_gnss_yaw_buffer_push, 2 * GNSS_YAW_MAX_INTERVAL);
+
+		const bool starting_conditions_passing = continuing_conditions_passing
+				&& _gps_checks_passed
+				&& !data_intermittent;
+
+		if (_control_status.flags.gnss_yaw) {
+			if (continuing_conditions_passing) {
+
+				fuseGnssYaw(gnss_sample.yaw_offset);
+
+				const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_max);
+
+				if (is_fusion_failing) {
+					stopGnssYawFusion();
+
+					// Before takeoff, we do not want to continue to rely on the current heading
+					// if we had to stop the fusion
+					if (!_control_status.flags.in_air) {
+						ECL_INFO("clearing yaw alignment");
+						_control_status.flags.yaw_align = false;
+					}
+				}
+
+			} else {
+				// Stop GNSS yaw fusion but do not declare it faulty
+				stopGnssYawFusion();
+			}
+
+		} else {
+			if (starting_conditions_passing) {
+				// Try to activate GNSS yaw fusion
+				const bool not_using_ne_aiding = !_control_status.flags.gps && !_control_status.flags.aux_gpos;
+
+				if (!_control_status.flags.in_air
+				    || !_control_status.flags.yaw_align
+				    || not_using_ne_aiding) {
+
+					// Reset before starting the fusion
+					if (resetYawToGnss(gnss_sample.yaw, gnss_sample.yaw_offset)) {
+
+						resetAidSourceStatusZeroInnovation(_aid_src_gnss_yaw);
+
+						_control_status.flags.gnss_yaw = true;
+						_control_status.flags.yaw_align = true;
+					}
+
+				} else if (!_aid_src_gnss_yaw.innovation_rejected) {
+					// Do not force a reset but wait for the consistency check to pass
+					_control_status.flags.gnss_yaw = true;
+					fuseGnssYaw(gnss_sample.yaw_offset);
+				}
+
+				if (_control_status.flags.gnss_yaw) {
+					ECL_INFO("starting GNSS yaw fusion");
+				}
+			}
+		}
+
+	} else if (_control_status.flags.gnss_yaw
+		   && !isNewestSampleRecent(_time_last_gnss_yaw_buffer_push, _params.reset_timeout_max)) {
+
+		// No yaw data in the message anymore. Stop until it comes back.
+		stopGnssYawFusion();
+	}
+}
+
+void Ekf::updateGnssYaw(const gnssSample &gnss_sample)
 {
 	// calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement
-	const float measured_hdg = wrap_pi(gps_sample.yaw + gps_sample.yaw_offset);
+	const float measured_hdg = wrap_pi(gnss_sample.yaw + gnss_sample.yaw_offset);
 
-	const float yaw_acc = PX4_ISFINITE(gps_sample.yaw_acc) ? gps_sample.yaw_acc : 0.f;
-	const float R_YAW = sq(fmaxf(yaw_acc, _params.gps_heading_noise));
+	const float yaw_acc = PX4_ISFINITE(gnss_sample.yaw_acc) ? gnss_sample.yaw_acc : 0.f;
+	const float R_YAW = sq(fmaxf(yaw_acc, _params.gnss_heading_noise));
 
 	float heading_pred;
 	float heading_innov_var;
 
 	VectorState H;
-	sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, gps_sample.yaw_offset, R_YAW, FLT_EPSILON,
+	sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, gnss_sample.yaw_offset, R_YAW, FLT_EPSILON,
 					    &heading_pred, &heading_innov_var, &H);
 
 	updateAidSourceStatus(_aid_src_gnss_yaw,
-			      gps_sample.time_us,                          // sample timestamp
+			      gnss_sample.time_us,                          // sample timestamp
 			      measured_hdg,                                // observation
 			      R_YAW,                                       // observation variance
 			      wrap_pi(heading_pred - measured_hdg),        // innovation
@@ -70,7 +155,7 @@ void Ekf::updateGpsYaw(const gnssSample &gps_sample)
 			      math::max(_params.heading_innov_gate, 1.f)); // innovation gate
 }
 
-void Ekf::fuseGpsYaw(float antenna_yaw_offset)
+void Ekf::fuseGnssYaw(float antenna_yaw_offset)
 {
 	auto &aid_src = _aid_src_gnss_yaw;
 
@@ -99,7 +184,8 @@ void Ekf::fuseGpsYaw(float antenna_yaw_offset)
 
 		// we reinitialise the covariance matrix and abort this fusion step
 		initialiseCovariance();
-		ECL_ERR("GPS yaw numerical error - covariance reset");
+		ECL_ERR("GNSS yaw numerical error - covariance reset");
+		stopGnssYawFusion();
 		return;
 	}
 
@@ -129,7 +215,7 @@ void Ekf::fuseGpsYaw(float antenna_yaw_offset)
 	}
 }
 
-bool Ekf::resetYawToGps(const float gnss_yaw, const float gnss_yaw_offset)
+bool Ekf::resetYawToGnss(const float gnss_yaw, const float gnss_yaw_offset)
 {
 	// define the predicted antenna array vector and rotate into earth frame
 	const Vector3f ant_vec_bf = {cosf(gnss_yaw_offset), sinf(gnss_yaw_offset), 0.0f};
@@ -140,11 +226,21 @@ bool Ekf::resetYawToGps(const float gnss_yaw, const float gnss_yaw_offset)
 		return false;
 	}
 
-	// GPS yaw measurement is alreday compensated for antenna offset in the driver
+	// GNSS yaw measurement is already compensated for antenna offset in the driver
 	const float measured_yaw = gnss_yaw;
 
-	const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.e-2f));
+	const float yaw_variance = sq(fmaxf(_params.gnss_heading_noise, 1.e-2f));
 	resetQuatStateYaw(measured_yaw, yaw_variance);
 
 	return true;
 }
+
+void Ekf::stopGnssYawFusion()
+{
+	if (_control_status.flags.gnss_yaw) {
+
+		_control_status.flags.gnss_yaw = false;
+
+		ECL_INFO("stopping GNSS yaw fusion");
+	}
+}

src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp

@@ -84,7 +84,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 			updateGnssPos(gnss_sample, _aid_src_gnss_pos);
 		}
 
-		updateGnssVel(gnss_sample, _aid_src_gnss_vel);
+		updateGnssVel(imu_delayed, gnss_sample, _aid_src_gnss_vel);
 
 	} else if (_control_status.flags.gps) {
 		if (!isNewestSampleRecent(_time_last_gps_buffer_push, _params.reset_timeout_max)) {
@@ -96,7 +96,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 	if (_gps_data_ready) {
 #if defined(CONFIG_EKF2_GNSS_YAW)
 		const gnssSample &gnss_sample = _gps_sample_delayed;
-		controlGpsYawFusion(gnss_sample);
+		controlGnssYawFusion(gnss_sample);
 #endif // CONFIG_EKF2_GNSS_YAW
 
 		controlGnssYawEstimator(_aid_src_gnss_vel);
@@ -130,17 +130,23 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 				}
 
 				if (do_vel_pos_reset) {
-					ECL_WARN("GPS fusion timeout, resetting velocity / position");
+					ECL_WARN("GPS fusion timeout, resetting");
+				}
 
-					if (gnss_vel_enabled) {
+				if (gnss_vel_enabled) {
+					if (do_vel_pos_reset) {
 						resetVelocityToGnss(_aid_src_gnss_vel);
-					}
 
-					if (gnss_pos_enabled) {
-						resetHorizontalPositionToGnss(_aid_src_gnss_pos);
+					} else if (isHeightResetRequired()) {
+						// reset vertical velocity if height is failing
+						resetVerticalVelocityTo(_aid_src_gnss_vel.observation[2], _aid_src_gnss_vel.observation_variance[2]);
 					}
 				}
 
+				if (gnss_pos_enabled && do_vel_pos_reset) {
+					resetHorizontalPositionToGnss(_aid_src_gnss_pos);
+				}
+
 			} else {
 				stopGpsFusion();
 			}
@@ -171,12 +177,13 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 	}
 }
 
-void Ekf::updateGnssVel(const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src)
+void Ekf::updateGnssVel(const imuSample &imu_sample, const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src)
 {
 	// correct velocity for offset relative to IMU
 	const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
 
-	const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
+	const Vector3f angular_velocity = imu_sample.delta_ang / imu_sample.delta_ang_dt - _state.gyro_bias;
+	const Vector3f vel_offset_body = angular_velocity % pos_offset_body;
 	const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
 	const Vector3f velocity = gnss_sample.vel - vel_offset_earth;
 
@@ -276,8 +283,8 @@ bool Ekf::tryYawEmergencyReset()
 
 #if defined(CONFIG_EKF2_GNSS_YAW)
 
-		if (_control_status.flags.gps_yaw) {
-			_control_status.flags.gps_yaw_fault = true;
+		if (_control_status.flags.gnss_yaw) {
+			_control_status.flags.gnss_yaw_fault = true;
 		}
 
 #endif // CONFIG_EKF2_GNSS_YAW
@@ -345,104 +352,6 @@ bool Ekf::shouldResetGpsFusion() const
 	return (is_reset_required || is_inflight_nav_failure);
 }
 
-#if defined(CONFIG_EKF2_GNSS_YAW)
-void Ekf::controlGpsYawFusion(const gnssSample &gps_sample)
-{
-	if (!(_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::YAW))
-	    || _control_status.flags.gps_yaw_fault) {
-
-		stopGpsYawFusion();
-		return;
-	}
-
-	const bool is_new_data_available = PX4_ISFINITE(gps_sample.yaw);
-
-	if (is_new_data_available) {
-
-		updateGpsYaw(gps_sample);
-
-		const bool continuing_conditions_passing = _control_status.flags.tilt_align;
-
-		// Maximum allowable time interval between GNSS yaw measurements (uSec)
-		static constexpr uint64_t GNSS_YAW_MAX_INTERVAL{1'500'000};
-		const bool data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, 2 * GNSS_YAW_MAX_INTERVAL);
-
-		const bool starting_conditions_passing = continuing_conditions_passing
-				&& _gps_checks_passed
-				&& !data_intermittent;
-
-		if (_control_status.flags.gps_yaw) {
-			if (continuing_conditions_passing) {
-
-				fuseGpsYaw(gps_sample.yaw_offset);
-
-				const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_max);
-
-				if (is_fusion_failing) {
-					stopGpsYawFusion();
-
-					// Before takeoff, we do not want to continue to rely on the current heading
-					// if we had to stop the fusion
-					if (!_control_status.flags.in_air) {
-						ECL_INFO("clearing yaw alignment");
-						_control_status.flags.yaw_align = false;
-					}
-				}
-
-			} else {
-				// Stop GPS yaw fusion but do not declare it faulty
-				stopGpsYawFusion();
-			}
-
-		} else {
-			if (starting_conditions_passing) {
-				// Try to activate GPS yaw fusion
-				const bool not_using_ne_aiding = !_control_status.flags.gps && !_control_status.flags.aux_gpos;
-
-				if (!_control_status.flags.in_air
-				    || !_control_status.flags.yaw_align
-				    || not_using_ne_aiding) {
-
-					// Reset before starting the fusion
-					if (resetYawToGps(gps_sample.yaw, gps_sample.yaw_offset)) {
-
-						resetAidSourceStatusZeroInnovation(_aid_src_gnss_yaw);
-
-						_control_status.flags.gps_yaw = true;
-						_control_status.flags.yaw_align = true;
-					}
-
-				} else if (!_aid_src_gnss_yaw.innovation_rejected) {
-					// Do not force a reset but wait for the consistency check to pass
-					_control_status.flags.gps_yaw = true;
-					fuseGpsYaw(gps_sample.yaw_offset);
-				}
-
-				if (_control_status.flags.gps_yaw) {
-					ECL_INFO("starting GPS yaw fusion");
-				}
-			}
-		}
-
-	} else if (_control_status.flags.gps_yaw
-		   && !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, _params.reset_timeout_max)) {
-
-		// No yaw data in the message anymore. Stop until it comes back.
-		stopGpsYawFusion();
-	}
-}
-
-void Ekf::stopGpsYawFusion()
-{
-	if (_control_status.flags.gps_yaw) {
-
-		_control_status.flags.gps_yaw = false;
-
-		ECL_INFO("stopping GPS yaw fusion");
-	}
-}
-#endif // CONFIG_EKF2_GNSS_YAW
-
 void Ekf::stopGpsFusion()
 {
 	if (_control_status.flags.gps) {
@@ -456,7 +365,7 @@ void Ekf::stopGpsFusion()
 
 	stopGpsHgtFusion();
 #if defined(CONFIG_EKF2_GNSS_YAW)
-	stopGpsYawFusion();
+	stopGnssYawFusion();
 #endif // CONFIG_EKF2_GNSS_YAW
 
 	_yawEstimator.reset();

src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp

@@ -44,7 +44,7 @@
 // Maximum allowable time interval between magnetic field measurements (uSec)
 static constexpr uint64_t MAG_MAX_INTERVAL{500'000};
 
-void Ekf::controlMagFusion()
+void Ekf::controlMagFusion(const imuSample &imu_sample)
 {
 	static constexpr const char *AID_SRC_NAME = "mag";
 	estimator_aid_source3d_s &aid_src = _aid_src_mag;
@@ -62,7 +62,7 @@ void Ekf::controlMagFusion()
 
 	magSample mag_sample;
 
-	if (_mag_buffer && _mag_buffer->pop_first_older_than(_time_delayed_us, &mag_sample)) {
+	if (_mag_buffer && _mag_buffer->pop_first_older_than(imu_sample.time_us, &mag_sample)) {
 
 		if (mag_sample.reset || (_mag_counter == 0)) {
 			// sensor or calibration has changed, reset low pass filter
@@ -155,7 +155,7 @@ void Ekf::controlMagFusion()
 						       && !_control_status.flags.mag_fault
 						       && !_control_status.flags.mag_field_disturbed
 						       && !_control_status.flags.ev_yaw
-						       && !_control_status.flags.gps_yaw;
+						       && !_control_status.flags.gnss_yaw;
 
 		_control_status.flags.mag_3D = common_conditions_passing
 					       && (_params.mag_fusion_type == MagFuseType::AUTO)
@@ -165,7 +165,6 @@ void Ekf::controlMagFusion()
 						&& ((_params.mag_fusion_type == MagFuseType::HEADING)
 						    || (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D));
 
-
 		if (_control_status.flags.mag_3D && !_control_status_prev.flags.mag_3D) {
 			ECL_INFO("starting mag 3D fusion");
 
@@ -185,7 +184,7 @@ void Ekf::controlMagFusion()
 				if (mag_sample.reset || checkHaglYawResetReq() || (wmm_updated && no_ne_aiding_or_pre_takeoff)) {
 					ECL_INFO("reset to %s", AID_SRC_NAME);
 					resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D);
-					aid_src.time_last_fuse = _time_delayed_us;
+					aid_src.time_last_fuse = imu_sample.time_us;
 
 				} else {
 					// The normal sequence is to fuse the magnetometer data first before fusing
@@ -213,7 +212,7 @@ void Ekf::controlMagFusion()
 					if (no_ne_aiding_or_pre_takeoff) {
 						ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
 						resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D);
-						aid_src.time_last_fuse = _time_delayed_us;
+						aid_src.time_last_fuse = imu_sample.time_us;
 
 					} else {
 						ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME);
@@ -242,7 +241,7 @@ void Ekf::controlMagFusion()
 					bool reset_heading = !_control_status.flags.yaw_align;
 
 					resetMagStates(_mag_lpf.getState(), reset_heading);
-					aid_src.time_last_fuse = _time_delayed_us;
+					aid_src.time_last_fuse = imu_sample.time_us;
 
 					if (reset_heading) {
 						_control_status.flags.yaw_align = true;

src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp

@@ -69,8 +69,6 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 					    : _params.flow_qual_min_gnd;
 
 		const bool is_quality_good = (flow_sample.quality >= min_quality);
-		const bool is_magnitude_good = flow_sample.flow_rate.isAllFinite()
-					       && !flow_sample.flow_rate.longerThan(_flow_max_rate);
 
 		bool is_tilt_good = true;
 
@@ -117,16 +115,21 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 					&& (_control_status.flags.inertial_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently
 					    || isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow));
 
-		const bool is_within_max_sensor_dist = getHagl() <= _flow_max_distance;
+		const bool is_within_sensor_dist = (getHagl() >= _flow_min_distance) && (getHagl() <= _flow_max_distance);
+
+		const bool is_magnitude_good = flow_sample.flow_rate.isAllFinite()
+					       && !flow_sample.flow_rate.longerThan(_flow_max_rate)
+					       && !flow_compensated.longerThan(_flow_max_rate);
 
 		const bool continuing_conditions_passing = (_params.flow_ctrl == 1)
 				&& _control_status.flags.tilt_align
-				&& is_within_max_sensor_dist;
+				&& is_within_sensor_dist;
 
 		const bool starting_conditions_passing = continuing_conditions_passing
 				&& is_quality_good
 				&& is_magnitude_good
 				&& is_tilt_good
+				&& (isTerrainEstimateValid() || isHorizontalAidingActive())
 				&& isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)2e6); // Prevent rapid switching
 
 		// If the height is relative to the ground, terrain height cannot be observed.
@@ -141,7 +144,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 
 				// handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period
 				if (isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max)) {
-					if (is_flow_required && is_quality_good) {
+					if (is_flow_required && is_quality_good && is_magnitude_good) {
 						resetFlowFusion();
 
 						if (_control_status.flags.opt_flow_terrain && !isTerrainEstimateValid()) {
@@ -216,15 +219,29 @@ void Ekf::resetFlowFusion()
 void Ekf::resetTerrainToFlow()
 {
 	ECL_INFO("reset hagl to flow");
+
 	// TODO: use the flow data
-	_state.terrain = fmaxf(0.0f, _state.pos(2));
+	const float new_terrain = fmaxf(0.0f, _state.pos(2));
+	const float delta_terrain = new_terrain - _state.terrain;
+	_state.terrain = new_terrain;
 	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, 100.f);
-	_terrain_vpos_reset_counter++;
 
 	resetAidSourceStatusZeroInnovation(_aid_src_optical_flow);
 
 	_innov_check_fail_status.flags.reject_optflow_X = false;
 	_innov_check_fail_status.flags.reject_optflow_Y = false;
+
+
+	// record the state change
+	if (_state_reset_status.reset_count.hagl == _state_reset_count_prev.hagl) {
+		_state_reset_status.hagl_change = delta_terrain;
+
+	} else {
+		// there's already a reset this update, accumulate total delta
+		_state_reset_status.hagl_change += delta_terrain;
+	}
+
+	_state_reset_status.reset_count.hagl++;
 }
 
 void Ekf::stopFlowFusion()

src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp

@@ -39,7 +39,7 @@
 #include "ekf.h"
 #include "ekf_derivation/generated/compute_hagl_innov_var.h"
 
-void Ekf::controlRangeHaglFusion()
+void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 {
 	static constexpr const char *HGT_SRC_NAME = "RNG";
 
@@ -47,7 +47,7 @@ void Ekf::controlRangeHaglFusion()
 
 	if (_range_buffer) {
 		// Get range data from buffer and check validity
-		rng_data_ready = _range_buffer->pop_first_older_than(_time_delayed_us, _range_sensor.getSampleAddress());
+		rng_data_ready = _range_buffer->pop_first_older_than(imu_sample.time_us, _range_sensor.getSampleAddress());
 		_range_sensor.setDataReadiness(rng_data_ready);
 
 		// update range sensor angle parameters in case they have changed
@@ -55,7 +55,7 @@ void Ekf::controlRangeHaglFusion()
 		_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
 		_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
 
-		_range_sensor.runChecks(_time_delayed_us, _R_to_earth);
+		_range_sensor.runChecks(imu_sample.time_us, _R_to_earth);
 
 		if (_range_sensor.isDataHealthy()) {
 			// correct the range data for position offset relative to the IMU
@@ -72,7 +72,7 @@ void Ekf::controlRangeHaglFusion()
 
 				_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
 				_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2),
-							      P(State::vel.idx + 2, State::vel.idx + 2), horizontal_motion, _time_delayed_us);
+							      P(State::vel.idx + 2, State::vel.idx + 2), horizontal_motion, imu_sample.time_us);
 			}
 
 		} else {
@@ -151,7 +151,7 @@ void Ekf::controlRangeHaglFusion()
 					_control_status.flags.rng_hgt = true;
 					stopRngTerrFusion();
 
-					aid_src.time_last_fuse = _time_delayed_us;
+					aid_src.time_last_fuse = imu_sample.time_us;
 				}
 
 			} else {
@@ -180,10 +180,12 @@ void Ekf::controlRangeHaglFusion()
 					_information_events.flags.reset_hgt_to_rng = true;
 					resetVerticalPositionTo(-(aid_src.observation - _state.terrain));
 
-					// reset vertical velocity
-					resetVerticalVelocityToZero();
+					// reset vertical velocity if no valid sources available
+					if (!isVerticalVelocityAidingActive()) {
+						resetVerticalVelocityToZero();
+					}
 
-					aid_src.time_last_fuse = _time_delayed_us;
+					aid_src.time_last_fuse = imu_sample.time_us;
 
 				} else if (is_fusion_failing) {
 					// Some other height source is still working
@@ -232,21 +234,20 @@ void Ekf::controlRangeHaglFusion()
 
 void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
 {
-	aid_src.observation = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance);
-	aid_src.innovation = getHagl() - aid_src.observation;
+	const float measurement = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance);
+	const float measurement_variance = getRngVar();
 
-	const float observation_variance = getRngVar();
 	float innovation_variance;
-	sym::ComputeHaglInnovVar(P, observation_variance, &innovation_variance);
+	sym::ComputeHaglInnovVar(P, measurement_variance, &innovation_variance);
 
 	const float innov_gate = math::max(_params.range_innov_gate, 1.f);
 	updateAidSourceStatus(aid_src,
-			      _range_sensor.getSampleAddress()->time_us,                           // sample timestamp
-			      math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance), // observation
-			      observation_variance,                                                // observation variance
-			      getHagl() - aid_src.observation,                                     // innovation
-			      innovation_variance,                                                 // innovation variance
-			      math::max(_params.range_innov_gate, 1.f));                           // innovation gate
+			      _range_sensor.getSampleAddress()->time_us, // sample timestamp
+			      measurement,                               // observation
+			      measurement_variance,                      // observation variance
+			      getHagl() - measurement,                   // innovation
+			      innovation_variance,                       // innovation variance
+			      innov_gate);                               // innovation gate
 
 	// z special case if there is bad vertical acceleration data, then don't reject measurement,
 	// but limit innovation to prevent spikes that could destabilise the filter
@@ -268,9 +269,23 @@ float Ekf::getRngVar() const
 
 void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src)
 {
-	_state.terrain = _state.pos(2) + aid_src.observation;
+	const float new_terrain = _state.pos(2) + aid_src.observation;
+	const float delta_terrain = new_terrain - _state.terrain;
+
+	_state.terrain = new_terrain;
 	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, aid_src.observation_variance);
-	_terrain_vpos_reset_counter++;
+
+	// record the state change
+	if (_state_reset_status.reset_count.hagl == _state_reset_count_prev.hagl) {
+		_state_reset_status.hagl_change = delta_terrain;
+
+	} else {
+		// there's already a reset this update, accumulate total delta
+		_state_reset_status.hagl_change += delta_terrain;
+	}
+
+	_state_reset_status.reset_count.hagl++;
+
 
 	aid_src.time_last_fuse = _time_delayed_us;
 }

src/modules/ekf2/EKF/aid_sources/zero_innovation_heading_update.cpp

@@ -41,7 +41,7 @@
 void Ekf::controlZeroInnovationHeadingUpdate()
 {
 	const bool yaw_aiding = _control_status.flags.mag_hdg || _control_status.flags.mag_3D
-				|| _control_status.flags.ev_yaw || _control_status.flags.gps_yaw;
+				|| _control_status.flags.ev_yaw || _control_status.flags.gnss_yaw;
 
 	// fuse zero innovation at a limited rate if the yaw variance is too large
 	if (!yaw_aiding && isTimedOut(_time_last_heading_fuse, 200'000)) {

src/modules/ekf2/EKF/common.h

@@ -316,7 +316,7 @@ struct parameters {
 
 # if defined(CONFIG_EKF2_GNSS_YAW)
 	// GNSS heading fusion
-	float gps_heading_noise{0.1f};          ///< measurement noise standard deviation used for GNSS heading fusion (rad)
+	float gnss_heading_noise{0.1f};          ///< measurement noise standard deviation used for GNSS heading fusion (rad)
 # endif // CONFIG_EKF2_GNSS_YAW
 
 	// Parameters used to control when yaw is reset to the EKF-GSF yaw estimator value
@@ -553,12 +553,12 @@ union filter_control_status_u {
 		uint64_t fuse_aspd               : 1; ///< 19 - airspeed measurements are being fused
 		uint64_t gnd_effect              : 1; ///< 20 - protection from ground effect induced static pressure rise is active
 		uint64_t rng_stuck               : 1; ///< 21 - rng data wasn't ready and new values haven't changed enough
-		uint64_t gps_yaw                 : 1; ///< 22 - yaw (not ground course) data fusion from a GPS receiver is intended
+		uint64_t gnss_yaw                : 1; ///< 22 - yaw (not ground course) data fusion from a GPS receiver is intended
 		uint64_t mag_aligned_in_flight   : 1; ///< 23 - the in-flight mag field alignment has been completed
 		uint64_t ev_vel                  : 1; ///< 24 - local frame velocity data fusion from vision measurements is intended
 		uint64_t synthetic_mag_z         : 1; ///< 25 - using a synthesized measurement for the magnetometer Z component
 		uint64_t vehicle_at_rest         : 1; ///< 26 - the vehicle is at rest
-		uint64_t gps_yaw_fault           : 1; ///< 27 - the GNSS heading has been declared faulty and is no longer being used
+		uint64_t gnss_yaw_fault          : 1; ///< 27 - the GNSS heading has been declared faulty and is no longer being used
 		uint64_t rng_fault               : 1; ///< 28 - the range finder has been declared faulty and is no longer being used
 		uint64_t inertial_dead_reckoning : 1; ///< 29 - longer fusing measurements that constrain horizontal velocity drift
 		uint64_t wind_dead_reckoning     : 1; ///< 30 - navigation reliant on wind relative measurements
@@ -596,16 +596,6 @@ union ekf_solution_status_u {
 	uint16_t value;
 };
 
-#if defined(CONFIG_EKF2_TERRAIN)
-union terrain_fusion_status_u {
-	struct {
-		bool range_finder : 1;  ///< 0 - fusing range finder data for terrain
-		bool flow         : 1;  ///< 1 - fusing flow data for terrain
-	} flags;
-	uint8_t value;
-};
-#endif // CONFIG_EKF2_TERRAIN
-
 // define structure used to communicate information events
 union information_event_status_u {
 	struct {

src/modules/ekf2/EKF/control.cpp

@@ -102,7 +102,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	// control use of observations for aiding
-	controlMagFusion();
+	controlMagFusion(imu_delayed);
 #endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
@@ -137,12 +137,12 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	// Additional data odometry data from an external estimator can be fused.
-	controlExternalVisionFusion();
+	controlExternalVisionFusion(imu_delayed);
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
 #if defined(CONFIG_EKF2_AUXVEL)
 	// Additional horizontal velocity data from an auxiliary sensor can be fused
-	controlAuxVelFusion();
+	controlAuxVelFusion(imu_delayed);
 #endif // CONFIG_EKF2_AUXVEL
 	//
 #if defined(CONFIG_EKF2_TERRAIN)

src/modules/ekf2/EKF/covariance.cpp

@@ -135,8 +135,8 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 
 	// calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states
 	P = sym::PredictCovariance(_state.vector(), P,
-				   imu_delayed.delta_vel / math::max(imu_delayed.delta_vel_dt, FLT_EPSILON), accel_var,
-				   imu_delayed.delta_ang / math::max(imu_delayed.delta_ang_dt, FLT_EPSILON), gyro_var,
+				   imu_delayed.delta_vel / imu_delayed.delta_vel_dt, accel_var,
+				   imu_delayed.delta_ang / imu_delayed.delta_ang_dt, gyro_var,
 				   dt);
 
 	// Construct the process noise variance diagonal for those states with a stationary process model

src/modules/ekf2/EKF/ekf.cpp

@@ -89,8 +89,6 @@ void Ekf::reset()
 	_control_status.flags.in_air = true;
 	_control_status_prev.flags.in_air = true;
 
-	_ang_rate_delayed_raw.zero();
-
 	_fault_status.value = 0;
 	_innov_check_fail_status.value = 0;
 
@@ -264,13 +262,6 @@ void Ekf::predictState(const imuSample &imu_delayed)
 	_state.vel = matrix::constrain(_state.vel, -1000.f, 1000.f);
 	_state.pos = matrix::constrain(_state.pos, -1.e6f, 1.e6f);
 
-	// some calculations elsewhere in code require a raw angular rate vector so calculate here to avoid duplication
-	// protect against possible small timesteps resulting from timing slip on previous frame that can drive spikes into the rate
-	// due to insufficient averaging
-	if (imu_delayed.delta_ang_dt > 0.25f * _dt_ekf_avg) {
-		_ang_rate_delayed_raw = imu_delayed.delta_ang / imu_delayed.delta_ang_dt;
-	}
-
 
 	// calculate a filtered horizontal acceleration with a 1 sec time constant
 	// this are used for manoeuvre detection elsewhere

src/modules/ekf2/EKF/ekf.h

@@ -104,9 +104,6 @@ public:
 	float getTerrainVertPos() const { return _state.terrain; };
 	float getHagl() const { return _state.terrain - _state.pos(2); }
 
-	// get the number of times the vertical terrain position has been reset
-	uint8_t getTerrainVertPosResetCounter() const { return _terrain_vpos_reset_counter; };
-
 	// get the terrain variance
 	float getTerrainVariance() const { return P(State::terrain.idx, State::terrain.idx); }
 
@@ -147,7 +144,7 @@ public:
 
 #if defined(CONFIG_EKF2_GNSS_YAW)
 
-		if (_control_status.flags.gps_yaw) {
+		if (_control_status.flags.gnss_yaw) {
 			return _aid_src_gnss_yaw.innovation;
 		}
 
@@ -176,7 +173,7 @@ public:
 
 #if defined(CONFIG_EKF2_GNSS_YAW)
 
-		if (_control_status.flags.gps_yaw) {
+		if (_control_status.flags.gnss_yaw) {
 			return _aid_src_gnss_yaw.innovation_variance;
 		}
 
@@ -205,7 +202,7 @@ public:
 
 #if defined(CONFIG_EKF2_GNSS_YAW)
 
-		if (_control_status.flags.gps_yaw) {
+		if (_control_status.flags.gnss_yaw) {
 			return _aid_src_gnss_yaw.test_ratio;
 		}
 
@@ -358,6 +355,13 @@ public:
 		*counter = _state_reset_status.reset_count.posD;
 	}
 
+	uint8_t get_hagl_reset_count() const { return _state_reset_status.reset_count.hagl; }
+	void get_hagl_reset(float *delta, uint8_t *counter) const
+	{
+		*delta = _state_reset_status.hagl_change;
+		*counter = _state_reset_status.reset_count.hagl;
+	}
+
 	// return the amount the local vertical velocity changed in the last reset and the number of reset events
 	uint8_t get_velD_reset_count() const { return _state_reset_status.reset_count.velD; }
 	void get_velD_reset(float *delta, uint8_t *counter) const
@@ -551,6 +555,7 @@ private:
 		uint8_t posNE{0};	///< number of horizontal position reset events (allow to wrap if count exceeds 255)
 		uint8_t posD{0};	///< number of vertical position reset events (allow to wrap if count exceeds 255)
 		uint8_t quat{0};	///< number of quaternion reset events (allow to wrap if count exceeds 255)
+		uint8_t hagl{0};	///< number of height above ground level reset events (allow to wrap if count exceeds 255)
 	};
 
 	struct StateResets {
@@ -559,6 +564,7 @@ private:
 		Vector2f posNE_change;	///< North, East position change due to last reset (m)
 		float posD_change;	///< Down position change due to last reset (m)
 		Quatf quat_change;	///< quaternion delta due to last reset - multiply pre-reset quaternion by this to get post-reset quaternion
+		float hagl_change;	///< Height above ground level change due to last reset (m)
 
 		StateResetCounts reset_count{};
 	};
@@ -566,8 +572,6 @@ private:
 	StateResets _state_reset_status{};	///< reset event monitoring structure containing velocity, position, height and yaw reset information
 	StateResetCounts _state_reset_count_prev{};
 
-	Vector3f _ang_rate_delayed_raw{};	///< uncorrected angular rate vector at fusion time horizon (rad/sec)
-
 	StateSample _state{};		///< state struct of the ekf running at the delayed time horizon
 
 	bool _filter_initialised{false};	///< true when the EKF sttes and covariances been initialised
@@ -601,8 +605,6 @@ private:
 
 #if defined(CONFIG_EKF2_TERRAIN)
 	// Terrain height state estimation
-	uint8_t _terrain_vpos_reset_counter{0};	///< number of times _terrain_vpos has been reset
-
 	float _last_on_ground_posD{0.0f};	///< last vertical position when the in_air status was false (m)
 #endif // CONFIG_EKF2_TERRAIN
 
@@ -841,7 +843,7 @@ private:
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	// range height
-	void controlRangeHaglFusion();
+	void controlRangeHaglFusion(const imuSample &imu_delayed);
 	bool isConditionalRangeAidSuitable();
 	void stopRngHgtFusion();
 	void stopRngTerrFusion();
@@ -934,16 +936,20 @@ private:
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	// control fusion of external vision observations
-	void controlExternalVisionFusion();
+	void controlExternalVisionFusion(const imuSample &imu_sample);
 	void updateEvAttitudeErrorFilter(extVisionSample &ev_sample, bool ev_reset);
-	void controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
-				   const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src);
-	void controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
-				const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src);
-	void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
-				const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src);
-	void controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
-				const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src);
+	void controlEvHeightFusion(const imuSample &imu_sample, const extVisionSample &ev_sample,
+				   const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient,
+				   estimator_aid_source1d_s &aid_src);
+	void controlEvPosFusion(const imuSample &imu_sample, const extVisionSample &ev_sample,
+				const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient,
+				estimator_aid_source2d_s &aid_src);
+	void controlEvVelFusion(const imuSample &imu_sample, const extVisionSample &ev_sample,
+				const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient,
+				estimator_aid_source3d_s &aid_src);
+	void controlEvYawFusion(const imuSample &imu_sample, const extVisionSample &ev_sample,
+				const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient,
+				estimator_aid_source1d_s &aid_src);
 	void resetVelocityToEV(const Vector3f &measurement, const Vector3f &measurement_var, const VelocityFrame &vel_frame);
 	Vector3f rotateVarianceToEkf(const Vector3f &measurement_var);
 
@@ -966,7 +972,7 @@ private:
 	// control fusion of GPS observations
 	void controlGpsFusion(const imuSample &imu_delayed);
 	void stopGpsFusion();
-	void updateGnssVel(const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src);
+	void updateGnssVel(const imuSample &imu_sample, const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src);
 	void updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s &aid_src);
 	void controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel);
 	bool tryYawEmergencyReset();
@@ -987,17 +993,17 @@ private:
 	void resetGpsDriftCheckFilters();
 
 # if defined(CONFIG_EKF2_GNSS_YAW)
-	void controlGpsYawFusion(const gnssSample &gps_sample);
-	void stopGpsYawFusion();
+	void controlGnssYawFusion(const gnssSample &gps_sample);
+	void stopGnssYawFusion();
 
 	// fuse the yaw angle obtained from a dual antenna GPS unit
-	void fuseGpsYaw(float antenna_yaw_offset);
+	void fuseGnssYaw(float antenna_yaw_offset);
 
 	// reset the quaternions states using the yaw angle obtained from a dual antenna GPS unit
 	// return true if the reset was successful
-	bool resetYawToGps(float gnss_yaw, float gnss_yaw_offset);
+	bool resetYawToGnss(float gnss_yaw, float gnss_yaw_offset);
 
-	void updateGpsYaw(const gnssSample &gps_sample);
+	void updateGnssYaw(const gnssSample &gps_sample);
 
 # endif // CONFIG_EKF2_GNSS_YAW
 
@@ -1015,7 +1021,7 @@ private:
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	// control fusion of magnetometer observations
-	void controlMagFusion();
+	void controlMagFusion(const imuSample &imu_sample);
 
 	bool checkHaglYawResetReq() const;
 
@@ -1048,7 +1054,7 @@ private:
 
 #if defined(CONFIG_EKF2_AUXVEL)
 	// control fusion of auxiliary velocity observations
-	void controlAuxVelFusion();
+	void controlAuxVelFusion(const imuSample &imu_sample);
 	void stopAuxVelFusion();
 #endif // CONFIG_EKF2_AUXVEL
 
@@ -1061,13 +1067,13 @@ private:
 	void checkHeightSensorRefFallback();
 
 #if defined(CONFIG_EKF2_BAROMETER)
-	void controlBaroHeightFusion();
+	void controlBaroHeightFusion(const imuSample &imu_sample);
 	void stopBaroHgtFusion();
 
 	void updateGroundEffect();
 
 # if defined(CONFIG_EKF2_BARO_COMPENSATION)
-	float compensateBaroForDynamicPressure(float baro_alt_uncompensated) const;
+	float compensateBaroForDynamicPressure(const imuSample &imu_sample, float baro_alt_uncompensated) const;
 # endif // CONFIG_EKF2_BARO_COMPENSATION
 
 #endif // CONFIG_EKF2_BAROMETER

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -333,7 +333,7 @@ float Ekf::getHeadingInnovationTestRatio() const
 
 #if defined(CONFIG_EKF2_GNSS_YAW)
 
-	if (_control_status.flags.gps_yaw) {
+	if (_control_status.flags.gnss_yaw) {
 		test_ratio = math::max(test_ratio, fabsf(_aid_src_gnss_yaw.test_ratio_filtered));
 	}
 

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -94,7 +94,17 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
 
 		_imu_updated = true;
 
-		_imu_buffer.push(_imu_down_sampler.getDownSampledImuAndTriggerReset());
+		imuSample imu_downsampled = _imu_down_sampler.getDownSampledImuAndTriggerReset();
+
+		// as a precaution constrain the integration delta time to prevent numerical problems
+		const float filter_update_period_s = _params.filter_update_interval_us * 1e-6f;
+		const float imu_min_dt = 0.5f * filter_update_period_s;
+		const float imu_max_dt = 2.0f * filter_update_period_s;
+
+		imu_downsampled.delta_ang_dt = math::constrain(imu_downsampled.delta_ang_dt, imu_min_dt, imu_max_dt);
+		imu_downsampled.delta_vel_dt = math::constrain(imu_downsampled.delta_vel_dt, imu_min_dt, imu_max_dt);
+
+		_imu_buffer.push(imu_downsampled);
 
 		// get the oldest data from the buffer
 		_time_delayed_us = _imu_buffer.get_oldest().time_us;
@@ -183,7 +193,7 @@ void EstimatorInterface::setGpsData(const gnssSample &gnss_sample)
 #if defined(CONFIG_EKF2_GNSS_YAW)
 
 		if (PX4_ISFINITE(gnss_sample.yaw)) {
-			_time_last_gps_yaw_buffer_push = _time_latest_us;
+			_time_last_gnss_yaw_buffer_push = _time_latest_us;
 		}
 
 #endif // CONFIG_EKF2_GNSS_YAW

src/modules/ekf2/EKF/estimator_interface.h

@@ -397,7 +397,7 @@ protected:
 
 # if defined(CONFIG_EKF2_GNSS_YAW)
 	// innovation consistency check monitoring ratios
-	uint64_t _time_last_gps_yaw_buffer_push{0};
+	uint64_t _time_last_gnss_yaw_buffer_push{0};
 # endif // CONFIG_EKF2_GNSS_YAW
 #endif // CONFIG_EKF2_GNSS
 

src/modules/ekf2/EKF/height_control.cpp

@@ -45,7 +45,7 @@ void Ekf::controlHeightFusion(const imuSample &imu_delayed)
 #if defined(CONFIG_EKF2_BAROMETER)
 	updateGroundEffect();
 
-	controlBaroHeightFusion();
+	controlBaroHeightFusion(imu_delayed);
 #endif // CONFIG_EKF2_BAROMETER
 
 #if defined(CONFIG_EKF2_GNSS)
@@ -53,7 +53,7 @@ void Ekf::controlHeightFusion(const imuSample &imu_delayed)
 #endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
-	controlRangeHaglFusion();
+	controlRangeHaglFusion(imu_delayed);
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 	checkHeightSensorRefFallback();

src/modules/ekf2/EKF/position_fusion.cpp

@@ -167,7 +167,18 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v
 
 #if defined(CONFIG_EKF2_TERRAIN)
 	_state.terrain += delta_z;
-#endif
+
+	// record the state change
+	if (_state_reset_status.reset_count.hagl == _state_reset_count_prev.hagl) {
+		_state_reset_status.hagl_change = delta_z;
+
+	} else {
+		// there's already a reset this update, accumulate total delta
+		_state_reset_status.hagl_change += delta_z;
+	}
+
+	_state_reset_status.reset_count.hagl++;
+#endif // CONFIG_EKF2_TERRAIN
 
 	// Reset the timout timer
 	_time_last_hgt_fuse = _time_delayed_us;

src/modules/ekf2/EKF/python/tuning_tools/mc_wind_estimator/requirements.txt

@@ -1,6 +1,6 @@
 matplotlib==3.5.1
 numpy==1.22.2
 pyulog==0.9.0
-quaternion==3.5.2.post4
+numpy-quaternion==2023.0.4
 scipy==1.8.0
 sympy==1.10.1

src/modules/ekf2/EKF2.cpp

@@ -1191,6 +1191,9 @@ void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp)
 			global_pos.terrain_alt_valid = true;
 		}
 
+		float delta_hagl = 0.f;
+		_ekf.get_hagl_reset(&delta_hagl, &global_pos.terrain_reset_counter);
+		global_pos.delta_terrain = -delta_z;
 #endif // CONFIG_EKF2_TERRAIN
 
 		global_pos.dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning
@@ -1619,9 +1622,10 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 
 #if defined(CONFIG_EKF2_TERRAIN)
 	// Distance to bottom surface (ground) in meters, must be positive
+	lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid();
 	lpos.dist_bottom = math::max(_ekf.getHagl(), 0.f);
 	lpos.dist_bottom_var = _ekf.getTerrainVariance();
-	lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid();
+	_ekf.get_hagl_reset(&lpos.delta_dist_bottom, &lpos.dist_bottom_reset_counter);
 
 	lpos.dist_bottom_sensor_bitfield = vehicle_local_position_s::DIST_BOTTOM_SENSOR_NONE;
 
@@ -1885,36 +1889,36 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		estimator_status_flags_s status_flags{};
 		status_flags.timestamp_sample = _ekf.time_delayed_us();
 
-		status_flags.control_status_changes   = _filter_control_status_changes;
-		status_flags.cs_tilt_align            = _ekf.control_status_flags().tilt_align;
-		status_flags.cs_yaw_align             = _ekf.control_status_flags().yaw_align;
-		status_flags.cs_gps                   = _ekf.control_status_flags().gps;
-		status_flags.cs_opt_flow              = _ekf.control_status_flags().opt_flow;
-		status_flags.cs_mag_hdg               = _ekf.control_status_flags().mag_hdg;
-		status_flags.cs_mag_3d                = _ekf.control_status_flags().mag_3D;
-		status_flags.cs_mag_dec               = _ekf.control_status_flags().mag_dec;
-		status_flags.cs_in_air                = _ekf.control_status_flags().in_air;
-		status_flags.cs_wind                  = _ekf.control_status_flags().wind;
-		status_flags.cs_baro_hgt              = _ekf.control_status_flags().baro_hgt;
-		status_flags.cs_rng_hgt               = _ekf.control_status_flags().rng_hgt;
-		status_flags.cs_gps_hgt               = _ekf.control_status_flags().gps_hgt;
-		status_flags.cs_ev_pos                = _ekf.control_status_flags().ev_pos;
-		status_flags.cs_ev_yaw                = _ekf.control_status_flags().ev_yaw;
-		status_flags.cs_ev_hgt                = _ekf.control_status_flags().ev_hgt;
-		status_flags.cs_fuse_beta             = _ekf.control_status_flags().fuse_beta;
-		status_flags.cs_mag_field_disturbed   = _ekf.control_status_flags().mag_field_disturbed;
-		status_flags.cs_fixed_wing            = _ekf.control_status_flags().fixed_wing;
-		status_flags.cs_mag_fault             = _ekf.control_status_flags().mag_fault;
-		status_flags.cs_fuse_aspd             = _ekf.control_status_flags().fuse_aspd;
-		status_flags.cs_gnd_effect            = _ekf.control_status_flags().gnd_effect;
-		status_flags.cs_rng_stuck             = _ekf.control_status_flags().rng_stuck;
-		status_flags.cs_gps_yaw               = _ekf.control_status_flags().gps_yaw;
-		status_flags.cs_mag_aligned_in_flight = _ekf.control_status_flags().mag_aligned_in_flight;
-		status_flags.cs_ev_vel                = _ekf.control_status_flags().ev_vel;
-		status_flags.cs_synthetic_mag_z       = _ekf.control_status_flags().synthetic_mag_z;
-		status_flags.cs_vehicle_at_rest       = _ekf.control_status_flags().vehicle_at_rest;
-		status_flags.cs_gps_yaw_fault         = _ekf.control_status_flags().gps_yaw_fault;
-		status_flags.cs_rng_fault             = _ekf.control_status_flags().rng_fault;
+		status_flags.control_status_changes     = _filter_control_status_changes;
+		status_flags.cs_tilt_align              = _ekf.control_status_flags().tilt_align;
+		status_flags.cs_yaw_align               = _ekf.control_status_flags().yaw_align;
+		status_flags.cs_gps                     = _ekf.control_status_flags().gps;
+		status_flags.cs_opt_flow                = _ekf.control_status_flags().opt_flow;
+		status_flags.cs_mag_hdg                 = _ekf.control_status_flags().mag_hdg;
+		status_flags.cs_mag_3d                  = _ekf.control_status_flags().mag_3D;
+		status_flags.cs_mag_dec                 = _ekf.control_status_flags().mag_dec;
+		status_flags.cs_in_air                  = _ekf.control_status_flags().in_air;
+		status_flags.cs_wind                    = _ekf.control_status_flags().wind;
+		status_flags.cs_baro_hgt                = _ekf.control_status_flags().baro_hgt;
+		status_flags.cs_rng_hgt                 = _ekf.control_status_flags().rng_hgt;
+		status_flags.cs_gps_hgt                 = _ekf.control_status_flags().gps_hgt;
+		status_flags.cs_ev_pos                  = _ekf.control_status_flags().ev_pos;
+		status_flags.cs_ev_yaw                  = _ekf.control_status_flags().ev_yaw;
+		status_flags.cs_ev_hgt                  = _ekf.control_status_flags().ev_hgt;
+		status_flags.cs_fuse_beta               = _ekf.control_status_flags().fuse_beta;
+		status_flags.cs_mag_field_disturbed     = _ekf.control_status_flags().mag_field_disturbed;
+		status_flags.cs_fixed_wing              = _ekf.control_status_flags().fixed_wing;
+		status_flags.cs_mag_fault               = _ekf.control_status_flags().mag_fault;
+		status_flags.cs_fuse_aspd               = _ekf.control_status_flags().fuse_aspd;
+		status_flags.cs_gnd_effect              = _ekf.control_status_flags().gnd_effect;
+		status_flags.cs_rng_stuck               = _ekf.control_status_flags().rng_stuck;
+		status_flags.cs_gnss_yaw                = _ekf.control_status_flags().gnss_yaw;
+		status_flags.cs_mag_aligned_in_flight   = _ekf.control_status_flags().mag_aligned_in_flight;
+		status_flags.cs_ev_vel                  = _ekf.control_status_flags().ev_vel;
+		status_flags.cs_synthetic_mag_z         = _ekf.control_status_flags().synthetic_mag_z;
+		status_flags.cs_vehicle_at_rest         = _ekf.control_status_flags().vehicle_at_rest;
+		status_flags.cs_gnss_yaw_fault          = _ekf.control_status_flags().gnss_yaw_fault;
+		status_flags.cs_rng_fault               = _ekf.control_status_flags().rng_fault;
 		status_flags.cs_inertial_dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning;
 		status_flags.cs_wind_dead_reckoning     = _ekf.control_status_flags().wind_dead_reckoning;
 		status_flags.cs_rng_kin_consistent      = _ekf.control_status_flags().rng_kin_consistent;
@@ -1925,8 +1929,8 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.cs_ev_yaw_fault            = _ekf.control_status_flags().ev_yaw_fault;
 		status_flags.cs_mag_heading_consistent  = _ekf.control_status_flags().mag_heading_consistent;
 		status_flags.cs_aux_gpos                = _ekf.control_status_flags().aux_gpos;
-		status_flags.cs_rng_terrain    = _ekf.control_status_flags().rng_terrain;
-		status_flags.cs_opt_flow_terrain    = _ekf.control_status_flags().opt_flow_terrain;
+		status_flags.cs_rng_terrain             = _ekf.control_status_flags().rng_terrain;
+		status_flags.cs_opt_flow_terrain        = _ekf.control_status_flags().opt_flow_terrain;
 
 		status_flags.fault_status_changes     = _filter_fault_status_changes;
 		status_flags.fs_bad_mag_x             = _ekf.fault_status_flags().bad_mag_x;

src/modules/ekf2/EKF2Selector.cpp

@@ -479,18 +479,32 @@ void EKF2Selector::PublishVehicleLocalPosition()
 				_delta_heading_reset = matrix::wrap_pi(local_position.heading - _local_position_last.heading);
 			}
 
+			// HAGL (dist_bottom) reset
+			if (!instance_change
+			    && (local_position.dist_bottom_reset_counter == _local_position_last.dist_bottom_reset_counter + 1)) {
+				++_hagl_reset_counter;
+				_delta_hagl_reset = local_position.delta_dist_bottom;
+
+			} else if (instance_change
+				   || (local_position.dist_bottom_reset_counter != _local_position_last.dist_bottom_reset_counter)) {
+				++_hagl_reset_counter;
+				_delta_hagl_reset = local_position.dist_bottom - _local_position_last.dist_bottom;
+			}
+
 		} else {
 			_xy_reset_counter = local_position.xy_reset_counter;
 			_z_reset_counter = local_position.z_reset_counter;
 			_vxy_reset_counter = local_position.vxy_reset_counter;
 			_vz_reset_counter = local_position.vz_reset_counter;
 			_heading_reset_counter = local_position.heading_reset_counter;
+			_hagl_reset_counter = local_position.dist_bottom_reset_counter;
 
 			_delta_xy_reset = Vector2f{local_position.delta_xy};
 			_delta_z_reset = local_position.delta_z;
 			_delta_vxy_reset = Vector2f{local_position.delta_vxy};
 			_delta_vz_reset = local_position.delta_vz;
 			_delta_heading_reset = local_position.delta_heading;
+			_delta_hagl_reset = local_position.delta_dist_bottom;
 		}
 
 		bool publish = true;
@@ -513,6 +527,7 @@ void EKF2Selector::PublishVehicleLocalPosition()
 			local_position.vxy_reset_counter = _vxy_reset_counter;
 			local_position.vz_reset_counter = _vz_reset_counter;
 			local_position.heading_reset_counter = _heading_reset_counter;
+			local_position.dist_bottom_reset_counter = _hagl_reset_counter;
 
 			_delta_xy_reset.copyTo(local_position.delta_xy);
 			local_position.delta_z = _delta_z_reset;
@@ -612,6 +627,16 @@ void EKF2Selector::PublishVehicleGlobalPosition()
 				_delta_alt_reset = global_position.delta_alt - _global_position_last.delta_alt;
 			}
 
+			// terrain reset
+			if (!instance_change && (global_position.terrain_reset_counter == _global_position_last.terrain_reset_counter + 1)) {
+				++_terrain_reset_counter;
+				_delta_terrain_reset = global_position.delta_terrain;
+
+			} else if (instance_change || (global_position.terrain_reset_counter != _global_position_last.terrain_reset_counter)) {
+				++_terrain_reset_counter;
+				_delta_terrain_reset = global_position.delta_terrain - _global_position_last.delta_terrain;
+			}
+
 		} else {
 			_lat_lon_reset_counter = global_position.lat_lon_reset_counter;
 			_alt_reset_counter = global_position.alt_reset_counter;

src/modules/ekf2/EKF2Selector.hpp

@@ -202,11 +202,14 @@ private:
 	matrix::Vector2f _delta_vxy_reset{};
 	float _delta_vz_reset{0.f};
 	float _delta_heading_reset{0};
+	float _delta_hagl_reset{0.f};
+
 	uint8_t _xy_reset_counter{0};
 	uint8_t _z_reset_counter{0};
 	uint8_t _vxy_reset_counter{0};
 	uint8_t _vz_reset_counter{0};
 	uint8_t _heading_reset_counter{0};
+	uint8_t _hagl_reset_counter{0};
 
 	// vehicle_odometry
 	vehicle_odometry_s _odometry_last{};
@@ -217,8 +220,11 @@ private:
 	double _delta_lat_reset{0};
 	double _delta_lon_reset{0};
 	float _delta_alt_reset{0.f};
+	float _delta_terrain_reset{0.f};
+
 	uint8_t _lat_lon_reset_counter{0};
 	uint8_t _alt_reset_counter{0};
+	uint8_t _terrain_reset_counter{0};
 
 	// wind estimate
 	wind_s _wind_last{};

src/modules/ekf2/test/CMakeLists.txt

@@ -45,7 +45,7 @@ px4_add_unit_gtest(SRC test_EKF_flow_generated.cpp LINKLIBS ecl_EKF ecl_sensor_s
 px4_add_unit_gtest(SRC test_EKF_gyroscope.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_fusionLogic.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_gps.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
-px4_add_unit_gtest(SRC test_EKF_gps_yaw.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
+px4_add_unit_gtest(SRC test_EKF_gnss_yaw.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_gnss_yaw_generated.cpp LINKLIBS ecl_EKF ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_height_fusion.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_imuSampling.cpp LINKLIBS ecl_EKF ecl_sensor_sim)

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp

@@ -132,7 +132,7 @@ void EkfWrapper::disableGpsHeadingFusion()
 
 bool EkfWrapper::isIntendingGpsHeadingFusion() const
 {
-	return _ekf->control_status_flags().gps_yaw;
+	return _ekf->control_status_flags().gnss_yaw;
 }
 
 void EkfWrapper::enableFlowFusion()

src/modules/ekf2/test/test_EKF_basics.cpp

@@ -123,7 +123,7 @@ TEST_F(EkfBasicsTest, initialControlMode)
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_fault);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().gnd_effect);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().rng_stuck);
-	EXPECT_EQ(0, (int) _ekf->control_status_flags().gps_yaw);
+	EXPECT_EQ(0, (int) _ekf->control_status_flags().gnss_yaw);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_aligned_in_flight);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().ev_vel);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().synthetic_mag_z);
@@ -178,7 +178,7 @@ TEST_F(EkfBasicsTest, gpsFusion)
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_fault);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().gnd_effect);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().rng_stuck);
-	EXPECT_EQ(0, (int) _ekf->control_status_flags().gps_yaw);
+	EXPECT_EQ(0, (int) _ekf->control_status_flags().gnss_yaw);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_aligned_in_flight);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().ev_vel);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().synthetic_mag_z);

src/modules/ekf2/test/test_EKF_terrain.cpp

@@ -207,9 +207,9 @@ TEST_F(EkfTerrainTest, testHeightReset)
 	const float new_baro_height = _sensor_simulator._baro.getData() + 50.f;
 	_sensor_simulator._baro.setData(new_baro_height);
 	_sensor_simulator.stopGps(); // prevent from switching to GNSS height
-	_sensor_simulator.runSeconds(6);
+	_sensor_simulator.runSeconds(10);
 
-	// THEN: a height reset occured and the estimated distance to the ground remains constant
+	// THEN: a height reset occurred and the estimated distance to the ground remains constant
 	reset_logging_checker.capturePostResetState();
 	EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1));
 	EXPECT_NEAR(estimated_distance_to_ground, _ekf->getHagl(), 1e-3f);

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

@@ -632,29 +632,27 @@ bool FlightTaskAuto::_evaluateGlobalReference()
 State FlightTaskAuto::_getCurrentState()
 {
 	// Calculate the vehicle current state based on the Navigator triplets and the current position.
-	const Vector2f u_prev_to_target_xy = Vector2f(_triplet_target - _triplet_prev_wp).unit_or_zero();
-	const Vector2f pos_to_target_xy = Vector2f(_triplet_target - _position);
-	const Vector2f prev_to_pos_xy = Vector2f(_position - _triplet_prev_wp);
+	const Vector3f u_prev_to_target = (_triplet_target - _triplet_prev_wp).unit_or_zero();
+	const Vector3f prev_to_pos = _position - _triplet_prev_wp;
+	const Vector3f pos_to_target = _triplet_target - _position;
 	// Calculate the closest point to the vehicle position on the line prev_wp - target
-	const Vector2f closest_pt_xy = Vector2f(_triplet_prev_wp) + u_prev_to_target_xy * (prev_to_pos_xy *
-				       u_prev_to_target_xy);
-	_closest_pt = Vector3f(closest_pt_xy(0), closest_pt_xy(1), _triplet_target(2));
+	_closest_pt = _triplet_prev_wp + u_prev_to_target * (prev_to_pos * u_prev_to_target);
 
 	State return_state = State::none;
 
-	if (u_prev_to_target_xy.length() < FLT_EPSILON) {
+	if (!u_prev_to_target.longerThan(FLT_EPSILON)) {
 		// Previous and target are the same point, so we better don't try to do any special line following
 		return_state = State::none;
 
-	} else if (u_prev_to_target_xy * pos_to_target_xy < 0.0f) {
+	} else if (u_prev_to_target * pos_to_target < 0.0f) {
 		// Target is behind
 		return_state = State::target_behind;
 
-	} else if (u_prev_to_target_xy * prev_to_pos_xy < 0.0f && prev_to_pos_xy.longerThan(_target_acceptance_radius)) {
+	} else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.longerThan(_target_acceptance_radius)) {
 		// Previous is in front
 		return_state = State::previous_infront;
 
-	} else if (Vector2f(_position - _closest_pt).longerThan(_target_acceptance_radius)) {
+	} else if ((_position - _closest_pt).longerThan(_target_acceptance_radius)) {
 		// Vehicle too far from the track
 		return_state = State::offtrack;
 

src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp

@@ -63,6 +63,11 @@ void FlightTask::_checkEkfResetCounters()
 		_reset_counters.z = _sub_vehicle_local_position.get().z_reset_counter;
 	}
 
+	if (_sub_vehicle_local_position.get().dist_bottom_reset_counter != _reset_counters.hagl) {
+		_ekfResetHandlerHagl(_sub_vehicle_local_position.get().delta_dist_bottom);
+		_reset_counters.hagl = _sub_vehicle_local_position.get().dist_bottom_reset_counter;
+	}
+
 	if (_sub_vehicle_local_position.get().vz_reset_counter != _reset_counters.vz) {
 		_ekfResetHandlerVelocityZ(_sub_vehicle_local_position.get().delta_vz);
 		_reset_counters.vz = _sub_vehicle_local_position.get().vz_reset_counter;
@@ -138,7 +143,7 @@ void FlightTask::_evaluateVehicleLocalPosition()
 		// distance to bottom
 		if (_sub_vehicle_local_position.get().dist_bottom_valid
 		    && PX4_ISFINITE(_sub_vehicle_local_position.get().dist_bottom)) {
-			_dist_to_bottom =  _sub_vehicle_local_position.get().dist_bottom;
+			_dist_to_bottom = _sub_vehicle_local_position.get().dist_bottom;
 		}
 
 		// global frame reference coordinates to enable conversions

src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp

@@ -61,6 +61,7 @@ struct ekf_reset_counters_s {
 	uint8_t z;
 	uint8_t vz;
 	uint8_t heading;
+	uint8_t hagl;
 };
 
 class FlightTask : public ModuleParams
@@ -191,6 +192,7 @@ protected:
 	virtual void _ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) {};
 	virtual void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) {};
 	virtual void _ekfResetHandlerPositionZ(float delta_z) {};
+	virtual void _ekfResetHandlerHagl(float delta_hagl) {};
 	virtual void _ekfResetHandlerVelocityZ(float delta_vz) {};
 	virtual void _ekfResetHandlerHeading(float delta_psi) {};