From 72fb7a5062989dcebc7607ebab0e870b72584106 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 21 Mar 2017 18:52:18 +0100 Subject: [PATCH] mc_pos_control: added gradual landing speed logic depending on two altitudes that can get set as parameter the logic linearly slows down from higher land altitude 1 to slower land altitude 2 --- .../mc_pos_control/mc_pos_control_main.cpp | 47 ++++++++++++++----- .../mc_pos_control/mc_pos_control_params.c | 30 ++++++++++++ 2 files changed, 64 insertions(+), 13 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 417e608fcb..a3f33bcba0 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -186,6 +186,8 @@ private: param_t z_vel_max_up; param_t z_vel_max_down; param_t z_ff; + param_t slow_land_alt1; + param_t slow_land_alt2; param_t xy_p; param_t xy_vel_p; param_t xy_vel_i; @@ -231,6 +233,8 @@ private: float vel_cruise_xy; float vel_max_up; float vel_max_down; + float slow_land_alt1; + float slow_land_alt2; uint32_t alt_mode; int opt_recover; @@ -313,6 +317,7 @@ private: void poll_subscriptions(); static float throttle_curve(float ctl, float ctr); + void _slow_land_gradual_velocity_limit(); /** * Update reference for local position projection @@ -515,6 +520,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.xy_vel_max = param_find("MPC_XY_VEL_MAX"); _params_handles.xy_vel_cruise = param_find("MPC_XY_CRUISE"); _params_handles.xy_ff = param_find("MPC_XY_FF"); + _params_handles.slow_land_alt1 = param_find("MPC_LAND_ALT1"); + _params_handles.slow_land_alt2 = param_find("MPC_LAND_ALT2"); _params_handles.tilt_max_air = param_find("MPC_TILTMAX_AIR"); _params_handles.land_speed = param_find("MPC_LAND_SPEED"); _params_handles.tko_speed = param_find("MPC_TKO_SPEED"); @@ -637,6 +644,11 @@ MulticopterPositionControl::parameters_update(bool force) /* make sure that vel_cruise_xy is always smaller than vel_max */ _params.vel_cruise_xy = math::min(_params.vel_cruise_xy, _params.vel_max_xy); + param_get(_params_handles.slow_land_alt2, &v); + _params.slow_land_alt2 = v; + param_get(_params_handles.slow_land_alt1, &v); + _params.slow_land_alt1 = math::max(v, _params.slow_land_alt2); + param_get(_params_handles.alt_mode, &v_i); _params.alt_mode = v_i; @@ -929,7 +941,28 @@ MulticopterPositionControl::get_cruising_speed_xy() _pos_sp_triplet.current.cruising_speed : _params.vel_cruise_xy); } +void +MulticopterPositionControl::_slow_land_gradual_velocity_limit() +{ + /* + * Make sure downward velocity (positive Z) is limited close to ground. + * for now we use the home altitude and assume that we want to land on a similar absolute altitude. + */ + float altitude_above_home = -_pos(2) + _home_pos.z; + float vel_limit = _params.vel_max_down; + + if (altitude_above_home < _params.slow_land_alt2) { + vel_limit = _params.land_speed; + } else if (altitude_above_home < _params.slow_land_alt1) { + /* linear function between the two altitudes */ + float a = (_params.vel_max_down - _params.land_speed) / (_params.slow_land_alt1 - _params.slow_land_alt2); + float b = _params.land_speed - a * _params.slow_land_alt2; + vel_limit = a * altitude_above_home + b; + } + + _vel_sp(2) = math::min(_vel_sp(2), vel_limit); +} void MulticopterPositionControl::control_manual(float dt) @@ -1734,19 +1767,7 @@ MulticopterPositionControl::control_position(float dt) _vel_sp(2) = -1.0f * _params.vel_max_up; } - /* - * 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) + _home_pos.z) < _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; - } + _slow_land_gradual_velocity_limit(); if (!_control_mode.flag_control_position_enabled) { _reset_pos_sp = 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 447d9c05f5..61e8b91f0c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -512,3 +512,33 @@ PARAM_DEFINE_FLOAT(MPC_XY_MAN_EXPO, 0.0f); * @group Multicopter Position Control */ PARAM_DEFINE_FLOAT(MPC_Z_MAN_EXPO, 0.0f); + +/** + * Altitude for 1. step of slow landing (descend) + * + * Below this altitude descending velocity gets limited + * to a value between "MPC_Z_VEL_MAX" and "MPC_LAND_SPEED" + * to enable a smooth descent experience + * Value needs to be higher than "MPC_LAND_ALT2" + * + * @unit m + * @min 0 + * @max 122 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_LAND_ALT1, 10.0f); + +/** + * Altitude for 2. step of slow landing (landing) + * + * Below this altitude descending velocity gets limited to "MPC_LAND_SPEED" + * Value needs to be lower than "MPC_LAND_ALT1" + * + * @unit m + * @min 0 + * @max 122 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_LAND_ALT2, 5.0f);