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

Convert Notch Q to BW

Browse source
This commit is contained in:
borisbstyle 2016-08-03 20:56:37 +02:00
parent 771feb8fde
commit cc7ea13e66
8 changed files with 22 additions and 14 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1215,12 +1215,14 @@ static bool blackboxWriteSysinfo()
BLACKBOX_PRINT_HEADER_LINE("rollPitchItermIgnoreRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.rollPitchItermIgnoreRate); BLACKBOX_PRINT_HEADER_LINE("rollPitchItermIgnoreRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.rollPitchItermIgnoreRate);
BLACKBOX_PRINT_HEADER_LINE("yawItermIgnoreRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawItermIgnoreRate); 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_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("deadband:%d", masterConfig.rcControlsConfig.deadband); BLACKBOX_PRINT_HEADER_LINE("deadband:%d", masterConfig.rcControlsConfig.deadband);
BLACKBOX_PRINT_HEADER_LINE("yaw_deadband:%d", masterConfig.rcControlsConfig.yaw_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_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_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_hz:%d", (int)(masterConfig.gyro_soft_notch_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_q:%d", (int)(masterConfig.gyro_soft_notch_q * 10.0f)); BLACKBOX_PRINT_HEADER_LINE("gyro_notch_bw:%d", (int)(masterConfig.gyro_soft_notch_bw * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz:%d", (int)(masterConfig.acc_lpf_hz * 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("acc_hardware:%d", masterConfig.acc_hardware);
BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", masterConfig.baro_hardware); BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", masterConfig.baro_hardware);

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_filter_type = FILTER_PT1;
pidProfile->dterm_lpf_hz = 100; // filtering ON by default pidProfile->dterm_lpf_hz = 100; // filtering ON by default
pidProfile->dterm_notch_hz = 0; pidProfile->dterm_notch_hz = 0;
pidProfile->dterm_notch_q = 5; pidProfile->dterm_notch_bw = 200;
pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT;
pidProfile->vbatPidCompensation = 0; pidProfile->vbatPidCompensation = 0;
pidProfile->zeroThrottleStabilisation = PID_STABILISATION_OFF; pidProfile->zeroThrottleStabilisation = PID_STABILISATION_OFF;
@ -482,7 +482,7 @@ static void resetConf(void)
masterConfig.gyro_soft_type = FILTER_PT1; masterConfig.gyro_soft_type = FILTER_PT1;
masterConfig.gyro_soft_lpf_hz = 80; masterConfig.gyro_soft_lpf_hz = 80;
masterConfig.gyro_soft_notch_hz = 0; masterConfig.gyro_soft_notch_hz = 0;
masterConfig.gyro_soft_notch_q = 5; masterConfig.gyro_soft_notch_bw = 200;
masterConfig.debug_mode = DEBUG_NONE; masterConfig.debug_mode = DEBUG_NONE;
@ -747,7 +747,7 @@ void activateConfig(void)
&currentProfile->pidProfile &currentProfile->pidProfile
); );
gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz, masterConfig.gyro_soft_notch_hz, masterConfig.gyro_soft_notch_q, masterConfig.gyro_soft_type); gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz, masterConfig.gyro_soft_notch_hz, masterConfig.gyro_soft_notch_bw, masterConfig.gyro_soft_type);
#ifdef TELEMETRY #ifdef TELEMETRY
telemetryUseConfig(&masterConfig.telemetryConfig); 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_type; // Gyro Filter Type
uint8_t gyro_soft_lpf_hz; // Biquad gyro lpf hz 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_hz; // Biquad gyro notch hz
uint8_t gyro_soft_notch_q; // Biquad gyro notch quality uint16_t gyro_soft_notch_bw; // Biquad gyro notch quality
uint16_t dcm_kp; // DCM filter proportional gain ( x 10000) uint16_t dcm_kp; // DCM filter proportional gain ( x 10000)
uint16_t dcm_ki; // DCM filter integral gain ( x 10000) uint16_t dcm_ki; // DCM filter integral gain ( x 10000)

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

@ -114,7 +114,10 @@ void initFilters(const pidProfile_t *pidProfile) {
int axis; int axis;
if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) { 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); 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);
} }
if (pidProfile->dterm_filter_type == FILTER_BIQUAD) { 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 dterm_lpf_hz; // Delta Filter in hz
uint16_t yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy 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_hz; // Biquad dterm notch hz
uint8_t dterm_notch_q; // Biquad dterm notch quality uint16_t dterm_notch_bw; // Biquad dterm notch quality
uint8_t deltaMethod; // Alternative delta Calculation uint8_t deltaMethod; // Alternative delta Calculation
uint16_t rollPitchItermIgnoreRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates 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 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_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_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_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 } }, { "gyro_notch_bw", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_bw, .config.minmax = { 1, 500 } },
{ "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } }, { "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_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 } }, { "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_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_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_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 } }, { "dterm_notch_bw", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_bw, .config.minmax = { 1, 500 } },
{ "vbat_pid_compensation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } }, { "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 } }, { "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 } }, { "pid_tolerance_band", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBand, .config.minmax = {0, 200 } },

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

@ -50,25 +50,28 @@ static biquadFilter_t gyroFilterNotch[XYZ_AXIS_COUNT];
static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT]; static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT];
static uint8_t gyroSoftLpfType; static uint8_t gyroSoftLpfType;
static uint16_t gyroSoftNotchHz; static uint16_t gyroSoftNotchHz;
static uint8_t gyroSoftNotchQ; static float gyroSoftNotchQ;
static uint8_t gyroSoftLpfHz; static uint8_t gyroSoftLpfHz;
static uint16_t calibratingG = 0; static uint16_t calibratingG = 0;
static float gyroDt; static float gyroDt;
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint8_t gyro_soft_notch_q, 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_bw, uint8_t gyro_soft_lpf_type)
{ {
gyroConfig = gyroConfigToUse; gyroConfig = gyroConfigToUse;
gyroSoftLpfHz = gyro_soft_lpf_hz; gyroSoftLpfHz = gyro_soft_lpf_hz;
gyroSoftNotchHz = gyro_soft_notch_hz; gyroSoftNotchHz = gyro_soft_notch_hz;
gyroSoftNotchQ = gyro_soft_notch_q; gyroSoftNotchQ = gyro_soft_notch_bw;
gyroSoftLpfType = gyro_soft_lpf_type; 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);
} }
void gyroInit(void) void gyroInit(void)
{ {
if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
biquadFilterInit(&gyroFilterNotch[axis], gyroSoftNotchHz, gyro.targetLooptime, ((float) gyroSoftNotchQ) / 10, FILTER_NOTCH); biquadFilterInit(&gyroFilterNotch[axis], gyroSoftNotchHz, gyro.targetLooptime, gyroSoftNotchQ, FILTER_NOTCH);
if (gyroSoftLpfType == FILTER_BIQUAD) if (gyroSoftLpfType == FILTER_BIQUAD)
biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime); biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime);
else else

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. 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; } gyroConfig_t;
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint8_t gyro_soft_notch_q, 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_bw, uint8_t gyro_soft_lpf_type);
void gyroSetCalibrationCycles(void); void gyroSetCalibrationCycles(void);
void gyroInit(void); void gyroInit(void);
void gyroUpdate(void); void gyroUpdate(void);