Orbit Yaw Vehicle Parameter (#23358)

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]

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

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();
 

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,

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);