diff --git a/make/source.mk b/make/source.mk index cefd2a1700..eca14161c8 100644 --- a/make/source.mk +++ b/make/source.mk @@ -75,7 +75,7 @@ FC_SRC = \ fc/fc_init.c \ fc/controlrate_profile.c \ drivers/camera_control.c \ - drivers/gyro_sync.c \ + drivers/accgyro/gyro_sync.c \ drivers/rx_nrf24l01.c \ drivers/rx_spi.c \ drivers/rx_xn297.c \ diff --git a/src/main/drivers/accgyro/accgyro_mpu6050.c b/src/main/drivers/accgyro/accgyro_mpu6050.c index 1bdeb73c44..6f235fe01d 100644 --- a/src/main/drivers/accgyro/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro/accgyro_mpu6050.c @@ -28,7 +28,6 @@ #include "drivers/bus_i2c.h" #include "drivers/exti.h" -#include "drivers/gyro_sync.h" #include "drivers/nvic.h" #include "drivers/sensor.h" #include "drivers/time.h" @@ -80,7 +79,7 @@ static void mpu6050GyroInit(gyroDev_t *gyro) busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1 delay(100); busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference) - busWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) + busWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops); //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 busWriteRegister(&gyro->bus, MPU_RA_CONFIG, gyro->lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz) busWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec diff --git a/src/main/drivers/accgyro/accgyro_mpu6500.c b/src/main/drivers/accgyro/accgyro_mpu6500.c index f211c98b50..453ea1dc1e 100644 --- a/src/main/drivers/accgyro/accgyro_mpu6500.c +++ b/src/main/drivers/accgyro/accgyro_mpu6500.c @@ -25,7 +25,6 @@ #include "common/maths.h" #include "drivers/exti.h" -#include "drivers/gyro_sync.h" #include "drivers/sensor.h" #include "drivers/time.h" @@ -69,7 +68,7 @@ void mpu6500GyroInit(gyroDev_t *gyro) delay(15); busWriteRegister(&gyro->bus, MPU_RA_CONFIG, gyro->lpf); delay(15); - busWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops + busWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops); // Get Divider Drops delay(100); // Data ready interrupt configuration diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20649.c b/src/main/drivers/accgyro/accgyro_spi_icm20649.c index c5e9ac65bc..61754d8196 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm20649.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm20649.c @@ -24,16 +24,15 @@ #include "common/axis.h" #include "common/maths.h" +#include "drivers/accgyro/accgyro.h" +#include "drivers/accgyro/accgyro_mpu.h" +#include "drivers/accgyro/accgyro_spi_icm20649.h" #include "drivers/bus_spi.h" #include "drivers/exti.h" -#include "drivers/gyro_sync.h" #include "drivers/io.h" #include "drivers/sensor.h" #include "drivers/time.h" -#include "accgyro.h" -#include "accgyro_mpu.h" -#include "accgyro_spi_icm20649.h" static void icm20649SpiInit(const busDevice_t *bus) { @@ -136,7 +135,7 @@ void icm20649GyroInit(gyroDev_t *gyro) raGyroConfigData |= gyro_fsr << 1 | gyro->lpf << 3; spiBusWriteRegister(&gyro->bus, ICM20649_RA_GYRO_CONFIG_1, raGyroConfigData); delay(15); - spiBusWriteRegister(&gyro->bus, ICM20649_RA_GYRO_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops + spiBusWriteRegister(&gyro->bus, ICM20649_RA_GYRO_SMPLRT_DIV, gyro->mpuDividerDrops); // Get Divider Drops delay(100); // Data ready interrupt configuration diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20689.c b/src/main/drivers/accgyro/accgyro_spi_icm20689.c index 1fe8f2fd5a..a76c0a0e27 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm20689.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm20689.c @@ -24,16 +24,15 @@ #include "common/axis.h" #include "common/maths.h" +#include "drivers/accgyro/accgyro.h" +#include "drivers/accgyro/accgyro_mpu.h" +#include "drivers/accgyro/accgyro_spi_icm20689.h" #include "drivers/bus_spi.h" #include "drivers/exti.h" -#include "drivers/gyro_sync.h" #include "drivers/io.h" #include "drivers/sensor.h" #include "drivers/time.h" -#include "accgyro.h" -#include "accgyro_mpu.h" -#include "accgyro_spi_icm20689.h" static void icm20689SpiInit(const busDevice_t *bus) { @@ -134,7 +133,7 @@ void icm20689GyroInit(gyroDev_t *gyro) delay(15); spiBusWriteRegister(&gyro->bus, MPU_RA_CONFIG, gyro->lpf); delay(15); - spiBusWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops + spiBusWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops); // Get Divider Drops delay(100); // Data ready interrupt configuration diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro/accgyro_spi_mpu6000.c index 9ccac7e483..c32d228c4d 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6000.c @@ -27,23 +27,21 @@ #include "platform.h" +#if defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_SPI_MPU6000) + #include "common/axis.h" #include "common/maths.h" +#include "drivers/accgyro/accgyro.h" +#include "drivers/accgyro/accgyro_mpu.h" +#include "drivers/accgyro/accgyro_spi_mpu6000.h" #include "drivers/bus_spi.h" #include "drivers/exti.h" -#include "drivers/gyro_sync.h" #include "drivers/io.h" #include "drivers/time.h" #include "drivers/sensor.h" #include "drivers/system.h" -#include "accgyro.h" -#include "accgyro_mpu.h" - -#if defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_SPI_MPU6000) - -#include "accgyro_spi_mpu6000.h" static void mpu6000AccAndGyroInit(gyroDev_t *gyro); @@ -200,7 +198,7 @@ static void mpu6000AccAndGyroInit(gyroDev_t *gyro) // Accel Sample Rate 1kHz // Gyroscope Output Rate = 1kHz when the DLPF is enabled - spiBusWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); + spiBusWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops); delayMicroseconds(15); // Gyro +/- 1000 DPS Full Scale diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu9250.c b/src/main/drivers/accgyro/accgyro_spi_mpu9250.c index ba846b1385..d5551f7ec3 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu9250.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu9250.c @@ -38,7 +38,6 @@ #include "drivers/accgyro/accgyro_spi_mpu9250.h" #include "drivers/bus_spi.h" #include "drivers/exti.h" -#include "drivers/gyro_sync.h" #include "drivers/io.h" #include "drivers/light_led.h" #include "drivers/sensor.h" @@ -155,7 +154,7 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro) { mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_CONFIG, 0); //8KHz, 250DLPF } - mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); + mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops); mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN diff --git a/src/main/drivers/gyro_sync.c b/src/main/drivers/accgyro/gyro_sync.c similarity index 93% rename from src/main/drivers/gyro_sync.c rename to src/main/drivers/accgyro/gyro_sync.c index e32754cec8..292120e174 100644 --- a/src/main/drivers/gyro_sync.c +++ b/src/main/drivers/accgyro/gyro_sync.c @@ -12,7 +12,7 @@ #include "drivers/sensor.h" #include "drivers/accgyro/accgyro.h" -#include "drivers/gyro_sync.h" +#include "drivers/accgyro/gyro_sync.h" bool gyroSyncCheckUpdate(gyroDev_t *gyro) @@ -70,8 +70,3 @@ uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenomin const uint32_t targetLooptime = (uint32_t)(gyroSyncDenominator * gyroSamplePeriod); return targetLooptime; } - -uint8_t gyroMPU6xxxGetDividerDrops(const gyroDev_t *gyro) -{ - return gyro->mpuDividerDrops; -} diff --git a/src/main/drivers/gyro_sync.h b/src/main/drivers/accgyro/gyro_sync.h similarity index 82% rename from src/main/drivers/gyro_sync.h rename to src/main/drivers/accgyro/gyro_sync.h index 335e1c279c..33633af0e1 100644 --- a/src/main/drivers/gyro_sync.h +++ b/src/main/drivers/accgyro/gyro_sync.h @@ -10,5 +10,4 @@ #include "drivers/accgyro/accgyro.h" bool gyroSyncCheckUpdate(gyroDev_t *gyro); -uint8_t gyroMPU6xxxGetDividerDrops(const gyroDev_t *gyro); uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenominator, bool gyro_use_32khz); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 48eeb52530..f731745e29 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -34,7 +34,6 @@ #include "config/parameter_group.h" #include "config/parameter_group_ids.h" -#include "drivers/gyro_sync.h" #include "drivers/light_led.h" #include "drivers/system.h" #include "drivers/time.h" diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 59d31a7508..2e4447f116 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -50,8 +50,8 @@ #include "drivers/accgyro/accgyro_spi_mpu6000.h" #include "drivers/accgyro/accgyro_spi_mpu6500.h" #include "drivers/accgyro/accgyro_spi_mpu9250.h" +#include "drivers/accgyro/gyro_sync.h" #include "drivers/bus_spi.h" -#include "drivers/gyro_sync.h" #include "drivers/io.h" #include "fc/runtime_config.h" diff --git a/src/test/Makefile b/src/test/Makefile index ec0243f4e5..79af7e1ade 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -74,7 +74,7 @@ blackbox_unittest_SRC := \ $(USER_DIR)/common/printf.c \ $(USER_DIR)/common/maths.c \ $(USER_DIR)/common/typeconversion.c \ - $(USER_DIR)/drivers/gyro_sync.c + $(USER_DIR)/drivers/accgyro/gyro_sync.c blackbox_encoding_unittest_SRC := \ $(USER_DIR)/blackbox/blackbox_encoding.c \ diff --git a/src/test/unit/blackbox_unittest.cc b/src/test/unit/blackbox_unittest.cc index a85c52eae2..c35fcbb231 100644 --- a/src/test/unit/blackbox_unittest.cc +++ b/src/test/unit/blackbox_unittest.cc @@ -27,7 +27,7 @@ extern "C" { #include "config/parameter_group_ids.h" #include "drivers/accgyro/accgyro.h" - #include "drivers/gyro_sync.h" + #include "drivers/accgyro/gyro_sync.h" #include "drivers/serial.h" #include "flight/failsafe.h"