From 2c7d06a94e9343ed105a82cc7711bf03ed3553f9 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 23 Nov 2016 19:04:14 +0000 Subject: [PATCH] Moved gyro filter settings into gyroConfig --- src/main/blackbox/blackbox.c | 12 ++++++------ src/main/cms/cms_menu_imu.c | 10 +++++----- src/main/config/config_master.h | 8 ++------ src/main/fc/config.c | 28 +++++++++++----------------- src/main/fc/fc_msp.c | 28 +++++++++++----------------- src/main/fc/mw.c | 2 +- src/main/io/serial_cli.c | 12 ++++++------ src/main/sensors/gyro.c | 24 +++++++++--------------- src/main/sensors/gyro.h | 16 ++++++++-------- 9 files changed, 59 insertions(+), 81 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 4d10e9077..ed4ba0137 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1263,12 +1263,12 @@ static bool blackboxWriteSysinfo() 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_soft_type:%d", masterConfig.gyro_soft_type); - BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d", masterConfig.gyro_soft_lpf_hz); - BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz:%d,%d", masterConfig.gyro_soft_notch_hz_1, - masterConfig.gyro_soft_notch_hz_2); - BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff:%d,%d", masterConfig.gyro_soft_notch_cutoff_1, - masterConfig.gyro_soft_notch_cutoff_2); + BLACKBOX_PRINT_HEADER_LINE("gyro_soft_type:%d", masterConfig.gyroConfig.gyro_soft_lpf_type); + BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d", masterConfig.gyroConfig.gyro_soft_lpf_hz); + BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz:%d,%d", masterConfig.gyroConfig.gyro_soft_notch_hz_1, + masterConfig.gyroConfig.gyro_soft_notch_hz_2); + BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff:%d,%d", masterConfig.gyroConfig.gyro_soft_notch_cutoff_1, + masterConfig.gyroConfig.gyro_soft_notch_cutoff_2); BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz:%d", (int)(masterConfig.acc_lpf_hz * 100.0f)); BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d", masterConfig.sensorSelectionConfig.acc_hardware); BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", masterConfig.sensorSelectionConfig.baro_hardware); diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 24b988cb0..57ecc67ba 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -282,11 +282,11 @@ static OSD_Entry cmsx_menuFilterGlobalEntries[] = { { "-- FILTER GLB --", OME_Label, NULL, NULL, 0 }, - { "GYRO LPF", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.gyro_soft_lpf_hz, 0, 255, 1 }, 0 }, - { "GYRO NF1", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyro_soft_notch_hz_1, 0, 500, 1 }, 0 }, - { "GYRO NF1C", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyro_soft_notch_cutoff_1, 0, 500, 1 }, 0 }, - { "GYRO NF2", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyro_soft_notch_hz_2, 0, 500, 1 }, 0 }, - { "GYRO NF2C", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyro_soft_notch_cutoff_2, 0, 500, 1 }, 0 }, + { "GYRO LPF", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.gyroConfig.gyro_soft_lpf_hz, 0, 255, 1 }, 0 }, + { "GYRO NF1", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyroConfig.gyro_soft_notch_hz_1, 0, 500, 1 }, 0 }, + { "GYRO NF1C", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyroConfig.gyro_soft_notch_cutoff_1, 0, 500, 1 }, 0 }, + { "GYRO NF2", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyroConfig.gyro_soft_notch_hz_2, 0, 500, 1 }, 0 }, + { "GYRO NF2C", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyroConfig.gyro_soft_notch_cutoff_2, 0, 500, 1 }, 0 }, { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 827be582b..e74ed5cbc 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -91,14 +91,10 @@ typedef struct master_s { int8_t yaw_control_direction; // change control direction of yaw (inverted, normal) uint8_t acc_for_fast_looptime; // shorten acc processing time by using 1 out of 9 samples. For combination with fast looptimes. + + uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. uint8_t gyro_sync_denom; // Gyro sample divider - uint8_t gyro_soft_type; // Gyro Filter Type - uint8_t gyro_soft_lpf_hz; // Biquad gyro lpf hz - uint16_t gyro_soft_notch_hz_1; // Biquad gyro notch hz - uint16_t gyro_soft_notch_cutoff_1; // Biquad gyro notch low cutoff - uint16_t gyro_soft_notch_hz_2; // Biquad gyro notch hz - uint16_t gyro_soft_notch_cutoff_2; // 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) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 4d002f16d..794bd669f 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -575,12 +575,12 @@ void createDefaultConfig(master_t *config) config->gyro_sync_denom = 4; config->pid_process_denom = 2; #endif - config->gyro_soft_type = FILTER_PT1; - config->gyro_soft_lpf_hz = 90; - config->gyro_soft_notch_hz_1 = 400; - config->gyro_soft_notch_cutoff_1 = 300; - config->gyro_soft_notch_hz_2 = 200; - config->gyro_soft_notch_cutoff_2 = 100; + config->gyroConfig.gyro_soft_lpf_type = FILTER_PT1; + config->gyroConfig.gyro_soft_lpf_hz = 90; + config->gyroConfig.gyro_soft_notch_hz_1 = 400; + config->gyroConfig.gyro_soft_notch_cutoff_1 = 300; + config->gyroConfig.gyro_soft_notch_hz_2 = 200; + config->gyroConfig.gyro_soft_notch_cutoff_2 = 100; config->debug_mode = DEBUG_NONE; @@ -830,13 +830,7 @@ void activateConfig(void) ¤tProfile->pidProfile ); - gyroUseConfig(&masterConfig.gyroConfig, - masterConfig.gyro_soft_lpf_hz, - masterConfig.gyro_soft_notch_hz_1, - masterConfig.gyro_soft_notch_cutoff_1, - masterConfig.gyro_soft_notch_hz_2, - masterConfig.gyro_soft_notch_cutoff_2, - masterConfig.gyro_soft_type); + gyroUseConfig(&masterConfig.gyroConfig); #ifdef TELEMETRY telemetryUseConfig(&masterConfig.telemetryConfig); @@ -1002,11 +996,11 @@ void validateAndFixConfig(void) void validateAndFixGyroConfig(void) { // Prevent invalid notch cutoff - if (masterConfig.gyro_soft_notch_cutoff_1 >= masterConfig.gyro_soft_notch_hz_1) { - masterConfig.gyro_soft_notch_hz_1 = 0; + if (masterConfig.gyroConfig.gyro_soft_notch_cutoff_1 >= masterConfig.gyroConfig.gyro_soft_notch_hz_1) { + masterConfig.gyroConfig.gyro_soft_notch_hz_1 = 0; } - if (masterConfig.gyro_soft_notch_cutoff_2 >= masterConfig.gyro_soft_notch_hz_2) { - masterConfig.gyro_soft_notch_hz_2 = 0; + if (masterConfig.gyroConfig.gyro_soft_notch_cutoff_2 >= masterConfig.gyroConfig.gyro_soft_notch_hz_2) { + masterConfig.gyroConfig.gyro_soft_notch_hz_2 = 0; } if (masterConfig.gyro_lpf != GYRO_LPF_256HZ && masterConfig.gyro_lpf != GYRO_LPF_NONE) { diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index adca0a366..664dc628e 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1094,15 +1094,15 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn break; case MSP_FILTER_CONFIG : - sbufWriteU8(dst, masterConfig.gyro_soft_lpf_hz); + sbufWriteU8(dst, masterConfig.gyroConfig.gyro_soft_lpf_hz); sbufWriteU16(dst, currentProfile->pidProfile.dterm_lpf_hz); sbufWriteU16(dst, currentProfile->pidProfile.yaw_lpf_hz); - sbufWriteU16(dst, masterConfig.gyro_soft_notch_hz_1); - sbufWriteU16(dst, masterConfig.gyro_soft_notch_cutoff_1); + sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_hz_1); + sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_cutoff_1); sbufWriteU16(dst, currentProfile->pidProfile.dterm_notch_hz); sbufWriteU16(dst, currentProfile->pidProfile.dterm_notch_cutoff); - sbufWriteU16(dst, masterConfig.gyro_soft_notch_hz_2); - sbufWriteU16(dst, masterConfig.gyro_soft_notch_cutoff_2); + sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_hz_2); + sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_cutoff_2); break; case MSP_PID_ADVANCED: @@ -1445,28 +1445,22 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_FILTER_CONFIG: - masterConfig.gyro_soft_lpf_hz = sbufReadU8(src); + masterConfig.gyroConfig.gyro_soft_lpf_hz = sbufReadU8(src); currentProfile->pidProfile.dterm_lpf_hz = sbufReadU16(src); currentProfile->pidProfile.yaw_lpf_hz = sbufReadU16(src); if (dataSize > 5) { - masterConfig.gyro_soft_notch_hz_1 = sbufReadU16(src); - masterConfig.gyro_soft_notch_cutoff_1 = sbufReadU16(src); + masterConfig.gyroConfig.gyro_soft_notch_hz_1 = sbufReadU16(src); + masterConfig.gyroConfig.gyro_soft_notch_cutoff_1 = sbufReadU16(src); currentProfile->pidProfile.dterm_notch_hz = sbufReadU16(src); currentProfile->pidProfile.dterm_notch_cutoff = sbufReadU16(src); } if (dataSize > 13) { - masterConfig.gyro_soft_notch_hz_2 = sbufReadU16(src); - masterConfig.gyro_soft_notch_cutoff_2 = sbufReadU16(src); + masterConfig.gyroConfig.gyro_soft_notch_hz_2 = sbufReadU16(src); + masterConfig.gyroConfig.gyro_soft_notch_cutoff_2 = sbufReadU16(src); } // reinitialize the gyro filters with the new values validateAndFixGyroConfig(); - gyroUseConfig(&masterConfig.gyroConfig, - masterConfig.gyro_soft_lpf_hz, - masterConfig.gyro_soft_notch_hz_1, - masterConfig.gyro_soft_notch_cutoff_1, - masterConfig.gyro_soft_notch_hz_2, - masterConfig.gyro_soft_notch_cutoff_2, - masterConfig.gyro_soft_type); + gyroUseConfig(&masterConfig.gyroConfig); gyroInit(); // reinitialize the PID filters with the new values pidInitFilters(¤tProfile->pidProfile); diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c index 5f5d4dc63..aab4497f7 100644 --- a/src/main/fc/mw.c +++ b/src/main/fc/mw.c @@ -803,7 +803,7 @@ void subTaskMotorUpdate(void) uint8_t setPidUpdateCountDown(void) { - if (masterConfig.gyro_soft_lpf_hz) { + if (masterConfig.gyroConfig.gyro_soft_lpf_hz) { return masterConfig.pid_process_denom - 1; } else { return 1; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index a23c0b15d..b3e6b8f52 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -809,12 +809,12 @@ const clivalue_t valueTable[] = { { "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &masterConfig.max_angle_inclination, .config.minmax = { 100, 900 } }, { "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_LOWPASS_TYPE } }, - { "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 255 } }, - { "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz_1, .config.minmax = { 0, 1000 } }, - { "gyro_notch1_cutoff", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_cutoff_1, .config.minmax = { 1, 1000 } }, - { "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz_2, .config.minmax = { 0, 1000 } }, - { "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_cutoff_2, .config.minmax = { 1, 1000 } }, + { "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyroConfig.gyro_soft_lpf_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, + { "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 255 } }, + { "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_notch_hz_1, .config.minmax = { 0, 1000 } }, + { "gyro_notch1_cutoff", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_notch_cutoff_1, .config.minmax = { 1, 1000 } }, + { "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_notch_hz_2, .config.minmax = { 0, 1000 } }, + { "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_notch_cutoff_2, .config.minmax = { 1, 1000 } }, { "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 } }, diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index f5b51ea1b..37855041e 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -57,25 +57,19 @@ static void *notchFilter1[3]; static filterApplyFnPtr notchFilter2ApplyFn; static void *notchFilter2[3]; -void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, - uint8_t gyro_soft_lpf_hz, - uint16_t gyro_soft_notch_hz_1, - uint16_t gyro_soft_notch_cutoff_1, - uint16_t gyro_soft_notch_hz_2, - uint16_t gyro_soft_notch_cutoff_2, - uint8_t gyro_soft_lpf_type) +void gyroUseConfig(const gyroConfig_t *gyroConfigToUse) { gyroConfig = gyroConfigToUse; - gyroSoftLpfHz = gyro_soft_lpf_hz; - gyroSoftNotchHz1 = gyro_soft_notch_hz_1; - if (gyro_soft_notch_hz_1) { - gyroSoftNotchQ1 = filterGetNotchQ(gyro_soft_notch_hz_1, gyro_soft_notch_cutoff_1); + gyroSoftLpfHz = gyroConfig->gyro_soft_lpf_hz; + gyroSoftNotchHz1 = gyroConfig->gyro_soft_notch_hz_1; + if (gyroConfig->gyro_soft_notch_hz_1) { + gyroSoftNotchQ1 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_1, gyroConfig->gyro_soft_notch_cutoff_1); } - gyroSoftNotchHz2 = gyro_soft_notch_hz_2; - if (gyro_soft_notch_hz_2) { - gyroSoftNotchQ2 = filterGetNotchQ(gyro_soft_notch_hz_2, gyro_soft_notch_cutoff_2); + gyroSoftNotchHz2 = gyroConfig->gyro_soft_notch_hz_2; + if (gyroConfig->gyro_soft_notch_hz_2) { + gyroSoftNotchQ2 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_2, gyroConfig->gyro_soft_notch_cutoff_2); } - gyroSoftLpfType = gyro_soft_lpf_type; + gyroSoftLpfType = gyroConfig->gyro_soft_lpf_type; } void gyroInit(void) diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 2946a4602..7f60d7dee 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -41,16 +41,16 @@ extern int32_t gyroADC[XYZ_AXIS_COUNT]; extern float gyroADCf[XYZ_AXIS_COUNT]; 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. + uint8_t gyro_soft_lpf_hz; + uint16_t gyro_soft_notch_hz_1; + uint16_t gyro_soft_notch_cutoff_1; + uint16_t gyro_soft_notch_hz_2; + uint16_t gyro_soft_notch_cutoff_2; + uint8_t gyro_soft_lpf_type; } gyroConfig_t; -void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, - uint8_t gyro_soft_lpf_hz, - uint16_t gyro_soft_notch_hz_1, - uint16_t gyro_soft_notch_cutoff_1, - uint16_t gyro_soft_notch_hz_2, - uint16_t gyro_soft_notch_cutoff_2, - uint8_t gyro_soft_lpf_type); +void gyroUseConfig(const gyroConfig_t *gyroConfigToUse); void gyroSetCalibrationCycles(void); void gyroInit(void); void gyroUpdate(void);