Add horizontal and yaw nudging for multicopter final descent

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

@@ -53,7 +53,7 @@ bool FlightTaskAuto::activate(const vehicle_local_position_setpoint_s &last_setp
 	bool ret = FlightTask::activate(last_setpoint);
 	_position_setpoint = _position;
 	_velocity_setpoint = _velocity;
-	_yaw_setpoint = _yaw_sp_prev = _yaw;
+	_yaw_setpoint = _yaw;
 	_yawspeed_setpoint = 0.0f;
 	_setDefaultConstraints();
 	return ret;
@@ -107,14 +107,15 @@ void FlightTaskAuto::_limitYawRate()
 		_yaw_sp_aligned = fabsf(matrix::wrap_pi(_yaw_setpoint - yaw_setpoint_sat)) < math::radians(_param_mis_yaw_err.get());
 
 		_yaw_setpoint = yaw_setpoint_sat;
-		_yaw_sp_prev = _yaw_setpoint;
 
 		if (!PX4_ISFINITE(_yawspeed_setpoint) && (_deltatime > FLT_EPSILON)) {
 			// Create a feedforward
 			_yawspeed_setpoint = dyaw / _deltatime;
 		}
 	}
 
+	_yaw_sp_prev = _yaw_setpoint;
+
 	if (PX4_ISFINITE(_yawspeed_setpoint)) {
 		// The yaw setpoint is aligned when its rate is not saturated
 		_yaw_sp_aligned = _yaw_sp_aligned && (fabsf(_yawspeed_setpoint) < yawrate_max);

src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.cpp

@@ -41,7 +41,8 @@
 using namespace matrix;
 
 FlightTaskAutoMapper::FlightTaskAutoMapper() :
-	_sticks(this)
+	_sticks(this),
+	_stick_acceleration_xy(this)
 {}
 
 bool FlightTaskAutoMapper::activate(const vehicle_local_position_setpoint_s &last_setpoint)
@@ -132,10 +133,46 @@ void FlightTaskAutoMapper::_prepareIdleSetpoints()
 
 void FlightTaskAutoMapper::_prepareLandSetpoints()
 {
-	// Keep xy-position and go down with landspeed
-	float land_speed = _getLandSpeed();
-	_position_setpoint = Vector3f(_target(0), _target(1), NAN);
-	_velocity_setpoint = Vector3f(Vector3f(NAN, NAN, land_speed));
+	_velocity_setpoint.setNaN(); // Don't take over any smoothed velocity setpoint
+
+	// Slow down automatic descend close to ground
+	float land_speed = math::gradual(_dist_to_ground,
+					 _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
+					 _param_mpc_land_speed.get(), _constraints.speed_down);
+
+	if (_type_previous != WaypointType::land) {
+		// initialize xy-position and yaw to waypoint such that home is reached exactly without user input
+		_land_position = Vector3f(_target(0), _target(1), NAN);
+		_land_heading = _yaw_setpoint;
+		_stick_acceleration_xy.resetPosition(Vector2f(_target(0), _target(1)));
+	}
+
+	// User input assisted landing
+	if (_param_mpc_land_rc_help.get() && _sticks.checkAndSetStickInputs()) {
+		// Stick full up -1 -> stop, stick full down 1 -> double the speed
+		land_speed *= (1 + _sticks.getPositionExpo()(2));
+
+		_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading,
+					       _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _deltatime);
+		_stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _land_heading, _position,
+				_velocity_setpoint_feedback.xy(), _deltatime);
+		_stick_acceleration_xy.getSetpoints(_land_position, _velocity_setpoint, _acceleration_setpoint);
+
+		// Hack to make sure the MPC_YAW_MODE 4 alignment doesn't stop the vehicle from descending when there's yaw input
+		if (fabsf(_yawspeed_setpoint) > FLT_EPSILON) {
+			_yaw_sp_aligned = true;
+		}
+
+	} else {
+		// Make sure we have a valid land position even in the case we loose RC while amending it
+		if (!PX4_ISFINITE(_land_position(0))) {
+			_land_position.xy() = Vector2f(_position);
+		}
+	}
+
+	_position_setpoint = _land_position; // The last element of the land position has to stay NAN
+	_yaw_setpoint = _land_heading;
+	_velocity_setpoint(2) = land_speed;
 	_gear.landing_gear = landing_gear_s::GEAR_DOWN;
 }
 
