diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 4d10e90778..e8df1a8ea4 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1199,7 +1199,7 @@ static bool blackboxWriteSysinfo() ); BLACKBOX_PRINT_HEADER_LINE("looptime:%d", gyro.targetLooptime); - BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom:%d", masterConfig.gyro_sync_denom); + BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom:%d", masterConfig.gyroConfig.gyro_sync_denom); BLACKBOX_PRINT_HEADER_LINE("pid_process_denom:%d", masterConfig.pid_process_denom); BLACKBOX_PRINT_HEADER_LINE("rcRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcRate8); BLACKBOX_PRINT_HEADER_LINE("rcExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcExpo8); @@ -1262,13 +1262,13 @@ 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_lpf:%d", masterConfig.gyroConfig.gyro_lpf); + 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 24b988cb07..57ecc67ba9 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 827be582b0..a0aec3753a 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -91,14 +91,7 @@ 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/drivers/accgyro.h b/src/main/drivers/accgyro.h index 0020d770fa..7e43b73078 100644 --- a/src/main/drivers/accgyro.h +++ b/src/main/drivers/accgyro.h @@ -23,6 +23,15 @@ #define MPU_I2C_INSTANCE I2C_DEVICE #endif +#define GYRO_LPF_256HZ 0 +#define GYRO_LPF_188HZ 1 +#define GYRO_LPF_98HZ 2 +#define GYRO_LPF_42HZ 3 +#define GYRO_LPF_20HZ 4 +#define GYRO_LPF_10HZ 5 +#define GYRO_LPF_5HZ 6 +#define GYRO_LPF_NONE 7 + typedef struct gyro_s { sensorGyroInitFuncPtr init; // initialize function sensorReadFuncPtr read; // read 3 axis data function diff --git a/src/main/drivers/gyro_sync.c b/src/main/drivers/gyro_sync.c index dbdc2244c9..4e9f02622b 100644 --- a/src/main/drivers/gyro_sync.c +++ b/src/main/drivers/gyro_sync.c @@ -23,15 +23,6 @@ bool gyroSyncCheckUpdate(const gyro_t *gyro) return gyro->intStatus(); } -#define GYRO_LPF_256HZ 0 -#define GYRO_LPF_188HZ 1 -#define GYRO_LPF_98HZ 2 -#define GYRO_LPF_42HZ 3 -#define GYRO_LPF_20HZ 4 -#define GYRO_LPF_10HZ 5 -#define GYRO_LPF_5HZ 6 -#define GYRO_LPF_NONE 7 - uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator) { int gyroSamplePeriod; diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 223fdfc9f6..ad3c3a5b89 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -564,23 +564,23 @@ void createDefaultConfig(master_t *config) config->current_profile_index = 0; // default profile config->dcm_kp = 2500; // 1.0 * 10000 config->dcm_ki = 0; // 0.003 * 10000 - config->gyro_lpf = 0; // 256HZ default + config->gyroConfig.gyro_lpf = GYRO_LPF_256HZ; // 256HZ default #ifdef STM32F10X - config->gyro_sync_denom = 8; + config->gyroConfig.gyro_sync_denom = 8; config->pid_process_denom = 1; #elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20689) - config->gyro_sync_denom = 1; + config->gyroConfig.gyro_sync_denom = 1; config->pid_process_denom = 4; #else - config->gyro_sync_denom = 4; + config->gyroConfig.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,21 +830,6 @@ void activateConfig(void) ¤tProfile->pidProfile ); - // 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.gyro_soft_notch_cutoff_2 >= masterConfig.gyro_soft_notch_hz_2) - masterConfig.gyro_soft_notch_hz_2 = 0; - - 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); - #ifdef TELEMETRY telemetryUseConfig(&masterConfig.telemetryConfig); #endif @@ -998,12 +983,30 @@ void validateAndFixConfig(void) if (!isSerialConfigValid(serialConfig)) { resetSerialConfig(serialConfig); } - + + validateAndFixGyroConfig(); + #if defined(TARGET_VALIDATECONFIG) targetValidateConfiguration(&masterConfig); #endif } +void validateAndFixGyroConfig(void) +{ + // Prevent invalid notch cutoff + 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.gyroConfig.gyro_soft_notch_cutoff_2 >= masterConfig.gyroConfig.gyro_soft_notch_hz_2) { + masterConfig.gyroConfig.gyro_soft_notch_hz_2 = 0; + } + + if (masterConfig.gyroConfig.gyro_lpf != GYRO_LPF_256HZ && masterConfig.gyroConfig.gyro_lpf != GYRO_LPF_NONE) { + masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed + masterConfig.gyroConfig.gyro_sync_denom = 1; + } +} + void readEEPROMAndNotify(void) { // re-read written data diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 6f903e5d83..3a3c4334a6 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -75,6 +75,7 @@ void ensureEEPROMContainsValidData(void); void saveConfigAndNotify(void); void validateAndFixConfig(void); +void validateAndFixGyroConfig(void); void activateConfig(void); uint8_t getCurrentProfile(void); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index df38554c72..808859860a 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1081,11 +1081,11 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn break; case MSP_ADVANCED_CONFIG: - if (masterConfig.gyro_lpf) { + if (masterConfig.gyroConfig.gyro_lpf) { sbufWriteU8(dst, 8); // If gyro_lpf != OFF then looptime is set to 1000 sbufWriteU8(dst, 1); } else { - sbufWriteU8(dst, masterConfig.gyro_sync_denom); + sbufWriteU8(dst, masterConfig.gyroConfig.gyro_sync_denom); sbufWriteU8(dst, masterConfig.pid_process_denom); } sbufWriteU8(dst, masterConfig.motorConfig.useUnsyncedPwm); @@ -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: @@ -1433,7 +1433,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_ADVANCED_CONFIG: - masterConfig.gyro_sync_denom = sbufReadU8(src); + masterConfig.gyroConfig.gyro_sync_denom = sbufReadU8(src); masterConfig.pid_process_denom = sbufReadU8(src); masterConfig.motorConfig.useUnsyncedPwm = sbufReadU8(src); #ifdef USE_DSHOT @@ -1445,19 +1445,24 @@ 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(); + gyroInit(&masterConfig.gyroConfig); + // reinitialize the PID filters with the new values + pidInitFilters(¤tProfile->pidProfile); break; case MSP_SET_PID_ADVANCED: diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c index 5f5d4dc63b..aab4497f79 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/flight/pid.c b/src/main/flight/pid.c index 21dfbcdd95..cccfad9ab0 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -56,7 +56,7 @@ int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; static float errorGyroIf[3]; -void setTargetPidLooptime(uint32_t pidLooptime) +void pidSetTargetLooptime(uint32_t pidLooptime) { targetPidLooptime = pidLooptime; } @@ -82,7 +82,7 @@ static void *dtermFilterLpf[2]; static filterApplyFnPtr ptermYawFilterApplyFn; static void *ptermYawFilter; -static void pidInitFilters(const pidProfile_t *pidProfile) +void pidInitFilters(const pidProfile_t *pidProfile) { static bool initialized = false; // !!TODO - remove this temporary measure once filter lazy initialization is removed static biquadFilter_t biquadFilterNotch[2]; @@ -166,8 +166,6 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio dT = (float)targetPidLooptime * 0.000001f; } - pidInitFilters(pidProfile); - float horizonLevelStrength = 1; if (FLIGHT_MODE(HORIZON_MODE)) { // Figure out the raw stick positions diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index f96652c92d..e059ed84c7 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -104,5 +104,6 @@ extern uint8_t PIDweight[3]; void pidResetErrorGyroState(void); void pidStabilisationState(pidStabilisationState_e pidControllerState); -void setTargetPidLooptime(uint32_t pidLooptime); +void pidSetTargetLooptime(uint32_t pidLooptime); +void pidInitFilters(const pidProfile_t *pidProfile); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index a23c0b15d2..b7039530af 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -807,14 +807,14 @@ const clivalue_t valueTable[] = { { "align_board_yaw", VAR_INT16 | MASTER_VALUE, &masterConfig.boardAlignment.yawDegrees, .config.minmax = { -180, 360 } }, { "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_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyroConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } }, + { "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_sync_denom, .config.minmax = { 1, 8 } }, + { "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/main.c b/src/main/main.c index 8cb9faf375..48849f012d 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -417,11 +417,16 @@ void init(void) } #endif +#ifdef SONAR + const sonarConfig_t *sonarConfig = &masterConfig.sonarConfig; +#else + const void *sonarConfig = NULL; +#endif if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, &masterConfig.sensorSelectionConfig, masterConfig.compassConfig.mag_declination, - masterConfig.gyro_lpf, - masterConfig.gyro_sync_denom)) { + &masterConfig.gyroConfig, + sonarConfig)) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); } @@ -443,10 +448,9 @@ void init(void) LED0_OFF; LED1_OFF; -#ifdef MAG - if (sensors(SENSOR_MAG)) - compassInit(); -#endif + // gyro.targetLooptime set in sensorsAutodetect(), so we are ready to call pidSetTargetLooptime() + pidSetTargetLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * masterConfig.pid_process_denom); // Initialize pid looptime + pidInitFilters(¤tProfile->pidProfile); imuInit(); @@ -478,12 +482,6 @@ void init(void) } #endif -#ifdef SONAR - if (feature(FEATURE_SONAR)) { - sonarInit(&masterConfig.sonarConfig); - } -#endif - #ifdef LED_STRIP ledStripInit(&masterConfig.ledStripConfig); @@ -537,13 +535,6 @@ void init(void) } #endif - if (masterConfig.gyro_lpf > 0 && masterConfig.gyro_lpf < 7) { - masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed - masterConfig.gyro_sync_denom = 1; - } - - setTargetPidLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * masterConfig.pid_process_denom); // Initialize pid looptime - #ifdef BLACKBOX initBlackbox(); #endif diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index d27c2eb17c..56c29bd83d 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -44,10 +44,6 @@ float gyroADCf[XYZ_AXIS_COUNT]; static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 }; static const gyroConfig_t *gyroConfig; -static uint8_t gyroSoftLpfType; -static uint16_t gyroSoftNotchHz1, gyroSoftNotchHz2; -static float gyroSoftNotchQ1, gyroSoftNotchQ2; -static uint8_t gyroSoftLpfHz; static uint16_t calibratingG = 0; static filterApplyFnPtr softLpfFilterApplyFn; @@ -57,24 +53,7 @@ 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) -{ - gyroConfig = gyroConfigToUse; - gyroSoftLpfHz = gyro_soft_lpf_hz; - gyroSoftNotchHz1 = gyro_soft_notch_hz_1; - gyroSoftNotchHz2 = gyro_soft_notch_hz_2; - gyroSoftLpfType = gyro_soft_lpf_type; - gyroSoftNotchQ1 = filterGetNotchQ(gyro_soft_notch_hz_1, gyro_soft_notch_cutoff_1); - gyroSoftNotchQ2 = filterGetNotchQ(gyro_soft_notch_hz_2, gyro_soft_notch_cutoff_2); -} - -void gyroInit(void) +void gyroInit(const gyroConfig_t *gyroConfigToUse) { static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT]; static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT]; @@ -82,45 +61,49 @@ void gyroInit(void) static biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT]; static biquadFilter_t gyroFilterNotch_2[XYZ_AXIS_COUNT]; + gyroConfig = gyroConfigToUse; + softLpfFilterApplyFn = nullFilterApply; notchFilter1ApplyFn = nullFilterApply; notchFilter2ApplyFn = nullFilterApply; - if (gyroSoftLpfHz) { // Initialisation needs to happen once samplingrate is known - if (gyroSoftLpfType == FILTER_BIQUAD) { + if (gyroConfig->gyro_soft_lpf_hz) { // Initialisation needs to happen once samplingrate is known + if (gyroConfig->gyro_soft_lpf_type == FILTER_BIQUAD) { softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; for (int axis = 0; axis < 3; axis++) { softLpfFilter[axis] = &gyroFilterLPF[axis]; - biquadFilterInitLPF(softLpfFilter[axis], gyroSoftLpfHz, gyro.targetLooptime); + biquadFilterInitLPF(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyro.targetLooptime); } - } else if (gyroSoftLpfType == FILTER_PT1) { + } else if (gyroConfig->gyro_soft_lpf_type == FILTER_BIQUAD) { softLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply; const float gyroDt = (float) gyro.targetLooptime * 0.000001f; for (int axis = 0; axis < 3; axis++) { softLpfFilter[axis] = &gyroFilterPt1[axis]; - pt1FilterInit(softLpfFilter[axis], gyroSoftLpfHz, gyroDt); + pt1FilterInit(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyroDt); } } else { softLpfFilterApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate; for (int axis = 0; axis < 3; axis++) { softLpfFilter[axis] = &gyroDenoiseState[axis]; - firFilterDenoiseInit(softLpfFilter[axis], gyroSoftLpfHz, gyro.targetLooptime); + firFilterDenoiseInit(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyro.targetLooptime); } } } - if (gyroSoftNotchHz1) { + if (gyroConfig->gyro_soft_notch_hz_1) { notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply; + const float gyroSoftNotchQ1 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_1, gyroConfig->gyro_soft_notch_cutoff_1); for (int axis = 0; axis < 3; axis++) { notchFilter1[axis] = &gyroFilterNotch_1[axis]; - biquadFilterInit(notchFilter1[axis], gyroSoftNotchHz1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH); + biquadFilterInit(notchFilter1[axis], gyroConfig->gyro_soft_notch_hz_1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH); } } - if (gyroSoftNotchHz1) { + if (gyroConfig->gyro_soft_notch_hz_2) { notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply; + const float gyroSoftNotchQ2 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_2, gyroConfig->gyro_soft_notch_cutoff_2); for (int axis = 0; axis < 3; axis++) { notchFilter2[axis] = &gyroFilterNotch_2[axis]; - biquadFilterInit(notchFilter2[axis], gyroSoftNotchHz2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH); + biquadFilterInit(notchFilter2[axis], gyroConfig->gyro_soft_notch_hz_2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH); } } } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 2946a4602f..bea1268e57 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -41,18 +41,19 @@ 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_sync_denom; // Gyro sample divider + uint8_t gyro_soft_lpf_type; + uint8_t gyro_soft_lpf_hz; + uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. + 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; } 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 gyroSetCalibrationCycles(void); -void gyroInit(void); +void gyroInit(const gyroConfig_t *gyroConfigToUse); void gyroUpdate(void); bool isGyroCalibrationComplete(void); diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 37bc9a20ec..d1812a9b15 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -24,6 +24,8 @@ #include "common/axis.h" +#include "config/feature.h" + #include "drivers/io.h" #include "drivers/system.h" #include "drivers/exti.h" @@ -61,6 +63,7 @@ #include "drivers/sonar_hcsr04.h" +#include "fc/config.h" #include "fc/runtime_config.h" #include "sensors/sensors.h" @@ -609,7 +612,20 @@ retry: } #endif -void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig) +#ifdef SONAR +static bool detectSonar(void) +{ + if (feature(FEATURE_SONAR)) { + // the user has set the sonar feature, so assume they have an HC-SR04 plugged in, + // since there is no way to detect it + sensorsSet(SENSOR_SONAR); + return true; + } + return false; +} +#endif + +static void reconfigureAlignment(const sensorAlignmentConfig_t *sensorAlignmentConfig) { if (sensorAlignmentConfig->gyro_align != ALIGN_DEFAULT) { gyroAlign = sensorAlignmentConfig->gyro_align; @@ -622,11 +638,11 @@ void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig) } } -bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, - sensorSelectionConfig_t *sensorSelectionConfig, +bool sensorsAutodetect(const sensorAlignmentConfig_t *sensorAlignmentConfig, + const sensorSelectionConfig_t *sensorSelectionConfig, int16_t magDeclinationFromConfig, - uint8_t gyroLpf, - uint8_t gyroSyncDenominator) + const gyroConfig_t *gyroConfig, + const sonarConfig_t *sonarConfig) { memset(&acc, 0, sizeof(acc)); memset(&gyro, 0, sizeof(gyro)); @@ -645,9 +661,9 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, // Now time to init things // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here. - gyro.targetLooptime = gyroSetSampleRate(gyroLpf, gyroSyncDenominator); // Set gyro sample rate before initialisation - gyro.init(gyroLpf); // driver initialisation - gyroInit(); // sensor initialisation + gyro.targetLooptime = gyroSetSampleRate(gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom); // Set gyro sample rate before initialisation + gyro.init(gyroConfig->gyro_lpf); // driver initialisation + gyroInit(gyroConfig); // sensor initialisation if (detectAcc(sensorSelectionConfig->acc_hardware)) { acc.acc_1G = 256; // set default @@ -664,6 +680,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, const int16_t deg = magDeclinationFromConfig / 100; const int16_t min = magDeclinationFromConfig % 100; magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units + compassInit(); } #else UNUSED(magDeclinationFromConfig); @@ -673,6 +690,14 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, detectBaro(sensorSelectionConfig->baro_hardware); #endif +#ifdef SONAR + if (detectSonar()) { + sonarInit(sonarConfig); + } +#else + UNUSED(sonarConfig); +#endif + reconfigureAlignment(sensorAlignmentConfig); return true; diff --git a/src/main/sensors/initialisation.h b/src/main/sensors/initialisation.h index 95eec1f459..c1b3204f91 100644 --- a/src/main/sensors/initialisation.h +++ b/src/main/sensors/initialisation.h @@ -16,5 +16,13 @@ */ #pragma once + +struct sensorAlignmentConfig_s; struct sensorSelectionConfig_s; -bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, struct sensorSelectionConfig_s *sensorSelectionConfig, int16_t magDeclinationFromConfig, uint8_t gyroLpf, uint8_t gyroSyncDenominator); +struct gyroConfig_s; +struct sonarConfig_s; +bool sensorsAutodetect(const struct sensorAlignmentConfig_s *sensorAlignmentConfig, + const struct sensorSelectionConfig_s *sensorSelectionConfig, + int16_t magDeclinationFromConfig, + const struct gyroConfig_s *gyroConfig, + const struct sonarConfig_s *sonarConfig); diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 36c6c7cddd..5774973293 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -977,7 +977,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) bstWrite8(masterConfig.rcControlsConfig.yaw_deadband); break; case BST_FC_FILTERS: - bstWrite16(constrain(masterConfig.gyro_lpf, 0, 1)); // Extra safety to prevent OSD setting corrupt values + bstWrite16(constrain(masterConfig.gyroConfig.gyro_lpf, 0, 1)); // Extra safety to prevent OSD setting corrupt values break; default: // we do not know how to handle the (valid) message, indicate error BST @@ -1404,7 +1404,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) masterConfig.rcControlsConfig.yaw_deadband = bstRead8(); break; case BST_SET_FC_FILTERS: - masterConfig.gyro_lpf = bstRead16(); + masterConfig.gyroConfig.gyro_lpf = bstRead16(); break; default: diff --git a/src/main/target/MOTOLAB/config.c b/src/main/target/MOTOLAB/config.c index 4cd38f98f2..94a217a364 100755 --- a/src/main/target/MOTOLAB/config.c +++ b/src/main/target/MOTOLAB/config.c @@ -26,6 +26,6 @@ // Motolab target supports 2 different type of boards Tornado / Cyclone. void targetConfiguration(master_t *config) { - config->gyro_sync_denom = 4; + config->gyroConfig.gyro_sync_denom = 4; config->pid_process_denom = 1; } diff --git a/src/main/target/MULTIFLITEPICO/config.c b/src/main/target/MULTIFLITEPICO/config.c index 5601a2c9eb..ddc186ccc7 100755 --- a/src/main/target/MULTIFLITEPICO/config.c +++ b/src/main/target/MULTIFLITEPICO/config.c @@ -70,7 +70,7 @@ void targetConfiguration(master_t *config) { config->motorConfig.motorPwmRate = 17000; - config->gyro_sync_denom = 4; + config->gyroConfig.gyro_sync_denom = 4; config->pid_process_denom = 1; config->profile[0].pidProfile.P8[ROLL] = 70; diff --git a/src/main/target/NAZE/config.c b/src/main/target/NAZE/config.c index 9fdbca437c..b4142093eb 100755 --- a/src/main/target/NAZE/config.c +++ b/src/main/target/NAZE/config.c @@ -43,10 +43,10 @@ void targetConfiguration(master_t *config) config->motorConfig.minthrottle = 1049; - config->gyro_lpf = 1; - config->gyro_soft_lpf_hz = 100; - config->gyro_soft_notch_hz_1 = 0; - config->gyro_soft_notch_hz_2 = 0; + config->gyroConfig.gyro_lpf = GYRO_LPF_188HZ; + config->gyroConfig.gyro_soft_lpf_hz = 100; + config->gyroConfig.gyro_soft_notch_hz_1 = 0; + config->gyroConfig.gyro_soft_notch_hz_2 = 0; /*for (int channel = 0; channel < NON_AUX_CHANNEL_COUNT; channel++) { config->rxConfig.channelRanges[channel].min = 1180;