EKF2: add validity flags to global pos message (#23787)

Co-authored-by: Mathieu Bresciani <[email protected]>

msg/VehicleGlobalPosition.msg

@@ -13,6 +13,9 @@ float64 lon			# Longitude, (degrees)
 float32 alt			# Altitude AMSL, (meters)
 float32 alt_ellipsoid		# Altitude above ellipsoid, (meters)
 
+bool lat_lon_valid
+bool alt_valid
+
 float32 delta_alt 	# Reset delta for altitude
 float32 delta_terrain   # Reset delta for terrain
 uint8 lat_lon_reset_counter	# Counter for reset events on horizontal position coordinates

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

@@ -746,8 +746,10 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
 		}
 	}
 
+	const bool global_pos_valid = gpos.lat_lon_valid && gpos.alt_valid;
+
 	failsafe_flags.global_position_invalid =
-		!checkPosVelValidity(now, xy_valid, gpos.eph, lpos_eph_threshold, gpos.timestamp,
+		!checkPosVelValidity(now, global_pos_valid, gpos.eph, lpos_eph_threshold, gpos.timestamp,
 				     _last_gpos_fail_time_us, !failsafe_flags.global_position_invalid);
 
 	// Additional warning if the system is about to enter position-loss failsafe after dead-reckoning period

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

@@ -202,7 +202,7 @@ void Ekf::stopBaroHgtFusion()
 #if defined(CONFIG_EKF2_BARO_COMPENSATION)
 float Ekf::compensateBaroForDynamicPressure(const imuSample &imu_sample, const float baro_alt_uncompensated) const
 {
-	if (_control_status.flags.wind && local_position_is_valid()) {
+	if (_control_status.flags.wind && isLocalHorizontalPositionValid()) {
 		// calculate static pressure error = Pmeas - Ptruth
 		// model position error sensitivity as a body fixed ellipse with a different scale in the positive and
 		// negative X and Y directions. Used to correct baro data for positional errors

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

@@ -88,7 +88,7 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 			bool origin_newer_than_last_mag = (global_origin().getProjectionReferenceTimestamp() > aid_src.time_last_fuse);
 
 			if (global_origin_valid()
-			    && (origin_newer_than_last_mag || (local_position_is_valid() && isTimedOut(_wmm_mag_time_last_checked, 10e6)))
+			    && (origin_newer_than_last_mag || (isLocalHorizontalPositionValid() && isTimedOut(_wmm_mag_time_last_checked, 10e6)))
 			   ) {
 				// position of local NED origin in GPS / WGS84 frame
 				double latitude_deg;

src/modules/ekf2/EKF/ekf.cpp

@@ -298,7 +298,7 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl
 	Vector3f pos_correction;
 
 	// apply a first order correction using velocity at the delayed time horizon and the delta time
-	if ((timestamp_observation > 0) && local_position_is_valid()) {
+	if ((timestamp_observation > 0) && isLocalHorizontalPositionValid()) {
 
 		timestamp_observation = math::min(_time_latest_us, timestamp_observation);
 

src/modules/ekf2/EKF/ekf.h

@@ -220,13 +220,17 @@ class Ekf final : public EstimatorInterface
 	// return true if the global position estimate is valid
 	// return true if the origin is set we are not doing unconstrained free inertial navigation
 	// and have not started using synthetic position observations to constrain drift
-	bool global_position_is_valid() const
+	bool isGlobalHorizontalPositionValid() const
 	{
-		return (_pos_ref.isInitialized() && local_position_is_valid());
+		return _pos_ref.isInitialized() && isLocalHorizontalPositionValid();
 	}
 
-	// return true if the local position estimate is valid
-	bool local_position_is_valid() const
+	bool isGlobalVerticalPositionValid() const
+	{
+		return _pos_ref.isInitialized() && isLocalVerticalPositionValid();
+	}
+
+	bool isLocalHorizontalPositionValid() const
 	{
 		return !_horizontal_deadreckon_time_exceeded;
 	}

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -109,7 +109,7 @@ bool Ekf::setLatLonOrigin(const double latitude, const double longitude, const f
 	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() && local_position_is_valid()) {
+	if (_pos_ref.isInitialized() && isLocalHorizontalPositionValid()) {
 		_pos_ref.reproject(_state.pos(0), _state.pos(1), current_lat, current_lon);
 		current_pos_available = true;
 	}
@@ -184,7 +184,7 @@ bool Ekf::setLatLonOriginFromCurrentPos(const double latitude, const double long
 	_pos_ref.initReference(latitude, longitude, _time_delayed_us);
 
 	// if we are already doing aiding, correct for the change in position since the EKF started navigating
-	if (local_position_is_valid()) {
+	if (isLocalHorizontalPositionValid()) {
 		double est_lat;
 		double est_lon;
 		_pos_ref.reproject(-_state.pos(0), -_state.pos(1), est_lat, est_lon);
@@ -671,16 +671,16 @@ uint16_t Ekf::get_ekf_soln_status() const
 	soln_status.flags.attitude = attitude_valid();
 
 	// 2	ESTIMATOR_VELOCITY_HORIZ	True if the horizontal velocity estimate is good
-	soln_status.flags.velocity_horiz = local_position_is_valid();
+	soln_status.flags.velocity_horiz = isLocalHorizontalPositionValid();
 
 	// 4	ESTIMATOR_VELOCITY_VERT	True if the vertical velocity estimate is good
 	soln_status.flags.velocity_vert = isLocalVerticalVelocityValid() || isLocalVerticalPositionValid();
 
 	// 8	ESTIMATOR_POS_HORIZ_REL	True if the horizontal position (relative) estimate is good
-	soln_status.flags.pos_horiz_rel = local_position_is_valid();
+	soln_status.flags.pos_horiz_rel = isLocalHorizontalPositionValid();
 
 	// 16	ESTIMATOR_POS_HORIZ_ABS	True if the horizontal position (absolute) estimate is good
-	soln_status.flags.pos_horiz_abs = global_position_is_valid();
+	soln_status.flags.pos_horiz_abs = isGlobalHorizontalPositionValid();
 
 	// 32	ESTIMATOR_POS_VERT_ABS	True if the vertical position (absolute) estimate is good
 	soln_status.flags.pos_vert_abs = isVerticalAidingActive();

src/modules/ekf2/EKF2.cpp

@@ -417,7 +417,7 @@ int EKF2::print_status(bool verbose)
 {
 	PX4_INFO_RAW("ekf2:%d EKF dt: %.4fs, attitude: %d, local position: %d, global position: %d\n",
 		     _instance, (double)_ekf.get_dt_ekf_avg(), _ekf.attitude_valid(),
-		     _ekf.local_position_is_valid(), _ekf.global_position_is_valid());
+		     _ekf.isLocalHorizontalPositionValid(), _ekf.isGlobalHorizontalPositionValid());
 
 	perf_print_counter(_ekf_update_perf);
 	perf_print_counter(_msg_missed_imu_perf);
@@ -1174,40 +1174,34 @@ void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp)
 
 		// Position of local NED origin in GPS / WGS84 frame
 		_ekf.global_origin().reproject(position(0), position(1), global_pos.lat, global_pos.lon);
+		global_pos.lat_lon_valid = _ekf.isGlobalHorizontalPositionValid();
 
 		global_pos.alt = -position(2) + _ekf.getEkfGlobalOriginAltitude(); // Altitude AMSL in meters
+		global_pos.alt_valid = _ekf.isGlobalVerticalPositionValid();
+
 #if defined(CONFIG_EKF2_GNSS)
 		global_pos.alt_ellipsoid = altAmslToEllipsoid(global_pos.alt);
-#else
-		global_pos.alt_ellipsoid = global_pos.alt;
 #endif
 
-		// delta_alt, alt_reset_counter
-		//  global altitude has opposite sign of local down position
+		// global altitude has opposite sign of local down position
 		float delta_z = 0.f;
 		uint8_t z_reset_counter = 0;
 		_ekf.get_posD_reset(&delta_z, &z_reset_counter);
 		global_pos.delta_alt = -delta_z;
 		global_pos.alt_reset_counter = z_reset_counter;
 
-		// lat_lon_reset_counter
 		float delta_xy[2] {};
 		uint8_t xy_reset_counter = 0;
 		_ekf.get_posNE_reset(delta_xy, &xy_reset_counter);
 		global_pos.lat_lon_reset_counter = xy_reset_counter;
 
 		_ekf.get_ekf_gpos_accuracy(&global_pos.eph, &global_pos.epv);
 
-		global_pos.terrain_alt = NAN;
-		global_pos.terrain_alt_valid = false;
-
 #if defined(CONFIG_EKF2_TERRAIN)
 
-		if (_ekf.isTerrainEstimateValid()) {
-			// Terrain altitude in m, WGS84
-			global_pos.terrain_alt = _ekf.getEkfGlobalOriginAltitude() - _ekf.getTerrainVertPos();
-			global_pos.terrain_alt_valid = true;
-		}
+		// Terrain altitude in m, WGS84
+		global_pos.terrain_alt = _ekf.getEkfGlobalOriginAltitude() - _ekf.getTerrainVertPos();
+		global_pos.terrain_alt_valid = _ekf.isTerrainEstimateValid();
 
 		float delta_hagl = 0.f;
 		_ekf.get_hagl_reset(&delta_hagl, &global_pos.terrain_reset_counter);
@@ -1566,8 +1560,8 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 	lpos.ay = vel_deriv(1);
 	lpos.az = vel_deriv(2);
 
-	lpos.xy_valid = _ekf.local_position_is_valid();
-	lpos.v_xy_valid = _ekf.local_position_is_valid();
+	lpos.xy_valid = _ekf.isLocalHorizontalPositionValid();
+	lpos.v_xy_valid = _ekf.isLocalHorizontalPositionValid();
 
 	// TODO: some modules (e.g.: mc_pos_control) don't handle v_z_valid != z_valid properly
 	lpos.z_valid = _ekf.isLocalVerticalPositionValid() || _ekf.isLocalVerticalVelocityValid();

src/modules/ekf2/test/test_EKF_airspeed.cpp

@@ -207,7 +207,7 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoning)
 	EXPECT_NEAR(vel_wind_earth(0), 0.f, .1f);
 	EXPECT_NEAR(vel_wind_earth(1), 0.f, .1f);
 
-	EXPECT_TRUE(_ekf->global_position_is_valid());
+	EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid());
 }
 
 TEST_F(EkfAirspeedTest, testAirspeedDeadReckoningLatLonAltReset)
@@ -239,7 +239,7 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoningLatLonAltReset)
 	_sensor_simulator.runSeconds(10.f);
 	EXPECT_TRUE(_ekf_wrapper.isIntendingAirspeedFusion());
 
-	EXPECT_TRUE(_ekf->global_position_is_valid());
+	EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid());
 
 	// WHEN: an external position reset is sent
 	ResetLoggingChecker reset_logging_checker(_ekf);
@@ -260,7 +260,7 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoningLatLonAltReset)
 	EXPECT_NEAR(altitude_est, altitude_new, 0.01f);
 	EXPECT_NEAR(latitude_est, latitude_new, 1e-3f);
 	EXPECT_NEAR(longitude_est, longitude_new, 1e-3f);
-	EXPECT_TRUE(_ekf->global_position_is_valid());
+	EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid());
 
 	reset_logging_checker.capturePostResetState();
 	EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(0));

src/modules/ekf2/test/test_EKF_basics.cpp

@@ -283,7 +283,7 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized)
 	// Global origin has been initialized but since there is no position aiding, the global
 	// position is still not valid
 	EXPECT_TRUE(_ekf->global_origin_valid());
-	EXPECT_FALSE(_ekf->global_position_is_valid());
+	EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid());
 
 	_sensor_simulator.runSeconds(1);
 
