From f961a12d9a31df67e0dc77319d4d21e9535575ba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 17 Jan 2017 10:05:11 +0100 Subject: [PATCH] MC position controller: Stop XY control once ground contact is established --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) 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 4d784d80dd..5f1166c4d9 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1750,6 +1750,16 @@ MulticopterPositionControl::control_position(float dt) thrust_sp(1) = 0.0f; } + /* if still or already on ground command a zero XY velocity */ + if (_vehicle_land_detected.ground_contact) { + 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); + } + if (!_control_mode.flag_control_climb_rate_enabled && !_control_mode.flag_control_acceleration_enabled) { thrust_sp(2) = 0.0f; } @@ -1818,6 +1828,10 @@ MulticopterPositionControl::control_position(float dt) /* 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.