FlightTaskDescend: Enable nudging by sticks when it's enabled

and stick input is available.

src/modules/flight_mode_manager/tasks/Descend/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2019-2023 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -35,5 +35,5 @@ px4_add_library(FlightTaskDescend
 	FlightTaskDescend.cpp
 )
 
-target_link_libraries(FlightTaskDescend PUBLIC FlightTask)
+target_link_libraries(FlightTaskDescend PUBLIC FlightTask FlightTaskUtility)
 target_include_directories(FlightTaskDescend PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -36,16 +36,6 @@
 
 #include "FlightTaskDescend.hpp"
 
-bool FlightTaskDescend::activate(const trajectory_setpoint_s &last_setpoint)
-{
-	bool ret = FlightTask::activate(last_setpoint);
-	// stay level to minimize horizontal drift
-	_acceleration_setpoint = matrix::Vector3f(0.f, 0.f, NAN);
-	// keep heading
-	_yaw_setpoint = _yaw;
-	return ret;
-}
-
 bool FlightTaskDescend::update()
 {
 	bool ret = FlightTask::update();
@@ -58,7 +48,28 @@ bool FlightTaskDescend::update()
 	} else {
 		// descend with constant acceleration (crash landing)
 		_velocity_setpoint(2) = NAN;
-		_acceleration_setpoint(2) = .3f;
+		_acceleration_setpoint(2) = .15f;
+	}
+
+	// Nudging
+	if (_param_mpc_land_rc_help.get() && _sticks.checkAndUpdateStickInputs()) {
+		_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw,
+					       _is_yaw_good_for_control, _deltatime);
+		_acceleration_setpoint.xy() = _stick_tilt_xy.generateAccelerationSetpoints(_sticks.getPitchRoll(), _deltatime, _yaw,
+					      _yaw_setpoint);
+
+		// Stick full up -1 -> stop, stick full down 1 -> double the value
+		_velocity_setpoint(2) *= (1 - _sticks.getThrottleZeroCenteredExpo());
+		_acceleration_setpoint(2) -= _sticks.getThrottleZeroCentered() * 10.f;
+
+	} else {
+		_acceleration_setpoint = matrix::Vector3f(0.f, 0.f, NAN); // stay level to minimize horizontal drift
+		_yawspeed_setpoint = NAN;
+
+		// keep heading
+		if (!PX4_ISFINITE(_yaw_setpoint)) {
+			_yaw_setpoint = _yaw;
+		}
 	}
 
 	return ret;

src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -38,6 +38,9 @@
 #pragma once
 
 #include "FlightTask.hpp"
+#include "Sticks.hpp"
+#include "StickTiltXY.hpp"
+#include "StickYaw.hpp"
 
 class FlightTaskDescend : public FlightTask
 {
@@ -46,11 +49,15 @@ public:
 	virtual ~FlightTaskDescend() = default;
 
 	bool update() override;
-	bool activate(const trajectory_setpoint_s &last_setpoint) override;
 
 private:
+	Sticks _sticks{this};
+	StickTiltXY _stick_tilt_xy{this};
+	StickYaw _stick_yaw{this};
+
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
-					(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover, ///< thrust at hover equilibrium
+					(ParamInt<px4::params::MPC_LAND_RC_HELP>) _param_mpc_land_rc_help,
-					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed ///< velocity for controlled descend
+					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed, ///< velocity for controlled descend
+					(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover ///< thrust at hover equilibrium
 				       )
 };