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

mpc: add parameter for ascent/descent speed in auto modes

Browse Source
This commit is contained in:
bresch
2022-01-12 13:33:48 +01:00
committed by Mathieu Bresciani
parent 463513f31f
commit ea7d2334c9
11 changed files with 97 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/MulticopterPositionControl.cpp
+24
View File
@@ -127,7 +127,9 @@ int MulticopterPositionControl::parameters_update(bool force)
if (_param_mpc_z_vel_all.get() >= 0.f) {
float z_vel = _param_mpc_z_vel_all.get();
num_changed += _param_mpc_z_v_auto_up.commit_no_notification(z_vel);
num_changed += _param_mpc_z_vel_max_up.commit_no_notification(z_vel);
num_changed += _param_mpc_z_v_auto_dn.commit_no_notification(z_vel * 0.75f);
num_changed += _param_mpc_z_vel_max_dn.commit_no_notification(z_vel * 0.75f);
num_changed += _param_mpc_tko_speed.commit_no_notification(z_vel * 0.6f);
num_changed += _param_mpc_land_speed.commit_no_notification(z_vel * 0.5f);
@@ -189,6 +191,28 @@ int MulticopterPositionControl::parameters_update(bool force)
"Manual speed has been constrained by maximum speed", _param_mpc_xy_vel_max.get());
}
if (_param_mpc_z_v_auto_up.get() > _param_mpc_z_vel_max_up.get()) {
_param_mpc_z_v_auto_up.set(_param_mpc_z_vel_max_up.get());
_param_mpc_z_v_auto_up.commit();
mavlink_log_critical(&_mavlink_log_pub, "Ascent speed has been constrained by max speed\t");
/* EVENT
* @description <param>MPC_Z_V_AUTO_UP</param> is set to {1:.0}.
*/
events::send<float>(events::ID("mc_pos_ctrl_up_vel_set"), events::Log::Warning,
"Ascent speed has been constrained by max speed", _param_mpc_z_vel_max_up.get());
}
if (_param_mpc_z_v_auto_dn.get() > _param_mpc_z_vel_max_dn.get()) {
_param_mpc_z_v_auto_dn.set(_param_mpc_z_vel_max_dn.get());
_param_mpc_z_v_auto_dn.commit();
mavlink_log_critical(&_mavlink_log_pub, "Descent speed has been constrained by max speed\t");
/* EVENT
* @description <param>MPC_Z_V_AUTO_DN</param> is set to {1:.0}.
*/
events::send<float>(events::ID("mc_pos_ctrl_down_vel_set"), events::Log::Warning,
"Descent speed has been constrained by max speed", _param_mpc_z_vel_max_dn.get());
}
if (_param_mpc_thr_hover.get() > _param_mpc_thr_max.get() ||
_param_mpc_thr_hover.get() < _param_mpc_thr_min.get()) {
_param_mpc_thr_hover.set(math::constrain(_param_mpc_thr_hover.get(), _param_mpc_thr_min.get(),
src/modules/mc_pos_control/MulticopterPositionControl.hpp
+2
View File
@@ -137,7 +137,9 @@ private:
(ParamFloat<px4::params::MPC_Z_VEL_I_ACC>) _param_mpc_z_vel_i_acc,
(ParamFloat<px4::params::MPC_Z_VEL_D_ACC>) _param_mpc_z_vel_d_acc,
(ParamFloat<px4::params::MPC_XY_VEL_MAX>) _param_mpc_xy_vel_max,
(ParamFloat<px4::params::MPC_Z_V_AUTO_UP>) _param_mpc_z_v_auto_up,
(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up,
(ParamFloat<px4::params::MPC_Z_V_AUTO_DN>) _param_mpc_z_v_auto_dn,
(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn,
(ParamFloat<px4::params::MPC_TILTMAX_AIR>) _param_mpc_tiltmax_air,
(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover,
src/modules/mc_pos_control/mc_pos_control_params.c
+42 -7
View File
@@ -200,29 +200,64 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_I_ACC, 2.0f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_D_ACC, 0.0f);
/**
* Maximum vertical ascent velocity
* Automatic ascent velocity
*
* Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL, POSCTRL).
* Ascent velocity in auto modes.
* For manual modes and offboard, see MPC_Z_VEL_MAX_UP
*
* @unit m/s
* @min 0.5
* @max 8.0
* @increment 0.1
* @decimal 1
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_UP, 3.0f);
PARAM_DEFINE_FLOAT(MPC_Z_V_AUTO_UP, 3.f);
/**
* Maximum vertical descent velocity
* Maximum ascent velocity
*
* Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL, POSCTRL).
* Ascent velocity in manual modes and offboard.
* For auto modes, see MPC_Z_V_AUTO_UP
*
* @unit m/s
* @min 0.5
* @max 8.0
* @increment 0.1
* @decimal 1
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_UP, 3.f);
/**
* Automatic descent velocity
*
* Descent velocity in auto modes.
* For manual modes and offboard, see MPC_Z_VEL_MAX_DN
*
* @unit m/s
* @min 0.5
* @max 4.0
* @increment 0.1
* @decimal 1
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_DN, 1.0f);
PARAM_DEFINE_FLOAT(MPC_Z_V_AUTO_DN, 1.f);
/**
* Maximum descent velocity
*
* Descent velocity in manual modes and offboard.
* For auto modes, see MPC_Z_V_AUTO_DN
*
* @unit m/s
* @min 0.5
* @max 4.0
* @increment 0.1
* @decimal 1
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_DN, 1.f);
/**
* Proportional gain for horizontal position error
@@ -664,7 +699,7 @@ PARAM_DEFINE_FLOAT(MPC_YAWRAUTO_MAX, 45.0f);
* Altitude for 1. step of slow landing (descend)
*
* Below this altitude descending velocity gets limited to a value
* between "MPC_Z_VEL_MAX_DN" and "MPC_LAND_SPEED"
* between "MPC_Z_VEL_MAX_DN" (or "MPC_Z_V_AUTO_DN") and "MPC_LAND_SPEED"
* Value needs to be higher than "MPC_LAND_ALT2"
*
* @unit m