dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Added mpc_alt_mode.

This commit is contained in:
James Goppert 2016-04-14 17:52:28 -04:00
parent 31cef095fe
commit f905276958
2 changed files with 27 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -196,6 +196,7 @@ private:
param_t hold_max_xy;
param_t hold_max_z;
param_t acc_hor_max;
param_t alt_mode;
} _params_handles; /**< handles for interesting parameters */
@ -218,6 +219,7 @@ private:
float hold_max_xy;
float hold_max_z;
float acc_hor_max;
uint32_t alt_mode;
math::Vector<3> pos_p;
math::Vector<3> vel_p;
@ -458,6 +460,7 @@ 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.alt_mode = param_find("MPC_ALT_MODE");
/* fetch initial parameter values */
parameters_update(true);
@ -517,6 +520,7 @@ MulticopterPositionControl::parameters_update(bool force)
_params.tilt_max_land = math::radians(_params.tilt_max_land);
float v;
uint32_t v_i;
param_get(_params_handles.xy_p, &v);
_params.pos_p(0) = v;
_params.pos_p(1) = v;
@ -564,6 +568,8 @@ 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.alt_mode, &v_i);
_params.alt_mode = v_i;
_params.sp_offs_max = _params.vel_cruise.edivide(_params.pos_p) * 2.0f;
@ -1264,7 +1270,11 @@ MulticopterPositionControl::task_main()
_pos(0) = _local_pos.x;
_pos(1) = _local_pos.y;
_pos(2) = _local_pos.z;
if (_params.alt_mode == 1 && _local_pos.dist_bottom_valid) {
_pos(2) = -_local_pos.dist_bottom;
} else {
_pos(2) = _local_pos.z;
}
}
if (PX4_ISFINITE(_local_pos.vx) &&
@ -1273,7 +1283,11 @@ MulticopterPositionControl::task_main()
_vel(0) = _local_pos.vx;
_vel(1) = _local_pos.vy;
_vel(2) = _local_pos.vz;
if (_params.alt_mode == 1 && _local_pos.dist_bottom_valid) {
_vel(2) = -_local_pos.dist_bottom_rate;
} else {
_vel(2) = _local_pos.vz;
}
}
_vel_err_d(0) = _vel_x_deriv.update(-_vel(0));

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

@ -402,3 +402,14 @@ PARAM_DEFINE_FLOAT(MPC_VELD_LP, 5.0f);
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_ACC_HOR_MAX, 10.0f);
/**
* Altitude control mode
*
* @min 0
* @max 1
* @value 0 Altitude following
* @value 1 Terrain following
* @group Multicopter Position Control
*/
PARAM_DEFINE_INT32(MPC_ALT_MODE, 0);