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: remove additional land-ground contact logic

Browse Source
This commit is contained in:
Dennis Mannhart
2017-06-30 10:53:16 +02:00
committed by Lorenz Meier
parent 2405abd859
commit 363ed43d38
1 changed files with 0 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -253,10 +253,6 @@ private:
math::Matrix<3, 3> _R; /**< rotation matrix from attitude quaternions */
float _yaw; /**< yaw angle (euler) */
float _yaw_takeoff; /**< home yaw angle present when vehicle was taking off (euler) */
bool _in_landing; /**< the vehicle is in the landing descent */
bool _lnd_reached_ground; /**< controller assumes the vehicle has reached the ground after landing */
float _vel_z_lp;
float _acc_z_lp;
float _vel_max_xy; /**< equal to vel_max except in auto mode when close to target */
bool _in_takeoff = false; /**< flag for smooth velocity setpoint takeoff ramp */
@@ -426,10 +422,6 @@ MulticopterPositionControl::MulticopterPositionControl() :
_pos_first_nonfinite(true),
_yaw(0.0f),
_yaw_takeoff(0.0f),
_in_landing(false),
_lnd_reached_ground(false),
_vel_z_lp(0),
_acc_z_lp(0),
_vel_max_xy(0.0f),
_takeoff_vel_limit(0.0f),
_z_reset_counter(0),
@@ -1862,13 +1854,6 @@ MulticopterPositionControl::calculate_thrust_setpoint(float dt)
float tilt_max = _params.tilt_max_air;
float thr_max = _params.thr_max;
/* filter vel_z over 1/8sec */
_vel_z_lp = _vel_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * _vel(2);
/* filter vel_z change over 1/8sec */
float vel_z_change = (_vel(2) - _vel_prev(2)) / dt;
_acc_z_lp = _acc_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * vel_z_change;
// We can only run the control if we're already in-air, have a takeoff setpoint,
// or if we're in offboard control.
// Otherwise, we should just bail out
@@ -1882,59 +1867,6 @@ MulticopterPositionControl::calculate_thrust_setpoint(float dt)
/* adjust limits for landing mode */
/* limit max tilt and min lift when landing */
tilt_max = _params.tilt_max_land;
if (thr_min < 0.0f) {
thr_min = 0.0f;
}
/* descend stabilized, we're landing */
if (!_in_landing && !_lnd_reached_ground
&& (fabsf(_acc_z_lp) < 0.1f)
&& _vel_z_lp > 0.6f * _params.land_speed) {
_in_landing = true;
}
float land_z_threshold = 0.1f;
/* assume ground, cut thrust */
if (_in_landing
&& _vel_z_lp < land_z_threshold) {
thr_max = 0.0f;
_in_landing = false;
_lnd_reached_ground = true;
} else if (_in_landing
&& _vel_z_lp < math::min(0.3f * _params.land_speed, 2.5f * land_z_threshold)) {
/* not on ground but with ground contact, stop position and velocity control */
thrust_sp(0) = 0.0f;
thrust_sp(1) = 0.0f;
_vel_sp(0) = _vel(0);
_vel_sp(1) = _vel(1);
_pos_sp(0) = _pos(0);
_pos_sp(1) = _pos(1);
}
/* once we assumed to have reached the ground always cut the thrust.
Only free fall detection below can revoke this
*/
if (!_in_landing && _lnd_reached_ground) {
thr_max = 0.0f;
}
/* if we suddenly fall, reset landing logic and remove thrust limit */
if (_lnd_reached_ground
/* XXX: magic value, assuming free fall above 4 m/s^2 acceleration */
&& (_acc_z_lp > 4.0f || _vel_z_lp > 2.0f * _params.land_speed)) {
thr_max = _params.thr_max;
_in_landing = true;
_lnd_reached_ground = false;
}
} else {
_in_landing = false;
_lnd_reached_ground = false;
}
/* limit min lift */