added RC Landing nudging

src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.cpp

@@ -67,11 +67,9 @@ FlightTaskLand::activate(const trajectory_setpoint_s &last_setpoint)
 	_position_setpoint = pos_prev;
 
 
+
 	// Initialize the Landing locations and parameters
-
 	// calculate where to land based on the current velocity and acceleration constraints
-	// set this as the target location for position smoothing
-
 	_CalculateBrakingLocation();
 	_initial_land_position = _land_position;
 
@@ -82,7 +80,6 @@ void
 FlightTaskLand::reActivate()
 {
 	FlightTask::reActivate();
-	PX4_ERR("FlightTaskLand reActivate was called!");
 	// On ground, reset acceleration and velocity to zero
 	_position_smoothing.reset({0.f, 0.f, 0.f}, {0.f, 0.f, 0.7f}, _position);
 }
@@ -93,27 +90,18 @@ FlightTaskLand::update()
 	bool ret = FlightTask::update();
 
 	if (!_is_initialized) {
-		PX4_ERR("initializing _position_smoothing");
-		PX4_ERR("_velocity_setpoint: %f, %f, %f", (double)_velocity_setpoint(0), (double)_velocity_setpoint(1),
-			(double)_velocity_setpoint(2));
-		PX4_ERR("_position_setpoint: %f, %f, %f", (double)_position_setpoint(0), (double)_position_setpoint(1),
-			(double)_position_setpoint(2));
-		PX4_ERR("_acceleration_setpoint: %f, %f, %f", (double)_acceleration_setpoint(0), (double)_acceleration_setpoint(1),
-			(double)_acceleration_setpoint(2));
 		_position_smoothing.reset(_acceleration_setpoint, _velocity, _position);
 		_is_initialized = true;
 	}
 
-
+	if (_velocity.norm() < 1.f && !_landing) {
+		_landing = true;
+	}
 
 	if (_landing) {
 		_PerformLanding();
-		PX4_WARN("_position: 		%f, 	%f, 	%f", (double)_position(0), (double)_position(1), (double)_position(2));
-		PX4_WARN("_land_position: 		%f, 	%f, 	%f", (double)_land_position(0), (double)_land_position(1),
-			 (double)_land_position(2));
 
 	} else {
-		//_CalculateBrakingLocation();
 		_SmoothBrakingPath();
 	}
 
@@ -138,6 +126,54 @@ FlightTaskLand::_PerformLanding()
 		vertical_speed = _param_mpc_land_crawl_speed.get();
 	}
 
+	// User input assisted landing
+	if (_param_mpc_land_rc_help.get() && _sticks.checkAndUpdateStickInputs()) {
+		// Stick full up -1 -> stop, stick full down 1 -> double the speed
+		vertical_speed *= (1 - _sticks.getThrottleZeroCenteredExpo());
+
+		if (fabsf(_sticks.getYawExpo()) > FLT_EPSILON) {
+			_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading, _sticks.getYawExpo(), _yaw, _deltatime);
+		}
+
+		Vector2f sticks_xy = _sticks.getPitchRollExpo();
+		Vector2f sticks_ne = sticks_xy;
+		Sticks::rotateIntoHeadingFrameXY(sticks_ne, _yaw, _land_heading);
+
+		const float distance_to_circle = math::trajectory::getMaxDistanceToCircle(_position.xy(), _initial_land_position.xy(),
+						 _param_mpc_land_radius.get(), sticks_ne);
+		float max_speed;
+
+		if (PX4_ISFINITE(distance_to_circle)) {
+			max_speed = math::trajectory::computeMaxSpeedFromDistance(_stick_acceleration_xy.getMaxJerk(),
+					_stick_acceleration_xy.getMaxAcceleration(), distance_to_circle, 0.f);
+
+			if (max_speed < 0.5f) {
+				sticks_xy.setZero();
+			}
+
+		} else {
+			max_speed = 0.f;
+			sticks_xy.setZero();
+		}
+
+		// If ground distance estimate valid (distance sensor) during nudging then limit horizontal speed
+		if (PX4_ISFINITE(_dist_to_bottom)) {
+			// Below 50cm no horizontal speed, above allow per meter altitude 0.5m/s speed
+			max_speed = math::max(0.f, math::min(max_speed, (_dist_to_bottom - .5f) * .5f));
+		}
+
+		_stick_acceleration_xy.setVelocityConstraint(max_speed);
+		_stick_acceleration_xy.generateSetpoints(sticks_xy, _yaw, _land_heading, _position,
+				_velocity_setpoint_feedback.xy(), _deltatime);
+		_stick_acceleration_xy.getSetpoints(_land_position, _velocity_setpoint, _acceleration_setpoint);
+
+	} 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(0), _land_position(1), NAN}; // The last element of the land position has to stay NAN
 	_yaw_setpoint = _land_heading;
 	_velocity_setpoint(2) = vertical_speed;
