adapted to enum change in mavlink

PX4 · Jul 22, 2024 · 7f36a7b · 7f36a7b
1 parent 68d1136
commit 7f36a7b
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 3 deletions.
diff --git 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
@@ -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