yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

mc_pos_control: added gradual landing speed logic

Browse Source 
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
This commit is contained in:
Matthias Grob
2017-03-21 18:52:18 +01:00
committed by Dennis Mannhart
parent 7434bcc693
commit 72fb7a5062
2 changed files with 64 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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;

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

@@ -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);