@@ -154,37 +190,24 @@ FlightTaskLand::_SmoothBrakingPath()
 	_position_smoothing.generateSetpoints(
 		_position,
 		_land_position,
-	{0.f, 0.f, 0.f},
-	_deltatime,
-	false,
-	out_setpoints
+		Vector3f{0.f, 0.f, 0.f},
+		_deltatime,
+		false,
+		out_setpoints
 	);
 
 	_jerk_setpoint = out_setpoints.jerk;
 	_acceleration_setpoint = out_setpoints.acceleration;
 	_velocity_setpoint = out_setpoints.velocity;
-	//_velocity_setpoint(2) = 0;
 	_position_setpoint = out_setpoints.position;
 	_yaw_setpoint = _land_heading;
-	PX4_WARN("_position_setpoint: 	%f, 	%f, 	%f", (double)_position_setpoint(0), (double)_position_setpoint(1),
-		 (double)_position_setpoint(2));
-	PX4_WARN("_land_position: 	%f, 	%f, 	%f", (double)_land_position(0), (double)_land_position(1),
-		 (double)_land_position(2));
-
-	// should be 3d vel?
-	if (_velocity.xy().norm() <
-	    _param_nav_mc_alt_rad.get()) { //} || _velocity(2) < _param_mpc_z_v_auto_dn.get()){ // not sure about the last parts, check!)
-		_landing = true;
-		PX4_WARN("Landing: Vehicle is not moving, perform landing");
-	}
-
 }
 
 void
 FlightTaskLand::_CalculateBrakingLocation()
 {
 	// Calculate the 3D point where we until where we can slow down smoothly and then land based on the current velocities and system constraints on jerk and acceleration.
-	_updateTrajConstraints();
+	_UpdateTrajConstraints();
 	float delay_scale = 0.4f; // delay scale factor
 	const float velocity_hor_abs = sqrtf(_velocity(0) * _velocity(0) + _velocity(1) * _velocity(1));
 	const float braking_dist_xy = math::trajectory::computeBrakingDistanceFromVelocity(velocity_hor_abs,
@@ -192,12 +215,10 @@ FlightTaskLand::_CalculateBrakingLocation()
 	float braking_dist_z = 0.0f;
 
 	if (_velocity(2) < -0.1f) {
-		PX4_WARN("Moving upwards: %f", (double)_velocity(2));
 		braking_dist_z = math::trajectory::computeBrakingDistanceFromVelocity(_velocity(2),
 				 _param_mpc_jerk_auto.get(), 9.81f +  _param_mpc_acc_down_max.get(), delay_scale * _param_mpc_jerk_auto.get());
 
 	} else if (_velocity(2) > 0.1f) {
-		PX4_WARN("Moving downwards");
 		braking_dist_z = math::trajectory::computeBrakingDistanceFromVelocity(_velocity(2),
 				 _param_mpc_jerk_auto.get(), _param_mpc_acc_up_max.get(), delay_scale * _param_mpc_jerk_auto.get());
 	}
@@ -226,11 +247,12 @@ FlightTaskLand::_HandleHighVelocities()
 		_exceeded_max_vel = false;
 		_position_smoothing.reset(_acceleration_setpoint, _velocity, _position);
 		_CalculateBrakingLocation();
+		_initial_land_position = _land_position;
 	}
 }
 
 void
