Skip to content

Commit

Permalink
WIP: circle mode for multicopters
Browse files Browse the repository at this point in the history
  • Loading branch information
@dagar
dagar committed Jan 12, 2017
1 parent 751909c commit bd1cab8
Show file tree
Hide file tree
Showing 3 changed files with 203 additions and 24 deletions.
2 changes: 2 additions & 0 deletions msg/vehicle_command.msg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,8 @@ uint32 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component |1 t
uint32 VEHICLE_CMD_START_RX_PAIR = 500 # Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX|
uint32 VEHICLE_CMD_DO_TRIGGER_CONTROL = 2003 # Enable or disable on-board camera triggering system
uint32 VEHICLE_CMD_DO_VTOL_TRANSITION = 3000 # Command VTOL transition
uint32 VEHICLE_CMD_POSCTRL_MODE_STANDARD = 4000 # Position control submode: Standard position control
uint32 VEHICLE_CMD_POSCTRL_MODE_CIRCLE = 4001 # Position control submode: Circle mode
uint32 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001 # Prepare a payload deployment in the flight plan
uint32 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 # Control a pre-programmed payload deployment
uint32 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243 # UAVCAN configuration. If param 1 == 1 actuator mapping and direction assignment should be started
Expand Down
14 changes: 14 additions & 0 deletions src/modules/commander/commander.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1152,6 +1152,20 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
}
}
break;

case vehicle_command_s::VEHICLE_CMD_POSCTRL_MODE_STANDARD:
case vehicle_command_s::VEHICLE_CMD_POSCTRL_MODE_CIRCLE: {
if (TRANSITION_DENIED != main_state_transition(&status, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state)) {
mavlink_and_console_log_info(&mavlink_log_pub, "Position mode");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;

} else {
mavlink_log_critical(&mavlink_log_pub, "Position mode denied");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
}
break;

case vehicle_command_s::VEHICLE_CMD_CUSTOM_0:
case vehicle_command_s::VEHICLE_CMD_CUSTOM_1:
case vehicle_command_s::VEHICLE_CMD_CUSTOM_2:
Expand Down
211 changes: 187 additions & 24 deletions src/modules/mc_pos_control/mc_pos_control_main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,6 +76,7 @@
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_command.h>

#include <float.h>
#include <systemlib/mavlink_log.h>
Expand All @@ -91,6 +92,8 @@
#define MIN_DIST 0.01f
#define MANUAL_THROTTLE_MAX_MULTICOPTER 0.9f
#define ONE_G 9.8066f
#define CIRCLE_RADIUS_DEFAULT 5.0f
#define CIRCLE_RADIUS_MAX 100.0f

/**
* Multicopter position control app start / stop handling function
Expand Down Expand Up @@ -139,6 +142,7 @@ class MulticopterPositionControl : public control::SuperBlock
int _pos_sp_triplet_sub; /**< position setpoint triplet */
int _local_pos_sp_sub; /**< offboard local position setpoint */
int _global_vel_sp_sub; /**< offboard global velocity setpoint */
int _vehicle_command_sub; /**< vehicle command subscription */

orb_advert_t _att_sp_pub; /**< attitude setpoint publication */
orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */
Expand All @@ -157,6 +161,7 @@ class MulticopterPositionControl : public control::SuperBlock
struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */
struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position setpoint */
struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */
struct vehicle_command_s _vehicle_command; /**< vehicle_command */

control::BlockParamFloat _manual_thr_min;
control::BlockParamFloat _manual_thr_max;
Expand Down Expand Up @@ -290,6 +295,17 @@ class MulticopterPositionControl : public control::SuperBlock

matrix::Dcmf _R_setpoint;

float _R_circle; /**< desired radius for circle mode */
float _v_tang_circle_sp; /**< tangential velocity setpoint in circle mode */
math::Vector<2> _circle_orig; /**< local origin of circle in circle mode */
bool _was_pos_ctrl_mode; /**< previous iteration was in position control mode */

// position control sub-modes
enum MAN_CTRL_MODE {
MODE_POS_CTRL = 0,
MODE_CIRCLE,
} _posctrl_sub_mode;

/**
* Update our local parameter cache.
*/
Expand Down Expand Up @@ -362,6 +378,8 @@ class MulticopterPositionControl : public control::SuperBlock
*/
void select_alt(bool global);

void control_circle(float center_x, float center_y, float R, float vel, float vel_sp[2], float *yaw_sp);

void update_velocity_derivative();

void do_control(float dt);
Expand Down Expand Up @@ -401,6 +419,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
_local_pos_sub(-1),
_pos_sp_triplet_sub(-1),
_global_vel_sp_sub(-1),
_vehicle_command_sub(-1),

/* publications */
_att_sp_pub(nullptr),
Expand All @@ -418,6 +437,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
_pos_sp_triplet{},
_local_pos_sp{},
_global_vel_sp{},
_vehicle_command{},
_manual_thr_min(this, "MANTHR_MIN"),
_manual_thr_max(this, "MANTHR_MAX"),
_vel_x_deriv(this, "VELD"),
Expand Down Expand Up @@ -446,7 +466,10 @@ MulticopterPositionControl::MulticopterPositionControl() :
_xy_reset_counter(0),
_vz_reset_counter(0),
_vxy_reset_counter(0),
_heading_reset_counter(0)
_heading_reset_counter(0),
_R_circle(-1.0f),
_v_tang_circle_sp(0.0f),
_was_pos_ctrl_mode(false)
{
// Make the quaternion valid for control state
_ctrl_state.q[0] = 1.0f;
Expand Down Expand Up @@ -477,6 +500,8 @@ MulticopterPositionControl::MulticopterPositionControl() :

_thrust_int.zero();

_circle_orig.zero();

_params_handles.thr_min = param_find("MPC_THR_MIN");
_params_handles.thr_max = param_find("MPC_THR_MAX");
_params_handles.thr_hover = param_find("MPC_THR_HOVER");
Expand Down Expand Up @@ -794,6 +819,56 @@ MulticopterPositionControl::poll_subscriptions()
_pos_sp_triplet.current.valid = false;
}
}

