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

Merge branch 'velocity_control'

This commit is contained in:
Lorenz Meier 2015-10-16 18:58:24 +02:00
parent 7ce0cbd8cb b3f912ecc1
commit 507f4d8df5
2 changed files with 170 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

194
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -177,6 +177,10 @@ private:
param_t man_pitch_max;
param_t man_yaw_max;
param_t mc_att_yaw_p;
param_t hold_xy_dz;
param_t hold_z_dz;
param_t hold_max_xy;
param_t hold_max_z;
} _params_handles; /**< handles for interesting parameters */
struct {
@ -189,6 +193,10 @@ private:
float man_pitch_max;
float man_yaw_max;
float mc_att_yaw_p;
float hold_xy_dz;
float hold_z_dz;
float hold_max_xy;
float hold_max_z;
math::Vector<3> pos_p;
math::Vector<3> vel_p;
@ -206,6 +214,10 @@ private:
bool _reset_pos_sp;
bool _reset_alt_sp;
bool _mode_auto;
bool _pos_hold_engaged;
bool _alt_hold_engaged;
bool _run_pos_control;
bool _run_alt_control;
math::Vector<3> _pos;
math::Vector<3> _pos_sp;
@ -213,7 +225,6 @@ private:
math::Vector<3> _vel_sp;
math::Vector<3> _vel_prev; /**< velocity on previous step */
math::Vector<3> _vel_ff;
math::Vector<3> _sp_move_rate;
/**
* Update our local parameter cache.
@ -325,7 +336,11 @@ MulticopterPositionControl::MulticopterPositionControl() :
_reset_pos_sp(true),
_reset_alt_sp(true),
_mode_auto(false)
_mode_auto(false),
_pos_hold_engaged(false),
_alt_hold_engaged(false),
_run_pos_control(true),
_run_alt_control(true)
{
memset(&_vehicle_status, 0, sizeof(_vehicle_status));
memset(&_att, 0, sizeof(_att));
@ -354,7 +369,6 @@ MulticopterPositionControl::MulticopterPositionControl() :
_vel_sp.zero();
_vel_prev.zero();
_vel_ff.zero();
_sp_move_rate.zero();
_params_handles.thr_min = param_find("MPC_THR_MIN");
_params_handles.thr_max = param_find("MPC_THR_MAX");
@ -377,6 +391,11 @@ MulticopterPositionControl::MulticopterPositionControl() :
_params_handles.man_pitch_max = param_find("MPC_MAN_P_MAX");
_params_handles.man_yaw_max = param_find("MPC_MAN_Y_MAX");
_params_handles.mc_att_yaw_p = param_find("MC_YAW_P");
_params_handles.hold_xy_dz = param_find("MPC_HOLD_XY_DZ");
_params_handles.hold_z_dz = param_find("MPC_HOLD_Z_DZ");
_params_handles.hold_max_xy = param_find("MPC_HOLD_MAX_XY");
_params_handles.hold_max_z = param_find("MPC_HOLD_MAX_Z");
/* fetch initial parameter values */
parameters_update(true);
@ -464,6 +483,16 @@ MulticopterPositionControl::parameters_update(bool force)
param_get(_params_handles.z_ff, &v);
v = math::constrain(v, 0.0f, 1.0f);
_params.vel_ff(2) = v;
param_get(_params_handles.hold_xy_dz, &v);
v = math::constrain(v, 0.0f, 1.0f);
_params.hold_xy_dz = v;
param_get(_params_handles.hold_z_dz, &v);
v = math::constrain(v, 0.0f, 1.0f);
_params.hold_z_dz = v;
param_get(_params_handles.hold_max_xy, &v);
_params.hold_max_xy = (v < 0.0f ? 0.0f : v);
param_get(_params_handles.hold_max_z, &v);
_params.hold_max_z = (v < 0.0f ? 0.0f : v);
_params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f;
@ -584,9 +613,9 @@ MulticopterPositionControl::reset_pos_sp()
_reset_pos_sp = false;
/* shift position setpoint to make attitude setpoint continuous */
_pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust / _params.vel_p(0)
- _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0);
- _params.vel_ff(0) * _vel_sp(0)) / _params.pos_p(0);
_pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp.R_body, 1, 2) * _att_sp.thrust / _params.vel_p(1)
- _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
- _params.vel_ff(1) * _vel_sp(1)) / _params.pos_p(1);
mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
}
}
@ -596,7 +625,7 @@ MulticopterPositionControl::reset_alt_sp()
{
if (_reset_alt_sp) {
_reset_alt_sp = false;
_pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2);
_pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _vel_sp(2)) / _params.pos_p(2);
mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2));
}
}
@ -627,31 +656,20 @@ MulticopterPositionControl::limit_pos_sp_offset()
void
MulticopterPositionControl::control_manual(float dt)
{
_sp_move_rate.zero();
math::Vector<3> req_vel_sp; // X,Y in local frame and Z in global (D), in [-1,1] normalized range
req_vel_sp.zero();
if (_control_mode.flag_control_altitude_enabled) {
/* move altitude setpoint with throttle stick */
_sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz);
/* set vertical velocity setpoint with throttle stick */
req_vel_sp(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz); // D
}
if (_control_mode.flag_control_position_enabled) {
/* move position setpoint with roll/pitch stick */
_sp_move_rate(0) = _manual.x;
_sp_move_rate(1) = _manual.y;
/* set horizontal velocity setpoint with roll/pitch stick */
req_vel_sp(0) = _manual.x;
req_vel_sp(1) = _manual.y;
}
/* limit setpoint move rate */
float sp_move_norm = _sp_move_rate.length();
if (sp_move_norm > 1.0f) {
_sp_move_rate /= sp_move_norm;
}
/* _sp_move_rate 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);
_sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max);
if (_control_mode.flag_control_altitude_enabled) {
/* reset alt setpoint to current altitude if needed */
reset_alt_sp();
@ -662,30 +680,70 @@ MulticopterPositionControl::control_manual(float dt)
reset_pos_sp();
}
/* feed forward setpoint move rate with weight vel_ff */
_vel_ff = _sp_move_rate.emult(_params.vel_ff);
/* limit velocity setpoint */
float req_vel_sp_norm = req_vel_sp.length();
/* move position setpoint */
_pos_sp += _sp_move_rate * dt;
/* check if position setpoint is too far from actual position */
math::Vector<3> pos_sp_offs;
pos_sp_offs.zero();
if (_control_mode.flag_control_position_enabled) {
pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
if (req_vel_sp_norm > 1.0f) {
req_vel_sp /= req_vel_sp_norm;
}
if (_control_mode.flag_control_altitude_enabled) {
pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(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_max); // 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
*/
/* horizontal axes */
if (_control_mode.flag_control_position_enabled)
{
/* check for pos. hold */
if (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz)
{
if (!_pos_hold_engaged && (_params.hold_max_xy < FLT_EPSILON ||
(fabsf(_vel(0)) < _params.hold_max_xy && fabsf(_vel(1)) < _params.hold_max_xy)))
{
_pos_hold_engaged = true;
}
}
else {
_pos_hold_engaged = false;
_pos_sp(0) = _pos(0);
_pos_sp(1) = _pos(1);
}
/* set requested velocity setpoint */
if (!_pos_hold_engaged) {
_run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */
_vel_sp(0) = req_vel_sp_scaled(0);
_vel_sp(1) = req_vel_sp_scaled(1);
}
}
float pos_sp_offs_norm = pos_sp_offs.length();
/* vertical axis */
if (_control_mode.flag_control_altitude_enabled)
{
/* check for pos. hold */
if (fabsf(req_vel_sp(2)) < _params.hold_z_dz)
{
if (!_alt_hold_engaged && (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z))
{
_alt_hold_engaged = true;
}
}
else {
_alt_hold_engaged = false;
_pos_sp(2) = _pos(2);
}
if (pos_sp_offs_norm > 1.0f) {
pos_sp_offs /= pos_sp_offs_norm;
_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
/* set requested velocity setpoint */
if (!_alt_hold_engaged) {
_run_alt_control = false; /* request velocity setpoint to be used, instead of altitude setpoint */
_vel_sp(2) = req_vel_sp_scaled(2);
}
}
}
@ -710,8 +768,10 @@ MulticopterPositionControl::control_offboard(float dt)
reset_pos_sp();
/* set position setpoint move rate */
_sp_move_rate(0) = _pos_sp_triplet.current.vx;
_sp_move_rate(1) = _pos_sp_triplet.current.vy;
_vel_sp(0) = _pos_sp_triplet.current.vx;
_vel_sp(1) = _pos_sp_triplet.current.vy;
_run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */
}
if (_pos_sp_triplet.current.yaw_valid) {
@ -728,15 +788,10 @@ MulticopterPositionControl::control_offboard(float dt)
reset_alt_sp();
/* set altitude setpoint move rate */
_sp_move_rate(2) = _pos_sp_triplet.current.vz;
_vel_sp(2) = _pos_sp_triplet.current.vz;
_run_alt_control = false; /* request velocity setpoint to be used, instead of position setpoint */
}
/* feed forward setpoint move rate with weight vel_ff */
_vel_ff = _sp_move_rate.emult(_params.vel_ff);
/* move position setpoint */
_pos_sp += _sp_move_rate * dt;
} else {
reset_pos_sp();
reset_alt_sp();
@ -1020,7 +1075,11 @@ MulticopterPositionControl::task_main()
_vel(2) = _local_pos.vz;
_vel_ff.zero();
_sp_move_rate.zero();
/* by default, run position/altitude controller. the control_* functions
* can disable this and run velocity controllers directly in this cycle */
_run_pos_control = true;
_run_alt_control = true;
/* select control source */
if (_control_mode.flag_control_manual_enabled) {
@ -1060,10 +1119,15 @@ MulticopterPositionControl::task_main()
}
} else {
/* run position & altitude controllers, calculate velocity setpoint */
math::Vector<3> pos_err = _pos_sp - _pos;
/* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */
if (_run_pos_control) {
_vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0);
_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
}
_vel_sp = pos_err.emult(_params.pos_p) + _vel_ff;
if (_run_alt_control) {
_vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2);
}
/* make sure velocity setpoint is saturated in xy*/
float vel_norm_xy = sqrtf(_vel_sp(0)*_vel_sp(0) +
@ -1080,17 +1144,23 @@ MulticopterPositionControl::task_main()
_vel_sp(2) = _vel_sp(2)*_params.vel_max(2)/vel_norm_z;
}
if (!_control_mode.flag_control_altitude_enabled) {
_reset_alt_sp = true;
_vel_sp(2) = 0.0f;
}
if (!_control_mode.flag_control_position_enabled) {
_reset_pos_sp = true;
}
if (!_control_mode.flag_control_altitude_enabled) {
_reset_alt_sp = true;
}
if (!_control_mode.flag_control_velocity_enabled) {
_vel_sp(0) = 0.0f;
_vel_sp(1) = 0.0f;
}
if (!_control_mode.flag_control_climb_rate_enabled) {
_vel_sp(2) = 0.0f;
}
/* use constant descend rate when landing, ignore altitude setpoint */
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
_vel_sp(2) = _params.land_speed;
@ -1148,7 +1218,7 @@ MulticopterPositionControl::task_main()
math::Vector<3> vel_err = _vel_sp - _vel;
/* derivative of velocity error, not includes setpoint acceleration */
math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt;
math::Vector<3> vel_err_d = (_vel - _vel_prev) / dt;
/* thrust vector in NED frame */
math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int;

38
src/modules/mc_pos_control/mc_pos_control_params.c
View File

@ -265,3 +265,41 @@ PARAM_DEFINE_FLOAT(MPC_MAN_P_MAX, 35.0f);
*/
PARAM_DEFINE_FLOAT(MPC_MAN_Y_MAX, 120.0f);
/**
* Deadzone of X,Y sticks where position hold is enabled
*
* @unit %
* @min 0.0
* @max 1.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_HOLD_XY_DZ, 0.1f);
/**
* Deadzone of Z stick where altitude hold is enabled
*
* @unit %
* @min 0.0
* @max 1.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_HOLD_Z_DZ, 0.1f);
/**
* Maximum horizontal velocity for which position hold is enabled (use 0 to disable check)
*
* @unit m/s
* @min 0.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_XY, 0.5f);
/**
* Maximum vertical velocity for which position hold is enabled (use 0 to disable check)
*
* @unit m/s
* @min 0.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_Z, 0.5f);