replace reset counter logic for estimator reset for manual controlled mode

src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp

@@ -82,7 +82,15 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
 			// ensure that minimum altitude is respected
 			_respectMinAltitude();
 
-		} else if (!apply_brake) {
+		} else if (PX4_ISFINITE(_position_setpoint(2)) && apply_brake) {
+			// Position is locked but check if a reset event has happened.
+			// We will shift the setpoints.
+			if (_sub_vehicle_local_position->get().z_reset_counter != _reset_counter) {
+				_position_setpoint(2) = _position(2);
+				_reset_counter = _sub_vehicle_local_position->get().z_reset_counter;
+			}
+
+		} else  {
 			// user demands velocity change
 			_position_setpoint(2) = NAN;
 		}

src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp

@@ -65,6 +65,8 @@ protected:
 					(ParamInt<px4::params::MPC_ALT_MODE>) MPC_ALT_MODE
 				       )
 private:
+	uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */
+
 	/**
 	 * Distance to ground during terrain following.
 	 * If user does not demand velocity change in D-direction and the vehcile

src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp

@@ -111,7 +111,17 @@ void FlightTaskManualPosition::_updateXYlock()
 		_position_setpoint(0) = _position(0);
 		_position_setpoint(1) = _position(1);
 
-	} else if (!apply_brake) {
+	} else if (PX4_ISFINITE(_position_setpoint(0)) && apply_brake) {
+		// Position is locked but check if a reset event has happened.
+		// We will shift the setpoints.
+		if (_sub_vehicle_local_position->get().xy_reset_counter
+				!= _reset_counter) {
+			_position_setpoint(0) = _position(0);
+			_position_setpoint(1) = _position(1);
+			_reset_counter = _sub_vehicle_local_position->get().xy_reset_counter;
+		}
+
+	} else {
 		/* don't lock*/
 		_position_setpoint(0) = NAN;
 		_position_setpoint(1) = NAN;

src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp

@@ -63,4 +63,5 @@ protected:
 				       )
 private:
 	float _velocity_scale; //scales the stick input to velocity
+	uint8_t _reset_counter = 0; /**< counter for estimator resets in xy-direction */
 };