From cc64b8447d747d9117d4e1bc35934be21a2ff857 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 +
 .../tasks/Orbit/FlightTaskOrbit.cpp                | 14 +++++++++++---
 2 files changed, 12 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..6347a0e23d8e 100644
--- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
+++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
@@ -90,10 +90,18 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command, b
 
 	// commanded heading behaviour
 	if (PX4_ISFINITE(command.param3)) {
-		_yaw_behaviour = command.param3;
+		PX4_WARN("commanded yaw behaviour: %f", (double)command.param3);
+		PX4_WARN("_is_position_on_circle: %d", _is_position_on_circle());
 
-	} else {
-		_yaw_behaviour = _param_mc_orbit_yaw_mod.get();
+		if (command.param3 - 5.0f < FLT_EPSILON) {
+			// only change yaw behaviour to the parameter if we are not in an active orbit.
+			if (!_is_position_on_circle()) {
+				_yaw_behaviour = _param_mc_orbit_yaw_mod.get();
+			}
+
+		} else {
+			_yaw_behaviour = command.param3;
+		}
 	}
 
 	// save current yaw estimate for ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING