ekf2: setEkfGlobalOrigin() reset baro and hgt sensor offsets if necessary

- handle uninitalized _gps_alt_ref
 - add basic lat/lon/alt sanity checks

src/modules/ekf2/EKF/ekf.h

@@ -169,9 +169,9 @@ public:
 	// get the ekf WGS-84 origin position and height and the system time it was last set
 	// return true if the origin is valid
 	bool getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const;
-	void setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude);
+	bool setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude);
 
-	float getEkfGlobalOriginAltitude() const { return _gps_alt_ref; }
+	float getEkfGlobalOriginAltitude() const { return PX4_ISFINITE(_gps_alt_ref) ? _gps_alt_ref : 0.f; }
 	bool setEkfGlobalOriginAltitude(const float altitude);
 
 
@@ -521,7 +521,7 @@ private:
 
 	// Variables used to publish the WGS-84 location of the EKF local NED origin
 	uint64_t _last_gps_origin_time_us{0};	///< time the origin was last set (uSec)
-	float _gps_alt_ref{0.0f};		///< WGS-84 height (m)
+	float _gps_alt_ref{NAN};		///< WGS-84 height (m)
 
 	// Variables used by the initial filter alignment
 	bool _is_first_imu_sample{true};

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -256,7 +256,7 @@ void Ekf::resetHeightToGps()
 	_information_events.flags.reset_hgt_to_gps = true;
 
 	const float z_pos_before_reset = _state.pos(2);
-	resetVerticalPositionTo(-_gps_sample_delayed.hgt + _gps_alt_ref);
+	resetVerticalPositionTo(-_gps_sample_delayed.hgt + getEkfGlobalOriginAltitude());
 
 	// the state variance is the same as the observation
 	P.uncorrelateCovarianceSetVariance<1>(9, getGpsHeightVariance());
@@ -695,40 +695,62 @@ bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &lo
 	origin_time = _last_gps_origin_time_us;
 	latitude = _pos_ref.getProjectionReferenceLat();
 	longitude = _pos_ref.getProjectionReferenceLon();
-	origin_alt  = _gps_alt_ref;
+	origin_alt  = getEkfGlobalOriginAltitude();
 	return _NED_origin_initialised;
 }
 
-void Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude)
+bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude)
 {
-	bool current_pos_available = false;
-	double current_lat = static_cast<double>(NAN);
-	double current_lon = static_cast<double>(NAN);
-	float current_alt  = 0.f;
+	// sanity check valid latitude/longitude and altitude anywhere between the Mariana Trench and edge of Space
+	if (PX4_ISFINITE(latitude) && (abs(latitude) <= 90)
+	&& PX4_ISFINITE(longitude) && (abs(longitude) <= 180)
+	&& PX4_ISFINITE(altitude) && (altitude > -12'000.f) && (altitude < 100'000.f)
+	) {
+		bool current_pos_available = false;
+		double current_lat = static_cast<double>(NAN);
+		double current_lon = static_cast<double>(NAN);
 
-	// if we are already doing aiding, correct for the change in position since the EKF started navigating
-	if (_pos_ref.isInitialized() && isHorizontalAidingActive()) {
-		_pos_ref.reproject(_state.pos(0), _state.pos(1), current_lat, current_lon);
-		current_alt = -_state.pos(2) + _gps_alt_ref;
-		current_pos_available = true;
+		// if we are already doing aiding, correct for the change in position since the EKF started navigating
+		if (_pos_ref.isInitialized() && isHorizontalAidingActive()) {
+			_pos_ref.reproject(_state.pos(0), _state.pos(1), current_lat, current_lon);
+			current_pos_available = true;
+		}
+
+		const float gps_alt_ref_prev = getEkfGlobalOriginAltitude();
+
+		// reinitialize map projection to latitude, longitude, altitude, and reset position
+		_pos_ref.initReference(latitude, longitude, _time_last_imu);
+		_gps_alt_ref = altitude;
+
+		// minimum change in position or height that triggers a reset
+		static constexpr float MIN_RESET_DIST_M = 0.01f;
+
+		if (current_pos_available) {
+			// reset horizontal position
+			Vector2f position = _pos_ref.project(current_lat, current_lon);
+
+			if (Vector2f(position - Vector2f(_state.pos)).longerThan(MIN_RESET_DIST_M)) {
+				resetHorizontalPositionTo(position);
+			}
+		}
+
+		// reset vertical position (if there's any change)
+		if (fabsf(altitude - gps_alt_ref_prev) > MIN_RESET_DIST_M) {
+			// determine current z
+			float current_alt = -_state.pos(2) + gps_alt_ref_prev;
+
+			resetVerticalPositionTo(_gps_alt_ref - current_alt);
+
+			_baro_hgt_offset -= _state_reset_status.posD_change;
+
+			_rng_hgt_offset -= _state_reset_status.posD_change;
+			_ev_hgt_offset -= _state_reset_status.posD_change;
+		}
+
+		return true;
 	}
 
-	// reinitialize map projection to latitude, longitude, altitude, and reset position
-	_pos_ref.initReference(latitude, longitude, _time_last_imu);
-
-	if (current_pos_available) {
-		// reset horizontal position
-		Vector2f position = _pos_ref.project(current_lat, current_lon);
-		resetHorizontalPositionTo(position);
-
-		// reset altitude
-		_gps_alt_ref = altitude;
-		resetVerticalPositionTo(_gps_alt_ref - current_alt);
-
-	} else {
-		// reset altitude
-		_gps_alt_ref = altitude;
-	}
+	return false;
 }
 
 // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
@@ -1283,7 +1305,7 @@ void Ekf::startGpsHgtFusion()
 			// switch out of range aid
 			// calculate height sensor offset such that current
 			// measurement matches our current height estimate
-			_gps_hgt_offset = (_gps_sample_delayed.hgt - _gps_alt_ref) + _state.pos(2);
+			_gps_hgt_offset = _gps_sample_delayed.hgt - getEkfGlobalOriginAltitude() + _state.pos(2);
 
 		} else {
 			_gps_hgt_offset = 0.f;
@@ -1381,7 +1403,7 @@ void Ekf::updateBaroHgtBias()
 	    && !_baro_hgt_faulty) {
 		// Use GPS altitude as a reference to compute the baro bias measurement
 		const float baro_bias = (_baro_sample_delayed.hgt - _baro_hgt_offset)
-					- (_gps_sample_delayed.hgt - _gps_alt_ref);
+					- (_gps_sample_delayed.hgt - getEkfGlobalOriginAltitude());
 		const float baro_bias_var = getGpsHeightVariance() + sq(_params.baro_noise);
 		_baro_b_est.fuseBias(baro_bias, baro_bias_var);
 	}

