Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 21:05:35 +03:00
Code Issues Wiki Activity

Refactor BiQuad // Set filters to floats

Browse source
This commit is contained in:
borisbstyle 2016-02-03 13:46:56 +01:00
parent 403812c45f
commit eebacc2542
10 changed files with 32 additions and 39 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -24,30 +24,23 @@
#include "common/filter.h"
#include "common/maths.h"
#include "drivers/gyro_sync.h"
#define M_LN2_FLOAT 0.69314718055994530942f
#define M_PI_FLOAT 3.14159265358979323846f
#define BIQUAD_BANDWIDTH 1.9f /* bandwidth in octaves */
/* sets up a biquad Filter */
void BiQuadNewLpf(uint8_t filterCutFreq, biquad_t *newState, float refreshRate)
void BiQuadNewLpf(float filterCutFreq, biquad_t *newState, uint32_t refreshRate)
{
float samplingRate;
float sampleRate;
if (!refreshRate) {
samplingRate = 1 / (targetLooptime * 0.000001f);
} else {
samplingRate = refreshRate;
}
sampleRate = 1 / ((float)refreshRate * 0.000001f);
float omega, sn, cs, alpha;
float a0, a1, a2, b0, b1, b2;
/* setup variables */
omega = 2 * M_PI_FLOAT * (float) filterCutFreq / samplingRate;
omega = 2 * M_PI_FLOAT * filterCutFreq / sampleRate;
sn = sinf(omega);
cs = cosf(omega);
alpha = sn * sinf(M_LN2_FLOAT /2 * BIQUAD_BANDWIDTH * omega /sn);
@ -60,11 +53,11 @@ void BiQuadNewLpf(uint8_t filterCutFreq, biquad_t *newState, float refreshRate)
a2 = 1 - alpha;
/* precompute the coefficients */
newState->a0 = b0 /a0;
newState->a1 = b1 /a0;
newState->a2 = b2 /a0;
newState->a3 = a1 /a0;
newState->a4 = a2 /a0;
newState->b0 = b0 /a0;
newState->b1 = b1 /a0;
newState->b2 = b2 /a0;
newState->a1 = a1 /a0;
newState->a2 = a2 /a0;
/* zero initial samples */
newState->x1 = newState->x2 = 0;
@ -77,8 +70,8 @@ float applyBiQuadFilter(float sample, biquad_t *state)
float result;
/* compute result */
result = state->a0 * sample + state->a1 * state->x1 + state->a2 * state->x2 -
state->a3 * state->y1 - state->a4 * state->y2;
result = state->b0 * sample + state->b1 * state->x1 + state->b2 * state->x2 -
state->a1 * state->y1 - state->a2 * state->y2;
/* shift x1 to x2, sample to x1 */
state->x2 = state->x1;

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

@ -17,9 +17,9 @@
/* this holds the data required to update samples thru a filter */
typedef struct biquad_s {
float a0, a1, a2, a3, a4;
float b0, b1, b2, a1, a2;
float x1, x2, y1, y2;
} biquad_t;
float applyBiQuadFilter(float sample, biquad_t *state);
void BiQuadNewLpf(uint8_t filterCutFreq, biquad_t *newState, float refreshRate);
void BiQuadNewLpf(float filterCutFreq, biquad_t *newState, uint32_t refreshRate);

6
src/main/config/config.c
View file

@ -134,7 +134,7 @@ static uint32_t activeFeaturesLatch = 0;
static uint8_t currentControlRateProfileIndex = 0;
controlRateConfig_t *currentControlRateProfile;
static const uint8_t EEPROM_CONF_VERSION = 119;
static const uint8_t EEPROM_CONF_VERSION = 120;
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
{
@ -176,7 +176,6 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->I8[PIDVEL] = 45;
pidProfile->D8[PIDVEL] = 1;
pidProfile->gyro_lpf_hz = 60; // filtering ON by default
pidProfile->dterm_lpf_hz = 0; // filtering ON by default
pidProfile->deltaFromGyro = 1;
pidProfile->airModeInsaneAcrobilityFactor = 0;
@ -406,6 +405,7 @@ static void resetConf(void)
masterConfig.dcm_kp = 2500; // 1.0 * 10000
masterConfig.dcm_ki = 0; // 0.003 * 10000
masterConfig.gyro_lpf = 1; // 188HZ
masterConfig.gyro_soft_lpf_hz = 60;
resetAccelerometerTrims(&masterConfig.accZero);
@ -770,7 +770,7 @@ void activateConfig(void)
&currentProfile->pidProfile
);
useGyroConfig(&masterConfig.gyroConfig, currentProfile->pidProfile.gyro_lpf_hz);
useGyroConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz);
#ifdef TELEMETRY
telemetryUseConfig(&masterConfig.telemetryConfig);

1
src/main/config/config_master.h
View file

