diff --git a/src/main/config/config.c b/src/main/config/config.c index c9f8f18d08..7f2b45743e 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -400,6 +400,7 @@ static void resetConf(void) masterConfig.current_profile_index = 0; // default profile masterConfig.dcm_kp = 2500; // 1.0 * 10000 masterConfig.dcm_ki = 0; // 0.003 * 10000 + masterConfig.gyro_lpf = 1; // 1KHZ or 8KHZ resetAccelerometerTrims(&masterConfig.accZero); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 24813a1d27..1fe6f3c55f 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -47,7 +47,7 @@ typedef struct master_t { int8_t yaw_control_direction; // change control direction of yaw (inverted, normal) uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device 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_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. 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_mpu.c b/src/main/drivers/accgyro_mpu.c index dffde4b9d8..aed61f2998 100644 --- a/src/main/drivers/accgyro_mpu.c +++ b/src/main/drivers/accgyro_mpu.c @@ -291,30 +291,6 @@ void mpuIntExtiInit(void) mpuExtiInitDone = true; } -uint8_t determineMPULPF(uint16_t lpf) -{ - uint8_t mpuLowPassFilter; - - if (lpf == 256) - mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2; - else if (lpf >= 188) - mpuLowPassFilter = INV_FILTER_188HZ; - else if (lpf >= 98) - mpuLowPassFilter = INV_FILTER_98HZ; - else if (lpf >= 42) - mpuLowPassFilter = INV_FILTER_42HZ; - else if (lpf >= 20) - mpuLowPassFilter = INV_FILTER_20HZ; - else if (lpf >= 10) - mpuLowPassFilter = INV_FILTER_10HZ; - else if (lpf > 0) - mpuLowPassFilter = INV_FILTER_5HZ; - else - mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2; - - return mpuLowPassFilter; -} - static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data) { bool ack = i2cRead(MPU_ADDRESS, reg, length, data); diff --git a/src/main/drivers/accgyro_mpu.h b/src/main/drivers/accgyro_mpu.h index 8dbfc4f9d2..dcbca700ba 100644 --- a/src/main/drivers/accgyro_mpu.h +++ b/src/main/drivers/accgyro_mpu.h @@ -126,17 +126,6 @@ typedef struct mpuConfiguration_s { extern mpuConfiguration_t mpuConfiguration; -enum lpf_e { - INV_FILTER_256HZ_NOLPF2 = 0, - INV_FILTER_188HZ, - INV_FILTER_98HZ, - INV_FILTER_42HZ, - INV_FILTER_20HZ, - INV_FILTER_10HZ, - INV_FILTER_5HZ, - INV_FILTER_2100HZ_NOLPF, - NUM_FILTER -}; enum gyro_fsr_e { INV_FSR_250DPS = 0, INV_FSR_500DPS, @@ -178,7 +167,6 @@ typedef struct mpuDetectionResult_s { extern mpuDetectionResult_t mpuDetectionResult; -uint8_t determineMPULPF(uint16_t lpf); void configureMPUDataReadyInterruptHandling(void); void mpuIntExtiInit(void); bool mpuAccRead(int16_t *accData); diff --git a/src/main/drivers/accgyro_mpu3050.c b/src/main/drivers/accgyro_mpu3050.c index a829393858..c974728156 100644 --- a/src/main/drivers/accgyro_mpu3050.c +++ b/src/main/drivers/accgyro_mpu3050.c @@ -47,7 +47,7 @@ #define MPU3050_USER_RESET 0x01 #define MPU3050_CLK_SEL_PLL_GX 0x01 -static void mpu3050Init(uint16_t lpf); +static void mpu3050Init(uint8_t lpf); static bool mpu3050ReadTemp(int16_t *tempData); bool mpu3050Detect(gyro_t *gyro) @@ -66,19 +66,17 @@ bool mpu3050Detect(gyro_t *gyro) return true; } -static void mpu3050Init(uint16_t lpf) +static void mpu3050Init(uint8_t lpf) { bool ack; - uint8_t mpuLowPassFilter = determineMPULPF(lpf); - delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe ack = mpuConfiguration.write(MPU3050_SMPLRT_DIV, 0); if (!ack) failureMode(FAILURE_ACC_INIT); - mpuConfiguration.write(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | mpuLowPassFilter); + mpuConfiguration.write(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | lpf); mpuConfiguration.write(MPU3050_INT_CFG, 0); mpuConfiguration.write(MPU3050_USER_CTRL, MPU3050_USER_RESET); mpuConfiguration.write(MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX); diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c index 38c418c711..19ce33c03a 100644 --- a/src/main/drivers/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro_mpu6050.c @@ -52,7 +52,7 @@ extern uint8_t mpuLowPassFilter; #define MPU6050_SMPLRT_DIV 0 // 8000Hz static void mpu6050AccInit(void); -static void mpu6050GyroInit(uint16_t lpf); +static void mpu6050GyroInit(uint8_t lpf); bool mpu6050AccDetect(acc_t *acc) { @@ -96,20 +96,18 @@ static void mpu6050AccInit(void) } } -static void mpu6050GyroInit(uint16_t lpf) +static void mpu6050GyroInit(uint8_t lpf) { bool ack; mpuIntExtiInit(); - uint8_t mpuLowPassFilter = determineMPULPF(lpf); - ack = mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1 delay(100); ack = mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference) ack = mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure - ack = mpuConfiguration.write(MPU_RA_CONFIG, mpuLowPassFilter); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz) + ack = mpuConfiguration.write(MPU_RA_CONFIG, lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz) ack = mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec // ACC Init stuff. diff --git a/src/main/drivers/accgyro_mpu6500.c b/src/main/drivers/accgyro_mpu6500.c index c2153240da..bfa963c291 100644 --- a/src/main/drivers/accgyro_mpu6500.c +++ b/src/main/drivers/accgyro_mpu6500.c @@ -71,7 +71,7 @@ void mpu6500AccInit(void) acc_1G = 512 * 8; } -void mpu6500GyroInit(uint16_t lpf) +void mpu6500GyroInit(uint8_t lpf) { mpuIntExtiInit(); @@ -90,8 +90,6 @@ void mpu6500GyroInit(uint16_t lpf) mpuIntExtiInit(); - uint8_t mpuLowPassFilter = determineMPULPF(lpf); - mpuConfiguration.write(MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET); delay(100); mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x07); @@ -101,7 +99,7 @@ void mpu6500GyroInit(uint16_t lpf) mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL); mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); - mpuConfiguration.write(MPU_RA_CONFIG, mpuLowPassFilter); + mpuConfiguration.write(MPU_RA_CONFIG, lpf); mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops // Data ready interrupt configuration diff --git a/src/main/drivers/accgyro_mpu6500.h b/src/main/drivers/accgyro_mpu6500.h index 0c64cfa160..f8cc65eb85 100644 --- a/src/main/drivers/accgyro_mpu6500.h +++ b/src/main/drivers/accgyro_mpu6500.h @@ -25,4 +25,4 @@ bool mpu6500AccDetect(acc_t *acc); bool mpu6500GyroDetect(gyro_t *gyro); void mpu6500AccInit(void); -void mpu6500GyroInit(uint16_t lpf); +void mpu6500GyroInit(uint8_t lpf); diff --git a/src/main/drivers/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro_spi_mpu6000.c index ef4c85cff6..d5973bf347 100644 --- a/src/main/drivers/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro_spi_mpu6000.c @@ -120,12 +120,10 @@ bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data) return true; } -void mpu6000SpiGyroInit(uint16_t lpf) +void mpu6000SpiGyroInit(uint8_t lpf) { mpuIntExtiInit(); - uint8_t mpuLowPassFilter = determineMPULPF(lpf); - mpu6000AccAndGyroInit(); mpuIntExtiInit(); @@ -135,7 +133,7 @@ void mpu6000SpiGyroInit(uint16_t lpf) spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER); // Accel and Gyro DLPF Setting - mpu6000WriteRegister(MPU6000_CONFIG, mpuLowPassFilter); + mpu6000WriteRegister(MPU6000_CONFIG, lpf); delayMicroseconds(1); int16_t data[3]; diff --git a/src/main/drivers/gyro_sync.c b/src/main/drivers/gyro_sync.c index 74213d25f1..b6fdb42251 100644 --- a/src/main/drivers/gyro_sync.c +++ b/src/main/drivers/gyro_sync.c @@ -41,15 +41,19 @@ bool gyroSyncCheckUpdate(void) { return getMpuDataStatus(&gyro); } -void gyroUpdateSampleRate(void) { - int gyroSamplePeriod; - int minLooptime; +void gyroUpdateSampleRate(uint8_t lpf) { + int gyroSamplePeriod, gyroSyncDenominator; - gyroSamplePeriod = 1000; // gyro sampling rate 1khz - minLooptime = 1000; // Full 1khz sampling + if (!lpf) { + gyroSamplePeriod = 125; + gyroSyncDenominator = 8; // Sample every 8th gyro measurement + } else { + gyroSamplePeriod = 1000; + gyroSyncDenominator = 1; // Full Sampling + } // calculate gyro divider and targetLooptime (expected cycleTime) - mpuDividerDrops = (minLooptime + gyroSamplePeriod -1 ) / gyroSamplePeriod - 1; + mpuDividerDrops = gyroSyncDenominator - 1; targetLooptime = (mpuDividerDrops + 1) * gyroSamplePeriod; } diff --git a/src/main/drivers/gyro_sync.h b/src/main/drivers/gyro_sync.h index 4d04a7968b..99b081eb40 100644 --- a/src/main/drivers/gyro_sync.h +++ b/src/main/drivers/gyro_sync.h @@ -9,4 +9,4 @@ extern uint32_t targetLooptime; bool gyroSyncCheckUpdate(void); uint8_t gyroMPU6xxxGetDividerDrops(void); -void gyroUpdateSampleRate(void); +void gyroUpdateSampleRate(uint8_t lpf); diff --git a/src/main/drivers/sensor.h b/src/main/drivers/sensor.h index fb776c9b2b..739e3e4b5b 100644 --- a/src/main/drivers/sensor.h +++ b/src/main/drivers/sensor.h @@ -19,5 +19,5 @@ typedef void (*sensorInitFuncPtr)(void); // sensor init prototype typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype -typedef void (*sensorGyroInitFuncPtr)(uint16_t lpf); // gyro sensor init prototype +typedef void (*sensorGyroInitFuncPtr)(uint8_t lpf); // gyro sensor init prototype typedef void (*sensorInterruptFuncPtr)(bool *data); // sensor Interrupt Data Ready diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 3f511fd902..6f9e32fc34 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -351,7 +351,12 @@ static const char * const lookupTableSerialRX[] = { }; static const char * const lookupTableGyroFilter[] = { - "OFF", "LOW", "MEDIUM", "HIGH" + "LOW", "MEDIUM", "HIGH" +}; + +static const char * const lookupTableGyroSampling[] = { + "8KHZ", + "1KHZ" }; @@ -373,6 +378,7 @@ typedef enum { TABLE_PID_CONTROLLER, TABLE_SERIAL_RX, TABLE_GYRO_FILTER, + TABLE_GYRO_SAMPLING, } lookupTableIndex_e; static const lookupTableEntry_t lookupTables[] = { @@ -387,7 +393,8 @@ static const lookupTableEntry_t lookupTables[] = { { lookupTableGimbalMode, sizeof(lookupTableGimbalMode) / sizeof(char *) }, { lookupTablePidController, sizeof(lookupTablePidController) / sizeof(char *) }, { lookupTableSerialRX, sizeof(lookupTableSerialRX) / sizeof(char *) }, - { lookupTableGyroFilter, sizeof(lookupTableGyroFilter) / sizeof(char *) } + { lookupTableGyroFilter, sizeof(lookupTableGyroFilter) / sizeof(char *) }, + { lookupTableGyroSampling, sizeof(lookupTableGyroSampling) / sizeof(char *) } }; #define VALUE_TYPE_OFFSET 0 @@ -527,7 +534,7 @@ const clivalue_t valueTable[] = { { "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &masterConfig.max_angle_inclination, .config.minmax = { 100, 900 } }, - { "gyro_lpf", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_lpf, .config.minmax = { 0, 256 } }, + { "gyro_sampling", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_SAMPLING } }, { "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 a05a7d5a90..e1381c5930 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -378,7 +378,7 @@ void init(void) } #endif - if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination)) { + if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination, masterConfig.gyro_lpf)) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 0358de3616..6fa5f5e40d 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -29,8 +29,6 @@ typedef enum { GYRO_FAKE } gyroSensor_e; -#define GYRO_LPF 188 - extern gyro_t gyro; extern sensor_align_e gyroAlign; diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 314324e5f3..2e1d527437 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -644,12 +644,10 @@ void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig) } } -bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig) +bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig, uint8_t gyroLpf) { int16_t deg, min; - uint16_t gyroLpf = GYRO_LPF; - memset(&acc, 0, sizeof(acc)); memset(&gyro, 0, sizeof(gyro)); @@ -672,7 +670,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t a if (sensors(SENSOR_ACC)) acc.init(); // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here. - gyroUpdateSampleRate(); // Set gyro refresh rate before initialisation + gyroUpdateSampleRate(gyroLpf); // Set gyro refresh rate before initialisation gyro.init(gyroLpf); detectMag(magHardwareToUse); diff --git a/src/main/sensors/initialisation.h b/src/main/sensors/initialisation.h index fd7cb83195..93de770a89 100644 --- a/src/main/sensors/initialisation.h +++ b/src/main/sensors/initialisation.h @@ -17,4 +17,4 @@ #pragma once -bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig); +bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig, uint8_t gyroLpf);