src/modules/ekf2/EKF/gps_checks.cpp

@@ -78,7 +78,10 @@ bool Ekf::collect_gps(const gpsMessage &gps)
 		}
 
 		// Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin
-		_gps_alt_ref = 1e-3f * (float)gps.alt + _state.pos(2);
+		if (!PX4_ISFINITE(_gps_alt_ref)) {
+			_gps_alt_ref = 1e-3f * (float)gps.alt + _state.pos(2);
+		}
+
 		_NED_origin_initialised = true;
 
 		_earth_rate_NED = calcEarthRateNED((float)math::radians(_pos_ref.getProjectionReferenceLat()));

src/modules/ekf2/EKF/gps_fusion.cpp

@@ -77,7 +77,7 @@ void Ekf::updateGpsPos(const gpsSample &gps_sample)
 	position(1) = gps_sample.pos(1);
 
 	// vertical position - gps measurement has opposite sign to earth z axis
-	position(2) = -(gps_sample.hgt - _gps_alt_ref - _gps_hgt_offset);
+	position(2) = -(gps_sample.hgt - getEkfGlobalOriginAltitude() - _gps_hgt_offset);
 
 	const float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
 

src/modules/ekf2/EKF2.cpp

@@ -343,18 +343,20 @@ void EKF2::Run()
 
 		if (_vehicle_command_sub.update(&vehicle_command)) {
 			if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN) {
-				if (!_ekf.control_status_flags().in_air) {
-
-					uint64_t origin_time {};
-					double latitude = vehicle_command.param5;
-					double longitude = vehicle_command.param6;
-					float altitude = vehicle_command.param7;
-
-					_ekf.setEkfGlobalOrigin(latitude, longitude, altitude);
+				double latitude = vehicle_command.param5;
+				double longitude = vehicle_command.param6;
+				float altitude = vehicle_command.param7;
 
+				if (_ekf.setEkfGlobalOrigin(latitude, longitude, altitude)) {
 					// Validate the ekf origin status.
+					uint64_t origin_time {};
 					_ekf.getEkfGlobalOrigin(origin_time, latitude, longitude, altitude);
-					PX4_INFO("New NED origin (LLA): %3.10f, %3.10f, %4.3f\n", latitude, longitude, static_cast<double>(altitude));
+					PX4_INFO("%d - New NED origin (LLA): %3.10f, %3.10f, %4.3f\n",
+						 _instance, latitude, longitude, static_cast<double>(altitude));
+
+				} else {
+					PX4_ERR("%d - Failed to set new NED origin (LLA): %3.10f, %3.10f, %4.3f\n",
+						_instance, latitude, longitude, static_cast<double>(altitude));
 				}
 			}
 		}