From cc7ea13e66ef7abcf22b430fdabc5a1be656d1ea Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 3 Aug 2016 20:56:37 +0200 Subject: [PATCH] Convert Notch Q to BW --- src/main/blackbox/blackbox.c | 4 +++- src/main/config/config.c | 6 +++--- src/main/config/config_master.h | 2 +- src/main/flight/pid.c | 5 ++++- src/main/flight/pid.h | 2 +- src/main/io/serial_cli.c | 4 ++-- src/main/sensors/gyro.c | 11 +++++++---- src/main/sensors/gyro.h | 2 +- 8 files changed, 22 insertions(+), 14 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 96281b05f..fc4a7ceb8 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -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("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("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_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_hardware:%d", masterConfig.acc_hardware); BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", masterConfig.baro_hardware); diff --git a/src/main/config/config.c b/src/main/config/config.c index 7e535ee36..c18c1ed34 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -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_q = 5; + pidProfile->dterm_notch_bw = 200; 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_q = 5; + masterConfig.gyro_soft_notch_bw = 200; masterConfig.debug_mode = DEBUG_NONE; @@ -747,7 +747,7 @@ void activateConfig(void) ¤tProfile->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 telemetryUseConfig(&masterConfig.telemetryConfig); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index e0bf2f769..48046bd22 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -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 - 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_ki; // DCM filter integral gain ( x 10000) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index db7165bf8..4b19dbe18 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -114,7 +114,10 @@ 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); + 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) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 7209f193d..9316af5e0 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -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 - uint8_t dterm_notch_q; // Biquad dterm notch quality + uint16_t dterm_notch_bw; // 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 diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 8bb22a953..edb7f4392 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -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_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 } }, { "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_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 } }, { "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 } }, diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 2905b9a29..7833ccc7c 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -50,25 +50,28 @@ static biquadFilter_t gyroFilterNotch[XYZ_AXIS_COUNT]; static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT]; static uint8_t gyroSoftLpfType; static uint16_t gyroSoftNotchHz; -static uint8_t gyroSoftNotchQ; +static float gyroSoftNotchQ; 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, 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; gyroSoftLpfHz = gyro_soft_lpf_hz; gyroSoftNotchHz = gyro_soft_notch_hz; - gyroSoftNotchQ = gyro_soft_notch_q; + 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); } 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); + biquadFilterInit(&gyroFilterNotch[axis], gyroSoftNotchHz, gyro.targetLooptime, gyroSoftNotchQ, FILTER_NOTCH); if (gyroSoftLpfType == FILTER_BIQUAD) biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime); else diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 63c99f1a6..385ae4a98 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -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, 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 gyroInit(void); void gyroUpdate(void);