From 590e569375f8e3099b8b6cb1d5121a683a9c128d Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 19 Nov 2016 14:11:03 +0000 Subject: [PATCH] Changed gyro init and read functions to take a gyro_t* parameter. Scaled gyro values to degrees per second in gyroUpdate. --- src/main/blackbox/blackbox.c | 3 +- src/main/common/filter.h | 2 + src/main/drivers/accgyro.h | 9 +++- src/main/drivers/accgyro_l3g4200d.c | 16 +++---- src/main/drivers/accgyro_l3gd20.c | 12 ++--- src/main/drivers/accgyro_mpu.c | 60 ++++++++++++++++--------- src/main/drivers/accgyro_mpu.h | 7 +-- src/main/drivers/accgyro_mpu3050.c | 6 +-- src/main/drivers/accgyro_mpu6050.c | 10 ++--- src/main/drivers/accgyro_mpu6500.c | 8 ++-- src/main/drivers/accgyro_mpu6500.h | 2 +- src/main/drivers/accgyro_spi_icm20689.c | 8 ++-- src/main/drivers/accgyro_spi_icm20689.h | 4 +- src/main/drivers/accgyro_spi_mpu6000.c | 13 +++--- src/main/drivers/accgyro_spi_mpu6500.c | 4 +- src/main/drivers/accgyro_spi_mpu6500.h | 2 +- src/main/drivers/accgyro_spi_mpu9250.c | 15 +++---- src/main/drivers/gyro_sync.c | 4 +- src/main/drivers/gyro_sync.h | 2 +- src/main/drivers/sensor.h | 7 ++- src/main/fc/fc_msp.c | 2 +- src/main/flight/imu.c | 5 +-- src/main/flight/pid.c | 2 +- src/main/io/dashboard.c | 3 +- src/main/sensors/gyro.c | 36 +++++++-------- src/main/sensors/gyro.h | 1 - src/main/sensors/initialisation.c | 15 ++++--- src/main/target/COLIBRI_RACE/i2c_bst.c | 2 +- 28 files changed, 136 insertions(+), 124 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 1f6f04e657..626ab8ef4b 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -18,6 +18,7 @@ #include #include #include +#include #include "platform.h" @@ -990,7 +991,7 @@ static void loadMainState(uint32_t currentTime) } for (i = 0; i < XYZ_AXIS_COUNT; i++) { - blackboxCurrent->gyroADC[i] = gyroADC[i]; + blackboxCurrent->gyroADC[i] = lrintf(gyroADCf[i]); } for (i = 0; i < XYZ_AXIS_COUNT; i++) { diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 0121c71b1d..35b5ca8f15 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -15,6 +15,8 @@ * along with Cleanflight. If not, see . */ +#pragma once + #ifdef STM32F10X #define MAX_FIR_DENOISE_WINDOW_SIZE 60 #else diff --git a/src/main/drivers/accgyro.h b/src/main/drivers/accgyro.h index 7e43b73078..f6f3a7718c 100644 --- a/src/main/drivers/accgyro.h +++ b/src/main/drivers/accgyro.h @@ -17,6 +17,8 @@ #pragma once +#include "common/axis.h" +#include "common/filter.h" #include "drivers/sensor.h" #ifndef MPU_I2C_INSTANCE @@ -34,11 +36,14 @@ typedef struct gyro_s { sensorGyroInitFuncPtr init; // initialize function - sensorReadFuncPtr read; // read 3 axis data function + sensorGyroReadFuncPtr read; // read 3 axis data function sensorReadFuncPtr temperature; // read temperature if available - sensorInterruptFuncPtr intStatus; + sensorGyroInterruptStatusFuncPtr intStatus; + int16_t gyroADCRaw[XYZ_AXIS_COUNT]; float scale; // scalefactor uint32_t targetLooptime; + uint16_t lpf; + volatile bool dataReady; } gyro_t; typedef struct acc_s { diff --git a/src/main/drivers/accgyro_l3g4200d.c b/src/main/drivers/accgyro_l3g4200d.c index 33bd2f1f80..26e58f44bd 100644 --- a/src/main/drivers/accgyro_l3g4200d.c +++ b/src/main/drivers/accgyro_l3g4200d.c @@ -54,8 +54,8 @@ #define L3G4200D_DLPF_78HZ 0x80 #define L3G4200D_DLPF_93HZ 0xC0 -static void l3g4200dInit(uint8_t lpf); -static bool l3g4200dRead(int16_t *gyroADC); +static void l3g4200dInit(gyro_t *gyro); +static bool l3g4200dRead(gyro_t *gyro); bool l3g4200dDetect(gyro_t *gyro) { @@ -76,13 +76,13 @@ bool l3g4200dDetect(gyro_t *gyro) return true; } -static void l3g4200dInit(uint8_t lpf) +static void l3g4200dInit(gyro_t *gyro) { bool ack; uint8_t mpuLowPassFilter = L3G4200D_DLPF_32HZ; - switch (lpf) { + switch (gyro->lpf) { default: case 32: mpuLowPassFilter = L3G4200D_DLPF_32HZ; @@ -109,7 +109,7 @@ static void l3g4200dInit(uint8_t lpf) } // Read 3 gyro values into user-provided buffer. No overrun checking is done. -static bool l3g4200dRead(int16_t *gyroADC) +static bool l3g4200dRead(gyro_t *gyro) { uint8_t buf[6]; @@ -117,9 +117,9 @@ static bool l3g4200dRead(int16_t *gyroADC) return false; } - gyroADC[X] = (int16_t)((buf[0] << 8) | buf[1]); - gyroADC[Y] = (int16_t)((buf[2] << 8) | buf[3]); - gyroADC[Z] = (int16_t)((buf[4] << 8) | buf[5]); + gyro->gyroADCRaw[X] = (int16_t)((buf[0] << 8) | buf[1]); + gyro->gyroADCRaw[Y] = (int16_t)((buf[2] << 8) | buf[3]); + gyro->gyroADCRaw[Z] = (int16_t)((buf[4] << 8) | buf[5]); return true; } diff --git a/src/main/drivers/accgyro_l3gd20.c b/src/main/drivers/accgyro_l3gd20.c index 3cb7bc1704..7432ead75d 100644 --- a/src/main/drivers/accgyro_l3gd20.c +++ b/src/main/drivers/accgyro_l3gd20.c @@ -84,9 +84,9 @@ static void l3gd20SpiInit(SPI_TypeDef *SPIx) spiSetDivisor(L3GD20_SPI, SPI_CLOCK_STANDARD); } -void l3gd20GyroInit(uint8_t lpf) +void l3gd20GyroInit(gyro_t *gyro) { - UNUSED(lpf); // FIXME use it! + UNUSED(gyro); // FIXME use it! l3gd20SpiInit(L3GD20_SPI); @@ -120,7 +120,7 @@ void l3gd20GyroInit(uint8_t lpf) delay(100); } -static bool l3gd20GyroRead(int16_t *gyroADC) +static bool l3gd20GyroRead(gyro_t *gyro) { uint8_t buf[6]; @@ -134,9 +134,9 @@ static bool l3gd20GyroRead(int16_t *gyroADC) DISABLE_L3GD20; - gyroADC[0] = (int16_t)((buf[0] << 8) | buf[1]); - gyroADC[1] = (int16_t)((buf[2] << 8) | buf[3]); - gyroADC[2] = (int16_t)((buf[4] << 8) | buf[5]); + gyro->gyroADCRaw[0] = (int16_t)((buf[0] << 8) | buf[1]); + gyro->gyroADCRaw[1] = (int16_t)((buf[2] << 8) | buf[3]); + gyro->gyroADCRaw[2] = (int16_t)((buf[4] << 8) | buf[5]); #if 0 debug[0] = (int16_t)((buf[1] << 8) | buf[0]); diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c index b577017bdc..88ceca8b78 100644 --- a/src/main/drivers/accgyro_mpu.c +++ b/src/main/drivers/accgyro_mpu.c @@ -25,7 +25,9 @@ #include "build/build_config.h" #include "build/debug.h" +#include "common/filter.h" #include "common/maths.h" +#include "common/utils.h" #include "nvic.h" @@ -52,8 +54,6 @@ static bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data); static void mpu6050FindRevision(void); -static volatile bool mpuDataReady; - #ifdef USE_SPI static bool detectSPISensorsAndUpdateDetectionResult(void); #endif @@ -221,12 +221,22 @@ static void mpu6050FindRevision(void) } } -extiCallbackRec_t mpuIntCallbackRec; +typedef struct mpuIntRec_s { + extiCallbackRec_t exti; + gyro_t *gyro; +} mpuIntRec_t; -void mpuIntExtiHandler(extiCallbackRec_t *cb) +mpuIntRec_t mpuIntRec; + +/* + * Gyro interrupt service routine + */ +#if defined(MPU_INT_EXTI) +static void mpuIntExtiHandler(extiCallbackRec_t *cb) { - UNUSED(cb); - mpuDataReady = true; + mpuIntRec_t *rec = container_of(cb, mpuIntRec_t, exti); + gyro_t *gyro = rec->gyro; + gyro->dataReady = true; #ifdef DEBUG_MPU_DATA_READY_INTERRUPT static uint32_t lastCalledAt = 0; @@ -236,17 +246,18 @@ void mpuIntExtiHandler(extiCallbackRec_t *cb) lastCalledAt = now; #endif } +#endif -void mpuIntExtiInit(void) +static void mpuIntExtiInit(gyro_t *gyro) { + mpuIntRec.gyro = gyro; +#if defined(MPU_INT_EXTI) static bool mpuExtiInitDone = false; if (mpuExtiInitDone || !mpuIntExtiConfig) { return; } -#if defined(USE_MPU_DATA_READY_SIGNAL) && defined(USE_EXTI) - IO_t mpuIntIO = IOGetByTag(mpuIntExtiConfig->tag); #ifdef ENSURE_MPU_DATA_READY_IS_LOW @@ -258,20 +269,20 @@ void mpuIntExtiInit(void) #if defined (STM32F7) IOInit(mpuIntIO, OWNER_MPU_EXTI, 0); - EXTIHandlerInit(&mpuIntCallbackRec, mpuIntExtiHandler); - EXTIConfig(mpuIntIO, &mpuIntCallbackRec, NVIC_PRIO_MPU_INT_EXTI, IO_CONFIG(GPIO_MODE_INPUT,0,GPIO_NOPULL)); // TODO - maybe pullup / pulldown ? + EXTIHandlerInit(&mpuIntRec.exti, mpuIntExtiHandler); + EXTIConfig(mpuIntIO, &mpuIntRec.exti, NVIC_PRIO_MPU_INT_EXTI, IO_CONFIG(GPIO_MODE_INPUT,0,GPIO_NOPULL)); // TODO - maybe pullup / pulldown ? #else IOInit(mpuIntIO, OWNER_MPU_EXTI, 0); IOConfigGPIO(mpuIntIO, IOCFG_IN_FLOATING); // TODO - maybe pullup / pulldown ? - EXTIHandlerInit(&mpuIntCallbackRec, mpuIntExtiHandler); - EXTIConfig(mpuIntIO, &mpuIntCallbackRec, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising); + EXTIHandlerInit(&mpuIntRec.exti, mpuIntExtiHandler); + EXTIConfig(mpuIntIO, &mpuIntRec.exti, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising); EXTIEnable(mpuIntIO, true); -#endif #endif mpuExtiInitDone = true; +#endif } static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data) @@ -302,28 +313,33 @@ bool mpuAccRead(int16_t *accData) return true; } -bool mpuGyroRead(int16_t *gyroADC) +bool mpuGyroRead(gyro_t *gyro) { uint8_t data[6]; - bool ack = mpuConfiguration.read(mpuConfiguration.gyroReadXRegister, 6, data); + const bool ack = mpuConfiguration.read(mpuConfiguration.gyroReadXRegister, 6, data); if (!ack) { return false; } - gyroADC[0] = (int16_t)((data[0] << 8) | data[1]); - gyroADC[1] = (int16_t)((data[2] << 8) | data[3]); - gyroADC[2] = (int16_t)((data[4] << 8) | data[5]); + gyro->gyroADCRaw[X] = (int16_t)((data[0] << 8) | data[1]); + gyro->gyroADCRaw[Y] = (int16_t)((data[2] << 8) | data[3]); + gyro->gyroADCRaw[Z] = (int16_t)((data[4] << 8) | data[5]); return true; } -bool checkMPUDataReady(void) +void mpuGyroInit(gyro_t *gyro) +{ + mpuIntExtiInit(gyro); +} + +bool checkMPUDataReady(gyro_t* gyro) { bool ret; - if (mpuDataReady) { + if (gyro->dataReady) { ret = true; - mpuDataReady= false; + gyro->dataReady= false; } else { ret = false; } diff --git a/src/main/drivers/accgyro_mpu.h b/src/main/drivers/accgyro_mpu.h index 465aa2ee23..125fbfb278 100644 --- a/src/main/drivers/accgyro_mpu.h +++ b/src/main/drivers/accgyro_mpu.h @@ -185,8 +185,9 @@ typedef struct mpuDetectionResult_s { extern mpuDetectionResult_t mpuDetectionResult; void configureMPUDataReadyInterruptHandling(void); -void mpuIntExtiInit(void); +struct gyro_s; +void mpuGyroInit(struct gyro_s *gyro); bool mpuAccRead(int16_t *accData); -bool mpuGyroRead(int16_t *gyroADC); +bool mpuGyroRead(struct gyro_s *gyro); mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse); -bool checkMPUDataReady(void); +bool checkMPUDataReady(struct gyro_s *gyro); diff --git a/src/main/drivers/accgyro_mpu3050.c b/src/main/drivers/accgyro_mpu3050.c index cefaf94b27..661b576d83 100644 --- a/src/main/drivers/accgyro_mpu3050.c +++ b/src/main/drivers/accgyro_mpu3050.c @@ -46,7 +46,7 @@ #define MPU3050_USER_RESET 0x01 #define MPU3050_CLK_SEL_PLL_GX 0x01 -static void mpu3050Init(uint8_t lpf); +static void mpu3050Init(gyro_t *gyro); static bool mpu3050ReadTemp(int16_t *tempData); bool mpu3050Detect(gyro_t *gyro) @@ -65,7 +65,7 @@ bool mpu3050Detect(gyro_t *gyro) return true; } -static void mpu3050Init(uint8_t lpf) +static void mpu3050Init(gyro_t *gyro) { bool ack; @@ -75,7 +75,7 @@ static void mpu3050Init(uint8_t lpf) if (!ack) failureMode(FAILURE_ACC_INIT); - mpuConfiguration.write(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | lpf); + mpuConfiguration.write(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->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 7b24b82a56..4cfa8e375e 100644 --- a/src/main/drivers/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro_mpu6050.c @@ -51,7 +51,7 @@ #define MPU6050_SMPLRT_DIV 0 // 8000Hz static void mpu6050AccInit(acc_t *acc); -static void mpu6050GyroInit(uint8_t lpf); +static void mpu6050GyroInit(gyro_t *gyro); bool mpu6050AccDetect(acc_t *acc) { @@ -83,8 +83,6 @@ bool mpu6050GyroDetect(gyro_t *gyro) static void mpu6050AccInit(acc_t *acc) { - mpuIntExtiInit(); - switch (mpuDetectionResult.resolution) { case MPU_HALF_RESOLUTION: acc->acc_1G = 256 * 4; @@ -95,16 +93,16 @@ static void mpu6050AccInit(acc_t *acc) } } -static void mpu6050GyroInit(uint8_t lpf) +static void mpu6050GyroInit(gyro_t *gyro) { - mpuIntExtiInit(); + mpuGyroInit(gyro); mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1 delay(100); 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) 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 - 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) + mpuConfiguration.write(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) 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 c37b7d1b58..ade07baf7b 100644 --- a/src/main/drivers/accgyro_mpu6500.c +++ b/src/main/drivers/accgyro_mpu6500.c @@ -64,14 +64,12 @@ bool mpu6500GyroDetect(gyro_t *gyro) void mpu6500AccInit(acc_t *acc) { - mpuIntExtiInit(); - acc->acc_1G = 512 * 4; } -void mpu6500GyroInit(uint8_t lpf) +void mpu6500GyroInit(gyro_t *gyro) { - mpuIntExtiInit(); + mpuGyroInit(gyro); mpuConfiguration.write(MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET); delay(100); @@ -85,7 +83,7 @@ void mpu6500GyroInit(uint8_t lpf) delay(15); mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); delay(15); - mpuConfiguration.write(MPU_RA_CONFIG, lpf); + mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); delay(15); mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops delay(100); diff --git a/src/main/drivers/accgyro_mpu6500.h b/src/main/drivers/accgyro_mpu6500.h index f761c835d3..06f6e13c65 100644 --- a/src/main/drivers/accgyro_mpu6500.h +++ b/src/main/drivers/accgyro_mpu6500.h @@ -32,4 +32,4 @@ bool mpu6500AccDetect(acc_t *acc); bool mpu6500GyroDetect(gyro_t *gyro); void mpu6500AccInit(acc_t *acc); -void mpu6500GyroInit(uint8_t lpf); +void mpu6500GyroInit(gyro_t *gyro); diff --git a/src/main/drivers/accgyro_spi_icm20689.c b/src/main/drivers/accgyro_spi_icm20689.c index 650219f89a..a73c7204d8 100644 --- a/src/main/drivers/accgyro_spi_icm20689.c +++ b/src/main/drivers/accgyro_spi_icm20689.c @@ -137,14 +137,12 @@ bool icm20689SpiGyroDetect(gyro_t *gyro) void icm20689AccInit(acc_t *acc) { - mpuIntExtiInit(); - acc->acc_1G = 512 * 4; } -void icm20689GyroInit(uint8_t lpf) +void icm20689GyroInit(gyro_t *gyro) { - mpuIntExtiInit(); + mpuGyroInit(gyro); spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); @@ -160,7 +158,7 @@ void icm20689GyroInit(uint8_t lpf) delay(15); mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); delay(15); - mpuConfiguration.write(MPU_RA_CONFIG, lpf); + mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); delay(15); mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops delay(100); diff --git a/src/main/drivers/accgyro_spi_icm20689.h b/src/main/drivers/accgyro_spi_icm20689.h index a87114cb7d..60f9bc945a 100644 --- a/src/main/drivers/accgyro_spi_icm20689.h +++ b/src/main/drivers/accgyro_spi_icm20689.h @@ -23,7 +23,7 @@ bool icm20689AccDetect(acc_t *acc); bool icm20689GyroDetect(gyro_t *gyro); void icm20689AccInit(acc_t *acc); -void icm20689GyroInit(uint8_t lpf); +void icm20689GyroInit(gyro_t *gyro); bool icm20689SpiDetect(void); @@ -31,4 +31,4 @@ bool icm20689SpiAccDetect(acc_t *acc); bool icm20689SpiGyroDetect(gyro_t *gyro); bool icm20689WriteRegister(uint8_t reg, uint8_t data); -bool icm20689ReadRegister(uint8_t reg, uint8_t length, uint8_t *data); \ No newline at end of file +bool icm20689ReadRegister(uint8_t reg, uint8_t length, uint8_t *data); diff --git a/src/main/drivers/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro_spi_mpu6000.c index d25ad8d269..7bf631928c 100644 --- a/src/main/drivers/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro_spi_mpu6000.c @@ -124,32 +124,29 @@ bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data) return true; } -void mpu6000SpiGyroInit(uint8_t lpf) +void mpu6000SpiGyroInit(gyro_t *gyro) { - mpuIntExtiInit(); + mpuGyroInit(gyro); mpu6000AccAndGyroInit(); spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); // Accel and Gyro DLPF Setting - mpu6000WriteRegister(MPU6000_CONFIG, lpf); + mpu6000WriteRegister(MPU6000_CONFIG, gyro->lpf); delayMicroseconds(1); spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST); // 18 MHz SPI clock - int16_t data[3]; - mpuGyroRead(data); + mpuGyroRead(gyro); - if (((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) { + if (((int8_t)gyro->gyroADCRaw[1]) == -1 && ((int8_t)gyro->gyroADCRaw[0]) == -1) { failureMode(FAILURE_GYRO_INIT_FAILED); } } void mpu6000SpiAccInit(acc_t *acc) { - mpuIntExtiInit(); - acc->acc_1G = 512 * 4; } diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c index a38e628097..fb1f52229a 100755 --- a/src/main/drivers/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro_spi_mpu6500.c @@ -99,12 +99,12 @@ void mpu6500SpiAccInit(acc_t *acc) mpu6500AccInit(acc); } -void mpu6500SpiGyroInit(uint8_t lpf) +void mpu6500SpiGyroInit(gyro_t *gyro) { spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_SLOW); delayMicroseconds(1); - mpu6500GyroInit(lpf); + mpu6500GyroInit(gyro); // Disable Primary I2C Interface mpu6500WriteRegister(MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS); diff --git a/src/main/drivers/accgyro_spi_mpu6500.h b/src/main/drivers/accgyro_spi_mpu6500.h index 3cd4ba89c8..969b913bcc 100755 --- a/src/main/drivers/accgyro_spi_mpu6500.h +++ b/src/main/drivers/accgyro_spi_mpu6500.h @@ -20,7 +20,7 @@ bool mpu6500SpiDetect(void); void mpu6500SpiAccInit(acc_t *acc); -void mpu6500SpiGyroInit(uint8_t lpf); +void mpu6500SpiGyroInit(gyro_t *gyro); bool mpu6500SpiAccDetect(acc_t *acc); bool mpu6500SpiGyroDetect(gyro_t *gyro); diff --git a/src/main/drivers/accgyro_spi_mpu9250.c b/src/main/drivers/accgyro_spi_mpu9250.c index 4ef2fc72d1..0fd502f644 100644 --- a/src/main/drivers/accgyro_spi_mpu9250.c +++ b/src/main/drivers/accgyro_spi_mpu9250.c @@ -96,22 +96,19 @@ bool mpu9250SlowReadRegister(uint8_t reg, uint8_t length, uint8_t *data) return true; } -void mpu9250SpiGyroInit(uint8_t lpf) +void mpu9250SpiGyroInit(gyro_t *gyro) { - (void)(lpf); + mpuGyroInit(gyro); - mpuIntExtiInit(); - - mpu9250AccAndGyroInit(lpf); + mpu9250AccAndGyroInit(gyro->lpf); spiResetErrorCounter(MPU9250_SPI_INSTANCE); spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST); //high speed now that we don't need to write to the slow registers - int16_t data[3]; - mpuGyroRead(data); + mpuGyroRead(gyro); - if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter(MPU9250_SPI_INSTANCE) != 0) { + if ((((int8_t)gyro->gyroADCRaw[1]) == -1 && ((int8_t)gyro->gyroADCRaw[0]) == -1) || spiGetErrorCounter(MPU9250_SPI_INSTANCE) != 0) { spiResetErrorCounter(MPU9250_SPI_INSTANCE); failureMode(FAILURE_GYRO_INIT_FAILED); } @@ -119,8 +116,6 @@ void mpu9250SpiGyroInit(uint8_t lpf) void mpu9250SpiAccInit(acc_t *acc) { - mpuIntExtiInit(); - acc->acc_1G = 512 * 8; } diff --git a/src/main/drivers/gyro_sync.c b/src/main/drivers/gyro_sync.c index 4e9f02622b..5d60aaffd2 100644 --- a/src/main/drivers/gyro_sync.c +++ b/src/main/drivers/gyro_sync.c @@ -16,11 +16,11 @@ static uint8_t mpuDividerDrops; -bool gyroSyncCheckUpdate(const gyro_t *gyro) +bool gyroSyncCheckUpdate(gyro_t *gyro) { if (!gyro->intStatus) return false; - return gyro->intStatus(); + return gyro->intStatus(gyro); } uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator) diff --git a/src/main/drivers/gyro_sync.h b/src/main/drivers/gyro_sync.h index f682e218ce..ca5111aa1f 100644 --- a/src/main/drivers/gyro_sync.h +++ b/src/main/drivers/gyro_sync.h @@ -6,6 +6,6 @@ */ struct gyro_s; -bool gyroSyncCheckUpdate(const struct gyro_s *gyro); +bool gyroSyncCheckUpdate(struct gyro_s *gyro); uint8_t gyroMPU6xxxGetDividerDrops(void); uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator); diff --git a/src/main/drivers/sensor.h b/src/main/drivers/sensor.h index 7ae17f5104..1cbe7725d3 100644 --- a/src/main/drivers/sensor.h +++ b/src/main/drivers/sensor.h @@ -21,5 +21,8 @@ struct acc_s; typedef void (*sensorInitFuncPtr)(void); // sensor init prototype typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype typedef void (*sensorAccInitFuncPtr)(struct acc_s *acc); // sensor init prototype -typedef void (*sensorGyroInitFuncPtr)(uint8_t lpf); // gyro sensor init prototype -typedef bool (*sensorInterruptFuncPtr)(void); // sensor Interrupt Data Ready +struct gyro_s; +typedef void (*sensorGyroInitFuncPtr)(struct gyro_s *gyro); +typedef bool (*sensorGyroReadFuncPtr)(struct gyro_s *gyro); +typedef bool (*sensorGyroInterruptStatusFuncPtr)(struct gyro_s *gyro); +typedef bool (*sensorInterruptFuncPtr)(void); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index e6f999722c..149977c66d 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -612,7 +612,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU16(dst, accSmooth[i] / scale); } for (int i = 0; i < 3; i++) { - sbufWriteU16(dst, gyroADC[i]); + sbufWriteU16(dst, lrintf(gyroADCf[i] / gyro.scale)); } for (int i = 0; i < 3; i++) { sbufWriteU16(dst, magADC[i]); diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 514e956aef..f733862cfc 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -75,8 +75,6 @@ static float rMat[3][3]; attitudeEulerAngles_t attitude = { { 0, 0, 0 } }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800 -static float gyroScale; - STATIC_UNIT_TESTED void imuComputeRotationMatrix(void) { float q1q1 = sq(q1); @@ -122,7 +120,6 @@ void imuConfigure( void imuInit(void) { smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig.small_angle)); - gyroScale = gyro.scale * (M_PIf / 180.0f); // gyro output scaled to rad per second accVelScale = 9.80665f / acc.acc_1G / 10000.0f; imuComputeRotationMatrix(); @@ -399,7 +396,7 @@ static void imuCalculateEstimatedAttitude(uint32_t currentTime) #endif imuMahonyAHRSupdate(deltaT * 1e-6f, - gyroADC[X] * gyroScale, gyroADC[Y] * gyroScale, gyroADC[Z] * gyroScale, + DEGREES_TO_RADIANS(gyroADCf[X]), DEGREES_TO_RADIANS(gyroADCf[Y]), DEGREES_TO_RADIANS(gyroADCf[Z]), useAcc, accSmooth[X], accSmooth[Y], accSmooth[Z], useMag, magADC[X], magADC[Y], magADC[Z], useYaw, rawYawError); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 20839a2775..ef488cf6cf 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -244,7 +244,7 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio } } - const float PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec + const float PVRate = gyroADCf[axis]; // Process variable from gyro output in deg/sec // --------low-level gyro-based PID based on 2DOF PID controller. ---------- // ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error). ---------- diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index c0cca5a918..151f9c58e8 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -18,6 +18,7 @@ #include #include #include +#include #include "platform.h" @@ -495,7 +496,7 @@ void showSensorsPage(void) } if (sensors(SENSOR_GYRO)) { - tfp_sprintf(lineBuffer, format, "GYR", gyroADC[X], gyroADC[Y], gyroADC[Z]); + tfp_sprintf(lineBuffer, format, "GYR", lrintf(gyroADCf[X]), lrintf(gyroADCf[Y]), lrintf(gyroADCf[Z])); padLineBuffer(); i2c_OLED_set_line(rowIndex++); i2c_OLED_send_string(lineBuffer); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 838a2805f7..6dfb8a4b06 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -39,7 +39,7 @@ gyro_t gyro; // gyro access functions sensor_align_e gyroAlign = 0; -int32_t gyroADC[XYZ_AXIS_COUNT]; +static int32_t gyroADC[XYZ_AXIS_COUNT]; float gyroADCf[XYZ_AXIS_COUNT]; static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 }; @@ -174,41 +174,39 @@ static void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold) void gyroUpdate(void) { - int16_t gyroADCRaw[XYZ_AXIS_COUNT]; - // range: +/- 8192; +/- 2000 deg/sec - if (!gyro.read(gyroADCRaw)) { + if (!gyro.read(&gyro)) { return; } - - gyroADC[X] = gyroADCRaw[X]; - gyroADC[Y] = gyroADCRaw[Y]; - gyroADC[Z] = gyroADCRaw[Z]; + gyro.dataReady = false; + gyroADC[X] = gyro.gyroADCRaw[X]; + gyroADC[Y] = gyro.gyroADCRaw[Y]; + gyroADC[Z] = gyro.gyroADCRaw[Z]; alignSensors(gyroADC, gyroAlign); - if (!isGyroCalibrationComplete()) { + const bool calibrationComplete = isGyroCalibrationComplete(); + if (!calibrationComplete) { performGyroCalibration(gyroConfig->gyroMovementCalibrationThreshold); } - gyroADC[X] -= gyroZero[X]; - gyroADC[Y] -= gyroZero[Y]; - gyroADC[Z] -= gyroZero[Z]; - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + gyroADC[axis] -= gyroZero[axis]; + // scale gyro output to degrees per second + gyroADCf[axis] = (float)gyroADC[axis] * gyro.scale; - if (debugMode == DEBUG_GYRO) - debug[axis] = gyroADC[axis]; + DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf[axis])); - gyroADCf[axis] = softLpfFilterApplyFn(softLpfFilter[axis], (float) gyroADC[axis]); + gyroADCf[axis] = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf[axis]); - if (debugMode == DEBUG_NOTCH) - debug[axis] = lrintf(gyroADCf[axis]); + DEBUG_SET(DEBUG_NOTCH, axis, lrintf(gyroADCf[axis])); gyroADCf[axis] = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf[axis]); gyroADCf[axis] = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf[axis]); - gyroADC[axis] = lrintf(gyroADCf[axis]); + if (!calibrationComplete) { + gyroADC[axis] = lrintf(gyroADCf[axis] / gyro.scale); + } } } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index bea1268e57..df2426e427 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -37,7 +37,6 @@ typedef enum { extern gyro_t gyro; -extern int32_t gyroADC[XYZ_AXIS_COUNT]; extern float gyroADCf[XYZ_AXIS_COUNT]; typedef struct gyroConfig_s { diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index d1812a9b15..b46787ef03 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -102,15 +102,15 @@ const extiConfig_t *selectMPUIntExtiConfig(void) #ifdef USE_FAKE_GYRO int16_t fake_gyro_values[XYZ_AXIS_COUNT] = { 0,0,0 }; -static void fakeGyroInit(uint8_t lpf) +static void fakeGyroInit(gyro_t *gyro) { - UNUSED(lpf); + UNUSED(gyro); } -static bool fakeGyroRead(int16_t *gyroADC) +static bool fakeGyroRead(gyro_t *gyro) { for (int i = 0; i < XYZ_AXIS_COUNT; ++i) { - gyroADC[i] = fake_gyro_values[i]; + gyro->gyroADCRaw[X] = fake_gyro_values[i]; } return true; @@ -123,7 +123,9 @@ static bool fakeGyroReadTemp(int16_t *tempData) } -static bool fakeGyroInitStatus(void) { +static bool fakeGyroInitStatus(gyro_t *gyro) +{ + UNUSED(gyro); return true; } @@ -662,7 +664,8 @@ bool sensorsAutodetect(const 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(gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom); // Set gyro sample rate before initialisation - gyro.init(gyroConfig->gyro_lpf); // driver initialisation + gyro.lpf = gyroConfig->gyro_lpf; + gyro.init(&gyro); // driver initialisation gyroInit(gyroConfig); // sensor initialisation if (detectAcc(sensorSelectionConfig->acc_hardware)) { diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 5774973293..95295c848c 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -613,7 +613,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) for (i = 0; i < 3; i++) bstWrite16(accSmooth[i] / scale); for (i = 0; i < 3; i++) - bstWrite16(gyroADC[i]); + bstWrite16(lrintf(gyroADCf[i])); for (i = 0; i < 3; i++) bstWrite16(magADC[i]); }