From 0312159f00bc91c1ad8f0aa9f143ca00724b3ea2 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 19 Nov 2019 15:58:08 +0100 Subject: [PATCH] mc_pos_control: reset velocity derivatives --- src/lib/controllib/BlockDerivative.hpp | 1 + src/modules/mc_pos_control/mc_pos_control_main.cpp | 5 +++++ 2 files changed, 6 insertions(+) diff --git a/src/lib/controllib/BlockDerivative.hpp b/src/lib/controllib/BlockDerivative.hpp index e7879c3dd2..1b8ed26a06 100644 --- a/src/lib/controllib/BlockDerivative.hpp +++ b/src/lib/controllib/BlockDerivative.hpp @@ -87,6 +87,7 @@ public: float update(float input); // accessors void setU(float u) {_u = u;} + void reset() { _initialized = false; }; float getU() {return _u;} float getLP() {return _lowPass.getFCut();} float getO() { return _lowPass.getState(); } 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 c0bfbd8f82..6486aaf2f6 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -731,6 +731,11 @@ MulticopterPositionControl::Run() q_sp.copyTo(_att_sp.q_d); _att_sp.q_d_valid = true; _att_sp.thrust_body[2] = 0.0f; + + // reset the numerical derivatives to not generate d term spikes when coming from non-position controlled operation + _vel_x_deriv.reset(); + _vel_y_deriv.reset(); + _vel_z_deriv.reset(); } }