Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 06:36:45 +08:00
Code Issues Actions 20 Packages Projects Releases Wiki Activity

separate _DZ parameter to XY and Z. this allows disabling pos hold for XY and not Z

Browse Source
This commit is contained in:
v01d
2015-09-29 15:51:05 -03:00
committed by tumbili
parent 9c49b30118
commit 44bb50e9ea
2 changed files with 25 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+13 -7
View File
@@ -177,7 +177,8 @@ private:
param_t man_pitch_max;
param_t man_yaw_max;
param_t mc_att_yaw_p;
param_t hold_dz;
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 */
@@ -192,7 +193,8 @@ private:
float man_pitch_max;
float man_yaw_max;
float mc_att_yaw_p;
float hold_dz;
float hold_xy_dz;
float hold_z_dz;
float hold_max_xy;
float hold_max_z;
@@ -389,7 +391,8 @@ 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_dz = param_find("MPC_HOLD_DZ");
_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");
@@ -480,9 +483,12 @@ 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_dz, &v);
param_get(_params_handles.hold_xy_dz, &v);
v = math::constrain(v, 0.0f, 1.0f);
_params.hold_dz = v;
_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);
@@ -695,7 +701,7 @@ MulticopterPositionControl::control_manual(float dt)
if (_control_mode.flag_control_position_enabled)
{
/* check for pos. hold */
if (fabsf(req_vel_sp(0)) < _params.hold_dz && fabsf(req_vel_sp(1)) < _params.hold_dz)
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)))
@@ -721,7 +727,7 @@ MulticopterPositionControl::control_manual(float dt)
if (_control_mode.flag_control_altitude_enabled)
{
/* check for pos. hold */
if (fabsf(req_vel_sp(2)) < _params.hold_dz)
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))
{
src/modules/mc_pos_control/mc_pos_control_params.c
+12 -2
View File
@@ -266,14 +266,24 @@ PARAM_DEFINE_FLOAT(MPC_MAN_P_MAX, 35.0f);
PARAM_DEFINE_FLOAT(MPC_MAN_Y_MAX, 120.0f);
/**
* Deadzone of X,Y,Z where position hold is enabled
* 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_DZ, 0.1f);
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)