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

MC pos ctrl: Force slow landing speed below min loiter altitude

Browse Source
This commit is contained in:
Lorenz Meier 2017-02-07 14:43:59 +01:00 committed by Lorenz Meier
parent 1d370f3470
commit 4ddc7e22fb
1 changed files with 13 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -160,6 +160,7 @@ private:
control::BlockParamFloat _manual_thr_min;
control::BlockParamFloat _manual_thr_max;
control::BlockParamFloat _manual_land_alt;
control::BlockDerivative _vel_x_deriv;
control::BlockDerivative _vel_y_deriv;
@ -420,6 +421,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
_global_vel_sp{},
_manual_thr_min(this, "MANTHR_MIN"),
_manual_thr_max(this, "MANTHR_MAX"),
_manual_land_alt(this, "MIS_LTRMIN_ALT", false),
_vel_x_deriv(this, "VELD"),
_vel_y_deriv(this, "VELD"),
_vel_z_deriv(this, "VELD"),
@ -1636,7 +1638,17 @@ MulticopterPositionControl::control_position(float dt)
_vel_sp(2) = -1.0f * _params.vel_max_up;
}
if (_vel_sp(2) > _params.vel_max_down) {
/*
* Make sure downward velocity (positive Z) is limited close to ground.
* for now we use the home altitude and assume that our Z coordinate
* is initialized close to home.
*/
bool close_to_ground = -_pos(2) < _manual_land_alt.get();
if (close_to_ground && (_vel_sp(2) > _params.land_speed)) {
_vel_sp(2) = _params.land_speed;
} else if (_vel_sp(2) > _params.vel_max_down) {
_vel_sp(2) = _params.vel_max_down;
}