From 7f36a7ba059863b4c5914a7c86dec3a9fc4a47de Mon Sep 17 00:00:00 2001
From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com>
Date: Mon, 22 Jul 2024 11:02:26 +0200
Subject: [PATCH] adapted to enum change in mavlink

---
 msg/OrbitStatus.msg                                       | 1 +
 .../flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp   | 8 +++++---
 2 files changed, 6 insertions(+), 3 deletions(-)

diff --git a/msg/OrbitStatus.msg b/msg/OrbitStatus.msg
index a04265db46c8..531fa4145306 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/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
index 6336ee416229..fffcbe60eb4f 100644
--- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
+++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
@@ -90,10 +90,12 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command, b
 
 	// commanded heading behaviour
 	if (PX4_ISFINITE(command.param3)) {
-		_yaw_behaviour = command.param3;
+		if (command.param3 - 5.0f < FLT_EPSILON && !_is_position_on_circle()) {
+			_yaw_behaviour = _param_mc_orbit_yaw_mod.get();
 
-	} else {
-		_yaw_behaviour = _param_mc_orbit_yaw_mod.get();
+		} else {
+			_yaw_behaviour = command.param3;
+		}
 	}
 
 	// save current yaw estimate for ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING