Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-20 14:55:18 +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; return filter->state;
} }
void filterResetPt1(filterStatePt1_t *filter, float input)
{
filter->state = input;
}
/** /**
* Typical quadcopter motor noise frequency (at 50% throttle): * Typical quadcopter motor noise frequency (at 50% throttle):
* 450-sized, 920kv, 9.4x4.3 props, 3S : 4622rpm = 77Hz * 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; } filterStatePt1_t;
float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dt); 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); 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]); 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 altholdInitialRCThrottle; // Throttle input when althold was activated
static int16_t rcCommandAdjustedThrottle; static int16_t rcCommandAdjustedThrottle;
static filterStatePt1_t altholdThrottleFilterState;
/* Calculate global altitude setpoint based on surface setpoint */ /* Calculate global altitude setpoint based on surface setpoint */
static void updateSurfaceTrackingAltitudeSetpoint_MC(void) static void updateSurfaceTrackingAltitudeSetpoint_MC(void)
@ -75,8 +76,6 @@ static void updateAltitudeVelocityController_MC(void)
static void updateAltitudeThrottleController_MC(uint32_t deltaMicros) static void updateAltitudeThrottleController_MC(uint32_t deltaMicros)
{ {
static filterStatePt1_t throttleFilterState;
// Calculate min and max throttle boundaries (to compensate for integral windup) // 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 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; 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]); 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); posControl.rcAdjustment[THROTTLE] = constrain(posControl.rcAdjustment[THROTTLE], thrAdjustmentMin, thrAdjustmentMax);
NAV_BLACKBOX_DEBUG(3, posControl.rcAdjustment[THROTTLE]); NAV_BLACKBOX_DEBUG(3, posControl.rcAdjustment[THROTTLE]);
@ -124,6 +123,7 @@ void setupMulticopterAltitudeController(void)
void resetMulticopterAltitudeController() void resetMulticopterAltitudeController()
{ {
navPidReset(&posControl.pids.vel[Z]); navPidReset(&posControl.pids.vel[Z]);
filterResetPt1(&altholdThrottleFilterState, 0.0f);
posControl.rcAdjustment[THROTTLE] = 0; posControl.rcAdjustment[THROTTLE] = 0;
} }
@ -198,8 +198,8 @@ void resetMulticopterPositionController(void)
for (axis = 0; axis < 2; axis++) { for (axis = 0; axis < 2; axis++) {
navPidReset(&posControl.pids.vel[axis]); navPidReset(&posControl.pids.vel[axis]);
posControl.rcAdjustment[axis] = 0; posControl.rcAdjustment[axis] = 0;
mcPosControllerAccFilterStateX.state = 0.0f; filterResetPt1(&mcPosControllerAccFilterStateX, 0.0f);
mcPosControllerAccFilterStateY.state = 0.0f; filterResetPt1(&mcPosControllerAccFilterStateY, 0.0f);
lastAccelTargetX = 0.0f; lastAccelTargetX = 0.0f;
lastAccelTargetY = 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 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 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_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 #define NAV_FW_VEL_CUTOFF_FREQENCY_HZ 2 // low-pass filter on Z-velocity for fixed wing