diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2014-01-20 16:46:39 +0100 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2014-01-20 16:46:39 +0100 |
| commit | 08099818003d0e5861dd312ff44674a61c0e6bc3 (patch) | |
| tree | ea3469275c6eab27ecd0ed2736dab59c6db94b0d | |
| parent | 0034c9f0e739e5478cc7005d611817ee08a7ff51 (diff) | |
| download | px4-firmware-08099818003d0e5861dd312ff44674a61c0e6bc3.tar.gz px4-firmware-08099818003d0e5861dd312ff44674a61c0e6bc3.tar.bz2 px4-firmware-08099818003d0e5861dd312ff44674a61c0e6bc3.zip | |
mc_pos_control: limit tilt when landing
| -rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 17 | ||||
| -rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_params.c | 1 |
2 files changed, 16 insertions, 2 deletions
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index c77d25b86..5b5d9f004 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -150,6 +150,7 @@ private: param_t xy_ff; param_t tilt_max; param_t land_speed; + param_t land_tilt_max; param_t rc_scale_pitch; param_t rc_scale_roll; @@ -161,6 +162,7 @@ private: float thr_max; float tilt_max; float land_speed; + float land_tilt_max; float rc_scale_pitch; float rc_scale_roll; @@ -281,6 +283,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.xy_ff = param_find("MPC_XY_FF"); _params_handles.tilt_max = param_find("MPC_TILT_MAX"); _params_handles.land_speed = param_find("MPC_LAND_SPEED"); + _params_handles.land_tilt_max = param_find("MPC_LAND_TILT_MAX"); _params_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH"); _params_handles.rc_scale_roll = param_find("RC_SCALE_ROLL"); _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); @@ -329,6 +332,7 @@ MulticopterPositionControl::parameters_update(bool force) param_get(_params_handles.thr_max, &_params.thr_max); param_get(_params_handles.tilt_max, &_params.tilt_max); param_get(_params_handles.land_speed, &_params.land_speed); + param_get(_params_handles.land_tilt_max, &_params.land_tilt_max); param_get(_params_handles.rc_scale_pitch, &_params.rc_scale_pitch); param_get(_params_handles.rc_scale_roll, &_params.rc_scale_roll); param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw); @@ -784,10 +788,19 @@ MulticopterPositionControl::task_main() /* limit max tilt */ float tilt = atan2f(math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(), -thrust_sp(2)); + float tilt_max = _params.tilt_max; + if (!_control_mode.flag_control_manual_enabled) { + if (_mission_items.current_valid && _mission_items.current.nav_cmd == NAV_CMD_LAND) { + /* limit max tilt and min lift when landing */ + tilt_max = _params.land_tilt_max; + if (thr_min < 0.0f) + thr_min = 0.0f; + } + } - if (tilt > _params.tilt_max && _params.thr_min >= 0.0f) { + if (tilt > tilt_max && thr_min >= 0.0f) { /* crop horizontal component */ - float k = tanf(_params.tilt_max) / tanf(tilt); + float k = tanf(tilt_max) / tanf(tilt); thrust_sp(0) *= k; thrust_sp(1) *= k; saturation_xy = true; diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index a00bc3644..8df88617a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -55,3 +55,4 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 1.0f); PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f); +PARAM_DEFINE_FLOAT(MPC_LAND_TILT_MAX, 0.4f); |