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
  42 // 8 MHz max SPI frequency
  43 #define ICM20649_MAX_SPI_CLK_HZ 8000000
  44
  45 static void icm20649SpiInit(const extDevice_t *dev)
  46 {
  47     static bool hardwareInitialised = false;
  48
  49     if (hardwareInitialised) {
  50         return;
  51     }
  52
  53     // all registers can be read/written at full speed (7MHz +-10%)
  54     // TODO verify that this works at 9MHz and 10MHz on non F7
  55     spiSetClkDivisor(dev, spiCalculateDivider(ICM20649_MAX_SPI_CLK_HZ));
  56
  57     hardwareInitialised = true;
  58 }
  59
  60 uint8_t icm20649SpiDetect(const extDevice_t *dev)
  61 {
  62
  63     icm20649SpiInit(dev);
  64
  65     spiSetClkDivisor(dev, spiCalculateDivider(ICM20649_MAX_SPI_CLK_HZ));
  66
  67     spiWriteReg(dev, ICM20649_RA_REG_BANK_SEL, 0 << 4); // select bank 0 just to be safe
  68     delay(15);
  69
  70     spiWriteReg(dev, ICM20649_RA_PWR_MGMT_1, ICM20649_BIT_RESET);
  71
  72     uint8_t icmDetected = MPU_NONE;
  73     uint8_t attemptsRemaining = 20;
  74     do {
  75         delay(150);
  76         const uint8_t whoAmI = spiReadRegMsk(bus, ICM20649_RA_WHO_AM_I);
  77         if (whoAmI == ICM20649_WHO_AM_I_CONST) {
  78             icmDetected = ICM_20649_SPI;
  79         } else {
  80             icmDetected = MPU_NONE;
  81         }
  82         if (icmDetected != MPU_NONE) {
  83             break;
  84         }
  85         if (!attemptsRemaining) {
  86             return MPU_NONE;
  87         }
  88     } while (attemptsRemaining--);
  89
  90     return icmDetected;
  91
  92 }
  93
  94 void icm20649AccInit(accDev_t *acc)
  95 {
  96     // 2,048 LSB/g 16g
  97     // 1,024 LSB/g 30g
  98     acc->acc_1G = acc->acc_high_fsr ? 1024 : 2048;
  99
 100     spiSetClkDivisor(&acc->dev, spiCalculateDivider(ICM20649_MAX_SPI_CLK_HZ));
 101
 102     spiWriteReg(&acc->dev, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2
 103     delay(15);
 104     const uint8_t acc_fsr = acc->acc_high_fsr ? ICM20649_FSR_30G : ICM20649_FSR_16G;
 105     spiWriteReg(&acc->dev, ICM20649_RA_ACCEL_CONFIG, acc_fsr << 1);
 106     delay(15);
 107     spiWriteReg(&acc->dev, ICM20649_RA_REG_BANK_SEL, 0 << 4); // back to bank 0
 108     delay(15);
 109 }
 110
 111 bool icm20649SpiAccDetect(accDev_t *acc)
 112 {
 113     if (acc->mpuDetectionResult.sensor != ICM_20649_SPI) {
 114         return false;
 115     }
 116
 117     acc->initFn = icm20649AccInit;
 118     acc->readFn = icm20649AccRead;
 119
 120     return true;
 121 }
 122
 123
 124 void icm20649GyroInit(gyroDev_t *gyro)
 125 {
 126     mpuGyroInit(gyro);
 127
 128     spiSetClkDivisor(dev, spiCalculateDivider(ICM20649_MAX_SPI_CLK_HZ)); // ensure proper speed
 129
 130     spiWriteReg(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // select bank 0 just to be safe
 131     delay(15);
 132     spiWriteReg(&gyro->bus, ICM20649_RA_PWR_MGMT_1, ICM20649_BIT_RESET);
 133     delay(100);
 134     spiWriteReg(&gyro->bus, ICM20649_RA_PWR_MGMT_1, INV_CLK_PLL);
 135     delay(15);
 136     spiWriteReg(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2
 137     delay(15);
 138     const uint8_t gyro_fsr = gyro->gyro_high_fsr ? ICM20649_FSR_4000DPS : ICM20649_FSR_2000DPS;
 139
 140     // If hardware_lpf is either GYRO_HARDWARE_LPF_NORMAL or GYRO_HARDWARE_LPF_EXPERIMENTAL then the
 141     // gyro is running in 9KHz sample mode and GYRO_FCHOICE should be 0, otherwise we're in 1.1KHz sample
 142     // mode and GYRO_FCHOICE = 1.  When in 1.1KHz mode select the 196.6Hz DLPF (GYRO_DLPFCFG = 0)
 143     // Unfortunately we can't configure any difference in DLPF based on NORMAL vs. EXPERIMENTAL because
 144     // the ICM20649 only has a single 9KHz DLPF cutoff.
 145     uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_1100_Hz ? 0 : 1; // deactivate GYRO_FCHOICE for sample rates over 1kHz (opposite of other invensense chips)
 146     raGyroConfigData |= gyro_fsr << 1;
 147     spiWriteReg(&gyro->bus, ICM20649_RA_GYRO_CONFIG_1, raGyroConfigData);
 148     delay(15);
 149     spiWriteReg(&gyro->bus, ICM20649_RA_GYRO_SMPLRT_DIV, gyro->mpuDividerDrops); // Get Divider Drops
 150     delay(100);
 151
 152     // Data ready interrupt configuration
 153     // back to bank 0
 154     spiWriteReg(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4);
 155     delay(15);
 156     spiWriteReg(&gyro->bus, ICM20649_RA_INT_PIN_CFG, 0x11);  // INT_ANYRD_2CLEAR, BYPASS_EN
 157     delay(15);
 158
 159 #ifdef USE_MPU_DATA_READY_SIGNAL
 160     spiWriteReg(&gyro->bus, ICM20649_RA_INT_ENABLE_1, 0x01);
 161 #endif
 162 }
 163
 164 bool icm20649SpiGyroDetect(gyroDev_t *gyro)
 165 {
 166     if (gyro->mpuDetectionResult.sensor != ICM_20649_SPI)
 167         return false;
 168
 169     gyro->initFn = icm20649GyroInit;
 170     gyro->readFn = icm20649GyroReadSPI;
 171
 172     // 16.384 dps/lsb scalefactor for 2000dps sensors
 173     //  8.192 dps/lsb scalefactor for 4000dps sensors
 174     gyro->scale = (gyro->gyro_high_fsr ? GYRO_SCALE_4000DPS : GYRO_SCALE_2000DPS);
 175
 176     return true;
 177 }
 178
 179 bool icm20649GyroReadSPI(gyroDev_t *gyro)
 180 {
 181     static const uint8_t dataToSend[7] = {ICM20649_RA_GYRO_XOUT_H | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
 182     uint8_t data[7];
 183
 184     const bool ack = spiReadWriteBufRB(&gyro->bus, dataToSend, data, 7);
 185     if (!ack) {
 186         return false;
 187     }
 188
 189     gyro->gyroADCRaw[X] = (int16_t)((data[1] << 8) | data[2]);
 190     gyro->gyroADCRaw[Y] = (int16_t)((data[3] << 8) | data[4]);
 191     gyro->gyroADCRaw[Z] = (int16_t)((data[5] << 8) | data[6]);
 192
 193     return true;
 194 }
 195
 196 bool icm20649AccRead(accDev_t *acc)
 197 {
 198     uint8_t data[6];
 199
 200     const bool ack = spiReadRegMskBufRB(&acc->bus, ICM20649_RA_ACCEL_XOUT_H, data, 6);
 201     if (!ack) {
 202         return false;
 203     }
 204
 205     acc->ADCRaw[X] = (int16_t)((data[0] << 8) | data[1]);
 206     acc->ADCRaw[Y] = (int16_t)((data[2] << 8) | data[3]);
 207     acc->ADCRaw[Z] = (int16_t)((data[4] << 8) | data[5]);
 208
 209     return true;
 210 }
 211 #endif