From 2890af7305c0addce875ed34d96ac3271f2c235f Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Wed, 24 May 2017 18:56:29 +0200 Subject: [PATCH] mc_pos_control: set thrust to zero once maybe_landed is reached --- .../mc_pos_control/mc_pos_control_main.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 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 090d8ecb59..a2f486b22d 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1818,6 +1818,10 @@ MulticopterPositionControl::calculate_thrust_setpoint(float dt) /* if still or already on ground command zero xy velcoity and zero xy thrust_sp in body frame to consider uneven ground */ if (_vehicle_land_detected.ground_contact && !in_auto_takeoff()) { + + /* if still or already on ground command zero xy thrust_sp in body + * frame to consider uneven ground */ + /* thrust setpoint in body frame*/ math::Vector<3> thrust_sp_body = _R.transposed() * thrust_sp; @@ -1831,11 +1835,12 @@ MulticopterPositionControl::calculate_thrust_setpoint(float dt) /* convert back to local frame (NED) */ thrust_sp = _R * thrust_sp_body; - /* set velocity setpoint to zero and reset position */ - _vel_sp(0) = 0.0f; - _vel_sp(1) = 0.0f; - _pos_sp(0) = _pos(0); - _pos_sp(1) = _pos(1); + + } else if (_vehicle_land_detected.maybe_landed) { + /* we set thrust to zero + * this will help to decide if we are actually landed or not + */ + thrust_sp.zero(); } if (!_control_mode.flag_control_climb_rate_enabled && !_control_mode.flag_control_acceleration_enabled) {