-FlightTaskLand::_updateTrajConstraints()
+FlightTaskLand::_UpdateTrajConstraints()
 {
 	// update params of the position smoothing
 	_position_smoothing.setCruiseSpeed(_param_mpc_xy_vel_max.get());
@@ -247,17 +269,14 @@ FlightTaskLand::_updateTrajConstraints()
 	// set the constraints for the vertical direction
 	// if moving up, acceleration constraint is always in deceleration direction, eg opposite to the velocity
 	if (_velocity(2) < 0.0f && !_landing) {
-		PX4_ERR("Moving upwards: %f", (double)_velocity(2));
 		_position_smoothing.setMaxAccelerationZ(_param_mpc_acc_down_max.get());
 		_position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_up.get());
 
 	} else if (!_landing) {
-		PX4_ERR("Moving downwards %f", (double)_velocity(2));
 		_position_smoothing.setMaxAccelerationZ(_param_mpc_acc_up_max.get());
 		_position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_dn.get());
 
 	} else {
-		PX4_ERR("Not moving Vertially");
 		_position_smoothing.setMaxAccelerationZ(_param_mpc_acc_down_max.get());
 		_position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_dn.get());
 	}

src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.hpp

@@ -47,6 +47,8 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include "Sticks.hpp"
+#include "StickAccelerationXY.hpp"
+#include "StickYaw.hpp"
 
 using matrix::Vector3f;
 using matrix::Vector2f;
@@ -59,12 +61,13 @@ public:
 
 	bool update() override;
 	bool activate(const trajectory_setpoint_s &last_setpoint) override;
-
 	void reActivate() override;
 
 protected:
 	PositionSmoothing _position_smoothing;
 	Sticks _sticks{this};
+	StickAccelerationXY _stick_acceleration_xy{this};
+	StickYaw _stick_yaw{this};
 
 	uORB::SubscriptionData<home_position_s>		_sub_home_position{ORB_ID(home_position)};
 	uORB::SubscriptionData<vehicle_status_s>	_sub_vehicle_status{ORB_ID(vehicle_status)};
@@ -79,6 +82,8 @@ protected:
 					(ParamFloat<px4::params::MPC_LAND_ALT2>) 	_param_mpc_land_alt2,
 					(ParamFloat<px4::params::MPC_LAND_ALT3>) 	_param_mpc_land_alt3,
 					(ParamFloat<px4::params::MPC_LAND_CRWL>) 	_param_mpc_land_crawl_speed,
+					(ParamFloat<px4::params::MPC_LAND_RADIUS>) 	_param_mpc_land_radius,
+					(ParamInt<px4::params::MPC_LAND_RC_HELP>) 	_param_mpc_land_rc_help,
 					(ParamFloat<px4::params::MPC_LAND_SPEED>) 	_param_mpc_land_speed,
 					(ParamFloat<px4::params::MPC_XY_ERR_MAX>) 	_param_mpc_xy_err_max,
 					(ParamFloat<px4::params::MPC_XY_TRAJ_P>) 	_param_mpc_xy_traj_p,
@@ -92,11 +97,12 @@ private:
 	void _HandleHighVelocities();
 	void _PerformLanding();
 	void _SmoothBrakingPath();
-	void _updateTrajConstraints();
+	void _UpdateTrajConstraints();
 
 	bool _landing{false};
 	bool _is_initialized{false};
 	bool _exceeded_max_vel{false};
+
 	Vector3f _initial_land_position;
 	Vector3f _land_position;
 	float _land_heading{0.0f};