From 1e06f6bbd233eaee3211ee0609e5a0875e2e7326 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Sat, 11 Jan 2020 22:02:34 +0100 Subject: [PATCH] mc_pos_control: fix derivative spike when regaining velocity estimate When having no velocity estimate the derivative was updated with zero. When losing the velocity estimate this is fine since the resulting derivative spike doesn't get used and acceleration is set to NAN. But when regaining the velocity estimate the spike from zero to the first estimated velocity gets used as acceleration in the position controller and results in a twitch. To solve this I use the derivative reset I introduced in pr #13522 commit b64abf48b211b66fbfc7a3b900adbbbdf0e2e14c --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 12 +++++------- 1 file changed, 5 insertions(+), 7 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 c5aa244aca..693a00932e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -456,10 +456,9 @@ MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z) } else { _states.velocity(0) = _states.velocity(1) = NAN; _states.acceleration(0) = _states.acceleration(1) = NAN; - - // since no valid velocity, update derivate with 0 - _vel_x_deriv.update(0.0f); - _vel_y_deriv.update(0.0f); + // reset derivatives to prevent acceleration spikes when regaining velocity + _vel_x_deriv.reset(); + _vel_y_deriv.reset(); } if (PX4_ISFINITE(_local_pos.vz) && _local_pos.v_z_valid) { @@ -476,9 +475,8 @@ MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z) } else { _states.velocity(2) = _states.acceleration(2) = NAN; - // since no valid velocity, update derivate with 0 - _vel_z_deriv.update(0.0f); - + // reset derivative to prevent acceleration spikes when regaining velocity + _vel_z_deriv.reset(); } }