flight mode manager: fix terrain hold

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

@@ -124,7 +124,6 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
 			if (stick_input || too_fast || !PX4_ISFINITE(_dist_to_bottom)) {
 				// Stop using distance to ground
 				_terrain_hold = false;
-				_terrain_follow = false;
 
 				// Adjust the setpoint to maintain the same height error to reduce control transients
 				if (PX4_ISFINITE(_dist_to_ground_lock) && PX4_ISFINITE(_dist_to_bottom)) {
@@ -141,7 +140,6 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
 			if (!stick_input && not_moving && PX4_ISFINITE(_dist_to_bottom)) {
 				// Start using distance to ground
 				_terrain_hold = true;
-				_terrain_follow = true;
 
 				// Adjust the setpoint to maintain the same height error to reduce control transients
 				if (PX4_ISFINITE(_position_setpoint(2))) {
@@ -152,7 +150,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
 
 	}
 
-	if ((_param_mpc_alt_mode.get() == 1 || _terrain_follow) && PX4_ISFINITE(_dist_to_bottom)) {
+	if ((_param_mpc_alt_mode.get() == 1 || _terrain_hold) && PX4_ISFINITE(_dist_to_bottom)) {
 		// terrain following
 		_terrainFollowing(apply_brake, stopped);
 		// respect maximum altitude

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

@@ -73,6 +73,7 @@ class FlightTaskManualAltitude : public FlightTask
 	StickYaw _stick_yaw{this};
 
 	bool _sticks_data_required = true; ///< let inherited task-class define if it depends on stick data
+	bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
 					(ParamFloat<px4::params::MPC_HOLD_MAX_Z>) _param_mpc_hold_max_z,
@@ -117,8 +118,6 @@ class FlightTaskManualAltitude : public FlightTask
 	bool _updateYawCorrection();
 
 	uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */
-	bool _terrain_follow{false}; /**< true when the vehicle is following the terrain height */
-	bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */
 
 	float _min_distance_to_ground{(float)(-INFINITY)}; /**< min distance to ground constraint */
 	float _max_distance_to_ground{(float)INFINITY};  /**< max distance to ground constraint */

src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp

@@ -106,5 +106,15 @@ void FlightTaskManualAltitudeSmoothVel::_setOutputState()
 	_jerk_setpoint(2) = _smoothing.getCurrentJerk();
 	_acceleration_setpoint(2) = _smoothing.getCurrentAcceleration();
 	_velocity_setpoint(2) = _smoothing.getCurrentVelocity();
-	_position_setpoint(2) = _smoothing.getCurrentPosition();
+
+	if (!_terrain_hold) {
+		if (_terrain_hold_previous) {
+			// Reset position setpoint to current position when switching from terrain hold to non-terrain hold
+			_smoothing.setCurrentPosition(_position(2));
+		}
+
+		_position_setpoint(2) = _smoothing.getCurrentPosition();
+	}
+
+	_terrain_hold_previous = _terrain_hold;
 }

src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp

@@ -67,4 +67,7 @@ class FlightTaskManualAltitudeSmoothVel : public FlightTaskManualAltitude
 					(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
 					(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max
 				       )
+
+private:
+	bool _terrain_hold_previous{false}; /**< true when vehicle was controlling height above a static ground position in the previous iteration */
 };

src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp

@@ -171,5 +171,15 @@ void FlightTaskManualPositionSmoothVel::_setOutputStateZ()
 	_jerk_setpoint(2) = _smoothing_z.getCurrentJerk();
 	_acceleration_setpoint(2) = _smoothing_z.getCurrentAcceleration();
 	_velocity_setpoint(2) = _smoothing_z.getCurrentVelocity();
-	_position_setpoint(2) = _smoothing_z.getCurrentPosition();
+
+	if (!_terrain_hold) {
+		if (_terrain_hold_previous) {
+			// Reset position setpoint to current position when switching from terrain hold to non-terrain hold
+			_smoothing_z.setCurrentPosition(_position(2));
+		}
+
+		_position_setpoint(2) = _smoothing_z.getCurrentPosition();
+	}
+
+	_terrain_hold_previous = _terrain_hold;
 }

src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp

@@ -87,4 +87,6 @@ class FlightTaskManualPositionSmoothVel : public FlightTaskManualPosition
 
 	ManualVelocitySmoothingXY _smoothing_xy; ///< Smoothing in x and y directions
 	ManualVelocitySmoothingZ _smoothing_z; ///< Smoothing in z direction
+
+	bool _terrain_hold_previous{false}; /**< true when vehicle was controlling height above a static ground position in the previous iteration */
 };