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