Remap orbit stick input when vehicle faces tangential

src/lib/mathlib/math/Functions.hpp

@@ -54,6 +54,17 @@ int signNoZero(T val)
 	return (T(0) <= val) - (val < T(0));
 }
 
+/**
+ * Sign function based on a boolean
+ *
+ * @param[in] positive Truth value to take the sign from
+ * @return 1 if positive is true, -1 if positive is false
+ */
+inline int signFromBool(bool positive)
+{
+	return positive ? 1 : -1;
+}
+
 template<typename T>
 T sq(T val)
 {

src/lib/mathlib/math/FunctionsTest.cpp

@@ -47,6 +47,15 @@ TEST(FunctionsTest, signNoZero)
 	EXPECT_FLOAT_EQ(signNoZero(123.456f), 1.f);
 }
 
+TEST(FunctionsTest, signFromBool)
+{
+	EXPECT_EQ(signFromBool(true), 1);
+	EXPECT_EQ(signFromBool(false), -1);
+	EXPECT_EQ(signFromBool(100), 1);
+	EXPECT_EQ(signFromBool(-100), 1);
+	EXPECT_EQ(signFromBool(0), -1);
+}
+
 TEST(FunctionsTest, expo)
 {
 	// input value limits

src/lib/matrix/matrix/helper_functions.hpp

@@ -143,7 +143,6 @@ Type unwrap(const Type last_x, const Type new_x, const Type low, const Type high
  *
  * @param[in] last_angle Last unwrapped angle [rad]
  * @param[in] new_angle New angle in [-pi, pi] [rad]
- * @param
  * @return New unwrapped angle [rad]
  */
 template<typename Type>

src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp

@@ -68,7 +68,8 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
 		new_absolute_velocity = command.param2;
 	}
 
-	float new_velocity = (new_is_clockwise ? 1.f : -1.f) * new_absolute_velocity;
+	float new_velocity = signFromBool(new_is_clockwise) * new_absolute_velocity;
+	_started_clockwise = new_is_clockwise;
 	_sanitizeParams(new_radius, new_velocity);
 	_orbit_radius = new_radius;
 	_orbit_velocity = new_velocity;
@@ -190,13 +191,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 +210,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 +257,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 -= signFromBool(_started_clockwise) * _sticks.getPositionExpo()(1) * _deltatime * _param_mpc_xy_cruise.get();
+		velocity += signFromBool(_started_clockwise) * _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);
@@ -318,7 +340,8 @@ void FlightTaskOrbit::_generate_circle_yaw_setpoints()
 		break;
 
 	case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE:
-		_yaw_setpoint = atan2f(sign(_orbit_velocity) * center_to_position(0), -sign(_orbit_velocity) * center_to_position(1));
+		_yaw_setpoint = atan2f(signFromBool(_started_clockwise) * center_to_position(0),
+				       -signFromBool(_started_clockwise) * center_to_position(1));
 		_yawspeed_setpoint = _orbit_velocity / _orbit_radius;
 		break;
 

src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp

@@ -106,6 +106,8 @@ class FlightTaskOrbit : public FlightTaskManualAltitudeSmoothVel
 	 */
 	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 */
@@ -122,6 +124,7 @@ class FlightTaskOrbit : public FlightTaskManualAltitudeSmoothVel
 
 	/** yaw behaviour during the orbit flight according to MAVLink's ORBIT_YAW_BEHAVIOUR enum */
 	int _yaw_behaviour = orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER;
+	bool _started_clockwise{true};
 	float _initial_heading = 0.f; /**< the heading of the drone when the orbit command was issued */
 	SlewRateYaw<float> _slew_rate_yaw;