orb_check(_vehicle_command_sub, &updated);

if (updated) {
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &_vehicle_command);

// update sub-mode for position control mode
if (_vehicle_command.command == vehicle_command_s::VEHICLE_CMD_POSCTRL_MODE_STANDARD) {
_posctrl_sub_mode = MODE_POS_CTRL;

} else if (_vehicle_command.command == vehicle_command_s::VEHICLE_CMD_POSCTRL_MODE_CIRCLE) {

_posctrl_sub_mode = MODE_CIRCLE;

// circle radius
if (PX4_ISFINITE(_vehicle_command.param1)) {
_R_circle = _vehicle_command.param1;

} else {
_R_circle = CIRCLE_RADIUS_DEFAULT;
}

_R_circle = math::constrain(_R_circle, CIRCLE_RADIUS_DEFAULT, CIRCLE_RADIUS_MAX);


// tanential velocity
// if tangential velocity is non-finite then manual control inputs from RC will be used to generate tangential velocity
if (PX4_ISFINITE(_vehicle_command.param2)) {
_v_tang_circle_sp = math::constrain(_vehicle_command.param2, -_params.vel_cruise(0), _params.vel_cruise(0));

} else {
_v_tang_circle_sp = _params.vel_cruise(0);
}

// altitude
if (PX4_ISFINITE(_vehicle_command.param3)) {
_pos_sp(2) = -_vehicle_command.param3;
}

if (PX4_ISFINITE(_vehicle_command.param5) && PX4_ISFINITE(_vehicle_command.param6)) {
map_projection_project(&_ref_pos,
_vehicle_command.param5, _vehicle_command.param6,
&_circle_orig.data[0], &_circle_orig.data[1]);

} else {
_circle_orig(0) = _pos(0) + cosf(_yaw) * _R_circle;
_circle_orig(1) = _pos(1) + sinf(_yaw) * _R_circle;
}
}
}
}

float
Expand Down Expand Up @@ -921,42 +996,76 @@ MulticopterPositionControl::control_manual(float dt)
}

math::Vector<3> req_vel_sp; // X,Y in local frame and Z in global (D), in [-1,1] normalized range
math::Vector<3> req_vel_sp_scaled; // desired velocity setpoint in global NED frame in m/s
req_vel_sp.zero();
req_vel_sp_scaled.zero();

if (_control_mode.flag_control_altitude_enabled) {
/* set vertical velocity setpoint with throttle stick */
req_vel_sp(2) = -scale_control(_manual.z - 0.5f, 0.5f, _params.alt_ctl_dz, _params.alt_ctl_dy); // D
if (_posctrl_sub_mode != MODE_CIRCLE) {
// reset the radius used for circle mode to invalid
_R_circle = -1.0f;
}

if (_control_mode.flag_control_position_enabled) {
/* set horizontal velocity setpoint with roll/pitch stick */
req_vel_sp(0) = _manual.x;
req_vel_sp(1) = _manual.y;
/* reset position setpoint to current position if needed */
reset_pos_sp();

if (_posctrl_sub_mode == MODE_POS_CTRL) {
// standard position control, user commands desired velocities
// otherwise drone will hold position

/* set horizontal velocity setpoint with roll/pitch stick */
req_vel_sp(0) = _manual.x;
req_vel_sp(1) = _manual.y;

/* limit velocity setpoint */
float req_vel_sp_norm = req_vel_sp.length();

if (req_vel_sp_norm > 1.0f) {
req_vel_sp /= req_vel_sp_norm;
}

/* _req_vel_sp scaled to 0..1, scale it to max speed and rotate around yaw */
math::Matrix<3, 3> R_yaw_sp;
R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);

// in NED and scaled to actual velocity
req_vel_sp_scaled = R_yaw_sp * req_vel_sp.emult(_params.vel_cruise);

} else if (_posctrl_sub_mode == MODE_CIRCLE) {
// vehicle flies along circle facing the center
// user can change command velocity and modify radius

if (_R_circle < 0.0f) {
// just switched into circle mode, set circle radius and origin
// if circle mode was activated via command then the radius was already set above
_R_circle = CIRCLE_RADIUS_DEFAULT;
_circle_orig(0) = _pos(0) + _R_circle * cosf(_yaw);
_circle_orig(1) = _pos(1) + _R_circle * sinf(_yaw);
}

// desired velocity along the circle
float vel_desired = PX4_ISFINITE(_v_tang_circle_sp) ? _v_tang_circle_sp : _manual.y * _params.vel_cruise(0);

// user can change radius with pitch stick
_R_circle += _manual.x * 2.0f * dt;
_R_circle = math::constrain(_R_circle, CIRCLE_RADIUS_DEFAULT, 100.0f);
control_circle(_circle_orig(0), _circle_orig(1), _R_circle, vel_desired, &req_vel_sp_scaled.data[0], &_att_sp.yaw_body);
req_vel_sp(0) = _params.vel_cruise(0) > FLT_EPSILON ? req_vel_sp_scaled(0) / _params.vel_cruise(0) : 0.0f;
req_vel_sp(1) = _params.vel_cruise(1) > FLT_EPSILON ? req_vel_sp_scaled(1) / _params.vel_cruise(1) : 0.0f;
}
}

