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

mpc: add sideways and backward speed for manual position modes

Browse Source
This commit is contained in:
bresch
2022-07-08 14:03:44 +02:00
committed by Mathieu Bresciani
parent 1de38c88d9
commit 3b26c611af
7 changed files with 81 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/MulticopterPositionControl.cpp
+24
View File
@@ -120,6 +120,8 @@ void MulticopterPositionControl::parameters_update(bool force)
if (_param_mpc_xy_vel_all.get() >= 0.f) {
float xy_vel = _param_mpc_xy_vel_all.get();
num_changed += _param_mpc_vel_manual.commit_no_notification(xy_vel);
num_changed += _param_mpc_vel_man_back.commit_no_notification(-1.f);
num_changed += _param_mpc_vel_man_side.commit_no_notification(-1.f);
num_changed += _param_mpc_xy_cruise.commit_no_notification(xy_vel);
num_changed += _param_mpc_xy_vel_max.commit_no_notification(xy_vel);
}
@@ -190,6 +192,28 @@ void MulticopterPositionControl::parameters_update(bool force)
"Manual speed has been constrained by maximum speed", _param_mpc_xy_vel_max.get());
}
if (_param_mpc_vel_man_back.get() > _param_mpc_vel_manual.get()) {
_param_mpc_vel_man_back.set(_param_mpc_vel_manual.get());
_param_mpc_vel_man_back.commit();
mavlink_log_critical(&_mavlink_log_pub, "Manual backward speed has been constrained by forward speed\t");
/* EVENT
* @description <param>MPC_VEL_MAN_BACK</param> is set to {1:.0}.
*/
events::send<float>(events::ID("mc_pos_ctrl_man_vel_back_set"), events::Log::Warning,
"Manual backward speed has been constrained by forward speed", _param_mpc_vel_manual.get());
}
if (_param_mpc_vel_man_side.get() > _param_mpc_vel_manual.get()) {
_param_mpc_vel_man_side.set(_param_mpc_vel_manual.get());
_param_mpc_vel_man_side.commit();
mavlink_log_critical(&_mavlink_log_pub, "Manual sideways speed has been constrained by forward speed\t");
/* EVENT
* @description <param>MPC_VEL_MAN_SIDE</param> is set to {1:.0}.
*/
events::send<float>(events::ID("mc_pos_ctrl_man_vel_side_set"), events::Log::Warning,
"Manual sideways speed has been constrained by forward speed", _param_mpc_vel_manual.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();
src/modules/mc_pos_control/MulticopterPositionControl.hpp
+2
View File
@@ -153,6 +153,8 @@ private:
(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
(ParamFloat<px4::params::MPC_VEL_MAN_BACK>) _param_mpc_vel_man_back,
(ParamFloat<px4::params::MPC_VEL_MAN_SIDE>) _param_mpc_vel_man_side,
(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise,
(ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2, /**< downwards speed limited below this altitude */
(ParamInt<px4::params::MPC_POS_MODE>) _param_mpc_pos_mode,
src/modules/mc_pos_control/mc_pos_control_params.c
+34 -1
View File
@@ -349,11 +349,14 @@ PARAM_DEFINE_FLOAT(MPC_XY_TRAJ_P, 0.5f);
PARAM_DEFINE_FLOAT(MPC_XY_ERR_MAX, 2.0f);
/**
* Maximum horizontal velocity setpoint for manual controlled mode
* Maximum horizontal velocity setpoint in Position mode
*
* If velocity setpoint larger than MPC_XY_VEL_MAX is set, then
* the setpoint will be capped to MPC_XY_VEL_MAX
*
* The maximum sideways and backward speed can be set differently
* using MPC_VEL_MAN_SIDE and MPC_VEL_MAN_BACK, respectively.
*
* @unit m/s
* @min 3.0
* @max 20.0
@@ -363,6 +366,36 @@ PARAM_DEFINE_FLOAT(MPC_XY_ERR_MAX, 2.0f);
*/
PARAM_DEFINE_FLOAT(MPC_VEL_MANUAL, 10.0f);
/**
* Maximum sideways velocity in Position mode
*
* If set to a negative value or larger than
* MPC_VEL_MANUAL then MPC_VEL_MANUAL is used.
*
* @unit m/s
* @min -1.0
* @max 20.0
* @increment 0.1
* @decimal 2
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_VEL_MAN_SIDE, -1.0f);
/**
* Maximum backward velocity in Position mode
*
* If set to a negative value or larger than
* MPC_VEL_MANUAL then MPC_VEL_MANUAL is used.
*
* @unit m/s
* @min -1.0
* @max 20.0
* @increment 0.1
* @decimal 2
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_VEL_MAN_BACK, -1.0f);
/**
* Maximum horizontal velocity
*