This commit splits out Landing out of the FlightTask Auto, any by tha…

…t is able to compensate initial velocities better.

src/modules/flight_mode_manager/FlightModeManager.cpp

@@ -153,6 +153,7 @@ void FlightModeManager::start_flight_task()
 	bool matching_task_running = true;
 	bool task_failure = false;
 	const bool nav_state_descend = (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND);
+	const bool nav_state_land = (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_LAND);
 
 	// Follow me
 	if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET) {
@@ -186,7 +187,7 @@ void FlightModeManager::start_flight_task()
 	}
 
 	// Landing
-	if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_LAND) {
+	if (nav_state_land) {
 		found_some_task = true;
 		FlightTaskError error = switchTask(FlightTaskIndex::Land);
 
@@ -198,7 +199,7 @@ void FlightModeManager::start_flight_task()
 
 	// Navigator interface for autonomous modes
 	if (_vehicle_control_mode_sub.get().flag_control_auto_enabled
-	    && !nav_state_descend) {
+	    && !nav_state_descend && !nav_state_land) {
 		found_some_task = true;
 
 		if (switchTask(FlightTaskIndex::Auto) != FlightTaskError::NoError) {

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

@@ -231,6 +231,8 @@ void FlightTaskAuto::rcHelpModifyYaw(float &yaw_sp)
 
 void FlightTaskAuto::_prepareLandSetpoints()
 {
+	// IMPORTANT: This Landing logic is only used for Precision Landing, for the other Landing see FlightTaskLand
+
 	_velocity_setpoint.setNaN(); // Don't take over any smoothed velocity setpoint
 
 	// Slow down automatic descend close to ground

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

@@ -44,7 +44,6 @@ FlightTaskLand::activate(const trajectory_setpoint_s &last_setpoint)
 	Vector3f vel_prev{last_setpoint.velocity};
 	Vector3f pos_prev{last_setpoint.position};
 	Vector3f accel_prev{last_setpoint.acceleration};
-	float yaw_prev = last_setpoint.yawspeed;
 
 	for (int i = 0; i < 3; i++) {
 		// If the position setpoint is unknown, set to the current position
@@ -56,84 +55,204 @@ FlightTaskLand::activate(const trajectory_setpoint_s &last_setpoint)
 		// If no acceleration estimate available, set to zero if the setpoint is NAN
 		if (!PX4_ISFINITE(accel_prev(i))) { accel_prev(i) = 0.f; }
 
-		// If the yaw setpoint is unknown, set to the current yawq
-		if (!PX4_ISFINITE(yaw_prev)) { yaw_prev = _yaw; }
+
 	}
 
+	_yaw_setpoint  = _land_heading = _yaw; // set the yaw setpoint to the current yaw
 	_position_smoothing.reset(pos_prev, vel_prev, accel_prev);
-	_yaw_setpoint = yaw_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
-	_updateTrajConstraints();
 
+	_acceleration_setpoint = accel_prev;
+	_velocity_setpoint = vel_prev;
+	_position_setpoint = pos_prev;
+
+
+
+	// Initialize the Landing locations and parameters
+	// calculate where to land based on the current velocity and acceleration constraints
+	_CalculateBrakingLocation();
+	_initial_land_position = _land_position;
 
-	PX4_INFO("FlightTaskMyTask activate was called! ret: %d", ret); // report if activation was successful
 	return ret;
 }
 
+void
+FlightTaskLand::reActivate()
+{
+	FlightTask::reActivate();
+	// On ground, reset acceleration and velocity to zero
+	_position_smoothing.reset({0.f, 0.f, 0.f}, {0.f, 0.f, 0.7f}, _position);
+}
+
 bool
 FlightTaskLand::update()
 {
+	bool ret = FlightTask::update();
 
-	// Check if we have a velocity
-	_landing = _velocity.xy().norm() > 0.1f
-		   || _velocity(2) < _param_mpc_z_v_auto_dn.get(); // not sure about the last parts, check!
+	if (!_is_initialized) {
+		_position_smoothing.reset(_acceleration_setpoint, _velocity, _position);
+		_is_initialized = true;
+	}
 
-	if (!_landing) { // smaller velocity as positive altitude is negative distance
-		PX4_WARN("Landing: Vehicle is moving, slow down first");
+	if (_velocity.norm() < 0.1f * _param_mpc_xy_vel_max.get() && !_landing) {
+		_landing = true;
+	}
 
+	if (_landing) {
+		_PerformLanding();
 
 	} else {
-		CalculateLandingLocation();
-		PositionSmoothing::PositionSmoothingSetpoints smoothed_setpoints;
-		_position_smoothing.generateSetpoints(
-			_position,
-			_initial_land_position,
-		{0, 0, 0}, _deltatime,
-		false,
-		smoothed_setpoints
-		);
+		_SmoothBrakingPath();
+	}
+
+	return ret;
+}
+
+void
+FlightTaskLand::_PerformLanding()
+{
+	// Perform 3 phase landing
+	_velocity_setpoint.setNaN();
+
+	// Calculate the vertical speed based on the distance to the ground
+	float vertical_speed = math::interpolate(_dist_to_ground,
+			       _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
+			       _param_mpc_land_speed.get(), _param_mpc_z_vel_max_dn.get());
+
+	bool range_dist_available = PX4_ISFINITE(_dist_to_bottom);
+
+	// If we are below the altitude of the third landing phase , use the crawl speed
+	if (range_dist_available && _dist_to_bottom <= _param_mpc_land_alt3.get()) {
+		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);
+		}
 	}
 
-	// update the yaw setpoint
+	_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;
+	_gear.landing_gear = landing_gear_s::GEAR_DOWN;
+
+}
+void
+FlightTaskLand::_SmoothBrakingPath()
+{
+	PositionSmoothing::PositionSmoothingSetpoints out_setpoints;
+
+	_HandleHighVelocities();
+
+	_position_smoothing.generateSetpoints(
+		_position,
+		_land_position,
+		Vector3f{0.f, 0.f, 0.f},
+		_deltatime,
+		false,
+		out_setpoints
+	);
 
-	PX4_INFO("FlightTaskMyTask update was called!"); // report update
-	return true;
+	_jerk_setpoint = out_setpoints.jerk;
+	_acceleration_setpoint = out_setpoints.acceleration;
+	_velocity_setpoint = out_setpoints.velocity;
+	_position_setpoint = out_setpoints.position;
+	_yaw_setpoint = _land_heading;
 }
 
 void
-FlightTaskLand::CalculateLandingLocation()
+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.
-
-	float delay_scale = 0.2f; // delay scale factor
-	const float braking_dist_xy = math::trajectory::computeBrakingDistanceFromVelocity(_velocity.xy().norm(),
+	_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,
 				      _param_mpc_jerk_auto.get(), _param_mpc_acc_hor.get(), delay_scale * _param_mpc_jerk_auto.get());
 	float braking_dist_z = 0.0f;
 
-	if (_velocity(2) < 0.0f) {
-		PX4_WARN("Moving upwards");
+	if (_velocity(2) < -0.1f) {
 		braking_dist_z = math::trajectory::computeBrakingDistanceFromVelocity(_velocity(2),
-				 _param_mpc_jerk_auto.get(), _param_mpc_acc_down_max.get(), delay_scale * _param_mpc_jerk_auto.get());
+				 _param_mpc_jerk_auto.get(), 9.81f +  _param_mpc_acc_down_max.get(), delay_scale * _param_mpc_jerk_auto.get());
 
-	} else {
-		PX4_WARN("Moving downwards");
+	} else if (_velocity(2) > 0.1f) {
 		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());
 	}
 
 	const Vector3f braking_dir = _velocity.unit_or_zero();
 	const Vector3f braking_dist = {braking_dist_xy, braking_dist_xy, braking_dist_z};
+	_land_position = _position + braking_dir.emult(braking_dist);
+}
+
+
+void
+FlightTaskLand::_HandleHighVelocities()
+{
+	// This logic here is to fix the problem that the trajectory generator will generate a smoot trajectory from the current velocity to zero velocity,
+	// but if the velocity is too high, the Position Controller will be able to comand higher accelerations than set by the parameter which means the vehicle will break faster than expected predicted by the trajectory generator.
+	// But if we then do a reset the deceleration will be smooth again.
+	const bool _exceeded_vel_z = fabsf(_velocity(2)) > math::max(_param_mpc_z_v_auto_dn.get(),
+				     _param_mpc_z_vel_max_up.get());
+	const bool _exceeded_vel_xy = _velocity.xy().norm() > _param_mpc_xy_vel_max.get();
+
+	if ((_exceeded_vel_xy || _exceeded_vel_z) && !_exceeded_max_vel) {
+		_exceeded_max_vel = true;
 
-	_initial_land_position = _position + braking_dir * braking_dist;
-	PX4_INFO("FlightTaskMyTask CalculateLandingLocation was called!"); // report calculation
+	} else if ((!_exceeded_vel_xy  && !_exceeded_vel_z)  && _exceeded_max_vel) {
+		// This Reset will be called when the velocity is again in the normal range and will be called only once.
+		_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());
@@ -161,6 +280,13 @@ FlightTaskLand::_updateTrajConstraints()
 		_position_smoothing.setMaxAccelerationZ(_param_mpc_acc_down_max.get());
 		_position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_dn.get());
 	}
