From 1211a457d7ec0990f67f0c5fe29bd514971b08cf Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 13 Dec 2021 15:51:06 +0100 Subject: [PATCH] MulticopterPositionControl: Overwrite vertical acceleration during takeoff rampup only --- src/modules/mc_pos_control/MulticopterPositionControl.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index d990f14200..43db19e69d 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -455,13 +455,15 @@ void MulticopterPositionControl::Run() const bool flying = (_takeoff.getTakeoffState() >= TakeoffState::flight); const bool flying_but_ground_contact = (flying && _vehicle_land_detected.ground_contact); - // make sure takeoff ramp is not amended by acceleration feed-forward if (!flying) { - _setpoint.acceleration[2] = NAN; - // hover_thrust maybe reset on takeoff _control.setHoverThrust(_param_mpc_thr_hover.get()); } + // make sure takeoff ramp is not amended by acceleration feed-forward + if (_takeoff.getTakeoffState() == TakeoffState::rampup) { + _setpoint.acceleration[2] = NAN; + } + if (not_taken_off || flying_but_ground_contact) { // we are not flying yet and need to avoid any corrections reset_setpoint_to_nan(_setpoint);