feat(control): velocity-only altitude hold during gnss altitude drift correction

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

@@ -132,6 +132,7 @@ 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,6 +206,7 @@ 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,6 +305,22 @@ 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,6 +126,7 @@ 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 */