+}
+
+void
+FlightTaskLand::updateParams()
+{
+	FlightTask::updateParams();
 
-	// should the constraints be different when switching from an auto mode compared to an manual mode?
+	// make sure that alt1 is above alt2
+	_param_mpc_land_alt1.set(math::max(_param_mpc_land_alt1.get(), _param_mpc_land_alt2.get()));
 }

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

@@ -43,7 +43,12 @@
 #include "FlightTask.hpp"
 #include <lib/motion_planning/PositionSmoothing.hpp>
 #include <lib/geo/geo.h>
+#include <uORB/topics/home_position.h>
+#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;
@@ -56,36 +61,53 @@ class FlightTaskLand : public FlightTask
 
 	bool update() override;
 	bool activate(const trajectory_setpoint_s &last_setpoint) override;
-
-	// void reActivate() override;
-	// bool updateInitialize() override;
-
+	void reActivate() override;
 
 protected:
+	void updateParams() override;
+
 	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)};
+	uORB::SubscriptionData<position_setpoint_triplet_s> _sub_triplet_setpoint{ORB_ID(position_setpoint_triplet)};
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
 					(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) 	_param_mpc_acc_down_max,
-					(ParamFloat<px4::params::MPC_ACC_UP_MAX>) 	_param_mpc_acc_up_max,
 					(ParamFloat<px4::params::MPC_ACC_HOR>) 		_param_mpc_acc_hor,
