FlightTasks: Stick library for auto land

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

@@ -64,7 +64,6 @@ bool FlightTaskAuto::updateInitialize()
 	bool ret = FlightTask::updateInitialize();
 
 	_sub_home_position.update();
-	_sub_manual_control_setpoint.update();
 	_sub_vehicle_status.update();
 	_sub_triplet_setpoint.update();
 

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

@@ -108,7 +108,6 @@ class FlightTaskAuto : public FlightTask
 	WaypointType _type{WaypointType::idle}; /**< Type of current target triplet. */
 
 	uORB::SubscriptionData<home_position_s>			_sub_home_position{ORB_ID(home_position)};
-	uORB::SubscriptionData<manual_control_setpoint_s>	_sub_manual_control_setpoint{ORB_ID(manual_control_setpoint)};
 	uORB::SubscriptionData<vehicle_status_s>		_sub_vehicle_status{ORB_ID(vehicle_status)};
 
 	State _current_state{State::none};

src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp

@@ -40,6 +40,10 @@
 
 using namespace matrix;
 
+FlightTaskAutoMapper::FlightTaskAutoMapper() :
+	_sticks(this)
+{};
+
 bool FlightTaskAutoMapper::activate(vehicle_local_position_setpoint_s last_setpoint)
 {
 	bool ret = FlightTaskAuto::activate(last_setpoint);
@@ -176,33 +180,17 @@ bool FlightTaskAutoMapper::_highEnoughForLandingGear()
 
 float FlightTaskAutoMapper::_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 (_dist_to_ground > _param_mpc_land_alt1.get()) {
-		speed = _constraints.speed_down;
-
-	} else {
-		const 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);
-		const 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;
-		}
+	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(_time_stamp_current)) {
+		// stick full up -1 -> stop, stick full down 1 -> double the speed
+		land_speed *= (1 + _sticks.getPositionExpo()(2));
 	}
 
-	return speed;
+	return land_speed;
 }

src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp

@@ -41,11 +41,12 @@
 #pragma once
 
 #include "FlightTaskAuto.hpp"
+#include "Sticks.hpp"
 
 class FlightTaskAutoMapper : public FlightTaskAuto
 {
 public:
-	FlightTaskAutoMapper() = default;
+	FlightTaskAutoMapper();
 	virtual ~FlightTaskAutoMapper() = default;
 	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
 	bool update() override;
@@ -76,7 +77,7 @@ class FlightTaskAutoMapper : public FlightTaskAuto
 				       );
 
 private:
-
+	Sticks _sticks;
 	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. */

