src/main/drivers/accgyro/accgyro_spi_icm20689.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 #include "common/axis.h"
  28 #include "common/maths.h"
  29
  30 #include "drivers/accgyro/accgyro.h"
  31 #include "drivers/accgyro/accgyro_mpu.h"
  32 #include "drivers/accgyro/accgyro_spi_icm20689.h"
  33 #include "drivers/bus_spi.h"
  34 #include "drivers/exti.h"
  35 #include "drivers/io.h"
  36 #include "drivers/sensor.h"
  37 #include "drivers/time.h"
  38
  39
  40
  41 // 10 MHz max SPI frequency
  42 #define ICM20689_MAX_SPI_CLK_HZ 10000000
  43
  44 // 10 MHz max SPI frequency for intialisation
  45 #define ICM20689_MAX_SPI_INIT_CLK_HZ 1000000
  46
  47 static void icm20689SpiInit(const extDevice_t *dev)
  48 {
  49     static bool hardwareInitialised = false;
  50
  51     if (hardwareInitialised) {
  52         return;
  53     }
  54
  55
  56     spiSetClkDivisor(dev, spiCalculateDivider(ICM20689_MAX_SPI_CLK_HZ));
  57
  58     hardwareInitialised = true;
  59 }
  60
  61 uint8_t icm20689SpiDetect(const extDevice_t *dev)
  62 {
  63     icm20689SpiInit(dev);
  64
  65     spiSetClkDivisor(dev, spiCalculateDivider(ICM20689_MAX_SPI_INIT_CLK_HZ));
  66
  67     // reset the device configuration
  68     spiWriteReg(dev, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
  69     delay(100);
  70
  71     // reset the device signal paths
  72     spiWriteReg(dev, MPU_RA_SIGNAL_PATH_RESET, 0x03);
  73     delay(100);
  74
  75     uint8_t icmDetected;
  76
  77     const uint8_t whoAmI = spiReadRegMsk(dev, MPU_RA_WHO_AM_I);
  78
  79     switch (whoAmI) {
  80     case ICM20601_WHO_AM_I_CONST:
  81         icmDetected = ICM_20601_SPI;
  82         break;
  83     case ICM20602_WHO_AM_I_CONST:
  84         icmDetected = ICM_20602_SPI;
  85         break;
  86     case ICM20608G_WHO_AM_I_CONST:
  87         icmDetected = ICM_20608_SPI;
  88         break;
  89     case ICM20689_WHO_AM_I_CONST:
  90         icmDetected = ICM_20689_SPI;
  91         break;
  92     default:
  93         icmDetected = MPU_NONE;
  94         break;
  95     }
  96
  97     spiSetClkDivisor(dev, spiCalculateDivider(ICM20689_MAX_SPI_CLK_HZ));
  98
  99     return icmDetected;
 100 }
 101
 102 void icm20689AccInit(accDev_t *acc)
 103 {
 104     acc->acc_1G = 512 * 4;
 105 }
 106
 107 bool icm20689SpiAccDetect(accDev_t *acc)
 108 {
 109     switch (acc->mpuDetectionResult.sensor) {
 110     case ICM_20602_SPI:
 111     case ICM_20689_SPI:
 112         break;
 113     default:
 114         return false;
 115     }
 116
 117     acc->initFn = icm20689AccInit;
 118     acc->readFn = mpuAccReadSPI;
 119
 120     return true;
 121 }
 122
 123 void icm20689GyroInit(gyroDev_t *gyro)
 124 {
 125     mpuGyroInit(gyro);
 126
 127     spiSetClkDivisor(&gyro->dev, spiCalculateDivider(ICM20689_MAX_SPI_INIT_CLK_HZ));
 128
 129     // Device was already reset during detection so proceed with configuration
 130
 131     spiWriteReg(&gyro->dev, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
 132     delay(15);
 133     spiWriteReg(&gyro->dev, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
 134     delay(15);
 135     spiWriteReg(&gyro->dev, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
 136     delay(15);
 137     spiWriteReg(&gyro->dev, MPU_RA_CONFIG, mpuGyroDLPF(gyro));
 138     delay(15);
 139     spiWriteReg(&gyro->dev, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops); // Get Divider Drops
 140     delay(100);
 141
 142     // Data ready interrupt configuration
 143 //    spiWriteReg(&gyro->dev, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0);  // INT_ANYRD_2CLEAR, BYPASS_EN
 144     spiWriteReg(&gyro->dev, MPU_RA_INT_PIN_CFG, 0x10);  // INT_ANYRD_2CLEAR, BYPASS_EN
 145
 146     delay(15);
 147
 148 #ifdef USE_MPU_DATA_READY_SIGNAL
 149     spiWriteReg(&gyro->dev, MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable
 150 #endif
 151
 152     spiSetClkDivisor(&gyro->dev, spiCalculateDivider(ICM20689_MAX_SPI_CLK_HZ));
 153 }
 154
 155 bool icm20689SpiGyroDetect(gyroDev_t *gyro)
 156 {
 157     switch (gyro->mpuDetectionResult.sensor) {
 158     case ICM_20601_SPI:
 159     case ICM_20602_SPI:
 160     case ICM_20608_SPI:
 161     case ICM_20689_SPI:
 162         break;
 163     default:
 164         return false;
 165     }
 166
 167     gyro->initFn = icm20689GyroInit;
 168     gyro->readFn = mpuGyroReadSPI;
 169
 170     gyro->scale = GYRO_SCALE_2000DPS;
 171
 172     return true;
 173 }