Multicopter Auto: Adjustable land speed via RC (#12220)

src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp

@@ -66,6 +66,10 @@ bool FlightTaskAuto::initializeSubscriptions(SubscriptionArray &subscription_arr
 		return false;
 	}
 
+	if (!subscription_array.get(ORB_ID(manual_control_setpoint), _sub_manual_control_setpoint)) {
+		return false;
+	}
+
 	if (!_obstacle_avoidance.initializeSubscriptions(subscription_array)) {
 		return false;
 	}

src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp

@@ -43,6 +43,7 @@
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/position_setpoint.h>
 #include <uORB/topics/home_position.h>
+#include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 #include <lib/ecl/geo/geo.h>
 #include <ObstacleAvoidance.hpp>
@@ -99,6 +100,8 @@ protected:
 	float _mc_cruise_speed{0.0f}; /**< Requested cruise speed. If not valid, default cruise speed is used. */
 	WaypointType _type{WaypointType::idle}; /**< Type of current target triplet. */
 	uORB::SubscriptionPollable<home_position_s> *_sub_home_position{nullptr};
+	uORB::SubscriptionPollable<manual_control_setpoint_s> *_sub_manual_control_setpoint{nullptr};
+	uORB::SubscriptionPollable<vehicle_status_s> *_sub_vehicle_status{nullptr};
 
 	State _current_state{State::none};
 	float _target_acceptance_radius = 0.0f; /**< Acceptances radius of the target */
@@ -122,7 +125,6 @@ private:
 	matrix::Vector2f _lock_position_xy{NAN, NAN}; /**< if no valid triplet is received, lock positition to current position */
 	bool _yaw_lock = false; /**< if within acceptance radius, lock yaw to current yaw */
 	uORB::SubscriptionPollable<position_setpoint_triplet_s> *_sub_triplet_setpoint{nullptr};
-	uORB::SubscriptionPollable<vehicle_status_s> *_sub_vehicle_status{nullptr};
 
 	matrix::Vector3f
 	_triplet_target; /**< current triplet from navigator which may differ from the intenal one (_target) depending on the vehicle state. */

src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp

@@ -129,11 +129,11 @@ void FlightTaskAutoMapper2::_prepareIdleSetpoints()
 
 void FlightTaskAutoMapper2::_prepareLandSetpoints()
 {
+	float land_speed = _getLandSpeed();
+
 	// Keep xy-position and go down with landspeed
 	_position_setpoint = Vector3f(_target(0), _target(1), NAN);
-	const float speed_lnd = (_alt_above_ground > _param_mpc_land_alt1.get()) ? _constraints.speed_down :
-				_param_mpc_land_speed.get();
-	_velocity_setpoint = Vector3f(Vector3f(NAN, NAN, speed_lnd));
+	_velocity_setpoint = Vector3f(Vector3f(NAN, NAN, land_speed));
 
 	// set constraints
 	_constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get());
@@ -195,3 +195,34 @@ bool FlightTaskAutoMapper2::_highEnoughForLandingGear()
 	// return true if altitude is above two meters
 	return _alt_above_ground > 2.0f;
 }
+
+float FlightTaskAutoMapper2::_getLandSpeed()
+{
+	bool rc_assist_enabled = _param_mpc_land_rc_help.get();
+	bool rc_is_valid = !_sub_vehicle_status->get().rc_signal_lost;
+
+	float throttle = 0.5f;
+
+	if (rc_is_valid && rc_assist_enabled) {
+		throttle = _sub_manual_control_setpoint->get().z;
+	}
+
+	float speed = 0;
+
+	if (_alt_above_ground > _param_mpc_land_alt1.get()) {
+		speed = _constraints.speed_down;
+
+	} else {
+		float land_speed = _param_mpc_land_speed.get();
+		float head_room = _constraints.speed_down - land_speed;
+
+		speed = land_speed + 2 * (0.5f - throttle) * head_room;
+
+		// Allow minimum assisted land speed to be half of parameter
+		if (speed < land_speed * 0.5f) {
+			speed = land_speed * 0.5f;
+		}
+	}
+
+	return speed;
+}

src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp

@@ -57,6 +57,7 @@ protected:
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto,
 					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
 					(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd,
+					(ParamInt<px4::params::MPC_LAND_RC_HELP>) _param_mpc_land_rc_help,
 					(ParamFloat<px4::params::MPC_LAND_ALT1>)
 					_param_mpc_land_alt1, // altitude at which speed limit downwards reaches maximum speed
 					(ParamFloat<px4::params::MPC_LAND_ALT2>)
@@ -80,4 +81,5 @@ private:
 	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/mc_pos_control/mc_pos_control_params.c

@@ -350,6 +350,18 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 12.0f);
  */
 PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 0.7f);
 
+/**
+ * Enable user assisted descent speed for autonomous land routine.
+ * When enabled, descent speed will be equal to MPC_LAND_SPEED at half throttle,
+ * MPC_Z_VEL_MAX_DN at zero throttle, and 0.5 * MPC_LAND_SPEED at full throttle.
+ *
+ * @min 0
+ * @max 1
+ * @value 0 Fixed descent speed of MPC_LAND_SPEED
+ * @value 1 User assisted descent speed
+ */
+PARAM_DEFINE_INT32(MPC_LAND_RC_HELP, 0);
+
 /**
  * Takeoff climb rate
  *