mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-25 17:25:18 +03:00
For discussion - updated filters to be more consistent and more efficient (#330)
* Made filters more consistent * Added pT1 init function. Made FIR filters consistent.
This commit is contained in:
Martin Budden
Konstantin Sharlaimov
parent
ef871ec50a
commit
dd770a0fb9
14 changed files with 135 additions and 88 deletions
|
|
@ -17,7 +17,7 @@
|
|||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "common/axis.h"
|
||||
|
|
@ -27,9 +27,10 @@
|
|||
#include "drivers/gyro_sync.h"
|
||||
|
||||
#define BIQUAD_Q (1.0f / 1.41421356f) /* quality factor - butterworth (1 / sqrt(2)) */
|
||||
#define M_PI_FLOAT 3.14159265358979323846f
|
||||
|
||||
/* sets up a biquad Filter */
|
||||
void filterInitBiQuad(uint8_t filterCutFreq, biquad_t *newState, int16_t samplingRate)
|
||||
void biquadFilterInit(biquadFilter_t *newState, uint8_t filterCutFreq, int16_t samplingRate)
|
||||
{
|
||||
float omega, sn, cs, alpha;
|
||||
float a0, a1, a2, b0, b1, b2;
|
||||
|
|
@ -64,7 +65,7 @@ void filterInitBiQuad(uint8_t filterCutFreq, biquad_t *newState, int16_t samplin
|
|||
}
|
||||
|
||||
/* Computes a biquad_t filter on a sample */
|
||||
float filterApplyBiQuad(float sample, biquad_t *state)
|
||||
float biquadFilterApply(biquadFilter_t *state, float sample)
|
||||
{
|
||||
float result;
|
||||
|
||||
|
|
@ -75,27 +76,40 @@ float filterApplyBiQuad(float sample, biquad_t *state)
|
|||
return result;
|
||||
}
|
||||
|
||||
// PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
|
||||
float filterApplyPt1(float input, filterStatePt1_t *filter, float f_cut, float dT)
|
||||
// PT1 Low Pass filter
|
||||
|
||||
// f_cut = cutoff frequency
|
||||
void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT)
|
||||
{
|
||||
// Pre calculate and store RC
|
||||
if (!filter->RC) {
|
||||
filter->RC = 1.0f / ( 2.0f * (float)M_PI * f_cut );
|
||||
}
|
||||
filter->RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut );
|
||||
filter->dT = dT;
|
||||
}
|
||||
|
||||
float pt1FilterApply(pt1Filter_t *filter, float input)
|
||||
{
|
||||
filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state);
|
||||
return filter->state;
|
||||
}
|
||||
|
||||
float pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dT)
|
||||
{
|
||||
// Pre calculate and store RC
|
||||
if (!filter->RC) {
|
||||
filter->RC = 1.0f / ( 2.0f * (float)M_PI * f_cut );
|
||||
}
|
||||
|
||||
filter->state = filter->state + dT / (filter->RC + dT) * (input - filter->state);
|
||||
return filter->state;
|
||||
}
|
||||
|
||||
// PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
|
||||
// f_cut = cutoff frequency
|
||||
// rate_limit = maximum rate of change of the output value in units per second
|
||||
float filterApplyPt1WithRateLimit(float input, filterStatePt1_t *filter, float f_cut, float rate_limit, float dT)
|
||||
float pt1FilterApplyWithRateLimit(pt1Filter_t *filter, float input, float f_cut, float rate_limit, float dT)
|
||||
{
|
||||
// Pre calculate and store RC
|
||||
if (!filter->RC) {
|
||||
filter->RC = 1.0f / ( 2.0f * (float)M_PI * f_cut );
|
||||
}
|
||||
// Pre calculate and store RC
|
||||
if (!filter->RC) {
|
||||
filter->RC = 1.0f / ( 2.0f * (float)M_PI * f_cut );
|
||||
}
|
||||
|
||||
const float newState = filter->state + dT / (filter->RC + dT) * (input - filter->state);
|
||||
const float rateLimitPerSample = rate_limit * dT;
|
||||
|
|
@ -104,26 +118,37 @@ float filterApplyPt1WithRateLimit(float input, filterStatePt1_t *filter, float f
|
|||
return filter->state;
|
||||
}
|
||||
|
||||
void filterResetPt1(filterStatePt1_t *filter, float input)
|
||||
void pt1FilterReset(pt1Filter_t *filter, float input)
|
||||
{
|
||||
filter->state = input;
|
||||
}
|
||||
|
||||
void filterUpdateFIR(int filterLength, float *shiftBuf, float newSample)
|
||||
// FIR filter
|
||||
void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength)
|
||||
{
|
||||
// Shift history buffer and push new sample
|
||||
for (int i = filterLength - 1; i > 0; i--)
|
||||
shiftBuf[i] = shiftBuf[i - 1];
|
||||
|
||||
shiftBuf[0] = newSample;
|
||||
filter->buf = buf;
|
||||
filter->bufLength = bufLength;
|
||||
filter->coeffs = coeffs;
|
||||
filter->coeffsLength = coeffsLength;
|
||||
memset(filter->buf, 0, sizeof(float) * filter->bufLength);
|
||||
}
|
||||
|
||||
float filterApplyFIR(int filterLength, const float *shiftBuf, const float *coeffBuf, float commonMultiplier)
|
||||
void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs)
|
||||
{
|
||||
float accum = 0;
|
||||
|
||||
for (int i = 0; i < filterLength; i++)
|
||||
accum += shiftBuf[i] * coeffBuf[i];
|
||||
|
||||
return accum * commonMultiplier;
|
||||
firFilterInit2(filter, buf, bufLength, coeffs, bufLength);
|
||||
}
|
||||
|
||||
void firFilterUpdate(firFilter_t *filter, float input)
|
||||
{
|
||||
memmove(&filter->buf[1], &filter->buf[0], (filter->bufLength-1) * sizeof(float));
|
||||
filter->buf[0] = input;
|
||||
}
|
||||
|
||||
float firFilterApply(firFilter_t *filter)
|
||||
{
|
||||
float ret = 0.0f;
|
||||
for (int ii = 0; ii < filter->coeffsLength; ++ii) {
|
||||
ret += filter->coeffs[ii] * filter->buf[ii];
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -17,24 +17,35 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
typedef struct filterStatePt1_s {
|
||||
float state;
|
||||
float RC;
|
||||
float constdT;
|
||||
} filterStatePt1_t;
|
||||
typedef struct pt1Filter_s {
|
||||
float state;
|
||||
float RC;
|
||||
float dT;
|
||||
} pt1Filter_t;
|
||||
|
||||
/* this holds the data required to update samples thru a filter */
|
||||
typedef struct biquad_s {
|
||||
typedef struct biquadFilter_s {
|
||||
float b0, b1, b2, a1, a2;
|
||||
float d1, d2;
|
||||
} biquad_t;
|
||||
} biquadFilter_t;
|
||||
|
||||
float filterApplyPt1(float input, filterStatePt1_t *filter, float f_cut, float dt);
|
||||
float filterApplyPt1WithRateLimit(float input, filterStatePt1_t *filter, float f_cut, float rate_limit, float dT);
|
||||
void filterResetPt1(filterStatePt1_t *filter, float input);
|
||||
typedef struct firFilter_s {
|
||||
float *buf;
|
||||
const float *coeffs;
|
||||
uint8_t bufLength;
|
||||
uint8_t coeffsLength;
|
||||
} firFilter_t;
|
||||
|
||||
void filterInitBiQuad(uint8_t filterCutFreq, biquad_t *newState, int16_t samplingRate);
|
||||
float filterApplyBiQuad(float sample, biquad_t *state);
|
||||
float pt1FilterApply(pt1Filter_t *filter, float input);
|
||||
float pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dt);
|
||||
float pt1FilterApplyWithRateLimit(pt1Filter_t *filter, float input, float f_cut, float rate_limit, float dT);
|
||||
void pt1FilterReset(pt1Filter_t *filter, float input);
|
||||
|
||||
void biquadFilterInit(biquadFilter_t *filter, uint8_t filterCutFreq, int16_t samplingRate);
|
||||
float biquadFilterApply(biquadFilter_t *filter, float sample);
|
||||
|
||||
void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs);
|
||||
void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength);
|
||||
void firFilterUpdate(firFilter_t *filter, float input);
|
||||
float firFilterApply(firFilter_t *filter);
|
||||
|
||||
void filterUpdateFIR(int filterLength, float *shiftBuf, float newSample);
|
||||
float filterApplyFIR(int filterLength, const float *shiftBuf, const float *coeffBuf, float commonMultiplier);
|
||||
|
|
@ -789,6 +789,8 @@ void activateConfig(void)
|
|||
|
||||
imuConfigure(&imuRuntimeConfig, ¤tProfile->pidProfile);
|
||||
|
||||
pidInit();
|
||||
|
||||
#ifdef NAV
|
||||
navigationUseConfig(&masterConfig.navConfig);
|
||||
navigationUsePIDs(¤tProfile->pidProfile);
|
||||
|
|
|
|||
|
|
@ -84,7 +84,7 @@ static uint8_t minServoIndex;
|
|||
static uint8_t maxServoIndex;
|
||||
|
||||
static servoParam_t *servoConf;
|
||||
static biquad_t servoFitlerState[MAX_SUPPORTED_SERVOS];
|
||||
static biquadFilter_t servoFitlerState[MAX_SUPPORTED_SERVOS];
|
||||
static bool servoFilterIsSet;
|
||||
#endif
|
||||
|
||||
|
|
@ -889,7 +889,7 @@ void filterServos(void)
|
|||
// Initialize servo lowpass filter (servos are calculated at looptime rate)
|
||||
if (!servoFilterIsSet) {
|
||||
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
|
||||
filterInitBiQuad(mixerConfig->servo_lowpass_freq, &servoFitlerState[servoIdx], 0);
|
||||
biquadFilterInit(&servoFitlerState[servoIdx], mixerConfig->servo_lowpass_freq, 0);
|
||||
}
|
||||
|
||||
servoFilterIsSet = true;
|
||||
|
|
@ -897,7 +897,7 @@ void filterServos(void)
|
|||
|
||||
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
|
||||
// Apply servo lowpass filter and do sanity cheching
|
||||
servo[servoIdx] = (int16_t) filterApplyBiQuad((float)servo[servoIdx], &servoFitlerState[servoIdx]);
|
||||
servo[servoIdx] = (int16_t) biquadFilterApply(&servoFitlerState[servoIdx], (float)servo[servoIdx]);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1236,7 +1236,7 @@ float navPidApply2(float setpoint, float measurement, float dt, pidController_t
|
|||
pid->last_input = measurement;
|
||||
}
|
||||
|
||||
newDerivative = pid->param.kD * filterApplyPt1(newDerivative, &pid->dterm_filter_state, NAV_DTERM_CUT_HZ, dt);
|
||||
newDerivative = pid->param.kD * pt1FilterApply4(&pid->dterm_filter_state, newDerivative, NAV_DTERM_CUT_HZ, dt);
|
||||
|
||||
/* Pre-calculate output and limit it if actuator is saturating */
|
||||
float outVal = newProportional + pid->integrator + newDerivative;
|
||||
|
|
|
|||
|
|
@ -96,7 +96,7 @@ bool adjustFixedWingAltitudeFromRCInput(void)
|
|||
// Position to velocity controller for Z axis
|
||||
static void updateAltitudeVelocityAndPitchController_FW(uint32_t deltaMicros)
|
||||
{
|
||||
static filterStatePt1_t velzFilterState;
|
||||
static pt1Filter_t velzFilterState;
|
||||
|
||||
// On a fixed wing we might not have a reliable climb rate source (if no BARO available), so we can't apply PID controller to
|
||||
// velocity error. We use PID controller on altitude error and calculate desired pitch angle from desired climb rate and forward velocity
|
||||
|
|
@ -109,7 +109,7 @@ static void updateAltitudeVelocityAndPitchController_FW(uint32_t deltaMicros)
|
|||
float maxVelocityDive = -forwardVelocity * sin_approx(DEGREES_TO_RADIANS(posControl.navConfig->fw_max_dive_angle));
|
||||
|
||||
posControl.desiredState.vel.V.Z = navPidApply2(posControl.desiredState.pos.V.Z, posControl.actualState.pos.V.Z, US2S(deltaMicros), &posControl.pids.fw_alt, maxVelocityDive, maxVelocityClimb, false);
|
||||
posControl.desiredState.vel.V.Z = filterApplyPt1(posControl.desiredState.vel.V.Z, &velzFilterState, NAV_FW_VEL_CUTOFF_FREQENCY_HZ, US2S(deltaMicros));
|
||||
posControl.desiredState.vel.V.Z = pt1FilterApply4(&velzFilterState, posControl.desiredState.vel.V.Z, NAV_FW_VEL_CUTOFF_FREQENCY_HZ, US2S(deltaMicros));
|
||||
|
||||
// Calculate climb angle ( >0 - climb, <0 - dive)
|
||||
int16_t climbAngleDeciDeg = RADIANS_TO_DECIDEGREES(atan2_approx(posControl.desiredState.vel.V.Z, forwardVelocity));
|
||||
|
|
@ -177,7 +177,7 @@ bool adjustFixedWingHeadingFromRCInput(void)
|
|||
* XY-position controller for multicopter aircraft
|
||||
*-----------------------------------------------------------*/
|
||||
static t_fp_vector virtualDesiredPosition;
|
||||
static filterStatePt1_t fwPosControllerCorrectionFilterState;
|
||||
static pt1Filter_t fwPosControllerCorrectionFilterState;
|
||||
|
||||
void resetFixedWingPositionController(void)
|
||||
{
|
||||
|
|
@ -189,7 +189,7 @@ void resetFixedWingPositionController(void)
|
|||
posControl.rcAdjustment[ROLL] = 0;
|
||||
isRollAdjustmentValid = false;
|
||||
|
||||
filterResetPt1(&fwPosControllerCorrectionFilterState, 0.0f);
|
||||
pt1FilterReset(&fwPosControllerCorrectionFilterState, 0.0f);
|
||||
}
|
||||
|
||||
static void calculateVirtualPositionTarget_FW(float trackingPeriod)
|
||||
|
|
@ -279,7 +279,7 @@ static void updatePositionHeadingController_FW(uint32_t deltaMicros)
|
|||
true);
|
||||
|
||||
// Apply low-pass filter to prevent rapid correction
|
||||
rollAdjustment = filterApplyPt1(rollAdjustment, &fwPosControllerCorrectionFilterState, NAV_FW_ROLL_CUTOFF_FREQUENCY_HZ, US2S(deltaMicros));
|
||||
rollAdjustment = pt1FilterApply4(&fwPosControllerCorrectionFilterState, rollAdjustment, NAV_FW_ROLL_CUTOFF_FREQUENCY_HZ, US2S(deltaMicros));
|
||||
|
||||
// Convert rollAdjustment to decidegrees (rcAdjustment holds decidegrees)
|
||||
posControl.rcAdjustment[ROLL] = CENTIDEGREES_TO_DECIDEGREES(rollAdjustment);
|
||||
|
|
|
|||
|
|
@ -56,7 +56,7 @@
|
|||
*-----------------------------------------------------------*/
|
||||
static int16_t rcCommandAdjustedThrottle;
|
||||
static int16_t altHoldThrottleRCZero = 1500;
|
||||
static filterStatePt1_t altholdThrottleFilterState;
|
||||
static pt1Filter_t altholdThrottleFilterState;
|
||||
static bool prepareForTakeoffOnReset = false;
|
||||
|
||||
/* Calculate global altitude setpoint based on surface setpoint */
|
||||
|
|
@ -107,7 +107,7 @@ static void updateAltitudeThrottleController_MC(uint32_t deltaMicros)
|
|||
|
||||
posControl.rcAdjustment[THROTTLE] = navPidApply2(posControl.desiredState.vel.V.Z, posControl.actualState.vel.V.Z, US2S(deltaMicros), &posControl.pids.vel[Z], thrAdjustmentMin, thrAdjustmentMax, false);
|
||||
|
||||
posControl.rcAdjustment[THROTTLE] = filterApplyPt1(posControl.rcAdjustment[THROTTLE], &altholdThrottleFilterState, NAV_THROTTLE_CUTOFF_FREQENCY_HZ, US2S(deltaMicros));
|
||||
posControl.rcAdjustment[THROTTLE] = pt1FilterApply4(&altholdThrottleFilterState, posControl.rcAdjustment[THROTTLE], NAV_THROTTLE_CUTOFF_FREQENCY_HZ, US2S(deltaMicros));
|
||||
posControl.rcAdjustment[THROTTLE] = constrain(posControl.rcAdjustment[THROTTLE], thrAdjustmentMin, thrAdjustmentMax);
|
||||
}
|
||||
|
||||
|
|
@ -174,7 +174,7 @@ void resetMulticopterAltitudeController(void)
|
|||
{
|
||||
navPidReset(&posControl.pids.vel[Z]);
|
||||
navPidReset(&posControl.pids.surface);
|
||||
filterResetPt1(&altholdThrottleFilterState, 0.0f);
|
||||
pt1FilterReset(&altholdThrottleFilterState, 0.0f);
|
||||
posControl.desiredState.vel.V.Z = posControl.actualState.vel.V.Z; // Gradually transition from current climb
|
||||
posControl.rcAdjustment[THROTTLE] = 0;
|
||||
|
||||
|
|
@ -247,7 +247,7 @@ bool adjustMulticopterHeadingFromRCInput(void)
|
|||
/*-----------------------------------------------------------
|
||||
* XY-position controller for multicopter aircraft
|
||||
*-----------------------------------------------------------*/
|
||||
static filterStatePt1_t mcPosControllerAccFilterStateX, mcPosControllerAccFilterStateY;
|
||||
static pt1Filter_t mcPosControllerAccFilterStateX, mcPosControllerAccFilterStateY;
|
||||
static float lastAccelTargetX = 0.0f, lastAccelTargetY = 0.0f;
|
||||
|
||||
void resetMulticopterPositionController(void)
|
||||
|
|
@ -256,8 +256,8 @@ void resetMulticopterPositionController(void)
|
|||
for (axis = 0; axis < 2; axis++) {
|
||||
navPidReset(&posControl.pids.vel[axis]);
|
||||
posControl.rcAdjustment[axis] = 0;
|
||||
filterResetPt1(&mcPosControllerAccFilterStateX, 0.0f);
|
||||
filterResetPt1(&mcPosControllerAccFilterStateY, 0.0f);
|
||||
pt1FilterReset(&mcPosControllerAccFilterStateX, 0.0f);
|
||||
pt1FilterReset(&mcPosControllerAccFilterStateY, 0.0f);
|
||||
lastAccelTargetX = 0.0f;
|
||||
lastAccelTargetY = 0.0f;
|
||||
}
|
||||
|
|
@ -394,8 +394,8 @@ static void updatePositionAccelController_MC(uint32_t deltaMicros, float maxAcce
|
|||
lastAccelTargetY = newAccelY;
|
||||
|
||||
// Apply LPF to jerk limited acceleration target
|
||||
float accelN = filterApplyPt1(newAccelX, &mcPosControllerAccFilterStateX, NAV_ACCEL_CUTOFF_FREQUENCY_HZ, US2S(deltaMicros));
|
||||
float accelE = filterApplyPt1(newAccelY, &mcPosControllerAccFilterStateY, NAV_ACCEL_CUTOFF_FREQUENCY_HZ, US2S(deltaMicros));
|
||||
float accelN = pt1FilterApply4(&mcPosControllerAccFilterStateX, newAccelX, NAV_ACCEL_CUTOFF_FREQUENCY_HZ, US2S(deltaMicros));
|
||||
float accelE = pt1FilterApply4(&mcPosControllerAccFilterStateY, newAccelY, NAV_ACCEL_CUTOFF_FREQUENCY_HZ, US2S(deltaMicros));
|
||||
|
||||
// Rotate acceleration target into forward-right frame (aircraft)
|
||||
float accelForward = accelN * posControl.actualState.cosYaw + accelE * posControl.actualState.sinYaw;
|
||||
|
|
|
|||
|
|
@ -93,7 +93,7 @@ typedef struct {
|
|||
|
||||
typedef struct {
|
||||
pidControllerParam_t param;
|
||||
filterStatePt1_t dterm_filter_state; // last derivative for low-pass filter
|
||||
pt1Filter_t dterm_filter_state; // last derivative for low-pass filter
|
||||
float integrator; // integrator value
|
||||
float last_input; // last input for derivative
|
||||
} pidController_t;
|
||||
|
|
|
|||
|
|
@ -59,8 +59,9 @@ typedef struct {
|
|||
float rateTarget;
|
||||
|
||||
// Buffer for derivative calculation
|
||||
#define DTERM_BUF_COUNT 5
|
||||
float dTermBuf[DTERM_BUF_COUNT];
|
||||
#define PID_GYRO_RATE_BUF_LENGTH 5
|
||||
float gyroRateBuf[PID_GYRO_RATE_BUF_LENGTH];
|
||||
firFilter_t gyroRateFilter;
|
||||
|
||||
// Rate integrator
|
||||
float errorGyroIf;
|
||||
|
|
@ -70,11 +71,11 @@ typedef struct {
|
|||
float axisLockAccum;
|
||||
|
||||
// Used for ANGLE filtering
|
||||
filterStatePt1_t angleFilterState;
|
||||
pt1Filter_t angleFilterState;
|
||||
|
||||
// Rate filtering
|
||||
filterStatePt1_t ptermLpfState;
|
||||
filterStatePt1_t deltaLpfState;
|
||||
pt1Filter_t ptermLpfState;
|
||||
pt1Filter_t deltaLpfState;
|
||||
} pidState_t;
|
||||
|
||||
extern uint8_t motorCount;
|
||||
|
|
@ -93,6 +94,17 @@ int32_t axisPID_P[FLIGHT_DYNAMICS_INDEX_COUNT], axisPID_I[FLIGHT_DYNAMICS_INDEX_
|
|||
|
||||
static pidState_t pidState[FLIGHT_DYNAMICS_INDEX_COUNT];
|
||||
|
||||
void pidInit(void)
|
||||
{
|
||||
// Calculate derivative using 5-point noise-robust differentiators without time delay (one-sided or forward filters)
|
||||
// by Pavel Holoborodko, see http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/
|
||||
// h[0] = 5/8, h[-1] = 1/4, h[-2] = -1, h[-3] = -1/4, h[-4] = 3/8
|
||||
static const float dtermCoeffs[PID_GYRO_RATE_BUF_LENGTH] = {5.0f/8, 2.0f/8, -8.0f/8, -2.0f/8, 3.0f/8};
|
||||
for (int axis = 0; axis < 3; ++ axis) {
|
||||
firFilterInit(&pidState[axis].gyroRateFilter, pidState[axis].gyroRateBuf, PID_GYRO_RATE_BUF_LENGTH, dtermCoeffs);
|
||||
}
|
||||
}
|
||||
|
||||
void pidResetErrorAccumulators(void)
|
||||
{
|
||||
// Reset R/P/Y integrator
|
||||
|
|
@ -245,7 +257,7 @@ static void pidLevel(const pidProfile_t *pidProfile, pidState_t *pidState, fligh
|
|||
// response to rapid attitude changes and smoothing out self-leveling reaction
|
||||
if (pidProfile->I8[PIDLEVEL]) {
|
||||
// I8[PIDLEVEL] is filter cutoff frequency (Hz). Practical values of filtering frequency is 5-10 Hz
|
||||
pidState->rateTarget = filterApplyPt1(pidState->rateTarget, &pidState->angleFilterState, pidProfile->I8[PIDLEVEL], dT);
|
||||
pidState->rateTarget = pt1FilterApply4(&pidState->angleFilterState, pidState->rateTarget, pidProfile->I8[PIDLEVEL], dT);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -262,7 +274,7 @@ static void pidApplyRateController(const pidProfile_t *pidProfile, pidState_t *p
|
|||
|
||||
// Additional P-term LPF on YAW axis
|
||||
if (axis == FD_YAW && pidProfile->yaw_lpf_hz) {
|
||||
newPTerm = filterApplyPt1(newPTerm, &pidState->ptermLpfState, pidProfile->yaw_lpf_hz, dT);
|
||||
newPTerm = pt1FilterApply4(&pidState->ptermLpfState, newPTerm, pidProfile->yaw_lpf_hz, dT);
|
||||
}
|
||||
|
||||
// Calculate new D-term
|
||||
|
|
@ -271,16 +283,12 @@ static void pidApplyRateController(const pidProfile_t *pidProfile, pidState_t *p
|
|||
// optimisation for when D8 is zero, often used by YAW axis
|
||||
newDTerm = 0;
|
||||
} else {
|
||||
// Calculate derivative using 5-point noise-robust differentiators without time delay (one-sided or forward filters)
|
||||
// by Pavel Holoborodko, see http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/
|
||||
// h[0] = 5/8, h[-1] = 1/4, h[-2] = -1, h[-3] = -1/4, h[-4] = 3/8
|
||||
static const float dtermCoeffs[DTERM_BUF_COUNT] = {5.0f, 2.0f, -8.0f, -2.0f, 3.0f};
|
||||
filterUpdateFIR(DTERM_BUF_COUNT, pidState->dTermBuf, pidState->gyroRate);
|
||||
newDTerm = filterApplyFIR(DTERM_BUF_COUNT, pidState->dTermBuf, dtermCoeffs, -pidState->kD / (8 * dT));
|
||||
firFilterUpdate(&pidState->gyroRateFilter, pidState->gyroRate);
|
||||
newDTerm = firFilterApply(&pidState->gyroRateFilter) * (-pidState->kD / dT);
|
||||
|
||||
// Apply additional lowpass
|
||||
if (pidProfile->dterm_lpf_hz) {
|
||||
newDTerm = filterApplyPt1(newDTerm, &pidState->deltaLpfState, pidProfile->dterm_lpf_hz, dT);
|
||||
newDTerm = pt1FilterApply4(&pidState->deltaLpfState, newDTerm, pidProfile->dterm_lpf_hz, dT);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -355,7 +363,7 @@ uint8_t getMagHoldState()
|
|||
float pidMagHold(const pidProfile_t *pidProfile)
|
||||
{
|
||||
|
||||
static filterStatePt1_t magHoldRateFilter;
|
||||
static pt1Filter_t magHoldRateFilter;
|
||||
float magHoldRate;
|
||||
|
||||
int16_t error = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - magHoldTargetHeading;
|
||||
|
|
@ -403,7 +411,7 @@ float pidMagHold(const pidProfile_t *pidProfile)
|
|||
|
||||
magHoldRate = error * pidProfile->P8[PIDMAG] / 30;
|
||||
magHoldRate = constrainf(magHoldRate, -pidProfile->mag_hold_rate_limit, pidProfile->mag_hold_rate_limit);
|
||||
magHoldRate = filterApplyPt1(magHoldRate, &magHoldRateFilter, MAG_HOLD_ERROR_LPF_FREQ, dT);
|
||||
magHoldRate = pt1FilterApply4(&magHoldRateFilter, magHoldRate, MAG_HOLD_ERROR_LPF_FREQ, dT);
|
||||
|
||||
return magHoldRate;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -69,6 +69,7 @@ typedef struct pidProfile_s {
|
|||
extern int16_t axisPID[];
|
||||
extern int32_t axisPID_P[], axisPID_I[], axisPID_D[], axisPID_Setpoint[];
|
||||
|
||||
void pidInit(void);
|
||||
void pidResetErrorAccumulators(void);
|
||||
void updatePIDCoefficients(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig, const rxConfig_t *rxConfig);
|
||||
|
||||
|
|
|
|||
|
|
@ -492,7 +492,7 @@ void filterRc(bool isRXDataNew)
|
|||
static int16_t lastCommand[4] = { 0, 0, 0, 0 };
|
||||
static int16_t deltaRC[4] = { 0, 0, 0, 0 };
|
||||
static int16_t factor, rcInterpolationFactor;
|
||||
static biquad_t filteredCycleTimeState;
|
||||
static biquadFilter_t filteredCycleTimeState;
|
||||
static bool filterInitialised;
|
||||
uint16_t filteredCycleTime;
|
||||
uint16_t rxRefreshRate;
|
||||
|
|
@ -502,11 +502,11 @@ void filterRc(bool isRXDataNew)
|
|||
|
||||
// Calculate average cycle time (1Hz LPF on cycle time)
|
||||
if (!filterInitialised) {
|
||||
filterInitBiQuad(1, &filteredCycleTimeState, 0);
|
||||
biquadFilterInit(&filteredCycleTimeState, 1, 0);
|
||||
filterInitialised = true;
|
||||
}
|
||||
|
||||
filteredCycleTime = filterApplyBiQuad((float) cycleTime, &filteredCycleTimeState);
|
||||
filteredCycleTime = biquadFilterApply(&filteredCycleTimeState, (float) cycleTime);
|
||||
|
||||
rcInterpolationFactor = rxRefreshRate / filteredCycleTime + 1;
|
||||
|
||||
|
|
|
|||
|
|
@ -49,7 +49,7 @@ static flightDynamicsTrims_t * accZero;
|
|||
static flightDynamicsTrims_t * accGain;
|
||||
|
||||
static int8_t accLpfCutHz = 0;
|
||||
static biquad_t accFilterState[XYZ_AXIS_COUNT];
|
||||
static biquadFilter_t accFilterState[XYZ_AXIS_COUNT];
|
||||
static bool accFilterInitialised = false;
|
||||
|
||||
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired)
|
||||
|
|
@ -184,7 +184,7 @@ void updateAccelerationReadings(void)
|
|||
if (!accFilterInitialised) {
|
||||
if (targetLooptime) { /* Initialisation needs to happen once sample rate is known */
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
filterInitBiQuad(accLpfCutHz, &accFilterState[axis], 0);
|
||||
biquadFilterInit(&accFilterState[axis], accLpfCutHz, 0);
|
||||
}
|
||||
|
||||
accFilterInitialised = true;
|
||||
|
|
@ -193,7 +193,7 @@ void updateAccelerationReadings(void)
|
|||
|
||||
if (accFilterInitialised) {
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
accADC[axis] = lrintf(filterApplyBiQuad((float) accADC[axis], &accFilterState[axis]));
|
||||
accADC[axis] = lrintf(biquadFilterApply(&accFilterState[axis], (float) accADC[axis]));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -65,11 +65,11 @@ uint16_t batteryAdcToVoltage(uint16_t src)
|
|||
static void updateBatteryVoltage(uint32_t vbatTimeDelta)
|
||||
{
|
||||
uint16_t vbatSample;
|
||||
static filterStatePt1_t vbatFilterState;
|
||||
static pt1Filter_t vbatFilterState;
|
||||
|
||||
// store the battery voltage with some other recent battery voltage readings
|
||||
vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY);
|
||||
vbatSample = filterApplyPt1(vbatSample, &vbatFilterState, VBATT_LPF_FREQ, vbatTimeDelta * 1e-6f);
|
||||
vbatSample = pt1FilterApply4(&vbatFilterState, vbatSample, VBATT_LPF_FREQ, vbatTimeDelta * 1e-6f);
|
||||
vbat = batteryAdcToVoltage(vbatSample);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -48,7 +48,7 @@ static int16_t gyroADCRaw[XYZ_AXIS_COUNT];
|
|||
static int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
|
||||
|
||||
static int8_t gyroLpfCutHz = 0;
|
||||
static biquad_t gyroFilterState[XYZ_AXIS_COUNT];
|
||||
static biquadFilter_t gyroFilterState[XYZ_AXIS_COUNT];
|
||||
static bool gyroFilterInitialised = false;
|
||||
|
||||
void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t initialGyroLpfCutHz)
|
||||
|
|
@ -137,7 +137,7 @@ void gyroUpdate(void)
|
|||
if (!gyroFilterInitialised) {
|
||||
if (targetLooptime) { /* Initialisation needs to happen once sample rate is known */
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
filterInitBiQuad(gyroLpfCutHz, &gyroFilterState[axis], 0);
|
||||
biquadFilterInit(&gyroFilterState[axis], gyroLpfCutHz, 0);
|
||||
}
|
||||
|
||||
gyroFilterInitialised = true;
|
||||
|
|
@ -146,7 +146,7 @@ void gyroUpdate(void)
|
|||
|
||||
if (gyroFilterInitialised) {
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
gyroADC[axis] = lrintf(filterApplyBiQuad((float) gyroADC[axis], &gyroFilterState[axis]));
|
||||
gyroADC[axis] = lrintf(biquadFilterApply(&gyroFilterState[axis], (float) gyroADC[axis]));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue