From e03585ca06a8dc318b6c6f5bc21dbbccb6cf341d Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Wed, 9 Mar 2022 19:29:17 +0100
Subject: [PATCH] FlightTaskOrbit: remap stick input when vehicle faces
 tangential

---
 .../tasks/Orbit/FlightTaskOrbit.cpp           | 37 +++++++++++++++----
 .../tasks/Orbit/FlightTaskOrbit.hpp           |  2 +
 2 files changed, 31 insertions(+), 8 deletions(-)

diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
index 7c84eb986b..ffe09f9432 100644
--- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
+++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
@@ -190,13 +190,7 @@ bool FlightTaskOrbit::update()
 {
 	bool ret = true;
 	_updateTrajectoryBoundaries();
-
-	// stick input adjusts parameters within a fixed time frame
-	float radius = _orbit_radius - _sticks.getPositionExpo()(0) * _deltatime * _param_mpc_xy_cruise.get();
-	float velocity = _orbit_velocity - _sticks.getPositionExpo()(1) * _deltatime * _param_mpc_acc_hor.get();
-	_sanitizeParams(radius, velocity);
-	_orbit_radius = radius;
-	_orbit_velocity = velocity;
+	_adjustParametersByStick();
 
 	if (_is_position_on_circle()) {
 		if (_in_circle_approach) {
@@ -215,7 +209,7 @@ bool FlightTaskOrbit::update()
 		// update altitude
 		ret = ret && FlightTaskManualAltitudeSmoothVel::update();
 
-		// this generates x / y setpoints
+		// this generates x, y and yaw setpoints
 		_generate_circle_setpoints();
 		_generate_circle_yaw_setpoints();
 	}
@@ -262,6 +256,33 @@ bool FlightTaskOrbit::_is_position_on_circle() const
 
 }
 
+void FlightTaskOrbit::_adjustParametersByStick()
+{
+	float radius = _orbit_radius;
+	float velocity = _orbit_velocity;
+
+	switch (_yaw_behaviour) {
+	case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE:
+		radius -= _sticks.getPositionExpo()(1) * _deltatime * _param_mpc_xy_cruise.get();
+		velocity += _sticks.getPositionExpo()(0) * _deltatime * _param_mpc_acc_hor.get();
+		break;
+
+	case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING:
+	case orbit_status_s::ORBIT_YAW_BEHAVIOUR_UNCONTROLLED:
+	case orbit_status_s::ORBIT_YAW_BEHAVIOUR_RC_CONTROLLED:
+	case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER:
+	default:
+		// stick input adjusts parameters within a fixed time frame
+		radius -= _sticks.getPositionExpo()(0) * _deltatime * _param_mpc_xy_cruise.get();
+		velocity -= _sticks.getPositionExpo()(1) * _deltatime * _param_mpc_acc_hor.get();
+		break;
+	}
+
+	_sanitizeParams(radius, velocity);
+	_orbit_radius = radius;
+	_orbit_velocity = velocity;
+}
+
 void FlightTaskOrbit::_generate_circle_approach_setpoints()
 {
 	const Vector2f center2d = Vector2f(_center);
diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp
index 9ebd760dde..6bc54bfa5b 100644
--- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp
+++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp
@@ -106,6 +106,8 @@ private:
 	 */
 	bool _is_position_on_circle() const;
 
+	/** Adjusts radius and speed according to stick input */
+	void _adjustParametersByStick();
 	/** generates setpoints to smoothly reach the closest point on the circle when starting from far away */
 	void _generate_circle_approach_setpoints();
 	/** generates xy setpoints to make the vehicle orbit */