From 62aaa50f93e6e5ca2837717c7dd702cee0e7bde9 Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Fri, 27 Nov 2015 12:36:22 +1000 Subject: [PATCH] Fix for throttle drops/jumps due to not reset throttle LPF filter from last attempt. Uppered throttle LPF cutoff to 4Hz --- src/main/common/filter.c | 5 +++++ src/main/common/filter.h | 1 + src/main/flight/navigation_rewrite_multicopter.c | 10 +++++----- src/main/flight/navigation_rewrite_private.h | 2 +- 4 files changed, 12 insertions(+), 6 deletions(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 2c1f709cff..62875654d9 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -37,6 +37,11 @@ float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float return filter->state; } +void filterResetPt1(filterStatePt1_t *filter, float input) +{ + filter->state = input; +} + /** * Typical quadcopter motor noise frequency (at 50% throttle): * 450-sized, 920kv, 9.4x4.3 props, 3S : 4622rpm = 77Hz diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 23c941de70..356df01e82 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -24,5 +24,6 @@ typedef struct filterStatePt1_s { } filterStatePt1_t; float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dt); +void filterResetPt1(filterStatePt1_t *filter, float input); int8_t * filterGetFIRCoefficientsTable(uint8_t filter_level, int16_t looptime); void filterApply9TapFIR(int16_t data[3], int16_t state[3][9], int8_t coeff[9]); diff --git a/src/main/flight/navigation_rewrite_multicopter.c b/src/main/flight/navigation_rewrite_multicopter.c index d33c9d9922..a89aad7f88 100755 --- a/src/main/flight/navigation_rewrite_multicopter.c +++ b/src/main/flight/navigation_rewrite_multicopter.c @@ -50,6 +50,7 @@ *-----------------------------------------------------------*/ static int16_t altholdInitialRCThrottle; // Throttle input when althold was activated static int16_t rcCommandAdjustedThrottle; +static filterStatePt1_t altholdThrottleFilterState; /* Calculate global altitude setpoint based on surface setpoint */ static void updateSurfaceTrackingAltitudeSetpoint_MC(void) @@ -75,8 +76,6 @@ static void updateAltitudeVelocityController_MC(void) static void updateAltitudeThrottleController_MC(uint32_t deltaMicros) { - static filterStatePt1_t throttleFilterState; - // Calculate min and max throttle boundaries (to compensate for integral windup) int16_t thrAdjustmentMin = (int16_t)posControl.escAndServoConfig->minthrottle - (int16_t)posControl.navConfig->mc_hover_throttle; int16_t thrAdjustmentMax = (int16_t)posControl.escAndServoConfig->maxthrottle - (int16_t)posControl.navConfig->mc_hover_throttle; @@ -85,7 +84,7 @@ static void updateAltitudeThrottleController_MC(uint32_t deltaMicros) NAV_BLACKBOX_DEBUG(2, posControl.rcAdjustment[THROTTLE]); - posControl.rcAdjustment[THROTTLE] = filterApplyPt1(posControl.rcAdjustment[THROTTLE], &throttleFilterState, NAV_THROTTLE_CUTOFF_FREQENCY_HZ, US2S(deltaMicros)); + posControl.rcAdjustment[THROTTLE] = filterApplyPt1(posControl.rcAdjustment[THROTTLE], &altholdThrottleFilterState, NAV_THROTTLE_CUTOFF_FREQENCY_HZ, US2S(deltaMicros)); posControl.rcAdjustment[THROTTLE] = constrain(posControl.rcAdjustment[THROTTLE], thrAdjustmentMin, thrAdjustmentMax); NAV_BLACKBOX_DEBUG(3, posControl.rcAdjustment[THROTTLE]); @@ -124,6 +123,7 @@ void setupMulticopterAltitudeController(void) void resetMulticopterAltitudeController() { navPidReset(&posControl.pids.vel[Z]); + filterResetPt1(&altholdThrottleFilterState, 0.0f); posControl.rcAdjustment[THROTTLE] = 0; } @@ -198,8 +198,8 @@ void resetMulticopterPositionController(void) for (axis = 0; axis < 2; axis++) { navPidReset(&posControl.pids.vel[axis]); posControl.rcAdjustment[axis] = 0; - mcPosControllerAccFilterStateX.state = 0.0f; - mcPosControllerAccFilterStateY.state = 0.0f; + filterResetPt1(&mcPosControllerAccFilterStateX, 0.0f); + filterResetPt1(&mcPosControllerAccFilterStateY, 0.0f); lastAccelTargetX = 0.0f; lastAccelTargetY = 0.0f; } diff --git a/src/main/flight/navigation_rewrite_private.h b/src/main/flight/navigation_rewrite_private.h index 726632f580..e6db143a2c 100755 --- a/src/main/flight/navigation_rewrite_private.h +++ b/src/main/flight/navigation_rewrite_private.h @@ -27,7 +27,7 @@ #define POSITION_TARGET_UPDATE_RATE_HZ 5 // Rate manual position target update (minumum possible speed in cms will be this value) #define MIN_POSITION_UPDATE_RATE_HZ 5 // Minimum position update rate at which XYZ controllers would be applied -#define NAV_THROTTLE_CUTOFF_FREQENCY_HZ 2 // low-pass filter on throttle output +#define NAV_THROTTLE_CUTOFF_FREQENCY_HZ 4 // low-pass filter on throttle output #define NAV_ACCEL_CUTOFF_FREQUENCY_HZ 2 // low-pass filter on XY-acceleration target #define NAV_FW_VEL_CUTOFF_FREQENCY_HZ 2 // low-pass filter on Z-velocity for fixed wing