feat(ekf2): absorb GPS altitude drift into origin instead of velocity fallback

msg/versioned/VehicleLocalPosition.msg

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

src/modules/ekf2/EKF/ekf.h

@@ -179,17 +179,11 @@ public:
 	// get the diagonal elements of the covariance matrix
 	matrix::Vector<float, State::size> covariances_diagonal() const { return P.diag(); }
 
-	template <size_t Width>
-	void uncorrelateCovariance(size_t first) { P.uncorrelateCovariance<Width>(first); }
-
-	// adjust baro bias for a GPS altitude drift correction so baro fusion
-	// doesn't slowly fight the corrected GPS reference
-	void adjustBaroBiasForDriftCorrection(float altitude_offset)
+	void shiftAltOrigin(float offset)
 	{
-#if defined(CONFIG_EKF2_BAROMETER)
-		const float delta_z = -altitude_offset;
-		_baro_b_est.setBias(_baro_b_est.getBias() + delta_z);
-#endif
+		if (PX4_ISFINITE(_local_origin_alt)) {
+			_local_origin_alt += offset;
+		}
 	}
 
 	matrix::Vector3f getRotVarBody() const;

src/modules/ekf2/EKF2.cpp

@@ -1618,7 +1618,6 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 	lpos.heading_var = _ekf.getYawVar();
 	lpos.delta_heading = Eulerf(delta_q_reset).psi();
 	lpos.heading_good_for_control = _ekf.isYawFinalAlignComplete();
-	lpos.altitude_good_for_local_control = _gps_alt_drift.altitude_good_for_local_control;
 	lpos.tilt_var = _ekf.getTiltVariance();
 
 #if defined(CONFIG_EKF2_TERRAIN)
@@ -2599,11 +2598,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 			_gps_alt_drift.update(vehicle_gps_position, _gps_alt_drift_pub);
 
 			if (fabsf(_gps_alt_drift.altitude_offset) > 0.f) {
-				_ekf.adjustBaroBiasForDriftCorrection(_gps_alt_drift.altitude_offset);
-			}
-
-			if (!_gps_alt_drift.altitude_good_for_local_control) {
-				_ekf.uncorrelateCovariance<1>(estimator::State::pos.idx + 2);
+				_ekf.shiftAltOrigin(_gps_alt_drift.altitude_offset);
 			}
 
 		} else {

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

@@ -132,7 +132,6 @@ void FlightTask::_evaluateVehicleLocalPosition()
 		_yaw = _sub_vehicle_local_position.get().heading;
 		_unaided_yaw = _sub_vehicle_local_position.get().unaided_heading;
 		_is_yaw_good_for_control = _sub_vehicle_local_position.get().heading_good_for_control;
-		_is_altitude_good_for_local_control = _sub_vehicle_local_position.get().altitude_good_for_local_control;
 
 		// position
 		if (_sub_vehicle_local_position.get().xy_valid) {

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

@@ -206,7 +206,6 @@ protected:
 	float _yaw{}; /**< current vehicle yaw heading */
 	float _unaided_yaw{};
 	bool _is_yaw_good_for_control{}; /**< true if the yaw estimate can be used for yaw control */
-	bool _is_altitude_good_for_local_control{true}; /**< true if the altitude estimate can be used for altitude control */
 	float _dist_to_bottom{}; /**< current height above ground level if dist_bottom is valid */
 	float _dist_to_ground{}; /**< equals _dist_to_bottom if available, height above home otherwise */
 

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -305,22 +305,6 @@ bool FlightTaskManualAltitude::update()
 	_updateConstraintsFromEstimator();
 	_scaleSticks();
 	_updateSetpoints();
-
-	// When altitude estimate is degraded, fall back to velocity-only control
-	if (!_is_altitude_good_for_local_control) {
-		if (_altitude_was_good_for_local_control) {
-		}
-
-		_position_setpoint(2) = NAN;
-		_dist_to_ground_lock = NAN;
-		_altitude_was_good_for_local_control = false;
-
-	} else if (!_altitude_was_good_for_local_control) {
-		// Altitude recovered: reset reference to current position
-		_position_setpoint(2) = _position(2);
-		_altitude_was_good_for_local_control = true;
-	}
-
 	_constraints.want_takeoff = _checkTakeoff();
 	_max_distance_to_ground = INFINITY;
 

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp

@@ -126,7 +126,6 @@ private:
 	bool _updateYawCorrection();
 
 	uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */
-	bool _altitude_was_good_for_local_control{true}; /**< tracks previous state for edge detection */
 
 	float _min_distance_to_ground{(float)(-INFINITY)}; /**< min distance to ground constraint */
 	float _max_distance_to_ground{(float)INFINITY};  /**< max distance to ground constraint */