fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 11:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

added different parameters for ascent and descent rates

Browse Source
This commit is contained in:
Adyasha Dash
2016-05-10 09:18:40 +02:00
committed by Lorenz Meier
parent ffb0d37c8a
commit 468b0b25de
2 changed files with 33 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+20 -12
View File
@@ -174,7 +174,8 @@ private:
param_t z_vel_p; param_t z_vel_p;
param_t z_vel_i; param_t z_vel_i;
param_t z_vel_d; param_t z_vel_d;
param_t z_vel_max; param_t z_vel_max_up;
param_t z_vel_max_down;
param_t z_ff; param_t z_ff;
param_t xy_p; param_t xy_p;
param_t xy_vel_p; param_t xy_vel_p;
@@ -219,6 +220,8 @@ private:
float hold_max_xy; float hold_max_xy;
float hold_max_z; float hold_max_z;
float acc_hor_max; float acc_hor_max;
float vel_max_up;
float vel_max_down;
uint32_t alt_mode; uint32_t alt_mode;
math::Vector<3> pos_p; math::Vector<3> pos_p;
@@ -438,7 +441,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
_params_handles.z_vel_p = param_find("MPC_Z_VEL_P"); _params_handles.z_vel_p = param_find("MPC_Z_VEL_P");
_params_handles.z_vel_i = param_find("MPC_Z_VEL_I"); _params_handles.z_vel_i = param_find("MPC_Z_VEL_I");
_params_handles.z_vel_d = param_find("MPC_Z_VEL_D"); _params_handles.z_vel_d = param_find("MPC_Z_VEL_D");
_params_handles.z_vel_max = param_find("MPC_Z_VEL_MAX"); _params_handles.z_vel_max_up = param_find("MPC_Z_VEL_MAX_UP");
_params_handles.z_vel_max_down = param_find("MPC_Z_VEL_MAX_DOWN");
_params_handles.z_ff = param_find("MPC_Z_FF"); _params_handles.z_ff = param_find("MPC_Z_FF");
_params_handles.xy_p = param_find("MPC_XY_P"); _params_handles.xy_p = param_find("MPC_XY_P");
_params_handles.xy_vel_p = param_find("MPC_XY_VEL_P"); _params_handles.xy_vel_p = param_find("MPC_XY_VEL_P");
@@ -544,13 +548,16 @@ MulticopterPositionControl::parameters_update(bool force)
param_get(_params_handles.xy_vel_max, &v); param_get(_params_handles.xy_vel_max, &v);
_params.vel_max(0) = v; _params.vel_max(0) = v;
_params.vel_max(1) = v; _params.vel_max(1) = v;
param_get(_params_handles.z_vel_max, &v); param_get(_params_handles.z_vel_max_up, &v);
_params.vel_max_up = v;
_params.vel_max(2) = v; _params.vel_max(2) = v;
param_get(_params_handles.z_vel_max_down, &v);
_params.vel_max_down = v;
param_get(_params_handles.xy_vel_cruise, &v); param_get(_params_handles.xy_vel_cruise, &v);
_params.vel_cruise(0) = v; _params.vel_cruise(0) = v;
_params.vel_cruise(1) = v; _params.vel_cruise(1) = v;
/* using Z max for now */ /* using Z max up for now */
param_get(_params_handles.z_vel_max, &v); param_get(_params_handles.z_vel_max_up, &v);
_params.vel_cruise(2) = v; _params.vel_cruise(2) = v;
param_get(_params_handles.xy_ff, &v); param_get(_params_handles.xy_ff, &v);
v = math::constrain(v, 0.0f, 1.0f); v = math::constrain(v, 0.0f, 1.0f);
@@ -588,8 +595,8 @@ MulticopterPositionControl::parameters_update(bool force)
_params.mc_att_yaw_p = v; _params.mc_att_yaw_p = v;
/* takeoff and land velocities should not exceed maximum */ /* takeoff and land velocities should not exceed maximum */
_params.tko_speed = fminf(_params.tko_speed, _params.vel_max(2)); _params.tko_speed = fminf(_params.tko_speed, _params.vel_max_up);
_params.land_speed = fminf(_params.land_speed, _params.vel_max(2)); _params.land_speed = fminf(_params.land_speed, _params.vel_max_down);
} }
return OK; return OK;
@@ -1448,10 +1455,11 @@ MulticopterPositionControl::task_main()
} }
/* make sure velocity setpoint is saturated in z*/ /* make sure velocity setpoint is saturated in z*/
float vel_norm_z = sqrtf(_vel_sp(2) * _vel_sp(2)); if (_vel_sp(2) < -1.0f * _params.vel_max_up){
_vel_sp(2) = -1.0f * _params.vel_max_up;
if (vel_norm_z > _params.vel_max(2)) { }
_vel_sp(2) = _vel_sp(2) * _params.vel_max(2) / vel_norm_z; if (_vel_sp(2) > _params.vel_max_down) {
_vel_sp(2) = _params.vel_max_down;
} }
if (!_control_mode.flag_control_position_enabled) { if (!_control_mode.flag_control_position_enabled) {
@@ -1617,7 +1625,7 @@ MulticopterPositionControl::task_main()
if (_control_mode.flag_control_acceleration_enabled && _pos_sp_triplet.current.acceleration_valid) { if (_control_mode.flag_control_acceleration_enabled && _pos_sp_triplet.current.acceleration_valid) {
thrust_sp = math::Vector<3>(_pos_sp_triplet.current.a_x,_pos_sp_triplet.current.a_y,_pos_sp_triplet.current.a_z); thrust_sp = math::Vector<3>(_pos_sp_triplet.current.a_x,_pos_sp_triplet.current.a_y,_pos_sp_triplet.current.a_z);
} else { } else {
thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d) + thrust_int; thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d) + thrust_int;
} }
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
src/modules/mc_pos_control/mc_pos_control_params.c
+13 -1
View File
@@ -199,7 +199,19 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
* @decimal 1 * @decimal 1
* @group Multicopter Position Control * @group Multicopter Position Control
*/ */
PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f); PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_UP, 3.0f);
/**
* Maximum vertical velocity in positive z direction (descending)
*
* Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL, POSCTRL).
*
* @unit m/s
* @min 0.0
* @max 8.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_DOWN, 1.0f);
/** /**
* Vertical velocity feed forward * Vertical velocity feed forward