diff --git a/src/main/drivers/accgyro.h b/src/main/drivers/accgyro.h index eb90ee217f..995a2b224e 100644 --- a/src/main/drivers/accgyro.h +++ b/src/main/drivers/accgyro.h @@ -37,6 +37,7 @@ typedef enum { GYRO_RATE_1_kHz, + GYRO_RATE_3200_Hz, GYRO_RATE_8_kHz, GYRO_RATE_32_kHz, } gyroRateKHz_e; diff --git a/src/main/drivers/accgyro_spi_bmi160.c b/src/main/drivers/accgyro_spi_bmi160.c new file mode 100644 index 0000000000..e18e92ef76 --- /dev/null +++ b/src/main/drivers/accgyro_spi_bmi160.c @@ -0,0 +1,458 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_BMI160 BMI160 Functions + * @brief Hardware functions to deal with the 6DOF gyro / accel sensor + * @{ + * + * @file pios_bmi160.c + * @author dRonin, http://dRonin.org/, Copyright (C) 2016 + * @brief BMI160 Gyro / Accel Sensor Routines + * @see The GNU Public License (GPL) Version 3 + ******************************************************************************/ + +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * Additional note on redistribution: The copyright and license notices above + * must be maintained in each individual source file that is a derivative work + * of this source file; otherwise redistribution is prohibited. + */ + +#include +#include + +#include "platform.h" + +#include "common/axis.h" +#include "common/maths.h" + +#include "system.h" +#include "io.h" +#include "exti.h" +#include "nvic.h" +#include "bus_spi.h" + +#include "gyro_sync.h" + +#include "sensor.h" +#include "accgyro.h" +#include "accgyro_spi_bmi160.h" + +#include "config/config_eeprom.h" +#include "config/config_profile.h" +#include "config/config_master.h" +#include "fc/runtime_config.h" + +#ifdef USE_ACCGYRO_BMI160 + +/* BMI160 Registers */ +#define BMI160_REG_CHIPID 0x00 +#define BMI160_REG_PMU_STAT 0x03 +#define BMI160_REG_GYR_DATA_X_LSB 0x0C +#define BMI160_REG_ACC_DATA_X_LSB 0x12 +#define BMI160_REG_STATUS 0x1B +#define BMI160_REG_TEMPERATURE_0 0x20 +#define BMI160_REG_ACC_CONF 0x40 +#define BMI160_REG_ACC_RANGE 0x41 +#define BMI160_REG_GYR_CONF 0x42 +#define BMI160_REG_GYR_RANGE 0x43 +#define BMI160_REG_INT_EN1 0x51 +#define BMI160_REG_INT_OUT_CTRL 0x53 +#define BMI160_REG_INT_MAP1 0x56 +#define BMI160_REG_FOC_CONF 0x69 +#define BMI160_REG_CONF 0x6A +#define BMI160_REG_OFFSET_0 0x77 +#define BMI160_REG_CMD 0x7E + +/* Register values */ +#define BMI160_PMU_CMD_PMU_ACC_NORMAL 0x11 +#define BMI160_PMU_CMD_PMU_GYR_NORMAL 0x15 +#define BMI160_INT_EN1_DRDY 0x10 +#define BMI160_INT_OUT_CTRL_INT1_CONFIG 0x0A +#define BMI160_REG_INT_MAP1_INT1_DRDY 0x80 +#define BMI160_CMD_START_FOC 0x03 +#define BMI160_CMD_PROG_NVM 0xA0 +#define BMI160_REG_STATUS_NVM_RDY 0x10 +#define BMI160_REG_STATUS_FOC_RDY 0x08 +#define BMI160_REG_CONF_NVM_PROG_EN 0x02 + +///* Global Variables */ +static volatile bool BMI160InitDone = false; +static volatile bool BMI160Detected = false; +static volatile bool bmi160DataReady = false; +static volatile bool bmi160ExtiInitDone = false; + +//! Private functions +static int32_t BMI160_Config(); +static int32_t BMI160_do_foc(); +static uint8_t BMI160_ReadReg(uint8_t reg); +static int32_t BMI160_WriteReg(uint8_t reg, uint8_t data); + +static IO_t bmi160CsPin = IO_NONE; +#define DISABLE_BMI160 IOHi(bmi160CsPin) +#define ENABLE_BMI160 IOLo(bmi160CsPin) + + +bool BMI160_Detect() +{ + if (BMI160Detected) + return true; + bmi160CsPin = IOGetByTag(IO_TAG(BMI160_CS_PIN)); + IOInit(bmi160CsPin, OWNER_MPU_CS, 0); + IOConfigGPIO(bmi160CsPin, SPI_IO_CS_CFG); + + spiSetDivisor(BMI160_SPI_INSTANCE, BMI160_SPI_DIVISOR); + + /* Read this address to acticate SPI (see p. 84) */ + BMI160_ReadReg(0x7F); + delay(10); // Give SPI some time to start up + + /* Check the chip ID */ + if (BMI160_ReadReg(BMI160_REG_CHIPID) != 0xd1){ + return false; + } + + BMI160Detected = true; + return true; +} + + +/** + * @brief Initialize the BMI160 6-axis sensor. + * @return 0 for success, -1 for failure to allocate, -10 for failure to get irq + */ +static void BMI160_Init() +{ + if (BMI160InitDone || !BMI160Detected) + return; + + /* Configure the BMI160 Sensor */ + if (BMI160_Config() != 0){ + return; + } + + bool do_foc = false; + + /* Perform fast offset compensation if requested */ + if (do_foc) { + BMI160_do_foc(); + } + + BMI160InitDone = true; +} + + +/** + * @brief Configure the sensor + */ +static int32_t BMI160_Config() +{ + + // Set normal power mode for gyro and accelerometer + if (BMI160_WriteReg(BMI160_REG_CMD, BMI160_PMU_CMD_PMU_GYR_NORMAL) != 0){ + return -1; + } + delay(100); // can take up to 80ms + + if (BMI160_WriteReg(BMI160_REG_CMD, BMI160_PMU_CMD_PMU_ACC_NORMAL) != 0){ + return -2; + } + delay(5); // can take up to 3.8ms + + // Verify that normal power mode was entered + uint8_t pmu_status = BMI160_ReadReg(BMI160_REG_PMU_STAT); + if ((pmu_status & 0x3C) != 0x14){ + return -3; + } + + // Set odr and ranges + // Set acc_us = 0 acc_bwp = 0b010 so only the first filter stage is used + if (BMI160_WriteReg(BMI160_REG_ACC_CONF, 0x20 | BMI160_ODR_800_Hz) != 0){ + return -3; + } + delay(1); + + // Set gyr_bwp = 0b010 so only the first filter stage is used + if (BMI160_WriteReg(BMI160_REG_GYR_CONF, 0x20 | BMI160_ODR_3200_Hz) != 0){ + return -4; + } + delay(1); + + if (BMI160_WriteReg(BMI160_REG_ACC_RANGE, BMI160_RANGE_8G) != 0){ + return -5; + } + delay(1); + + if (BMI160_WriteReg(BMI160_REG_GYR_RANGE, BMI160_RANGE_2000DPS) != 0){ + return -6; + } + delay(1); + + // Enable offset compensation + uint8_t val = BMI160_ReadReg(BMI160_REG_OFFSET_0); + if (BMI160_WriteReg(BMI160_REG_OFFSET_0, val | 0xC0) != 0){ + return -7; + } + + // Enable data ready interrupt + if (BMI160_WriteReg(BMI160_REG_INT_EN1, BMI160_INT_EN1_DRDY) != 0){ + return -8; + } + delay(1); + + // Enable INT1 pin + if (BMI160_WriteReg(BMI160_REG_INT_OUT_CTRL, BMI160_INT_OUT_CTRL_INT1_CONFIG) != 0){ + return -9; + } + delay(1); + + // Map data ready interrupt to INT1 pin + if (BMI160_WriteReg(BMI160_REG_INT_MAP1, BMI160_REG_INT_MAP1_INT1_DRDY) != 0){ + return -10; + } + delay(1); + + return 0; +} + +static int32_t BMI160_do_foc() +{ + // assume sensor is mounted on top + uint8_t val = 0x7D;; + if (BMI160_WriteReg(BMI160_REG_FOC_CONF, val) != 0) { + return -1; + } + + // Start FOC + if (BMI160_WriteReg(BMI160_REG_CMD, BMI160_CMD_START_FOC) != 0) { + return -2; + } + + // Wait for FOC to complete + for (int i=0; i<50; i++) { + val = BMI160_ReadReg(BMI160_REG_STATUS); + if (val & BMI160_REG_STATUS_FOC_RDY) { + break; + } + delay(10); + } + if (!(val & BMI160_REG_STATUS_FOC_RDY)) { + return -3; + } + + // Program NVM + val = BMI160_ReadReg(BMI160_REG_CONF); + if (BMI160_WriteReg(BMI160_REG_CONF, val | BMI160_REG_CONF_NVM_PROG_EN) != 0) { + return -4; + } + + if (BMI160_WriteReg(BMI160_REG_CMD, BMI160_CMD_PROG_NVM) != 0) { + return -5; + } + + // Wait for NVM programming to complete + for (int i=0; i<50; i++) { + val = BMI160_ReadReg(BMI160_REG_STATUS); + if (val & BMI160_REG_STATUS_NVM_RDY) { + break; + } + delay(10); + } + if (!(val & BMI160_REG_STATUS_NVM_RDY)) { + return -6; + } + + return 0; +} + +/** + * @brief Read a register from BMI160 + * @returns The register value + * @param reg[in] Register address to be read + */ +static uint8_t BMI160_ReadReg(uint8_t reg) +{ + uint8_t data; + + ENABLE_BMI160; + + spiTransferByte(BMI160_SPI_INSTANCE, 0x80 | reg); // request byte + spiTransfer(BMI160_SPI_INSTANCE, &data, NULL, 1); // receive response + + DISABLE_BMI160; + + return data; +} + + +/** + * @brief Writes one byte to the BMI160 register + * \param[in] reg Register address + * \param[in] data Byte to write + * @returns 0 when success + */ +static int32_t BMI160_WriteReg(uint8_t reg, uint8_t data) +{ + ENABLE_BMI160; + + spiTransferByte(BMI160_SPI_INSTANCE, 0x7f & reg); + spiTransferByte(BMI160_SPI_INSTANCE, data); + + DISABLE_BMI160; + + return 0; +} + + +extiCallbackRec_t bmi160IntCallbackRec; + +void bmi160ExtiHandler(extiCallbackRec_t *cb) +{ + gyroDev_t *gyro = container_of(cb, gyroDev_t, exti); + gyro->dataReady = true; +} + +static void bmi160IntExtiInit(gyroDev_t *gyro) +{ + static bool bmi160ExtiInitDone = false; + + if (bmi160ExtiInitDone) { + return; + } + + IO_t mpuIntIO = IOGetByTag(IO_TAG(BMI160_INT_EXTI)); + + IOInit(mpuIntIO, OWNER_MPU_EXTI, 0); + IOConfigGPIO(mpuIntIO, IOCFG_IN_FLOATING); // TODO - maybe pullup / pulldown ? + + EXTIHandlerInit(&gyro->exti, bmi160ExtiHandler); + EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising); + EXTIEnable(mpuIntIO, true); + + bmi160ExtiInitDone = true; +} + + +bool bmi160AccRead(accDev_t *acc) +{ + enum { + IDX_REG = 0, + IDX_ACCEL_XOUT_L, + IDX_ACCEL_XOUT_H, + IDX_ACCEL_YOUT_L, + IDX_ACCEL_YOUT_H, + IDX_ACCEL_ZOUT_L, + IDX_ACCEL_ZOUT_H, + BUFFER_SIZE, + }; + + uint8_t bmi160_rec_buf[BUFFER_SIZE]; + uint8_t bmi160_tx_buf[BUFFER_SIZE] = {BMI160_REG_ACC_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0}; + + ENABLE_BMI160; + spiTransfer(BMI160_SPI_INSTANCE, bmi160_rec_buf, bmi160_tx_buf, BUFFER_SIZE); // receive response + DISABLE_BMI160; + + acc->ADCRaw[X] = (int16_t)((bmi160_rec_buf[IDX_ACCEL_XOUT_H] << 8) | bmi160_rec_buf[IDX_ACCEL_XOUT_L]); + acc->ADCRaw[Y] = (int16_t)((bmi160_rec_buf[IDX_ACCEL_YOUT_H] << 8) | bmi160_rec_buf[IDX_ACCEL_YOUT_L]); + acc->ADCRaw[Z] = (int16_t)((bmi160_rec_buf[IDX_ACCEL_ZOUT_H] << 8) | bmi160_rec_buf[IDX_ACCEL_ZOUT_L]); + + return true; +} + + +bool bmi160GyroRead(gyroDev_t *gyro) +{ + enum { + IDX_REG = 0, + IDX_GYRO_XOUT_L, + IDX_GYRO_XOUT_H, + IDX_GYRO_YOUT_L, + IDX_GYRO_YOUT_H, + IDX_GYRO_ZOUT_L, + IDX_GYRO_ZOUT_H, + BUFFER_SIZE, + }; + + uint8_t bmi160_rec_buf[BUFFER_SIZE]; + uint8_t bmi160_tx_buf[BUFFER_SIZE] = {BMI160_REG_GYR_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0}; + + ENABLE_BMI160; + spiTransfer(BMI160_SPI_INSTANCE, bmi160_rec_buf, bmi160_tx_buf, BUFFER_SIZE); // receive response + DISABLE_BMI160; + + gyro->gyroADCRaw[X] = (int16_t)((bmi160_rec_buf[IDX_GYRO_XOUT_H] << 8) | bmi160_rec_buf[IDX_GYRO_XOUT_L]); + gyro->gyroADCRaw[Y] = (int16_t)((bmi160_rec_buf[IDX_GYRO_YOUT_H] << 8) | bmi160_rec_buf[IDX_GYRO_YOUT_L]); + gyro->gyroADCRaw[Z] = (int16_t)((bmi160_rec_buf[IDX_GYRO_ZOUT_H] << 8) | bmi160_rec_buf[IDX_GYRO_ZOUT_L]); + + return true; +} + + +bool checkBMI160DataReady(gyroDev_t* gyro) +{ + bool ret; + if (gyro->dataReady) { + ret = true; + gyro->dataReady= false; + } else { + ret = false; + } + return ret; +} + +void bmi160SpiGyroInit(gyroDev_t *gyro) +{ + BMI160_Init(); + bmi160IntExtiInit(gyro); +} + +void bmi160SpiAccInit(accDev_t *acc) +{ + BMI160_Init(); + + acc->acc_1G = 512 * 8; +} + + +bool bmi160SpiAccDetect(accDev_t *acc) +{ + if (!BMI160_Detect()) { + return false; + } + + acc->init = bmi160SpiAccInit; + acc->read = bmi160AccRead; + + return true; +} + + +bool bmi160SpiGyroDetect(gyroDev_t *gyro) +{ + if (!BMI160_Detect()) { + return false; + } + + gyro->init = bmi160SpiGyroInit; + gyro->read = bmi160GyroRead; + gyro->intStatus = checkBMI160DataReady; + gyro->scale = 1.0f / 16.4f; + + return true; +} +#endif // USE_ACCGYRO_BMI160 diff --git a/src/main/drivers/accgyro_spi_bmi160.h b/src/main/drivers/accgyro_spi_bmi160.h new file mode 100644 index 0000000000..0ce0c1d970 --- /dev/null +++ b/src/main/drivers/accgyro_spi_bmi160.h @@ -0,0 +1,70 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_BMI160 BMI160 Functions + * @brief Hardware functions to deal with the 6DOF gyro / accel sensor + * @{ + * + * @file pios_bmi160.c + * @author dRonin, http://dRonin.org/, Copyright (C) 2016 + * @brief BMI160 Gyro / Accel Sensor Routines + * @see The GNU Public License (GPL) Version 3 + ******************************************************************************/ + +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * Additional note on redistribution: The copyright and license notices above + * must be maintained in each individual source file that is a derivative work + * of this source file; otherwise redistribution is prohibited. + */ + +#pragma once + +enum pios_bmi160_orientation { // clockwise rotation from board forward + PIOS_BMI160_TOP_0DEG, + PIOS_BMI160_TOP_90DEG, + PIOS_BMI160_TOP_180DEG, + PIOS_BMI160_TOP_270DEG, + PIOS_BMI160_BOTTOM_0DEG, + PIOS_BMI160_BOTTOM_90DEG, + PIOS_BMI160_BOTTOM_180DEG, + PIOS_BMI160_BOTTOM_270DEG, +}; + +enum bmi160_odr { + BMI160_ODR_800_Hz = 0x0B, + BMI160_ODR_1600_Hz = 0x0C, + BMI160_ODR_3200_Hz = 0x0D, +}; + +enum bmi160_acc_range { + BMI160_RANGE_2G = 0x03, + BMI160_RANGE_4G = 0x05, + BMI160_RANGE_8G = 0x08, + BMI160_RANGE_16G = 0x0C, +}; + +enum bmi160_gyro_range { + BMI160_RANGE_125DPS = 0x04, + BMI160_RANGE_250DPS = 0x03, + BMI160_RANGE_500DPS = 0x02, + BMI160_RANGE_1000DPS = 0x01, + BMI160_RANGE_2000DPS = 0x00, +}; + +bool bmi160SpiAccDetect(accDev_t *acc); +bool bmi160SpiGyroDetect(gyroDev_t *gyro); diff --git a/src/main/drivers/gyro_sync.c b/src/main/drivers/gyro_sync.c index 8449fc1df2..82e0dc6586 100644 --- a/src/main/drivers/gyro_sync.c +++ b/src/main/drivers/gyro_sync.c @@ -31,8 +31,13 @@ uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenomin gyro->gyroRateKHz = GYRO_RATE_32_kHz; gyroSamplePeriod = 31.5f; } else { +#ifdef USE_ACCGYRO_BMI160 + gyro->gyroRateKHz = GYRO_RATE_3200_Hz; + gyroSamplePeriod = 312.0f; +#else gyro->gyroRateKHz = GYRO_RATE_8_kHz; gyroSamplePeriod = 125.0f; +#endif } } else { gyro->gyroRateKHz = GYRO_RATE_1_kHz; diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 49ae521d8a..57336843b0 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -43,6 +43,7 @@ #include "drivers/accgyro_mpu3050.h" #include "drivers/accgyro_mpu6050.h" #include "drivers/accgyro_mpu6500.h" +#include "drivers/accgyro_spi_bmi160.h" #include "drivers/accgyro_spi_icm20689.h" #include "drivers/accgyro_spi_mpu6000.h" #include "drivers/accgyro_spi_mpu6500.h" @@ -251,13 +252,22 @@ retry: ; // fallthrough case ACC_ICM20689: #ifdef USE_ACC_SPI_ICM20689 - - if (icm20689SpiAccDetect(dev)) - { + if (icm20689SpiAccDetect(dev)) { + accHardware = ACC_ICM20689; #ifdef ACC_ICM20689_ALIGN dev->accAlign = ACC_ICM20689_ALIGN; #endif - accHardware = ACC_ICM20689; + break; + } +#endif + ; // fallthrough + case ACC_BMI160: +#ifdef USE_ACCGYRO_BMI160 + if (bmi160SpiAccDetect(dev)) { + accHardware = ACC_BMI160; +#ifdef ACC_BMI160_ALIGN + dev->accAlign = ACC_BMI160_ALIGN; +#endif break; } #endif diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 35de76374e..8ea59f1e01 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -36,6 +36,7 @@ typedef enum { ACC_MPU9250, ACC_ICM20608G, ACC_ICM20602, + ACC_BMI160, ACC_FAKE } accelerationSensor_e; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index e3d368a863..c20f0fd670 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -43,6 +43,7 @@ #include "drivers/accgyro_mpu3050.h" #include "drivers/accgyro_mpu6050.h" #include "drivers/accgyro_mpu6500.h" +#include "drivers/accgyro_spi_bmi160.h" #include "drivers/accgyro_spi_icm20689.h" #include "drivers/accgyro_spi_mpu6000.h" #include "drivers/accgyro_spi_mpu6500.h" @@ -239,8 +240,7 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) #ifdef USE_GYRO_SPI_ICM20689 case GYRO_ICM20689: - if (icm20689SpiGyroDetect(dev)) - { + if (icm20689SpiGyroDetect(dev)) { gyroHardware = GYRO_ICM20689; #ifdef GYRO_ICM20689_ALIGN dev->gyroAlign = GYRO_ICM20689_ALIGN; @@ -249,6 +249,17 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) } #endif +#ifdef USE_ACCGYRO_BMI160 + case GYRO_BMI160: + if (bmi160SpiGyroDetect(dev)) { + gyroHardware = GYRO_BMI160; +#ifdef GYRO_BMI160_ALIGN + dev->gyroAlign = GYRO_BMI160_ALIGN; +#endif + break; + } +#endif + #ifdef USE_FAKE_GYRO case GYRO_FAKE: if (fakeGyroDetect(dev)) { diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 5dd80d3a48..98a9c585ef 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -34,6 +34,7 @@ typedef enum { GYRO_ICM20689, GYRO_ICM20608G, GYRO_ICM20602, + GYRO_BMI160, GYRO_FAKE } gyroSensor_e;