// altitude control, common for all alitude controlled modes
if (_control_mode.flag_control_altitude_enabled) {
/* reset alt setpoint to current altitude if needed */
reset_alt_sp();
}

if (_control_mode.flag_control_position_enabled) {
/* reset position setpoint to current position if needed */
reset_pos_sp();
}

/* limit velocity setpoint */
float req_vel_sp_norm = req_vel_sp.length();

if (req_vel_sp_norm > 1.0f) {
req_vel_sp /= req_vel_sp_norm;
/* set vertical velocity setpoint with throttle stick */
req_vel_sp(2) = -scale_control(_manual.z - 0.5f, 0.5f, _params.alt_ctl_dz, _params.alt_ctl_dy); // D
req_vel_sp(2) = math::constrain(req_vel_sp(2), -1.0f, 1.0f);
req_vel_sp_scaled(2) = req_vel_sp(2) * _params.vel_cruise(2);
}

/* _req_vel_sp scaled to 0..1, scale it to max speed and rotate around yaw */
math::Matrix<3, 3> R_yaw_sp;
R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
math::Vector<3> req_vel_sp_scaled = R_yaw_sp * req_vel_sp.emult(
_params.vel_cruise); // in NED and scaled to actual velocity

/*
* assisted velocity mode: user controls velocity, but if velocity is small enough, position
* hold is activated for the corresponding axis
Expand Down Expand Up @@ -1053,6 +1162,48 @@ MulticopterPositionControl::control_manual(float dt)

}

void
MulticopterPositionControl::control_circle(float center_x, float center_y, float R, float vel,
float vel_sp[2], float *yaw_sp)
{
// radial control. D is position of drone, C center of circle
// rAB denotes a position vector connecting point A and B
// rA denotes a position vector connecting the origin with point A
matrix::Vector3f rC(center_x, center_y, 0.0f);
matrix::Vector3f rD(_pos(0), _pos(1), 0.0f);
matrix::Vector3f rCD = rD - rC;

matrix::Vector3f radial_error = rCD.unit() * (R - rCD.norm());

// tangential control, compute demanded velocity vector tangential to circle
matrix::Vector3f z(0, 0, 1);
matrix::Vector3f rTang = z % rCD;

// limit centripetal acceleration
float maxG = ONE_G / 8;

// a_centripetal = vel * vel / R
// vel = sqrt(a_centripetal * R)
float vel_max = sqrtf(maxG * R);

if (vel > vel_max) {
vel = vel_max;

} else if (vel < -vel_max) {
vel = -vel_max;
}

rTang.normalize();
rTang *= vel;

// velocity setpoint is combination of radial and tangential velocity setpoints
vel_sp[0] = _params.pos_p(0) * radial_error(0) + rTang(0);
vel_sp[1] = _params.pos_p(1) * radial_error(1) + rTang(1);

// choose heading such that drone faces the center of the circle
*yaw_sp = atan2f(-rCD(1), -rCD(0));
}

void
MulticopterPositionControl::control_non_manual(float dt)
{
Expand Down Expand Up @@ -2180,6 +2331,7 @@ MulticopterPositionControl::task_main()
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
_local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
_global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));

parameters_update(true);

Expand Down Expand Up @@ -2249,6 +2401,17 @@ MulticopterPositionControl::task_main()
_reset_alt_sp = true;
}

bool in_pos_control_mode = _control_mode.flag_control_manual_enabled
&& _control_mode.flag_control_position_enabled;

// by default position control mode should always start in
// manual position control submode
if (!_was_pos_ctrl_mode && in_pos_control_mode) {
_posctrl_sub_mode = MODE_POS_CTRL;
}

_was_pos_ctrl_mode = in_pos_control_mode;

//Update previous arming state
was_armed = _control_mode.flag_armed;

Expand Down

0 comments on commit bd1cab8

Please sign in to comment.