adapted to enum change in mavlink

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

Hide file tree

Showing 2 changed files with 12 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,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