@@ -307,9 +307,9 @@ TEST_F(EkfBasicsTest, global_position_from_local_ev)
 	_sensor_simulator.runSeconds(1);
 
 	// THEN; since there is no origin, only the local position can be valid
-	EXPECT_TRUE(_ekf->local_position_is_valid());
+	EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid());
 	EXPECT_FALSE(_ekf->global_origin_valid());
-	EXPECT_FALSE(_ekf->global_position_is_valid());
+	EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid());
 
 	_latitude_new  = 45.0000005;
 	_longitude_new = 111.0000005;
@@ -320,8 +320,8 @@ TEST_F(EkfBasicsTest, global_position_from_local_ev)
 
 	// THEN: local and global positions are valid
 	EXPECT_TRUE(_ekf->global_origin_valid());
-	EXPECT_TRUE(_ekf->global_position_is_valid());
-	EXPECT_TRUE(_ekf->local_position_is_valid());
+	EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid());
+	EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid());
 }
 
 TEST_F(EkfBasicsTest, global_position_from_opt_flow)
@@ -338,9 +338,9 @@ TEST_F(EkfBasicsTest, global_position_from_opt_flow)
 	_sensor_simulator.runSeconds(1);
 
 	// THEN; since there is no origin, only the local position can be valid
-	EXPECT_TRUE(_ekf->local_position_is_valid());
+	EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid());
 	EXPECT_FALSE(_ekf->global_origin_valid());
