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);