From 50b084cf171de92903699dba4e2665e39261639a Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Thu, 9 Feb 2023 14:27:35 +0100
Subject: [PATCH] MC auto: allow yawing during hold

---
 .../tasks/Auto/FlightTaskAuto.cpp             | 29 ++++++++++++-------
 .../tasks/Auto/FlightTaskAuto.hpp             |  2 ++
 2 files changed, 20 insertions(+), 11 deletions(-)

diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
index 23743cf3d0..9431e69d56 100644
--- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
+++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
@@ -141,9 +141,13 @@ bool FlightTaskAuto::update()
 		_velocity_setpoint(2) = NAN;
 		break;
 
+	case WaypointType::loiter:
+		if (_param_mpc_land_rc_help.get() && _sticks.checkAndUpdateStickInputs()) {
+			rcHelpModifyYaw(_yaw_setpoint);
+		}
+
 	// FALLTHROUGH
 	case WaypointType::takeoff:
-	case WaypointType::loiter:
 	case WaypointType::position:
 	default:
 		// Simple waypoint navigation: go to xyz target, with standard limitations
@@ -213,6 +217,18 @@ void FlightTaskAuto::overrideCruiseSpeed(const float cruise_speed_m_s)
 	_time_last_cruise_speed_override = hrt_absolute_time();
 }
 
+void FlightTaskAuto::rcHelpModifyYaw(float &yaw_sp)
+{
+	// Only set a yawrate setpoint if weather vane is not active or the yaw stick is out of its dead-zone
+	if (!_weathervane.isActive() || fabsf(_sticks.getYawExpo()) > FLT_EPSILON) {
+		_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, yaw_sp, _sticks.getYawExpo(), _yaw, _is_yaw_good_for_control,
+					       _deltatime);
+
+		// Hack to make sure the MPC_YAW_MODE 4 alignment doesn't stop the vehicle from descending when there's yaw input
+		_yaw_sp_aligned = true;
+	}
+}
+
 void FlightTaskAuto::_prepareLandSetpoints()
 {
 	_velocity_setpoint.setNaN(); // Don't take over any smoothed velocity setpoint
@@ -243,11 +259,7 @@ void FlightTaskAuto::_prepareLandSetpoints()
 		// Stick full up -1 -> stop, stick full down 1 -> double the speed
 		vertical_speed *= (1 - _sticks.getThrottleZeroCenteredExpo());
 
-		// Only set a yawrate setpoint if weather vane is not active or the yaw stick is out of its dead-zone
-		if (!_weathervane.isActive() || fabsf(_sticks.getYawExpo()) > FLT_EPSILON) {
-			_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading, _sticks.getYawExpo(), _yaw, _is_yaw_good_for_control,
-						       _deltatime);
-		}
+		rcHelpModifyYaw(_land_heading);
 
 		Vector2f sticks_xy = _sticks.getPitchRollExpo();
 		Vector2f sticks_ne = sticks_xy;
@@ -275,11 +287,6 @@ void FlightTaskAuto::_prepareLandSetpoints()
 				_velocity_setpoint_feedback.xy(), _deltatime);
 		_stick_acceleration_xy.getSetpoints(_land_position, _velocity_setpoint, _acceleration_setpoint);
 
-		// Hack to make sure the MPC_YAW_MODE 4 alignment doesn't stop the vehicle from descending when there's yaw input
-		if (fabsf(_yawspeed_setpoint) > FLT_EPSILON) {
-			_yaw_sp_aligned = true;
-		}
-
 	} 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))) {
diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
index 8fead25cbb..fc671cd5e5 100644
--- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
+++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
@@ -109,6 +109,8 @@ protected:
 	bool isTargetModified() const;
 	void _updateTrajConstraints();
 
+	void rcHelpModifyYaw(float &yaw_sp);
+
 	/** determines when to trigger a takeoff (ignored in flight) */
 	bool _checkTakeoff() override { return _want_takeoff; };