From 28a0de63c5787c10b366aeb0371c5bfe710d0ca7 Mon Sep 17 00:00:00 2001
From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com>
Date: Wed, 7 Aug 2024 11:12:52 +0200
Subject: [PATCH] Orbit Yaw Vehicle Parameter (#23358)

---
 msg/OrbitStatus.msg                                |  1 +
 msg/VehicleCommand.msg                             |  8 ++++++++
 .../tasks/Orbit/FlightTaskOrbit.cpp                | 11 ++++++++++-
 .../tasks/Orbit/FlightTaskOrbit.hpp                |  2 ++
 .../tasks/Orbit/flight_task_orbit_params.c         | 14 +++++++++++++-
 5 files changed, 34 insertions(+), 2 deletions(-)

diff --git a/msg/OrbitStatus.msg b/msg/OrbitStatus.msg
index a04265db46..531fa41453 100644
--- a/msg/OrbitStatus.msg
+++ b/msg/OrbitStatus.msg
@@ -4,6 +4,7 @@ uint8 ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING = 1
 uint8 ORBIT_YAW_BEHAVIOUR_UNCONTROLLED = 2
 uint8 ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE = 3
 uint8 ORBIT_YAW_BEHAVIOUR_RC_CONTROLLED = 4
+uint8 ORBIT_YAW_BEHAVIOUR_UNCHANGED = 5
 
 uint64 timestamp # time since system start (microseconds)
 float32 radius   # Radius of the orbit circle. Positive values orbit clockwise, negative values orbit counter-clockwise. [m]
diff --git a/msg/VehicleCommand.msg b/msg/VehicleCommand.msg
index 0c1ce85d5f..8df0ca56b0 100644
--- a/msg/VehicleCommand.msg
+++ b/msg/VehicleCommand.msg
@@ -158,6 +158,14 @@ uint8 SPEED_TYPE_GROUNDSPEED = 1
 uint8 SPEED_TYPE_CLIMB_SPEED = 2
 uint8 SPEED_TYPE_DESCEND_SPEED = 3
 
+# used as param3 in CMD_DO_ORBIT
+uint8 ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER = 0
+uint8 ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING = 1
+uint8 ORBIT_YAW_BEHAVIOUR_UNCONTROLLED = 2
+uint8 ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE = 3
+uint8 ORBIT_YAW_BEHAVIOUR_RC_CONTROLLED = 4
+uint8 ORBIT_YAW_BEHAVIOUR_UNCHANGED = 5
+
 # used as param1 in ARM_DISARM command
 int8 ARMING_ACTION_DISARM = 0
 int8 ARMING_ACTION_ARM = 1
diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
index f639e49249..18adf4d3eb 100644
--- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
+++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
@@ -90,7 +90,14 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command, b
 
 	// commanded heading behaviour
 	if (PX4_ISFINITE(command.param3)) {
-		_yaw_behaviour = command.param3;
+		if (static_cast<uint8_t>(command.param3 + .5f) == vehicle_command_s::ORBIT_YAW_BEHAVIOUR_UNCHANGED) {
+			if (!_currently_orbiting) {	// only change the yaw behaviour if we are not actively orbiting
+				_yaw_behaviour = _param_mc_orbit_yaw_mod.get();
+			}
+
+		} else {
+			_yaw_behaviour = command.param3;
+		}
 	}
 
 	// save current yaw estimate for ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING
@@ -165,6 +172,7 @@ void FlightTaskOrbit::_sanitizeParams(float &radius, float &velocity) const
 bool FlightTaskOrbit::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTaskManualAltitude::activate(last_setpoint);
+	_currently_orbiting = false;
 	_orbit_radius = _radius_min;
 	_orbit_velocity = 1.f;
 	_center = _position;
@@ -199,6 +207,7 @@ bool FlightTaskOrbit::activate(const trajectory_setpoint_s &last_setpoint)
 bool FlightTaskOrbit::update()
 {
 	bool ret = true;
+	_currently_orbiting = true;
 	_updateTrajectoryBoundaries();
 	_adjustParametersByStick();
 
diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp
index 187a47de41..2c55bbc15b 100644
--- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp
+++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp
@@ -124,6 +124,7 @@ private:
 	/** 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};
+	bool _currently_orbiting{false};
 	float _initial_heading = 0.f; /**< the heading of the drone when the orbit command was issued */
 	SlewRateYaw<float> _slew_rate_yaw;
 
@@ -132,6 +133,7 @@ private:
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MC_ORBIT_RAD_MAX>) _param_mc_orbit_rad_max,
+		(ParamInt<px4::params::MC_ORBIT_YAW_MOD>) _param_mc_orbit_yaw_mod,
 		(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise, /**< cruise speed for circle approach */
 		(ParamFloat<px4::params::MPC_YAWRAUTO_MAX>) _param_mpc_yawrauto_max,
 		(ParamFloat<px4::params::MPC_XY_TRAJ_P>) _param_mpc_xy_traj_p,
diff --git a/src/modules/flight_mode_manager/tasks/Orbit/flight_task_orbit_params.c b/src/modules/flight_mode_manager/tasks/Orbit/flight_task_orbit_params.c
index 3e655c80e2..c8f66eb9f1 100644
--- a/src/modules/flight_mode_manager/tasks/Orbit/flight_task_orbit_params.c
+++ b/src/modules/flight_mode_manager/tasks/Orbit/flight_task_orbit_params.c
@@ -39,6 +39,18 @@
  * @max 10000.0
  * @increment 0.5
  * @decimal 1
- * @group FlightTaskOrbit
+ * @group Flight Task Orbit
  */
 PARAM_DEFINE_FLOAT(MC_ORBIT_RAD_MAX, 1000.0f);
+
+/**
+ * Yaw behaviour during orbit flight.
+ *
+ * @value 0 Front to Circle Center
+ * @value 1 Hold Initial Heading
+ * @value 2 Uncontrolled
+ * @value 3 Hold Front Tangent to Circle
+ * @value 4 RC Controlled
+ * @group Flight Task Orbit
+ */
+PARAM_DEFINE_INT32(MC_ORBIT_YAW_MOD, 0);