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

also filter acceleration to filter out the bump on the ground on landing

Browse Source
This commit is contained in:
Andreas Antener
2015-12-22 10:41:12 +01:00
committed by Lorenz Meier
parent e0405617ef
commit 98bec0e175
1 changed files with 12 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -249,6 +249,7 @@ private:
hrt_abstime _landing_started;
bool _takeoff_jumped;
float _vel_z_lp;
float _acc_z_lp;
/**
* Update our local parameter cache.
@@ -382,7 +383,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
_landing_thrust(1.0f),
_landing_started(0),
_takeoff_jumped(false),
_vel_z_lp(0)
_vel_z_lp(0),
_acc_z_lp(0)
{
memset(&_vehicle_status, 0, sizeof(_vehicle_status));
memset(&_ctrl_state, 0, sizeof(_ctrl_state));
@@ -1478,6 +1480,9 @@ MulticopterPositionControl::task_main()
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;
/* adjust limits for landing mode */
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid &&
@@ -1492,12 +1497,11 @@ MulticopterPositionControl::task_main()
if (_landing_started == 0) {
_landing_started = hrt_absolute_time();
}
float vel_z_change = (_vel(2) - _vel_prev(2)) / dt;
/* don't let it throttle up again during landing */
if (thrust_sp(2) < 0.0f && thrust_abs < _landing_thrust
/* fix landing thrust after a certain time when velocity change is minimal */
&& (float)fabs(vel_z_change) < 0.05f
&& (float)fabs(_acc_z_lp) < 0.1f
&& _vel_z_lp > 0.5f * _params.land_speed
&& hrt_elapsed_time(&_landing_started) > 15e5) {
_landing_thrust = thrust_abs;
@@ -1506,7 +1510,8 @@ MulticopterPositionControl::task_main()
/* assume ground, reduce thrust */
if (hrt_elapsed_time(&_landing_started) > 15e5
&& _landing_thrust > FLT_EPSILON
&& _vel_z_lp < 0.1f) {
&& _vel_z_lp < 0.1f
) {
_landing_thrust = 0.0f;
}
@@ -1514,7 +1519,9 @@ MulticopterPositionControl::task_main()
if (hrt_elapsed_time(&_landing_started) > 15e5
&& _landing_thrust < FLT_EPSILON
/* XXX: magic value, assuming free fall above 4m/s2 acceleration */
&& vel_z_change > 4.0f) {
&& (_acc_z_lp > 4.0f
|| _vel_z_lp > 2.0f * _params.land_speed)
) {
_landing_thrust = _params.thr_max;
_landing_started = 0;
}