+					(ParamFloat<px4::params::MPC_ACC_UP_MAX>) 	_param_mpc_acc_up_max,
 					(ParamFloat<px4::params::MPC_JERK_AUTO>) 	_param_mpc_jerk_auto,
 					(ParamFloat<px4::params::MPC_LAND_ALT1>) 	_param_mpc_land_alt1,
 					(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_VEL_MAX>) 	_param_mpc_xy_vel_max,
 					(ParamFloat<px4::params::MPC_XY_ERR_MAX>) 	_param_mpc_xy_err_max,
 					(ParamFloat<px4::params::MPC_XY_TRAJ_P>) 	_param_mpc_xy_traj_p,
-					(ParamFloat<px4::params::MPC_Z_V_AUTO_UP>) 	_param_mpc_z_v_auto_up,
+					(ParamFloat<px4::params::MPC_XY_VEL_MAX>) 	_param_mpc_xy_vel_max,
 					(ParamFloat<px4::params::MPC_Z_V_AUTO_DN>) 	_param_mpc_z_v_auto_dn,
+					(ParamFloat<px4::params::MPC_Z_V_AUTO_UP>) 	_param_mpc_z_v_auto_up,
 					(ParamFloat<px4::params::NAV_MC_ALT_RAD>) 	_param_nav_mc_alt_rad
 				       );
 private:
-	void CalculateLandingLocation();
-	void _updateTrajConstraints();
+	void _CalculateBrakingLocation();
+	void _HandleHighVelocities();
+	void _PerformLanding();
+	void _SmoothBrakingPath();
+	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};
 
 
 };