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

Add more filter freedom

Browse source
This commit is contained in:
borisbstyle 2016-08-03 11:51:12 +02:00
parent ca703b1ff1
commit 771feb8fde
8 changed files with 66 additions and 20 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -60,7 +60,7 @@ void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refr
{
biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF);
}
void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, filterType_e filterType)
void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType)
{
// setup variables
const float sampleRate = 1 / ((float)refreshRate * 0.000001f);

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

@ -30,12 +30,17 @@ typedef struct biquadFilter_s {
} biquadFilter_t;
typedef enum {
FILTER_LPF,
FILTER_NOTCH
FILTER_PT1 = 0,
FILTER_BIQUAD,
} filterType_e;
typedef enum {
FILTER_LPF,
FILTER_NOTCH
} biquadFilterType_e;
void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, filterType_e filterType);
void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
float biquadFilterApply(biquadFilter_t *filter, float input);
void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT);

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

@ -20,6 +20,7 @@
#include <string.h>
#include "platform.h"
#include "debug.h"
#include "build_config.h"
@ -171,7 +172,7 @@ static uint32_t activeFeaturesLatch = 0;
static uint8_t currentControlRateProfileIndex = 0;
controlRateConfig_t *currentControlRateProfile;
static const uint8_t EEPROM_CONF_VERSION = 141;
static const uint8_t EEPROM_CONF_VERSION = 142;
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
{
@ -233,7 +234,10 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->yaw_lpf_hz = 80;
pidProfile->rollPitchItermIgnoreRate = 200;
pidProfile->yawItermIgnoreRate = 50;
pidProfile->dterm_filter_type = FILTER_PT1;
pidProfile->dterm_lpf_hz = 100; // filtering ON by default
pidProfile->dterm_notch_hz = 0;
pidProfile->dterm_notch_q = 5;
pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT;
pidProfile->vbatPidCompensation = 0;
pidProfile->zeroThrottleStabilisation = PID_STABILISATION_OFF;
@ -475,7 +479,7 @@ static void resetConf(void)
masterConfig.gyro_sync_denom = 4;
masterConfig.pid_process_denom = 2;
#endif
masterConfig.gyro_soft_type = GYRO_FILTER_PT1;
masterConfig.gyro_soft_type = FILTER_PT1;
masterConfig.gyro_soft_lpf_hz = 80;
masterConfig.gyro_soft_notch_hz = 0;
masterConfig.gyro_soft_notch_q = 5;

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

@ -66,7 +66,6 @@ uint8_t PIDweight[3];
static int32_t errorGyroI[3];
static float errorGyroIf[3];
static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig);
#ifdef SKIP_PID_FLOAT
@ -107,6 +106,23 @@ const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
static pt1Filter_t deltaFilter[3];
static pt1Filter_t yawFilter;
static biquadFilter_t dtermFilterLpf[3];
static biquadFilter_t dtermFilterNotch[3];
static bool dtermNotchInitialised, dtermBiquadLpfInitialised;
void initFilters(const pidProfile_t *pidProfile) {
int axis;
if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) {
for (axis = 0; axis < 3; axis++) biquadFilterInit(&dtermFilterNotch[axis], pidProfile->dterm_notch_hz, gyro.targetLooptime, ((float) pidProfile->dterm_notch_q) / 10, FILTER_NOTCH);
}
if (pidProfile->dterm_filter_type == FILTER_BIQUAD) {
if (pidProfile->dterm_lpf_hz && !dtermBiquadLpfInitialised) {
for (axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, gyro.targetLooptime);
}
}
}
#ifndef SKIP_PID_FLOAT
// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. Based on 2DOF reference design (matlab)
@ -126,6 +142,8 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float
initFilters(pidProfile);
if (FLIGHT_MODE(HORIZON_MODE)) {
// Figure out the raw stick positions
const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc));
@ -265,7 +283,15 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc
if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd * delta * dynReduction;
// Filter delta
if (pidProfile->dterm_lpf_hz) delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT());
if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta);
if (pidProfile->dterm_lpf_hz) {
if (dtermBiquadLpfInitialised) {
delta = biquadFilterApply(&dtermFilterLpf[axis], delta);
} else {
delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT());
}
}
DTerm = constrainf(Kd * delta * dynReduction, -300.0f, 300.0f);
@ -317,6 +343,8 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina
int8_t horizonLevelStrength = 100;
initFilters(pidProfile);
if (FLIGHT_MODE(HORIZON_MODE)) {
// Figure out the raw stick positions
const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc));
@ -417,7 +445,15 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina
if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
// Filter delta
if (pidProfile->dterm_lpf_hz) delta = pt1FilterApply4(&deltaFilter[axis], (float)delta, pidProfile->dterm_lpf_hz, getdT());
if (pidProfile->dterm_lpf_hz) {
float deltaf = delta; // single conversion
if (dtermBiquadLpfInitialised) {
delta = biquadFilterApply(&dtermFilterLpf[axis], delta);
} else {
delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT());
}
delta = lrintf(deltaf);
}
DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;

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

