Orbit: Add RC controlled yaw mode

For the RC controlled yaw behaviour, we do a yaw setpoint according to
the stick expo. The uncontrolled yaw behaviour behaves undefined.
Switching between yaw behaviours makes the drone stand still for a
moment, which probably can be improved.

Author: dayjaby

msg/orbit_status.msg

@@ -1,6 +1,14 @@
+# ORBIT_YAW_BEHAVIOUR
+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
+
 uint64 timestamp # time since system start (microseconds)
 float32 radius   # Radius of the orbit circle. Positive values orbit clockwise, negative values orbit counter-clockwise. [m]
 uint8 frame      # The coordinate system of the fields: x, y, z.
 float64 x        # X coordinate of center point. Coordinate system depends on frame field: local = x position in meters * 1e4, global = latitude in degrees * 1e7.
 float64 y        # Y coordinate of center point. Coordinate system depends on frame field: local = y position in meters * 1e4, global = latitude in degrees * 1e7.
 float32 z        # Altitude of center point. Coordinate system depends on frame field.
+uint8 yaw_behaviour

src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp

@@ -68,26 +68,9 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
 
 	ret = ret && setVelocity(v * (clockwise ? 1.f : -1.f));
 
-	// commanded heading behavior
+	// commanded heading behaviour
 	if (PX4_ISFINITE(command.param3)) {
-		switch ((int)command.param3) {
-		case 1:
-			_yaw_behavior = YawBehavior::hold_last_heading;
-			break;
-
-		case 2:
-			_yaw_behavior = YawBehavior::leave_uncontrolled;
-			break;
-
-		case 3:
-			_yaw_behavior = YawBehavior::turn_towards_flight_direction;
-			break;
-
-		case 0:
-		default:
-			_yaw_behavior = YawBehavior::point_to_center;
-			break;
-		}
+		_yaw_behaviour = command.param3;
 	}
 
 	// TODO: apply x,y / z independently in geo library
@@ -121,6 +104,7 @@ bool FlightTaskOrbit::sendTelemetry()
 	orbit_status.timestamp = hrt_absolute_time();
 	orbit_status.radius = math::signNoZero(_v) * _r;
 	orbit_status.frame = 0; // MAV_FRAME::MAV_FRAME_GLOBAL
+	orbit_status.yaw_behaviour = _yaw_behaviour;
 
 	if (globallocalconverter_toglobal(_center(0), _center(1), _position_setpoint(2), &orbit_status.x, &orbit_status.y,
 					  &orbit_status.z)) {
@@ -196,12 +180,14 @@ bool FlightTaskOrbit::update()
 	setVelocity(v);
 
 	Vector2f center_to_position = Vector2f(_position) - _center;
+	Vector2f start_to_circle = (_r - center_to_position.norm()) * center_to_position.unit_or_zero();
 
 	if (_in_circle_approach) {
-		generate_circle_approach_setpoints();
+		generate_circle_approach_setpoints(start_to_circle);
 
 	} else {
 		generate_circle_setpoints(center_to_position);
+		generate_circle_yaw_setpoints(center_to_position);
 	}
 
 	// publish information to UI
@@ -210,10 +196,8 @@ bool FlightTaskOrbit::update()
 	return ret;
 }
 
