Commit Graph

35 Commits

Author SHA1 Message Date
etracer65 062ef77276 Rework gyro sample rate and DLPF configuration and expose additional filter cutoffs (#5483)
The old gyro_lpf setting was based on the DLPF_CFG values for the MPU6050 gyro and the enumeration was inaccurate and misleading.  For example, the default "OFF" setting did not disable the DLPF, but actually set it to around 250hz.  The actual cutoff frequency for each setting varies by gyro hardware so the literal frequencies in the enumeration were also incorrect.

Removed gyro_lpf and replaced it with gyro_hardware_lpf (8KHz) and gyro_32khz_hardware_lpf (32KHz).  The parameters were renamed to indicate that they are hardware filtering options to differentiate from the many software lowpass filtering options.

gyro_hardware_lpf - This parameter sets the filtering and sample rate options for 8KHz gyros (or 32KHz capable gyros running in 8KHz mode).

- NORMAL - default setting that is equivalent to the previous "OFF" setting.  Configures 8KHz sampling with ~250Hz filter cutoff.
- EXPERIMENTAL - 8KHz sampling with a higher frequency filter cutoff (around 3000hz).  Considerably more noisy and requires additional software filtering.  Note that for the MPU6000 Invensense doesn't officially document the filter cutoff frequency for this selection and simply lists it as "reserved".  In testing it's clear that a higher frequency filter cutoff is being selected due to the increased noise, but the actual cutoff frequency is unknown.
- 1KHZ_SAMPLING - 1KHz sample rate with and approximate 188Hz filter cutoff.

Note that the following additional 1KHz sample rate options with lower filter cutoffs have been eliminated - "98HZ", "42HZ", "20HZ", "10HZ", "5HZ".  It seems unlikely that these are still needed are probably no longer viable and flight performance would be very poor.

gyro_32khz_hardware_lpf - This parameter sets the filtering options while running in 32KHz mode on capable gyros.  It also exposes a new high frequency filter cutoff mode.

- NORMAL - The default and matches the current settings used for 32KHz mode.  Provides a filter cutoff around 3000Hz.
- EXPERIMENTAL - Selects a filter cutoff around 8000Hz.  This is a very noisy setting and will require substantial software filtering.

The default values for both 8KHz and 32KHz sample rates were chosen to match the previous defaults and users should not experience any performance differences.

Normalized the gyro initialization.  Previously there was little consistency on how the initialization was performed and the settings interpreted.  For example, MPU9250 used a completely different logic tree when configuring the registers.

Disconnected the literal parameter value from the gyro initialization.  The gyro_lpf parameter contained a number from 0-7 that was literally applied to the configuration register during the gyro initialization.  This caused some older gyro initializations to be incorrect as they used a different register layout (MPU3050 and L3G4200D).  By transitioning to a logical selection the actual value applied to the hardware register is abstracted.  This will better future-proof the design as new gyros may have a different register structure that may be incompatible with the old method.

Added a gyroregisters command to the CLI that is used to read the current register settings from the gyro and dump them to the CLI.  This is used to verify the configuration in comparison to the datasheets for the various gyros.  Testing empirically by looking at the relative noise from the gyros can give a rough estimate whether the different options are selecting correctly, but it's not very precise.  The code for the gyroregisters CLI command is wrapped inside #ifdef USE_GYRO_REGISTER_DUMP blocks to allow easy disabling.  It's currently enabled for all targets but we may decide to disable before release or only limit to targets with more available space (>=F4).
2018-03-22 14:02:30 +13:00
Andrey Mironov 8319c6c6d1 Unified enabling of EXTI for gyro on F7 with other targets (#5364) 2018-03-04 11:49:36 +13:00
Dominic Clifton cde9a9517b SPRacingF7DUAL - Dual SIMULTANEOUS gyro support. (#5264)
* CF/BF - Set STM32F7 SPI FAST clock to 13.5Mhz - Gyros not stable at
27mhz.

* CF/BF - Initial SPRacingF7DUAL commit.

Support two simultaneous gyro support (code by Dominic Clifton and Martin Budden)
There are new debug modes so you can see the difference between each gyro.

Notes:
* spi bus instance caching broke spi mpu detection because the detection
tries I2C first which overwrites the selected bus instance when using
dual gyro.
* ALL other dual-gyro boards have one sensor per bus.  SPRacingF7DUAL is has two per bus and thus commit has a lot of changes to fix SPI/BUS/GYRO initialisation issues.

* CF/BF - Add SPRacingF4EVODG target.

This target adds a second gyro to the board using the SPI pads on the back of the board.

* CF/BF - Temporarily disable Gyro EXTI pin to allow NEO target to build.
2018-03-04 11:29:31 +13:00
Bruce Luckcuck e9686c0ef8 Change gyro_overflow_detect to use filtered instead of raw gyro data - 2nd try
Noise and momentary spikes in gyro data (particularly in 32K mode) were causing false triggering of the gyro overflow detection. Changing to using filtered instead of raw gyro data eliminates the false triggering.

Also adjusted the trigger and reset rates slightly (approx. 1950dps and 1850dps respectively).

Fixed bug for gyros that may not be +-2000dps full scale. The trigger threshold will be 97.5% full-scale and reset at 92.5% (corresponds to 1950 and 1850 for 2000dps gyros).
2018-02-22 18:56:15 -05:00
Michael Keller e5364ee764
Revert "Change gyro_overflow_detect to use filtered instead of raw gyro data" 2018-02-18 09:25:04 +13:00
Bruce Luckcuck b49a9ec928 Change gyro_overflow_detect to use filtered instead of raw gyro data
Noise and momentary spikes in gyro data (particularly in 32K mode) were causing false triggering of the gyro overflow detection.

Also adjusted the trigger and reset rates slightly (approx. 1950dps and 1850dps respectively).

Optimized the gyro loop slightly by eliminating 3 floating point multiplies.

Fixed bug for gyros that may not be +-2000dps full scale.
2018-02-14 17:50:09 -05:00
Martin Budden b26ff88fd9 Added gyro overflow checking and handling. Helps avoid YSTTM 2017-12-18 05:21:02 +00:00
Martin Budden 03d1409fd2 Remove accgyro mpu read, write and ISR update function pointers 2017-11-13 07:18:59 +00:00
Brian Balogh afec0258c7 Add support for ICM-20649 acc/gyro 2017-09-01 10:57:54 -04:00
Martin Budden eadaff0bdd Code tidy and comments update 2017-08-30 08:50:40 +01:00
Martin Budden e5e121e5f5 Move whoami constants to accgyro_mpu.h 2017-07-20 20:30:48 +01:00
Martin Budden 2fd20c2bd9 Rename SPI functions. Rationalise parameter order. 2017-07-20 16:22:59 +01:00
jflyper 363cfae650 Merge branch 'master' into bfdev-configurable-baro 2017-07-19 18:18:23 +09:00
Martin Budden 3385884e56 Removed mpuConfiguration.gyroReadXRegister 2017-07-18 12:53:10 +01:00
jflyper 8c5c6c1385 Converted BMI160 & ICM20289 2017-07-16 13:31:37 +09:00
jflyper 93089e754c Converted MPU6500 (SPI) and MPU9250 2017-07-16 02:32:34 +09:00
jflyper 52d447d2ef Configurable baro (BMP280)
- busDevice_t is now has a discriminator.
- busDevice_t is added to baroDev_t.
- BMP280 I2C and SPI drivers are consolidated.
2017-07-15 18:05:56 +09:00
Martin Budden 43d8657e5e Merge pull request #3502 from martinbudden/bf_gyro_spi_detect2
Gyro SPI detect function now returns gyro type
2017-07-12 15:54:13 +01:00
Martin Budden 693ca4f0c4 Merge pull request #3492 from martinbudden/bf_gyro_status_fnptr
Removed unnecessary gyro interrupt status function pointer
2017-07-12 15:15:07 +01:00
Martin Budden 78b1c1ab1a Gyro SPI detect function now returns gyro type 2017-07-12 15:12:17 +01:00
Martin Budden 1ee5929b82 Improved gyro SPI detect function 2017-07-12 14:40:08 +01:00
Martin Budden 1448b5040a Removed unnecessary gyro interrupt status function pointer 2017-07-11 07:08:20 +01:00
Martin Budden b78a8bd519 Improved efficiency of gyro read for SPI 2017-07-02 05:32:18 +01:00
Martin Budden 6edb4b9c19 Cache SPI handle to improve performance 2017-07-01 20:39:38 +01:00
Martin Budden 6157ebe5e3 Removed abbreviations from SPI read/write functions 2017-07-01 16:28:10 +01:00
Martin Budden 1c7777ecc5 BMI160 conversion 2017-07-01 16:28:10 +01:00
Martin Budden 5d7f8cdd6e Function renaming as per ledvinap's suggestion 2017-07-01 16:28:10 +01:00
Martin Budden bdd876b6aa MPU9250 conversion 2017-07-01 16:28:10 +01:00
Martin Budden aedfa1e49c ICM20689 conversion 2017-07-01 16:28:09 +01:00
Martin Budden 79589f99ca MPU6500 conversion 2017-07-01 16:28:09 +01:00
Martin Budden 8df87bc47d Make SPI read and write register generic 2017-07-01 16:28:09 +01:00
Martin Budden 23f158913e Moved time functions out of system.h into time.h 2017-05-26 14:03:28 +01:00
Martin Budden 83d49e503e Add Fn suffix to accgyro function pointers 2017-05-07 23:21:44 +01:00
Sean Kelly af06c36515 Adding target for Crazyflie 2.0 Nanocopter
- STM32F4-based flight controller
- Onboard NRF51 soc with custom serial rx protocol (Bluetooth or ESB)

Two general changes to Betaflight included:
- Ability to customize MPU I2C address in target.h (MPU6500's address
  can be changed in hardware by holding a pin high)
- Addition of a target-specific serial type. Targets must opt-in in
  their target.h and provide an implementation of the interface to
convert their custom Rx protocol into RawRC for Betaflight
2017-04-12 15:17:32 -07:00
Martin Budden 2493c214b0 Created subdirectories in drivers directory 2017-04-12 08:06:22 +01:00