fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-20 12:59:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_pos_control: limit slewrate different in up and down direction

Browse Source
This commit is contained in:
Dennis Mannhart 2017-02-01 15:06:16 +01:00 committed by Lorenz Meier
parent c976a26156
commit f718b3a97a
2 changed files with 50 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -198,6 +198,8 @@ private:
param_t hold_max_xy;
param_t hold_max_z;
param_t acc_hor_max;
param_t acc_up_max;
param_t acc_down_max;
param_t alt_mode;
param_t opt_recover;
@ -222,6 +224,8 @@ private:
float hold_max_xy;
float hold_max_z;
float acc_hor_max;
float acc_up_max;
float acc_down_max;
float vel_max_up;
float vel_max_down;
uint32_t alt_mode;
@ -354,7 +358,7 @@ private:
void control_position(float dt);
void limit_acceleration(float dt);
void vel_sp_slewrate(float dt);
/**
* Select between barometric and global (AMSL) altitudes
@ -515,6 +519,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
_params_handles.hold_max_xy = param_find("MPC_HOLD_MAX_XY");
_params_handles.hold_max_z = param_find("MPC_HOLD_MAX_Z");
_params_handles.acc_hor_max = param_find("MPC_ACC_HOR_MAX");
_params_handles.acc_up_max = param_find("MPC_ACC_UP_MAX");
_params_handles.acc_down_max = param_find("MPC_ACC_DOWN_MAX");
_params_handles.alt_mode = param_find("MPC_ALT_MODE");
_params_handles.opt_recover = param_find("VT_OPT_RECOV_EN");
@ -628,6 +634,10 @@ MulticopterPositionControl::parameters_update(bool force)
_params.hold_max_z = (v < 0.0f ? 0.0f : v);
param_get(_params_handles.acc_hor_max, &v);
_params.acc_hor_max = v;
param_get(_params_handles.acc_up_max, &v);
_params.acc_up_max = v;
param_get(_params_handles.acc_down_max, &v);
_params.acc_down_max = v;
/*
* increase the maximum horizontal acceleration such that stopping
* within 1 s from full speed is feasible
@ -1283,29 +1293,30 @@ MulticopterPositionControl::control_offboard(float dt)
}
void
MulticopterPositionControl::limit_acceleration(float dt)
MulticopterPositionControl::vel_sp_slewrate(float dt)
{
// limit total horizontal acceleration
/* limit total horizontal acceleration */
math::Vector<2> acc_hor;
acc_hor(0) = (_vel_sp(0) - _vel_sp_prev(0)) / dt;
acc_hor(1) = (_vel_sp(1) - _vel_sp_prev(1)) / dt;
if (acc_hor.length() > _params.acc_hor_max) {
acc_hor.normalize();
acc_hor *= _params.acc_hor_max;
math::Vector<2> vel_sp_hor_prev(_vel_sp_prev(0), _vel_sp_prev(1));
math::Vector<2> vel_sp_hor = acc_hor * dt + vel_sp_hor_prev;
_vel_sp(0) = vel_sp_hor(0);
_vel_sp(1) = vel_sp_hor(1);
acc_hor = acc_hor.normalized() * _params.acc_hor_max;
_vel_sp(0) = acc_hor(0) * dt + _vel_sp_prev(0);
_vel_sp(1) = acc_hor(1) * dt + _vel_sp_prev(1);
}
// limit vertical acceleration
/* limit vertical acceleration */
float acc_v = (_vel_sp(2) - _vel_sp_prev(2)) / dt;
// TODO: vertical acceleration is not just 2 * horizontal acceleration
if (fabsf(acc_v) > 2 * _params.acc_hor_max) {
acc_v /= fabsf(acc_v);
_vel_sp(2) = acc_v * 2 * _params.acc_hor_max * dt + _vel_sp_prev(2);
/* accelerate up */
if (acc_v < 0.0f && fabsf(acc_v) > _params.acc_up_max) {
_vel_sp(2) = -_params.acc_up_max * dt + _vel_sp_prev(2);
}
/*accelerate down */
if (acc_v >= 0.0f && fabsf(acc_v) > _params.acc_down_max) {
_vel_sp(2) = _params.acc_down_max * dt + _vel_sp_prev(2);
}
}
@ -1688,7 +1699,7 @@ MulticopterPositionControl::control_position(float dt)
_takeoff_thrust_sp = 0.0f;
}
limit_acceleration(dt);
vel_sp_slewrate(dt);
_vel_sp_prev = _vel_sp;

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

@ -458,6 +458,30 @@ PARAM_DEFINE_FLOAT(MPC_VELD_LP, 5.0f);
*/
PARAM_DEFINE_FLOAT(MPC_ACC_HOR_MAX, 5.0f);
/**
* Maximum vertical acceleration in velocity controlled modes upward
*
* @unit m/s/s
* @min 2.0
* @max 15.0
* @increment 1
* @decimal 2
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_ACC_UP_MAX, 10.0f);
/**
* Maximum vertical acceleration in velocity controlled modes down
*
* @unit m/s/s
* @min 2.0
* @max 15.0
* @increment 1
* @decimal 2
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_ACC_DOWN_MAX, 5.0f);
/**
* Altitude control mode, note mode 1 only tested with LPE
*