-void FlightTaskOrbit::generate_circle_approach_setpoints()
+void FlightTaskOrbit::generate_circle_approach_setpoints(Vector2f start_to_circle)
 {
-	Vector2f start_to_center = _center - Vector2f(_position);
-	Vector2f start_to_circle = (start_to_center.norm() - _r) * start_to_center.unit_or_zero();
 
 	if (_circle_approach_line.isEndReached()) {
 		// calculate target point on circle and plan a line trajectory
@@ -242,34 +226,45 @@ void FlightTaskOrbit::generate_circle_setpoints(Vector2f center_to_position)
 	_velocity_setpoint(0) = velocity_xy(0);
 	_velocity_setpoint(1) = velocity_xy(1);
 	_position_setpoint(0) = _position_setpoint(1) = NAN;
+}
 
-	// yawspeed feed-forward because we know the necessary angular rate
-	_yawspeed_setpoint = _v / _r;
-
-	switch (_yaw_behavior) {
-	case YawBehavior::hold_last_heading:
+void FlightTaskOrbit::generate_circle_yaw_setpoints(Vector2f center_to_position)
+{
+	switch (_yaw_behaviour) {
+	case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING:
 		// make vehicle keep the same heading as when the orbit was commanded
 		_yaw_setpoint = _initial_heading;
+		_yawspeed_setpoint = NAN;
 		break;
 
-	case YawBehavior::leave_uncontrolled:
+	case orbit_status_s::ORBIT_YAW_BEHAVIOUR_UNCONTROLLED:
 		// no yaw setpoint
 		_yaw_setpoint = NAN;
+		_yawspeed_setpoint = NAN;
 		break;
 
-	case YawBehavior::turn_towards_flight_direction:
+	case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE:
 		if (_r > 0) {
 			_yaw_setpoint = atan2f(center_to_position(1), center_to_position(0)) + M_PI_F / 2.f;
 
 		} else {
 			_yaw_setpoint = atan2f(center_to_position(1), center_to_position(0)) - M_PI_F / 2.f;
 		}
 
+		_yawspeed_setpoint = _v / _r;
+
+		break;
+
+	case orbit_status_s::ORBIT_YAW_BEHAVIOUR_RC_CONTROLLED:
+		_yaw_setpoint = NAN;
+		_yawspeed_setpoint = _sticks_expo(3);
 		break;
 
-	case YawBehavior::point_to_center:
+	case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER:
 	default:
 		_yaw_setpoint = atan2f(center_to_position(1), center_to_position(0)) + M_PI_F;
+		// yawspeed feed-forward because we know the necessary angular rate
+		_yawspeed_setpoint = _v / _r;
 		break;
 	}
 }

src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.hpp

@@ -46,13 +46,6 @@
 #include <uORB/topics/orbit_status.h>
 #include <StraightLine.hpp>
 
-enum class YawBehavior : int {
-	point_to_center = 0,
-	hold_last_heading = 1,
-	leave_uncontrolled = 2,
-	turn_towards_flight_direction = 3,
-};
-
 class FlightTaskOrbit : public FlightTaskManualAltitudeSmooth
 {
 public:
@@ -95,8 +88,11 @@ class FlightTaskOrbit : public FlightTaskManualAltitudeSmooth
 	bool setVelocity(const float v);
 
 private:
-	void generate_circle_approach_setpoints(); /**< generates setpoints to smoothly reach the closest point on the circle when starting from far away */
+	void generate_circle_approach_setpoints(matrix::Vector2f
+						start_to_circle); /**< generates setpoints to smoothly reach the closest point on the circle when starting from far away */
 	void generate_circle_setpoints(matrix::Vector2f center_to_position); /**< generates xy setpoints to orbit the vehicle */
+	void generate_circle_yaw_setpoints(matrix::Vector2f
+					   center_to_position); /**< generates yaw setpoints to control the vehicle's heading */
 
 	float _r = 0.f; /**< radius with which to orbit the target */
 	float _v = 0.f; /**< clockwise tangential velocity for orbiting in m/s */
@@ -111,8 +107,8 @@ class FlightTaskOrbit : public FlightTaskManualAltitudeSmooth
 	const float _velocity_max = 10.f;
 	const float _acceleration_max = 2.f;
 
-	YawBehavior _yaw_behavior =
-		YawBehavior::point_to_center; /**<  the direction during the orbit task in which the drone looks */
+	int _yaw_behaviour =
+		orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER; /**<  yaw behaviour during the orbit flight according to MAVLink's ORBIT_YAW_BEHAVIOUR enum */
 	float _initial_heading = 0.f; /**< the heading of the drone when the orbit command was issued */
 
 	uORB::Publication<orbit_status_s> _orbit_status_pub{ORB_ID(orbit_status)};