-	EXPECT_FALSE(_ekf->global_position_is_valid());
+	EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid());
 
 	_latitude_new  = 45.0000005;
 	_longitude_new = 111.0000005;
@@ -351,8 +351,8 @@ TEST_F(EkfBasicsTest, global_position_from_opt_flow)
 
 	// THEN: local and global positions are valid
 	EXPECT_TRUE(_ekf->global_origin_valid());
-	EXPECT_TRUE(_ekf->global_position_is_valid());
-	EXPECT_TRUE(_ekf->local_position_is_valid());
+	EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid());
+	EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid());
 }
 
 // TODO: Add sampling tests

src/modules/ekf2/test/test_EKF_externalVision.cpp

@@ -85,8 +85,8 @@ TEST_F(EkfExternalVisionTest, checkVisionFusionLogic)
 	EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion());
 	EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion());
 
-	EXPECT_TRUE(_ekf->local_position_is_valid());
-	EXPECT_FALSE(_ekf->global_position_is_valid());
+	EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid());
+	EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid());
 
 	_ekf_wrapper.enableExternalVisionVelocityFusion();
 	_sensor_simulator.runSeconds(2);
@@ -95,8 +95,8 @@ TEST_F(EkfExternalVisionTest, checkVisionFusionLogic)
 	EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion());
 	EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion());
 
-	EXPECT_TRUE(_ekf->local_position_is_valid());
-	EXPECT_FALSE(_ekf->global_position_is_valid());
+	EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid());
+	EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid());
 
 	_ekf_wrapper.enableExternalVisionHeadingFusion();
 	_sensor_simulator.runSeconds(2);
@@ -105,8 +105,8 @@ TEST_F(EkfExternalVisionTest, checkVisionFusionLogic)
 	EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion());
 	EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion());
 
-	EXPECT_TRUE(_ekf->local_position_is_valid());
-	EXPECT_FALSE(_ekf->global_position_is_valid());
+	EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid());
+	EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid());
 }
 
 TEST_F(EkfExternalVisionTest, visionVelocityReset)

src/modules/ekf2/test/test_EKF_fake_pos.cpp

@@ -117,5 +117,5 @@ TEST_F(EkfFakePosTest, testValidFakePosValidLocalPos)
 	_sensor_simulator.runSeconds(60);
 
 	EXPECT_EQ(1, (int) _ekf->control_status_flags().valid_fake_pos);
-	EXPECT_TRUE(_ekf->local_position_is_valid());
+	EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid());
 }