src/main/drivers/accgyro/accgyro_spi_icm20649.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <stdlib.h>
  24
  25 #include "platform.h"
  26
  27 #ifdef USE_GYRO_SPI_ICM20649
  28
  29 #include "common/axis.h"
  30 #include "common/maths.h"
  31
  32 #include "drivers/accgyro/accgyro.h"
  33 #include "drivers/accgyro/accgyro_mpu.h"
  34 #include "drivers/accgyro/accgyro_spi_icm20649.h"
  35 #include "drivers/bus_spi.h"
  36 #include "drivers/exti.h"
  37 #include "drivers/io.h"
  38 #include "drivers/sensor.h"
  39 #include "drivers/time.h"
  40
  41 // 8 MHz max SPI frequency
  42 #define ICM20649_MAX_SPI_CLK_HZ 8000000
  43
  44 uint8_t icm20649SpiDetect(const extDevice_t *dev)
  45 {
  46     spiWriteReg(dev, ICM20649_RA_REG_BANK_SEL, 0 << 4); // select bank 0 just to be safe
  47     delay(15);
  48
  49     spiWriteReg(dev, ICM20649_RA_PWR_MGMT_1, ICM20649_BIT_RESET);
  50
  51     uint8_t icmDetected = MPU_NONE;
  52     uint8_t attemptsRemaining = 20;
  53     do {
  54         delay(150);
  55         const uint8_t whoAmI = spiReadRegMsk(dev, ICM20649_RA_WHO_AM_I);
  56         if (whoAmI == ICM20649_WHO_AM_I_CONST) {
  57             icmDetected = ICM_20649_SPI;
  58         } else {
  59             icmDetected = MPU_NONE;
  60         }
  61         if (icmDetected != MPU_NONE) {
  62             break;
  63         }
  64         if (!attemptsRemaining) {
  65             return MPU_NONE;
  66         }
  67     } while (attemptsRemaining--);
  68
  69     return icmDetected;
  70
  71 }
  72
  73 void icm20649AccInit(accDev_t *acc)
  74 {
  75     // 2,048 LSB/g 16g
  76     // 1,024 LSB/g 30g
  77     acc->acc_1G = acc->acc_high_fsr ? 1024 : 2048;
  78
  79     spiWriteReg(&acc->gyro->dev, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2
  80     delay(15);
  81     const uint8_t acc_fsr = acc->acc_high_fsr ? ICM20649_FSR_30G : ICM20649_FSR_16G;
  82     spiWriteReg(&acc->gyro->dev, ICM20649_RA_ACCEL_CONFIG, acc_fsr << 1);
  83     delay(15);
  84     spiWriteReg(&acc->gyro->dev, ICM20649_RA_REG_BANK_SEL, 0 << 4); // back to bank 0
  85     delay(15);
  86 }
  87
  88 bool icm20649SpiAccDetect(accDev_t *acc)
  89 {
  90     if (acc->mpuDetectionResult.sensor != ICM_20649_SPI) {
  91         return false;
  92     }
  93
  94     acc->initFn = icm20649AccInit;
  95     acc->readFn = icm20649AccRead;
  96
  97     return true;
  98 }
  99
 100 void icm20649GyroInit(gyroDev_t *gyro)
 101 {
 102     mpuGyroInit(gyro);
 103
 104     spiSetClkDivisor(&gyro->dev, spiCalculateDivider(ICM20649_MAX_SPI_CLK_HZ)); // ensure proper speed
 105
 106     spiWriteReg(&gyro->dev, ICM20649_RA_REG_BANK_SEL, 0 << 4); // select bank 0 just to be safe
 107     delay(15);
 108     spiWriteReg(&gyro->dev, ICM20649_RA_PWR_MGMT_1, ICM20649_BIT_RESET);
 109     delay(100);
 110     spiWriteReg(&gyro->dev, ICM20649_RA_PWR_MGMT_1, INV_CLK_PLL);
 111     delay(15);
 112     spiWriteReg(&gyro->dev, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2
 113     delay(15);
 114     const uint8_t gyro_fsr = gyro->gyro_high_fsr ? ICM20649_FSR_4000DPS : ICM20649_FSR_2000DPS;
 115
 116     // If hardware_lpf is either GYRO_HARDWARE_LPF_NORMAL or GYRO_HARDWARE_LPF_EXPERIMENTAL then the
 117     // gyro is running in 9KHz sample mode and GYRO_FCHOICE should be 0, otherwise we're in 1.1KHz sample
 118     // mode and GYRO_FCHOICE = 1.  When in 1.1KHz mode select the 196.6Hz DLPF (GYRO_DLPFCFG = 0)
 119     // Unfortunately we can't configure any difference in DLPF based on NORMAL vs. EXPERIMENTAL because
 120     // the ICM20649 only has a single 9KHz DLPF cutoff.
 121     uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_1100_Hz ? 0 : 1; // deactivate GYRO_FCHOICE for sample rates over 1kHz (opposite of other invensense chips)
 122     raGyroConfigData |= gyro_fsr << 1;
 123     spiWriteReg(&gyro->dev, ICM20649_RA_GYRO_CONFIG_1, raGyroConfigData);
 124     delay(15);
 125     spiWriteReg(&gyro->dev, ICM20649_RA_GYRO_SMPLRT_DIV, gyro->mpuDividerDrops); // Get Divider Drops
 126     delay(100);
 127
 128     // Data ready interrupt configuration
 129     // back to bank 0
 130     spiWriteReg(&gyro->dev, ICM20649_RA_REG_BANK_SEL, 0 << 4);
 131     delay(15);
 132     spiWriteReg(&gyro->dev, ICM20649_RA_INT_PIN_CFG, 0x11);  // INT_ANYRD_2CLEAR, BYPASS_EN
 133     delay(15);
 134
 135     spiWriteReg(&gyro->dev, ICM20649_RA_INT_ENABLE_1, 0x01);
 136 }
 137
 138 bool icm20649SpiGyroDetect(gyroDev_t *gyro)
 139 {
 140     if (gyro->mpuDetectionResult.sensor != ICM_20649_SPI)
 141         return false;
 142
 143     gyro->initFn = icm20649GyroInit;
 144     gyro->readFn = icm20649GyroReadSPI;
 145
 146     // 16.384 dps/lsb scalefactor for 2000dps sensors
 147     //  8.192 dps/lsb scalefactor for 4000dps sensors
 148     gyro->scale = (gyro->gyro_high_fsr ? GYRO_SCALE_4000DPS : GYRO_SCALE_2000DPS);
 149
 150     return true;
 151 }
 152
 153 bool icm20649GyroReadSPI(gyroDev_t *gyro)
 154 {
 155     uint8_t dataToSend[7] = {ICM20649_RA_GYRO_XOUT_H | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
 156     uint8_t data[7];
 157
 158     const bool ack = spiReadWriteBufRB(&gyro->dev, dataToSend, data, 7);
 159     if (!ack) {
 160         return false;
 161     }
 162
 163     gyro->gyroADCRaw[X] = (int16_t)((data[1] << 8) | data[2]);
 164     gyro->gyroADCRaw[Y] = (int16_t)((data[3] << 8) | data[4]);
 165     gyro->gyroADCRaw[Z] = (int16_t)((data[5] << 8) | data[6]);
 166
 167     return true;
 168 }
 169
 170 bool icm20649AccRead(accDev_t *acc)
 171 {
 172     uint8_t data[6];
 173
 174     const bool ack = spiReadRegMskBufRB(&acc->gyro->dev, ICM20649_RA_ACCEL_XOUT_H, data, 6);
 175     if (!ack) {
 176         return false;
 177     }
 178
 179     acc->ADCRaw[X] = (int16_t)((data[0] << 8) | data[1]);
 180     acc->ADCRaw[Y] = (int16_t)((data[2] << 8) | data[3]);
 181     acc->ADCRaw[Z] = (int16_t)((data[4] << 8) | data[5]);
 182
 183     return true;
 184 }
 185 #endif