diff --git a/msg/orbit_status.msg b/msg/orbit_status.msg index 7a3d061ac1..a04265db46 100644 --- a/msg/orbit_status.msg +++ b/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 diff --git a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp index 951fd857d2..f9abb1defa 100644 --- a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp +++ b/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,22 +226,24 @@ 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; @@ -265,11 +251,20 @@ void FlightTaskOrbit::generate_circle_setpoints(Vector2f center_to_position) _yaw_setpoint = atan2f(center_to_position(1), center_to_position(0)) - M_PI_F / 2.f; } + _yawspeed_setpoint = _v / _r; + break; - case YawBehavior::point_to_center: + case orbit_status_s::ORBIT_YAW_BEHAVIOUR_RC_CONTROLLED: + _yaw_setpoint = NAN; + _yawspeed_setpoint = _sticks_expo(3); + break; + + 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; } } diff --git a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.hpp b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.hpp index 68425d1507..3b86c336db 100644 --- a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.hpp +++ b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.hpp @@ -46,13 +46,6 @@ #include #include -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 @@ protected: 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 @@ private: 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_pub{ORB_ID(orbit_status)};