From a38bdcfc9df71f6ba9c73bd936a0c1c2c3362118 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 9 Nov 2022 10:54:21 +0100 Subject: [PATCH] MulticopterPositionControl: fix amending existing idle setpoint from before takeoff once the rampup starts. The rampup requires a valid vertical velocity setpoint. The corner case is: - We are before takeoff and amending the setpoint to be 0,0,100 acceleration in order to idle - The rampup starts BUT the setpoint is not yet overwritten by the trajectory setpoint topic - The idle setpoint gets amended to not contain a feed-forward vertical acceleration because the rampup is velocity based - The result is a brief invalid 0,0,NAN acceleration setpoint - That invalid setpoint gets overridden by a failsafe that holds zero velocity - Zero velocity leads to applying ~hover thrust briefly --- src/modules/mc_pos_control/MulticopterPositionControl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 039bef28fd..9f7722541f 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -480,7 +480,7 @@ void MulticopterPositionControl::Run() } // make sure takeoff ramp is not amended by acceleration feed-forward - if (_takeoff.getTakeoffState() == TakeoffState::rampup) { + if (_takeoff.getTakeoffState() == TakeoffState::rampup && PX4_ISFINITE(_setpoint.velocity[2])) { _setpoint.acceleration[2] = NAN; }