@ -80,8 +80,11 @@ typedef struct pidProfile_s {
uint8_t I8[PID_ITEM_COUNT];
uint8_t D8[PID_ITEM_COUNT];
uint8_t dterm_filter_type; // Filter selection for dterm
uint16_t dterm_lpf_hz; // Delta Filter in hz
uint16_t yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy
uint16_t dterm_notch_hz; // Biquad dterm notch hz
uint8_t dterm_notch_q; // Biquad dterm notch quality
uint8_t deltaMethod; // Alternative delta Calculation
uint16_t rollPitchItermIgnoreRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates
uint16_t yawItermIgnoreRate; // Experimental threshold for resetting iterm for yaw on certain rates

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

@ -497,7 +497,7 @@ static const char * const lookupTableRcSmoothing[] = {
"OFF", "DEFAULT", "AUTO", "MANUAL"
};
static const char * const lookupTableGyroSoftLowpassType[] = {
static const char * const lookupTableLowpassType[] = {
"NORMAL", "HIGH"
};
@ -538,7 +538,7 @@ typedef enum {
TABLE_MOTOR_PWM_PROTOCOL,
TABLE_DELTA_METHOD,
TABLE_RC_SMOOTHING,
TABLE_GYRO_LOWPASS_TYPE,
TABLE_LOWPASS_TYPE,
#ifdef OSD
TABLE_OSD,
#endif
@ -576,7 +576,7 @@ static const lookupTableEntry_t lookupTables[] = {
{ lookupTablePwmProtocol, sizeof(lookupTablePwmProtocol) / sizeof(char *) },
{ lookupTableDeltaMethod, sizeof(lookupTableDeltaMethod) / sizeof(char *) },
{ lookupTableRcSmoothing, sizeof(lookupTableRcSmoothing) / sizeof(char *) },
{ lookupTableGyroSoftLowpassType, sizeof(lookupTableGyroSoftLowpassType) / sizeof(char *) },
{ lookupTableLowpassType, sizeof(lookupTableLowpassType) / sizeof(char *) },
#ifdef OSD
{ lookupTableOsdType, sizeof(lookupTableOsdType) / sizeof(char *) },
#endif
@ -748,7 +748,7 @@ const clivalue_t valueTable[] = {
{ "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } },
{ "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
{ "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_sync_denom, .config.minmax = { 1, 8 } },
{ "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_soft_type, .config.lookup = { TABLE_GYRO_LOWPASS_TYPE } },
{ "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_soft_type, .config.lookup = { TABLE_LOWPASS_TYPE } },
{ "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 255 } },
{ "gyro_notch_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz, .config.minmax = { 0, 500 } },
{ "gyro_notch_q", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_notch_q, .config.minmax = { 1, 100 } },
@ -821,7 +821,10 @@ const clivalue_t valueTable[] = {
{ "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } },
{ "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } },
#endif
{ "dterm_filter_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dterm_filter_type, .config.lookup = { TABLE_LOWPASS_TYPE } },
{ "dterm_lowpass", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } },
{ "dterm_notch_hz", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_hz, .config.minmax = { 0, 500 } },
{ "dterm_notch_q", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_q, .config.minmax = { 1, 100 } },
{ "vbat_pid_compensation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } },
{ "zero_throttle_stabilisation",VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.zeroThrottleStabilisation, .config.lookup = { TABLE_OFF_ON } },
{ "pid_tolerance_band", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBand, .config.minmax = {0, 200 } },

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

@ -69,7 +69,7 @@ void gyroInit(void)
if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known
for (int axis = 0; axis < 3; axis++) {
biquadFilterInit(&gyroFilterNotch[axis], gyroSoftNotchHz, gyro.targetLooptime, ((float) gyroSoftNotchQ) / 10, FILTER_NOTCH);
if (gyroSoftLpfType == GYRO_FILTER_BIQUAD)
if (gyroSoftLpfType == FILTER_BIQUAD)
biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime);
else
gyroDt = gyro.targetLooptime / 1000.0f;
@ -182,7 +182,7 @@ void gyroUpdate(void)
debug[axis*2 + 1] = lrintf(sample);
}
if (gyroSoftLpfType == GYRO_FILTER_BIQUAD) {
if (gyroSoftLpfType == FILTER_BIQUAD) {
gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], sample);
} else {
gyroADCf[axis] = pt1FilterApply4(&gyroFilterPt1[axis], sample, gyroSoftLpfHz, gyroDt);

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

@ -31,11 +31,6 @@ typedef enum {
GYRO_MAX = GYRO_FAKE
} gyroSensor_e;
typedef enum {
GYRO_FILTER_PT1 = 0,
GYRO_FILTER_BIQUAD,
} gyroFilter_e;
extern gyro_t gyro;
extern int32_t gyroADC[XYZ_AXIS_COUNT];