diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 2caa6039ca..66040caa45 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1703,7 +1703,6 @@ float navPidApply2(pidController_t *pid, const float setpoint, const float measu return navPidApply3(pid, setpoint, measurement, dt, outMin, outMax, pidFlags, 1.0f); } - void navPidReset(pidController_t *pid) { pid->reset = true; @@ -1713,8 +1712,7 @@ void navPidReset(pidController_t *pid) pid->integrator = 0.0f; pid->last_input = 0.0f; pid->feedForward = 0.0f; - pid->dterm_filter_state.state = 0.0f; - pid->dterm_filter_state.RC = 0.0f; + pt1FilterReset(&pid->dterm_filter_state, 0.0f); pid->output_constrained = 0.0f; } diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index f1b5b16911..9b380723ea 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -263,7 +263,6 @@ bool adjustMulticopterHeadingFromRCInput(void) /*----------------------------------------------------------- * XY-position controller for multicopter aircraft *-----------------------------------------------------------*/ -static pt1Filter_t mcPosControllerAccFilterStateX, mcPosControllerAccFilterStateY; static float lastAccelTargetX = 0.0f, lastAccelTargetY = 0.0f; void resetMulticopterBrakingMode(void) @@ -363,8 +362,6 @@ void resetMulticopterPositionController(void) for (int axis = 0; axis < 2; axis++) { navPidReset(&posControl.pids.vel[axis]); posControl.rcAdjustment[axis] = 0; - pt1FilterReset(&mcPosControllerAccFilterStateX, 0.0f); - pt1FilterReset(&mcPosControllerAccFilterStateY, 0.0f); lastAccelTargetX = 0.0f; lastAccelTargetY = 0.0f; }