From d41d1fc5104c5d17fddc4bc9040df3722106d9ba Mon Sep 17 00:00:00 2001 From: Harry Kalogirou Date: Thu, 9 Apr 2020 11:59:07 +0300 Subject: [PATCH] Apply low pass filters with configurable value --- src/main/navigation/navigation_fixedwing.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 9b01c2c679..81eaf77aa0 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -136,7 +136,9 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros) // PID controller to translate energy balance error [J] into pitch angle [decideg] float targetPitchAngle = navPidApply3(&posControl.pids.fw_alt, demSEB, estSEB, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, 0, pitchGainInv); - targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, NAV_FW_PITCH_CUTOFF_FREQENCY_HZ, US2S(deltaMicros)); + + const float cutoff_freq = constrain((10.f - navConfig()->fw.control_smoothness) / 5.0, 0.5, 2.0); // Pitch cutoff set to 1/5 the roll cutoff + targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, cutoff_freq, US2S(deltaMicros)); // Reconstrain pitch angle ( >0 - climb, <0 - dive) targetPitchAngle = constrainf(targetPitchAngle, minDiveDeciDeg, maxClimbDeciDeg); @@ -367,7 +369,8 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta pidFlags); // Apply low-pass filter to prevent rapid correction - rollAdjustment = pt1FilterApply4(&fwPosControllerCorrectionFilterState, rollAdjustment, NAV_FW_ROLL_CUTOFF_FREQUENCY_HZ, US2S(deltaMicros)); + uint8_t cutoff_freq = constrain(10 - navConfig()->fw.control_smoothness, 1, 10); + rollAdjustment = pt1FilterApply4(&fwPosControllerCorrectionFilterState, rollAdjustment, cutoff_freq, US2S(deltaMicros)); // Convert rollAdjustment to decidegrees (rcAdjustment holds decidegrees) posControl.rcAdjustment[ROLL] = CENTIDEGREES_TO_DECIDEGREES(rollAdjustment);