@@ -177,20 +214,3 @@ bool FlightTaskAutoMapper::_highEnoughForLandingGear()
 	// return true if altitude is above two meters
 	return _dist_to_ground > 2.0f;
 }
-
-float FlightTaskAutoMapper::_getLandSpeed()
-{
-	float land_speed = math::gradual(_dist_to_ground,
-					 _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
-					 _param_mpc_land_speed.get(), _constraints.speed_down);
-
-	// user input assisted land speed
-	if (_param_mpc_land_rc_help.get()
-	    && (_dist_to_ground < _param_mpc_land_alt1.get())
-	    && _sticks.checkAndSetStickInputs()) {
-		// stick full up -1 -> stop, stick full down 1 -> double the speed
-		land_speed *= (1 + _sticks.getPositionExpo()(2));
-	}
-
-	return land_speed;
-}

src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp

@@ -42,6 +42,8 @@
 
 #include "FlightTaskAuto.hpp"
 #include "Sticks.hpp"
+#include "StickAccelerationXY.hpp"
+#include "StickYaw.hpp"
 
 class FlightTaskAutoMapper : public FlightTaskAuto
 {
@@ -72,13 +74,17 @@ class FlightTaskAutoMapper : public FlightTaskAuto
 					_param_mpc_land_alt2, // altitude at which speed limit downwards reached minimum speed
 					(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed,
 					(ParamFloat<px4::params::MPC_TKO_RAMP_T>)
-					_param_mpc_tko_ramp_t // time constant for smooth takeoff ramp
+					_param_mpc_tko_ramp_t, // time constant for smooth takeoff ramp
+					(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max
 				       );
 
 private:
 	Sticks _sticks;
+	StickAccelerationXY _stick_acceleration_xy;
+	StickYaw _stick_yaw;
+	matrix::Vector3f _land_position;
+	float _land_heading;
 	void _reset(); /**< Resets member variables to current vehicle state */
 	WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */
 	bool _highEnoughForLandingGear(); /**< Checks if gears can be lowered. */
-	float _getLandSpeed(); /**< Returns landing descent speed. */
 };

src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp

@@ -54,6 +54,11 @@ void StickAccelerationXY::resetPosition()
 	_position_setpoint.setNaN();
 }
 
+void StickAccelerationXY::resetPosition(const matrix::Vector2f &position)
+{
+	_position_setpoint = position;
+}
+
 void StickAccelerationXY::resetVelocity(const matrix::Vector2f &velocity)
 {
 	_velocity_setpoint = velocity;

src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp

@@ -54,6 +54,7 @@ class StickAccelerationXY : public ModuleParams
 	~StickAccelerationXY() = default;
 
 	void resetPosition();
+	void resetPosition(const matrix::Vector2f &position);
 	void resetVelocity(const matrix::Vector2f &velocity);
 	void resetAcceleration(const matrix::Vector2f &acceleration);
 	void generateSetpoints(matrix::Vector2f stick_xy, const float yaw, const float yaw_sp, const matrix::Vector3f &pos,

src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp

@@ -39,10 +39,14 @@
 
 #include <px4_platform_common/defines.h>
 
+StickYaw::StickYaw()
+{
+	_yawspeed_slew_rate.setSlewRate(2.f * M_PI_F);
+}
+
 void StickYaw::generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, const float desired_yawspeed,
 				   const float yaw, const float deltatime)
 {
-	_yawspeed_slew_rate.setSlewRate(2.f * M_PI_F);
 	yawspeed_setpoint = _yawspeed_slew_rate.update(desired_yawspeed, deltatime);
 	yaw_setpoint = updateYawLock(yaw, yawspeed_setpoint, yaw_setpoint);
 }

src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp

@@ -44,7 +44,7 @@
 class StickYaw
 {
 public:
-	StickYaw() = default;
+	StickYaw();
 	~StickYaw() = default;
 
 	void generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, const float desired_yawspeed, const float yaw,

src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp

@@ -42,7 +42,7 @@ using namespace matrix;
 
 Sticks::Sticks(ModuleParams *parent) :
 	ModuleParams(parent)
-{};
+{}
 
 bool Sticks::checkAndSetStickInputs()
 {