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

set lower filter cutoff instead of Q

Browse source 
fix error in gyro pt1 filter
This commit is contained in:
rav 2016-08-03 23:45:52 +02:00
parent 258e05e9c5
commit 0e905e01d3
10 changed files with 21 additions and 20 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/main/blackbox/blackbox.c
View file

@ -1218,13 +1218,13 @@ static bool blackboxWriteSysinfo()
BLACKBOX_PRINT_HEADER_LINE("yawItermIgnoreRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawItermIgnoreRate);
BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_lpf_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("dterm_notch_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_notch_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("dterm_notch_bw:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_notch_bw * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("dterm_notch_cutoff:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_notch_cutoff * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("deadband:%d", masterConfig.rcControlsConfig.deadband);
BLACKBOX_PRINT_HEADER_LINE("yaw_deadband:%d", masterConfig.rcControlsConfig.yaw_deadband);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf:%d", masterConfig.gyro_lpf);
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d", (int)(masterConfig.gyro_soft_lpf_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz:%d", (int)(masterConfig.gyro_soft_notch_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_bw:%d", (int)(masterConfig.gyro_soft_notch_bw * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff:%d", (int)(masterConfig.gyro_soft_notch_cutoff * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz:%d", (int)(masterConfig.acc_lpf_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d", masterConfig.acc_hardware);
BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", masterConfig.baro_hardware);

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

@ -55,6 +55,11 @@ float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT)
return filter->state;
}
float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff) {
float octaves = log2f((float) centerFreq / (float) cutoff) * 2;
return sqrtf(powf(2, octaves)) / (powf(2, octaves) - 1);
}
/* sets up a biquad Filter */
void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate)
{

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

@ -42,6 +42,7 @@ typedef enum {
void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
float biquadFilterApply(biquadFilter_t *filter, float input);
float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff);
void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT);
float pt1FilterApply(pt1Filter_t *filter, float input);

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

@ -237,7 +237,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->dterm_filter_type = FILTER_PT1;
pidProfile->dterm_lpf_hz = 100; // filtering ON by default
pidProfile->dterm_notch_hz = 0;
pidProfile->dterm_notch_bw = 200;
pidProfile->dterm_notch_cutoff = 150;
pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT;
pidProfile->vbatPidCompensation = 0;
pidProfile->zeroThrottleStabilisation = PID_STABILISATION_OFF;
@ -482,7 +482,7 @@ static void resetConf(void)
masterConfig.gyro_soft_type = FILTER_PT1;
masterConfig.gyro_soft_lpf_hz = 80;
masterConfig.gyro_soft_notch_hz = 0;
masterConfig.gyro_soft_notch_bw = 200;
masterConfig.gyro_soft_notch_cutoff = 150;
masterConfig.debug_mode = DEBUG_NONE;
@ -747,7 +747,7 @@ void activateConfig(void)
&currentProfile->pidProfile
);
gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz, masterConfig.gyro_soft_notch_hz, masterConfig.gyro_soft_notch_bw, masterConfig.gyro_soft_type);
gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz, masterConfig.gyro_soft_notch_hz, masterConfig.gyro_soft_notch_cutoff, masterConfig.gyro_soft_type);
#ifdef TELEMETRY
telemetryUseConfig(&masterConfig.telemetryConfig);

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

@ -61,7 +61,7 @@ typedef struct master_t {
uint8_t gyro_soft_type; // Gyro Filter Type
uint8_t gyro_soft_lpf_hz; // Biquad gyro lpf hz
uint16_t gyro_soft_notch_hz; // Biquad gyro notch hz
uint16_t gyro_soft_notch_bw; // Biquad gyro notch quality
uint16_t gyro_soft_notch_cutoff; // Biquad gyro notch low cutoff
uint16_t dcm_kp; // DCM filter proportional gain ( x 10000)
uint16_t dcm_ki; // DCM filter integral gain ( x 10000)

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

@ -114,10 +114,8 @@ void initFilters(const pidProfile_t *pidProfile) {
int axis;
if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) {
float octaves = log2f(((float) pidProfile->dterm_notch_hz + pidProfile->dterm_notch_bw / 2) / ((float) pidProfile->dterm_notch_hz - pidProfile->dterm_notch_bw / 2));
float dtermNotchQ = sqrtf(powf(2, octaves)) / (powf(2, octaves) - 1);
for (axis = 0; axis < 3; axis++) biquadFilterInit(&dtermFilterNotch[axis], pidProfile->dterm_notch_hz, gyro.targetLooptime, dtermNotchQ, FILTER_NOTCH);
float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff);
for (axis = 0; axis < 3; axis++) biquadFilterInit(&dtermFilterNotch[axis], pidProfile->dterm_notch_hz, gyro.targetLooptime, notchQ, FILTER_NOTCH);
}
if (pidProfile->dterm_filter_type == FILTER_BIQUAD) {

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

@ -84,7 +84,7 @@ typedef struct pidProfile_s {
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
uint16_t dterm_notch_bw; // Biquad dterm notch quality
uint16_t dterm_notch_cutoff; // Biquad dterm notch low cutoff
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

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

@ -751,7 +751,7 @@ const clivalue_t valueTable[] = {
{ "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_bw", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_bw, .config.minmax = { 1, 500 } },
{ "gyro_notch_cutoff", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_cutoff, .config.minmax = { 1, 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 } },
@ -824,7 +824,7 @@ const clivalue_t valueTable[] = {
{ "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_bw", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_bw, .config.minmax = { 1, 500 } },
{ "dterm_notch_cutoff", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_cutoff, .config.minmax = { 1, 500 } },
{ "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 } },

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

@ -55,16 +55,13 @@ static uint8_t gyroSoftLpfHz;
static uint16_t calibratingG = 0;
static float gyroDt;
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint16_t gyro_soft_notch_bw, uint8_t gyro_soft_lpf_type)
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint16_t gyro_soft_notch_cutoff, uint8_t gyro_soft_lpf_type)
{
gyroConfig = gyroConfigToUse;
gyroSoftLpfHz = gyro_soft_lpf_hz;
gyroSoftNotchHz = gyro_soft_notch_hz;
gyroSoftNotchQ = gyro_soft_notch_bw;
gyroSoftLpfType = gyro_soft_lpf_type;
float octaves = log2f(((float) gyro_soft_notch_hz + gyro_soft_notch_bw / 2) / ((float) gyro_soft_notch_hz - gyro_soft_notch_bw / 2));
gyroSoftNotchQ = sqrtf(powf(2, octaves)) / (powf(2, octaves) - 1);
gyroSoftNotchQ = filterGetNotchQ(gyro_soft_notch_hz, gyro_soft_notch_cutoff);
}
void gyroInit(void)
@ -75,7 +72,7 @@ void gyroInit(void)
if (gyroSoftLpfType == FILTER_BIQUAD)
biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime);
else
gyroDt = gyro.targetLooptime / 1000.0f;
gyroDt = (float) gyro.targetLooptime * 0.000001f;
}
}
}

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

@ -40,7 +40,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 gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint16_t gyro_soft_notch_bw, uint8_t gyro_soft_lpf_type);
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint16_t gyro_soft_notch_cutoff, uint8_t gyro_soft_lpf_type);
void gyroSetCalibrationCycles(void);
void gyroInit(void);
void gyroUpdate(void);