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

Configurable Soft LPF values (gyro_lpf_hz and dterm_lpf_hz)

Browse source
This commit is contained in:
borisbstyle 2016-01-14 14:34:46 +01:00
parent a8916a3ddd
commit 93d917af3b
8 changed files with 88 additions and 98 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -27,6 +27,13 @@
#include "drivers/gyro_sync.h"
#define M_LN2_FLOAT 0.69314718055994530942f
#define M_PI_FLOAT 3.14159265358979323846f
#define BIQUAD_GAIN 6.0f /* gain in db */
#define BIQUAD_BANDWIDTH 1.9f /* bandwidth in octaves */
// PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dT) {
@ -40,69 +47,48 @@ float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float
return filter->state;
}
void setBiQuadCoefficients(int type, biquad_t *state) {
/* sets up a biquad Filter */
biquad_t *BiQuadNewLpf(uint8_t filterCutFreq)
{
float samplingRate;
samplingRate = 1 / (targetLooptime * 0.000001f);
biquad_t *b;
float omega, sn, cs, alpha;
float a0, a1, a2, b0, b1, b2;
b = malloc(sizeof(biquad_t));
if (b == NULL)
return NULL;
/* setup variables */
omega = 2 * M_PI_FLOAT * (float) filterCutFreq / samplingRate;
sn = sinf(omega);
cs = cosf(omega);
alpha = sn * sinf(M_LN2_FLOAT /2 * BIQUAD_BANDWIDTH * omega /sn);
b0 = (1 - cs) /2;
b1 = 1 - cs;
b2 = (1 - cs) /2;
a0 = 1 + alpha;
a1 = -2 * cs;
a2 = 1 - alpha;
/* precompute the coefficients */
b->a0 = b0 /a0;
b->a1 = b1 /a0;
b->a2 = b2 /a0;
b->a3 = a1 /a0;
b->a4 = a2 /a0;
/* zero initial samples */
state->x1=0;
state->x2=0;
state->y1=0;
state->y2=0;
b->x1 = b->x2 = 0;
b->y1 = b->y2 = 0;
/* set coefficients */
switch(type) {
case(GYRO_FILTER):
switch (targetLooptime) {
case(SAMPLE_RATE_2KHZ):
state->a0= 0.007820199;
state->a1= 0.015640399;
state->a2= 0.007820199;
state->a3= -1.73472382;
state->a4= 0.766004619;
break;
case(SAMPLE_RATE_2K6HZ):
state->a0= 0.004538377;
state->a1= 0.009076754;
state->a2= 0.004538377;
state->a3= -1.80059391;
state->a4= 0.818747415;
break;
default:
case(SAMPLE_RATE_1KHZ):
state->a0= 0.027859711;
state->a1= 0.055719422;
state->a2= 0.027859711;
state->a3= -1.47547752;
state->a4= 0.586916365;
}
break;
case(DELTA_FILTER):
switch (targetLooptime) {
case(SAMPLE_RATE_2KHZ):
state->a0= 0.003621679;
state->a1= 0.007243357;
state->a2= 0.003621679;
state->a3= -1.82269350;
state->a4= 0.837180216;
break;
case(SAMPLE_RATE_2K6HZ):
state->a0= 0.002081573;
state->a1= 0.004163147;
state->a2= 0.002081573;
state->a3= -1.86685796;
state->a4= 0.875184256;
break;
default:
case(SAMPLE_RATE_1KHZ):
state->a0= 0.013359181;
state->a1= 0.026718362;
state->a2= 0.013359181;
state->a3= -1.64745762;
state->a4= 0.700894342;
}
}
return b;
}
/* Computes a BiQuad filter on a sample */
/* Computes a biquad_t filter on a sample */
float applyBiQuadFilter(float sample, biquad_t *state)
{
float result;

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

@ -29,19 +29,6 @@ typedef struct biquad_s {
float x1, x2, y1, y2;
} biquad_t;
typedef enum {
GYRO_FILTER,
ACCELEROMETER_FILTER,
DELTA_FILTER
} filterType_e;
typedef enum {
SAMPLE_RATE_1KHZ = 1000,
SAMPLE_RATE_2KHZ = 500,
SAMPLE_RATE_2K6HZ = 375,
SAMPLE_RATE_4KHZ = 250
} sampleRates_e;
float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dt);
float applyBiQuadFilter(float sample, biquad_t * b);
void setBiQuadCoefficients(int type, biquad_t *state);
float applyBiQuadFilter(float sample, biquad_t *state);
biquad_t *BiQuadNewLpf(uint8_t filterCutFreq);

7
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 = 117;
static const uint8_t EEPROM_CONF_VERSION = 118;
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
{
@ -176,7 +176,8 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->I8[PIDVEL] = 45;
pidProfile->D8[PIDVEL] = 1;
pidProfile->gyro_soft_lpf = 1; // filtering ON by default
pidProfile->gyro_lpf_hz = 60; // filtering ON by default
pidProfile->dterm_lpf_hz = 50; // filtering ON by default
pidProfile->airModeInsaneAcrobilityFactor = 0;
pidProfile->P_f[ROLL] = 1.5f; // new PID with preliminary defaults test carefully
@ -747,7 +748,7 @@ void activateConfig(void)
&currentProfile->pidProfile
);
useGyroConfig(&masterConfig.gyroConfig, currentProfile->pidProfile.gyro_soft_lpf);
useGyroConfig(&masterConfig.gyroConfig, &currentProfile->pidProfile.gyro_lpf_hz);
#ifdef TELEMETRY
telemetryUseConfig(&masterConfig.telemetryConfig);

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

@ -113,8 +113,8 @@ void airModePlus(airModePlus_t *axisState, int axis, pidProfile_t *pidProfile) {
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
static airModePlus_t airModePlusAxisState[3];
static bool deltaStateIsSet = false;
static biquad_t deltaBiQuadState[3];
static bool deltaStateIsSet;
static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
@ -127,8 +127,8 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
float horizonLevelStrength = 1;
static float previousErrorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
if (!deltaStateIsSet) {
for (axis = 0; axis < 3; axis++) setBiQuadCoefficients(DELTA_FILTER, &deltaBiQuadState[axis]);
if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
for (axis = 0; axis < 3; axis++) deltaBiQuadState[axis] = *(biquad_t *)BiQuadNewLpf(pidProfile->dterm_lpf_hz);
deltaStateIsSet = true;
}
@ -214,7 +214,9 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
delta = RateError - lastError[axis];
lastError[axis] = RateError;
delta = applyBiQuadFilter(delta, &deltaBiQuadState[axis]);
if (deltaStateIsSet) {
delta = applyBiQuadFilter(delta, &deltaBiQuadState[axis]);
}
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
// would be scaled by different dt each time. Division by dT fixes that.
@ -256,8 +258,8 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
int8_t horizonLevelStrength = 100;
if (!deltaStateIsSet) {
for (axis = 0; axis < 3; axis++) setBiQuadCoefficients(DELTA_FILTER, &deltaBiQuadState[axis]);
if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
for (axis = 0; axis < 3; axis++) deltaBiQuadState[axis] = *(biquad_t *)BiQuadNewLpf(pidProfile->dterm_lpf_hz);
deltaStateIsSet = true;
}
@ -345,7 +347,9 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastError[axis] = RateError;
delta = lrintf(applyBiQuadFilter((float) delta, &deltaBiQuadState[axis]));
if (deltaStateIsSet) {
delta = lrintf(applyBiQuadFilter((float) delta, &deltaBiQuadState[axis]));
}
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
// would be scaled by different dt each time. Division by dT fixes that.

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

@ -59,7 +59,8 @@ typedef struct pidProfile_s {
uint8_t H_sensitivity;
uint16_t airModeInsaneAcrobilityFactor; // Air mode acrobility factor
uint8_t gyro_soft_lpf; // Gyro FIR filter
uint8_t gyro_lpf_hz; // Gyro Soft filter in hz
uint8_t dterm_lpf_hz; // Delta Filter in hz
#ifdef GTUNE
uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune

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

@ -657,7 +657,8 @@ 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 } },
{ "gyro_soft_lpf", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.gyro_soft_lpf, .config.lookup = { TABLE_OFF_ON } },
{ "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 } },
{ "insane_acro_factor", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.airModeInsaneAcrobilityFactor, .config.minmax = {0, 100 } },
#ifdef BLACKBOX

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

@ -17,8 +17,10 @@
#include <stdbool.h>
#include <stdint.h>
#include <math.h>
#include "platform.h"
#include "debug.h"
#include "common/axis.h"
#include "common/maths.h"
@ -26,6 +28,7 @@
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/gyro_sync.h"
#include "sensors/sensors.h"
#include "io/beeper.h"
#include "io/statusindicator.h"
@ -39,18 +42,23 @@ int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
static gyroConfig_t *gyroConfig;
static biquad_t gyroBiQuadState[3];
static bool useSoftFilter;
static bool gyroFilterStateIsSet;
static uint8_t gyroLpfCutFreq;
int axis;
gyro_t gyro; // gyro access functions
sensor_align_e gyroAlign = 0;
void useGyroConfig(gyroConfig_t *gyroConfigToUse, uint8_t useFilter)
void useGyroConfig(gyroConfig_t *gyroConfigToUse, uint8_t *gyro_lpf_hz)
{
int axis;
gyroConfig = gyroConfigToUse;
if (useFilter) {
useSoftFilter = true;
for (axis = 0; axis < 3; axis++) setBiQuadCoefficients(GYRO_FILTER, &gyroBiQuadState[axis]);
gyroLpfCutFreq = *gyro_lpf_hz;
}
void initGyroFilterCoefficients(void) {
if (gyroLpfCutFreq && targetLooptime) { /* Initialisation needs to happen once samplingrate is known */
for (axis = 0; axis < 3; axis++) gyroBiQuadState[axis] = *(biquad_t *)BiQuadNewLpf(gyroLpfCutFreq);
gyroFilterStateIsSet = true;
}
}
@ -129,14 +137,16 @@ void gyroUpdate(void)
return;
}
if (useSoftFilter) {
int axis;
//for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) filterApplyFIR(&gyroADC[axis], gyroFIRState[axis], gyroFIRTable);
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = applyBiQuadFilter((float) gyroADC[axis], &gyroBiQuadState[axis]);
}
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 (!isGyroCalibrationComplete()) {
performAcclerationCalibration(gyroConfig->gyroMovementCalibrationThreshold);
}

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 useFilter);
void useGyroConfig(gyroConfig_t *gyroConfigToUse, uint8_t *gyro_lpf_hz);
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
void gyroUpdate(void);
bool isGyroCalibrationComplete(void);