dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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.
This commit is contained in:
David Jablonski 2019-11-18 10:20:46 +01:00 committed by Matthias Grob
parent 21f2e9b654
commit 589aff7e0d
3 changed files with 40 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
msg/orbit_status.msg
View File

@ -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

57
src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp
View File

@ -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;
}
}

16
src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.hpp
View File

@ -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 @@ 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_s> _orbit_status_pub{ORB_ID(orbit_status)};