@ -51,6 +51,7 @@ typedef struct master_t {
uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device
uint8_t acc_for_fast_looptime; // shorten acc processing time by using 1 out of 9 samples. For combination with fast looptimes.
uint8_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
float gyro_soft_lpf_hz; // Biqyad gyro lpf hz
uint16_t dcm_kp; // DCM filter proportional gain ( x 10000)
uint16_t dcm_ki; // DCM filter integral gain ( x 10000)

4
src/main/flight/pid.c
View file

@ -142,7 +142,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
float horizonLevelStrength = 1;
if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], 0);
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetLooptime);
deltaStateIsSet = true;
}
@ -284,7 +284,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
int8_t horizonLevelStrength = 100;
if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], 0);
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetLooptime);
deltaStateIsSet = true;
}

3
src/main/flight/pid.h
View file

@ -60,8 +60,7 @@ typedef struct pidProfile_s {
uint8_t H_sensitivity;
uint16_t airModeInsaneAcrobilityFactor; // Air mode acrobility factor
uint8_t gyro_lpf_hz; // Gyro Soft filter in hz
uint8_t dterm_lpf_hz; // Delta Filter in hz
float dterm_lpf_hz; // Delta Filter in hz
uint8_t deltaFromGyro; // Alternative delta Calculation
#ifdef GTUNE

4
src/main/io/serial_cli.c
View file

@ -601,6 +601,7 @@ const clivalue_t valueTable[] = {
{ "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &masterConfig.max_angle_inclination, .config.minmax = { 100, 900 } },
{ "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
{ "gyro_soft_lpf", VAR_FLOAT | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 500 } },
{ "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } },
{ "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, .config.minmax = { 0, 50000 } },
{ "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, .config.minmax = { 0, 50000 } },
@ -705,8 +706,7 @@ const clivalue_t valueTable[] = {
{ "i_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDVEL], .config.minmax = { 0, 200 } },
{ "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], .config.minmax = { 0, 200 } },
{ "soft_gyro_lpf_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gyro_lpf_hz, .config.minmax = {0, 255 } },
{ "dterm_lpf_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 255 } },
{ "dterm_lpf_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } },
{ "acro_plus_factor", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.airModeInsaneAcrobilityFactor, .config.minmax = {0, 100 } },
#ifdef BLACKBOX

2
src/main/mw.c
View file

@ -184,7 +184,7 @@ void filterRc(void){
/* Initialize cycletime filter */
if (!filterIsSet) {
BiQuadNewLpf(1, &filteredCycleTimeState, 0);
BiQuadNewLpf(0.5f, &filteredCycleTimeState, targetLooptime);
filterIsSet = true;
}

16
src/main/sensors/gyro.c
View file

@ -41,15 +41,15 @@ int16_t gyroADC[XYZ_AXIS_COUNT];
int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
static gyroConfig_t *gyroConfig;
static biquad_t gyroBiQuadState[3];
static biquad_t gyroFilterState[3];
static bool gyroFilterStateIsSet;
static uint8_t gyroLpfCutFreq;
static float gyroLpfCutFreq;
int axis;
gyro_t gyro; // gyro access functions
sensor_align_e gyroAlign = 0;
void useGyroConfig(gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz)
void useGyroConfig(gyroConfig_t *gyroConfigToUse, float gyro_lpf_hz)
{
gyroConfig = gyroConfigToUse;
gyroLpfCutFreq = gyro_lpf_hz;
@ -57,7 +57,7 @@ void useGyroConfig(gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz)
void initGyroFilterCoefficients(void) {
if (gyroLpfCutFreq && targetLooptime) { /* Initialisation needs to happen once samplingrate is known */
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(gyroLpfCutFreq, &gyroBiQuadState[axis], 0);
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(gyroLpfCutFreq, &gyroFilterState[axis], targetLooptime);
gyroFilterStateIsSet = true;
}
}
@ -140,10 +140,10 @@ void gyroUpdate(void)
alignSensors(gyroADC, gyroADC, gyroAlign);
if (gyroLpfCutFreq) {
if (!gyroFilterStateIsSet) {
initGyroFilterCoefficients();
} else {
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = lrintf(applyBiQuadFilter((float) gyroADC[axis], &gyroBiQuadState[axis]));
if (!gyroFilterStateIsSet) initGyroFilterCoefficients(); /* initialise filter coefficients */
if (gyroFilterStateIsSet) {
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = lrintf(applyBiQuadFilter((float) gyroADC[axis], &gyroFilterState[axis]));
}
}

2
src/main/sensors/gyro.h
View file

@ -39,7 +39,7 @@ typedef struct gyroConfig_s {
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
} gyroConfig_t;
void useGyroConfig(gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz);
void useGyroConfig(gyroConfig_t *gyroConfigToUse, float gyro_lpf_hz);
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
void gyroUpdate(void);
bool isGyroCalibrationComplete(void);