Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Orbit: Adding orbit yaw behaviours #13426

Merged
merged 3 commits into from
Jul 13, 2020
Merged

Orbit: Adding orbit yaw behaviours #13426

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
e99197e
Orbit: add more yaw behaviours
dayjaby Nov 11, 2019
3eb3240
Orbit: Adjust yaw setpoint on circle approach
dayjaby Nov 11, 2019
f92f168
Orbit: Add RC controlled yaw mode
dayjaby Nov 18, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 8 additions & 0 deletions msg/orbit_status.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
66 changes: 54 additions & 12 deletions src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,6 +68,11 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)

ret = ret && setVelocity(v * (clockwise ? 1.f : -1.f));

// commanded heading behaviour
if (PX4_ISFINITE(command.param3)) {
_yaw_behaviour = command.param3;
}

// TODO: apply x,y / z independently in geo library
// commanded center coordinates
// if(PX4_ISFINITE(command.param5) && PX4_ISFINITE(command.param6)) {
Expand Down Expand Up @@ -99,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)) {
Expand Down Expand Up @@ -148,6 +154,8 @@ bool FlightTaskOrbit::activate(vehicle_local_position_setpoint_s last_setpoint)
_center = Vector2f(_position);
_center(0) -= _r;

_initial_heading = _yaw;

// need a valid position and velocity
ret = ret && PX4_ISFINITE(_position(0))
&& PX4_ISFINITE(_position(1))
Expand All @@ -172,15 +180,14 @@ bool FlightTaskOrbit::update()
setVelocity(v);

Vector2f center_to_position = Vector2f(_position) - _center;

// make vehicle front always point towards the center
_yaw_setpoint = atan2f(center_to_position(1), center_to_position(0)) + M_PI_F;
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
Expand All @@ -189,24 +196,21 @@ bool FlightTaskOrbit::update()
return true;
}

void FlightTaskOrbit::generate_circle_approach_setpoints()
void FlightTaskOrbit::generate_circle_approach_setpoints(Vector2f start_to_circle)
{

if (_circle_approach_line.isEndReached()) {
// calculate target point on circle and plan a line trajectory
Vector2f start_to_center = _center - Vector2f(_position);
Vector2f start_to_circle = (start_to_center.norm() - _r) * start_to_center.unit_or_zero();
Vector2f closest_circle_point = Vector2f(_position) + start_to_circle;
Vector3f target = Vector3f(closest_circle_point(0), closest_circle_point(1), _position(2));
_circle_approach_line.setLineFromTo(_position, target);
_circle_approach_line.setSpeed(_param_mpc_xy_cruise.get());
_yaw_setpoint = atan2f(start_to_circle(1), start_to_circle(0));
}

// follow the planned line and switch to orbiting once the circle is reached
_circle_approach_line.generateSetpoints(_position_setpoint, _velocity_setpoint);
_in_circle_approach = !_circle_approach_line.isEndReached();

// yaw stays constant
_yawspeed_setpoint = NAN;
}

void FlightTaskOrbit::generate_circle_setpoints(Vector2f center_to_position)
Expand All @@ -222,7 +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;
void FlightTaskOrbit::generate_circle_yaw_setpoints(Vector2f center_to_position)
{
switch (_yaw_behaviour) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I was asking to put this logic in its own function. Now it's not in the main update anymore but just uses another already existing one.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is generate_circle_setpoints not the proper place? Or do you prefer an additional generate_circle_yaw_setpoints function?

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 orbit_status_s::ORBIT_YAW_BEHAVIOUR_UNCONTROLLED:
// no yaw setpoint
_yaw_setpoint = NAN;
_yawspeed_setpoint = NAN;
break;

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 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;
}
}
9 changes: 8 additions & 1 deletion src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -88,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 */
Expand All @@ -104,6 +107,10 @@ class FlightTaskOrbit : public FlightTaskManualAltitudeSmooth
const float _velocity_max = 10.f;
const float _acceleration_max = 2.f;

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 */
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@dayjaby this is actually the heading of the vehicle when the vehicle command DO_ORBIT gets processed, or when we can say when the MAV_CMD_DO_ORBIT is received and processed in the flight controller.


uORB::Publication<orbit_status_s> _orbit_status_pub{ORB_ID(orbit_status)};

DEFINE_PARAMETERS(
Expand Down