src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -50,10 +50,11 @@ bool FlightTaskManualAltitude::updateInitialize()
 {
 	bool ret = FlightTask::updateInitialize();
 
-	const bool sticks_available = _sticks.evaluateSticks(_time_stamp_current, _gear);
+	_sticks.checkAndSetStickInputs(_time_stamp_current);
+	_sticks.setGearAccordingToSwitch(_gear);
 
 	if (_sticks_data_required) {
-		ret = ret && sticks_available;
+		ret = ret && _sticks.isAvailable();
 	}
 
 	// in addition to manual require valid position and velocity in D-direction and valid yaw

src/lib/flight_tasks/tasks/Utility/Sticks.cpp

@@ -43,15 +43,14 @@ Sticks::Sticks(ModuleParams *parent) :
 	ModuleParams(parent)
 {};
 
-bool Sticks::evaluateSticks(hrt_abstime now, landing_gear_s &gear)
+bool Sticks::checkAndSetStickInputs(hrt_abstime now)
 {
 	_sub_manual_control_setpoint.update();
 
 	hrt_abstime rc_timeout = (_param_com_rc_loss_t.get() * 1.5f) * 1_s;
 
 	// Sticks are rescaled linearly and exponentially to [-1,1]
 	if ((now - _sub_manual_control_setpoint.get().timestamp) < rc_timeout) {
-
 		// Linear scale
 		_positions(0) = _sub_manual_control_setpoint.get().x; // NED x, pitch [-1,1]
 		_positions(1) = _sub_manual_control_setpoint.get().y; // NED y, roll [-1,1]
@@ -64,41 +63,48 @@ bool Sticks::evaluateSticks(hrt_abstime now, landing_gear_s &gear)
 		_positions_expo(2) = math::expo_deadzone(_positions(2), _param_mpc_z_man_expo.get(), _param_mpc_hold_dz.get());
 		_positions_expo(3) = math::expo_deadzone(_positions(3), _param_mpc_yaw_expo.get(), _param_mpc_hold_dz.get());
 
-		// Only switch the landing gear up if the user switched from gear down to gear up.
-		// If the user had the switch in the gear up position and took off ignore it
-		// until he toggles the switch to avoid retracting the gear immediately on takeoff.
-		int8_t gear_switch = _sub_manual_control_setpoint.get().gear_switch;
-
-		if (_gear_switch_old != gear_switch) {
-			applyGearSwitch(gear_switch, gear);
-		}
-
-		_gear_switch_old = gear_switch;
-
 		// valid stick inputs are required
 		const bool valid_sticks =  PX4_ISFINITE(_positions(0))
 					   && PX4_ISFINITE(_positions(1))
 					   && PX4_ISFINITE(_positions(2))
 					   && PX4_ISFINITE(_positions(3));
 
-		return valid_sticks;
+		_input_available = valid_sticks;
 
 	} else {
+		_input_available = false;
+	}
+
+	if (!_input_available) {
 		// Timeout: set all sticks to zero
 		_positions.zero();
 		_positions_expo.zero();
-		gear.landing_gear = landing_gear_s::GEAR_KEEP;
-		return false;
 	}
+
+	return _input_available;
 }
 
-void Sticks::applyGearSwitch(uint8_t gear_switch, landing_gear_s &gear)
+void Sticks::setGearAccordingToSwitch(landing_gear_s &gear)
 {
-	if (gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
-		gear.landing_gear = landing_gear_s::GEAR_DOWN;
-	}
+	// Only switch the landing gear up if the user switched from gear down to gear up.
+	// If the user had the switch in the gear up position and took off ignore it
+	// until he toggles the switch to avoid retracting the gear immediately on takeoff.
+	if (!isAvailable()) {
+		gear.landing_gear = landing_gear_s::GEAR_KEEP;
+
+	} else {
+		const int8_t gear_switch = _sub_manual_control_setpoint.get().gear_switch;
 
-	if (gear_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
-		gear.landing_gear = landing_gear_s::GEAR_UP;
+		if (_gear_switch_old != gear_switch) {
+			if (gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
+				gear.landing_gear = landing_gear_s::GEAR_DOWN;
+			}
+
+			if (gear_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
+				gear.landing_gear = landing_gear_s::GEAR_UP;
+			}
+		}
+
+		_gear_switch_old = gear_switch;
 	}
 }

src/lib/flight_tasks/tasks/Utility/Sticks.hpp

@@ -53,11 +53,14 @@ class Sticks : public ModuleParams
 	Sticks(ModuleParams *parent);
 	~Sticks() = default;
 
-	bool evaluateSticks(hrt_abstime now, landing_gear_s &gear); ///< checks and sets stick inputs
-	void applyGearSwitch(uint8_t gear_switch, landing_gear_s &gear); ///< Sets gears according to switch
+	bool checkAndSetStickInputs(hrt_abstime now);
+	void setGearAccordingToSwitch(landing_gear_s &gear);
+	bool isAvailable() { return _input_available; };
 	const matrix::Vector<float, 4> &getPosition() { return _positions; };
 	const matrix::Vector<float, 4> &getPositionExpo() { return _positions_expo; };
+
 private:
+	bool _input_available = false;
 	matrix::Vector<float, 4> _positions; ///< unmodified manual stick inputs
 	matrix::Vector<float, 4> _positions_expo; ///< modified manual sticks using expo function
 	int _gear_switch_old = manual_control_setpoint_s::SWITCH_POS_NONE; ///< old switch state

src/modules/mc_pos_control/mc_pos_control_params.c

@@ -359,8 +359,11 @@ PARAM_DEFINE_FLOAT(MPC_LAND_VEL_XY, 10.0f);
 
 /**
  * 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.
+ *
+ * When enabled, descent speed will be:
+ * stick full up - 0
+ * stick centered - MPC_LAND_SPEED
+ * stick full down - 2 * MPC_LAND_SPEED
  *
  * @min 0
  * @max 1