From b64abf48b211b66fbfc7a3b900adbbbdf0e2e14c 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 | 6 ++++++ 2 files changed, 7 insertions(+) diff --git a/src/lib/controllib/BlockDerivative.hpp b/src/lib/controllib/BlockDerivative.hpp index 72d9aea412..637c23ca53 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 3ffbd23420..e9709087c0 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -702,6 +702,12 @@ MulticopterPositionControl::Run() } _old_landing_gear_position = gear.landing_gear; + + } else { + // 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(); } }