Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-19 14:25:16 +03:00
Code Issues Wiki Activity

Fix for throttle drops/jumps due to not reset throttle LPF filter from last attempt. Uppered throttle LPF cutoff to 4Hz

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2015-11-27 12:36:22 +10:00
parent 68ba45f24d
commit 62aaa50f93
4 changed files with 12 additions and 6 deletions
Download patch file Download diff file Expand all files Collapse all files

5
src/main/common/filter.c
View file

@ -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

1
src/main/common/filter.h
View file

@ -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]);

10
src/main/flight/navigation_rewrite_multicopter.c
View file

@ -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;
}

2
src/main/flight/navigation_rewrite_private.h
View file

@ -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