src/main/drivers/accgyro/accgyro_spi_icm42688p.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 /*
  22  * Author: Dominic Clifton
  23  */
  24
  25 #include <stdbool.h>
  26 #include <stdint.h>
  27 #include <stdlib.h>
  28
  29 #include "platform.h"
  30
  31 #ifdef USE_GYRO_SPI_ICM42688P
  32
  33 #include "common/axis.h"
  34 #include "common/maths.h"
  35 #include "build/debug.h"
  36
  37 #include "drivers/accgyro/accgyro.h"
  38 #include "drivers/accgyro/accgyro_mpu.h"
  39 #include "drivers/accgyro/accgyro_spi_icm42688p.h"
  40 #include "drivers/bus_spi.h"
  41 #include "drivers/exti.h"
  42 #include "drivers/io.h"
  43 #include "drivers/sensor.h"
  44 #include "drivers/time.h"
  45
  46 // 24 MHz max SPI frequency
  47 #define ICM42688P_MAX_SPI_CLK_HZ 24000000
  48
  49 // 10 MHz max SPI frequency for intialisation
  50 #define ICM42688P_MAX_SPI_INIT_CLK_HZ 1000000
  51
  52 #define ICM42688P_RA_PWR_MGMT0                       0x4E
  53
  54 #define ICM42688P_PWR_MGMT0_ACCEL_MODE_LN            (3 << 0)
  55 #define ICM42688P_PWR_MGMT0_GYRO_MODE_LN             (3 << 2)
  56 #define ICM42688P_PWR_MGMT0_TEMP_DISABLE_OFF         (0 << 5)
  57 #define ICM42688P_PWR_MGMT0_TEMP_DISABLE_ON          (1 << 5)
  58
  59 #define ICM42688P_RA_GYRO_CONFIG0                    0x4F
  60 #define ICM42688P_RA_ACCEL_CONFIG0                   0x50
  61
  62 #define ICM42688P_RA_GYRO_ACCEL_CONFIG0              0x52
  63
  64 #define ICM42688P_ACCEL_UI_FILT_BW_LOW_LATENCY       (14 << 4)
  65 #define ICM42688P_GYRO_UI_FILT_BW_LOW_LATENCY        (14 << 0)
  66
  67 #define ICM42688P_RA_GYRO_DATA_X1                    0x25
  68 #define ICM42688P_RA_ACCEL_DATA_X1                   0x1F
  69
  70 #define ICM42688P_RA_INT_CONFIG                      0x14
  71 #define ICM42688P_INT1_MODE_PULSED                   (0 << 2)
  72 #define ICM42688P_INT1_MODE_LATCHED                  (1 << 2)
  73 #define ICM42688P_INT1_DRIVE_CIRCUIT_OD              (0 << 1)
  74 #define ICM42688P_INT1_DRIVE_CIRCUIT_PP              (1 << 1)
  75 #define ICM42688P_INT1_POLARITY_ACTIVE_LOW           (0 << 0)
  76 #define ICM42688P_INT1_POLARITY_ACTIVE_HIGH          (1 << 0)
  77
  78 #define ICM42688P_RA_INT_CONFIG0                     0x63
  79 #define ICM42688P_UI_DRDY_INT_CLEAR_ON_SBR           ((0 << 5) || (0 << 4))
  80 #define ICM42688P_UI_DRDY_INT_CLEAR_ON_SBR_DUPLICATE ((0 << 5) || (1 << 4)) // duplicate settings in datasheet, Rev 1.3.
  81 #define ICM42688P_UI_DRDY_INT_CLEAR_ON_F1BR          ((1 << 5) || (0 << 4))
  82 #define ICM42688P_UI_DRDY_INT_CLEAR_ON_SBR_AND_F1BR  ((1 << 5) || (1 << 4))
  83
  84 #define ICM42688P_RA_INT_CONFIG1                     0x64
  85 #define ICM42688P_INT_ASYNC_RESET_BIT                4
  86 #define ICM42688P_INT_TDEASSERT_DISABLE_BIT          5
  87 #define ICM42688P_INT_TDEASSERT_ENABLED              (0 << ICM42688P_INT_TDEASSERT_DISABLE_BIT)
  88 #define ICM42688P_INT_TDEASSERT_DISABLED             (1 << ICM42688P_INT_TDEASSERT_DISABLE_BIT)
  89 #define ICM42688P_INT_TPULSE_DURATION_BIT            6
  90 #define ICM42688P_INT_TPULSE_DURATION_100            (0 << ICM42688P_INT_TPULSE_DURATION_BIT)
  91 #define ICM42688P_INT_TPULSE_DURATION_8              (1 << ICM42688P_INT_TPULSE_DURATION_BIT)
  92
  93
  94 #define ICM42688P_RA_INT_SOURCE0                     0x65
  95 #define ICM42688P_UI_DRDY_INT1_EN_DISABLED           (0 << 3)
  96 #define ICM42688P_UI_DRDY_INT1_EN_ENABLED            (1 << 3)
  97
  98 static void icm42688PSpiInit(const extDevice_t *dev)
  99 {
 100     static bool hardwareInitialised = false;
 101
 102     if (hardwareInitialised) {
 103         return;
 104     }
 105
 106
 107     spiSetClkDivisor(dev, spiCalculateDivider(ICM42688P_MAX_SPI_CLK_HZ));
 108
 109     hardwareInitialised = true;
 110 }
 111
 112 uint8_t icm42688PSpiDetect(const extDevice_t *dev)
 113 {
 114     icm42688PSpiInit(dev);
 115
 116     spiSetClkDivisor(dev, spiCalculateDivider(ICM42688P_MAX_SPI_INIT_CLK_HZ));
 117
 118     spiWriteReg(dev, ICM42688P_RA_PWR_MGMT0, 0x00);
 119
 120     uint8_t icmDetected = MPU_NONE;
 121     uint8_t attemptsRemaining = 20;
 122     do {
 123         delay(150);
 124         const uint8_t whoAmI = spiReadRegMsk(dev, MPU_RA_WHO_AM_I);
 125         switch (whoAmI) {
 126         case ICM42605_WHO_AM_I_CONST:
 127             icmDetected = ICM_42605_SPI;
 128             break;
 129         case ICM42688P_WHO_AM_I_CONST:
 130             icmDetected = ICM_42688P_SPI;
 131             break;
 132         default:
 133             icmDetected = MPU_NONE;
 134             break;
 135         }
 136         if (icmDetected != MPU_NONE) {
 137             break;
 138         }
 139         if (!attemptsRemaining) {
 140             return MPU_NONE;
 141         }
 142     } while (attemptsRemaining--);
 143
 144     spiSetClkDivisor(dev, spiCalculateDivider(ICM42688P_MAX_SPI_CLK_HZ));
 145
 146     return icmDetected;
 147 }
 148
 149 void icm42688PAccInit(accDev_t *acc)
 150 {
 151     acc->acc_1G = 512 * 4;
 152 }
 153
 154 bool icm42688PAccRead(accDev_t *acc)
 155 {
 156     uint8_t data[6];
 157
 158     const bool ack = busReadRegisterBuffer(&acc->gyro->dev, ICM42688P_RA_ACCEL_DATA_X1, data, 6);
 159     if (!ack) {
 160         return false;
 161     }
 162
 163     acc->ADCRaw[X] = (int16_t)((data[0] << 8) | data[1]);
 164     acc->ADCRaw[Y] = (int16_t)((data[2] << 8) | data[3]);
 165     acc->ADCRaw[Z] = (int16_t)((data[4] << 8) | data[5]);
 166
 167     return true;
 168 }
 169 bool icm42688PSpiAccDetect(accDev_t *acc)
 170 {
 171     switch (acc->mpuDetectionResult.sensor) {
 172     case ICM_42688P_SPI:
 173         break;
 174     default:
 175         return false;
 176     }
 177
 178     acc->initFn = icm42688PAccInit;
 179     acc->readFn = icm42688PAccRead;
 180
 181     return true;
 182 }
 183
 184 typedef struct odrEntry_s {
 185     uint8_t khz;
 186     uint8_t odr; // See GYRO_ODR in datasheet.
 187 } odrEntry_t;
 188
 189 static odrEntry_t icm42688PkhzToSupportedODRMap[] = {
 190     { 8, 3 },
 191     { 4, 4 },
 192     { 2, 5 },
 193     { 1, 6 },
 194 };
 195
 196 void icm42688PGyroInit(gyroDev_t *gyro)
 197 {
 198     mpuGyroInit(gyro);
 199
 200     spiSetClkDivisor(&gyro->dev, spiCalculateDivider(ICM42688P_MAX_SPI_INIT_CLK_HZ));
 201
 202     spiWriteReg(&gyro->dev, ICM42688P_RA_PWR_MGMT0, ICM42688P_PWR_MGMT0_TEMP_DISABLE_OFF | ICM42688P_PWR_MGMT0_ACCEL_MODE_LN | ICM42688P_PWR_MGMT0_GYRO_MODE_LN);
 203     delay(15);
 204
 205     uint8_t outputDataRate = 0;
 206     bool supportedODRFound = false;
 207
 208     if (gyro->gyroRateKHz) {
 209         uint8_t gyroSyncDenominator = gyro->mpuDividerDrops + 1; // rebuild it in here, see gyro_sync.c
 210         uint8_t desiredODRKhz = 8 / gyroSyncDenominator;
 211         for (uint32_t i = 0; i < ARRAYLEN(icm42688PkhzToSupportedODRMap); i++) {
 212             if (icm42688PkhzToSupportedODRMap[i].khz == desiredODRKhz) {
 213                 outputDataRate = icm42688PkhzToSupportedODRMap[i].odr;
 214                 supportedODRFound = true;
 215                 break;
 216             }
 217         }
 218     }
 219
 220     if (!supportedODRFound) {
 221         outputDataRate = 6;
 222         gyro->gyroRateKHz = GYRO_RATE_1_kHz;
 223     }
 224
 225     STATIC_ASSERT(INV_FSR_2000DPS == 3, "INV_FSR_2000DPS must be 3 to generate correct value");
 226     spiWriteReg(&gyro->dev, ICM42688P_RA_GYRO_CONFIG0, (3 - INV_FSR_2000DPS) << 5 | (outputDataRate & 0x0F));
 227     delay(15);
 228
 229     STATIC_ASSERT(INV_FSR_16G == 3, "INV_FSR_16G must be 3 to generate correct value");
 230     spiWriteReg(&gyro->dev, ICM42688P_RA_ACCEL_CONFIG0, (3 - INV_FSR_16G) << 5 | (outputDataRate & 0x0F));
 231     delay(15);
 232
 233     spiWriteReg(&gyro->dev, ICM42688P_RA_GYRO_ACCEL_CONFIG0, ICM42688P_ACCEL_UI_FILT_BW_LOW_LATENCY | ICM42688P_GYRO_UI_FILT_BW_LOW_LATENCY);
 234
 235     spiWriteReg(&gyro->dev, ICM42688P_RA_INT_CONFIG, ICM42688P_INT1_MODE_PULSED | ICM42688P_INT1_DRIVE_CIRCUIT_PP | ICM42688P_INT1_POLARITY_ACTIVE_HIGH);
 236     spiWriteReg(&gyro->dev, ICM42688P_RA_INT_CONFIG0, ICM42688P_UI_DRDY_INT_CLEAR_ON_SBR);
 237
 238 #ifdef USE_MPU_DATA_READY_SIGNAL
 239     spiWriteReg(&gyro->dev, ICM42688P_RA_INT_SOURCE0, ICM42688P_UI_DRDY_INT1_EN_ENABLED);
 240
 241     uint8_t intConfig1Value = spiReadRegMsk(&gyro->dev, ICM42688P_RA_INT_CONFIG1);
 242     // Datasheet says: "User should change setting to 0 from default setting of 1, for proper INT1 and INT2 pin operation"
 243     intConfig1Value &= ~(1 << ICM42688P_INT_ASYNC_RESET_BIT);
 244     intConfig1Value |= (ICM42688P_INT_TPULSE_DURATION_8 | ICM42688P_INT_TDEASSERT_DISABLED);
 245
 246     spiWriteReg(&gyro->dev, ICM42688P_RA_INT_CONFIG1, intConfig1Value);
 247 #endif
 248     //
 249
 250     spiSetClkDivisor(&gyro->dev, spiCalculateDivider(ICM42688P_MAX_SPI_CLK_HZ));
 251 }
 252
 253 bool icm42688PGyroReadSPI(gyroDev_t *gyro)
 254 {
 255     STATIC_DMA_DATA_AUTO uint8_t dataToSend[7] = {ICM42688P_RA_GYRO_DATA_X1 | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
 256     STATIC_DMA_DATA_AUTO uint8_t data[7];
 257
 258     const bool ack = spiReadWriteBufRB(&gyro->dev, dataToSend, data, 7);
 259     if (!ack) {
 260         return false;
 261     }
 262
 263     gyro->gyroADCRaw[X] = (int16_t)((data[1] << 8) | data[2]);
 264     gyro->gyroADCRaw[Y] = (int16_t)((data[3] << 8) | data[4]);
 265     gyro->gyroADCRaw[Z] = (int16_t)((data[5] << 8) | data[6]);
 266
 267     return true;
 268 }
 269
 270 bool icm42688PSpiGyroDetect(gyroDev_t *gyro)
 271 {
 272     switch (gyro->mpuDetectionResult.sensor) {
 273     case ICM_42688P_SPI:
 274         break;
 275     default:
 276         return false;
 277     }
 278
 279     gyro->initFn = icm42688PGyroInit;
 280     gyro->readFn = icm42688PGyroReadSPI;
 281
 282     gyro->scale = GYRO_SCALE_2000DPS;
 283
 284     return true;
 285 }
 286 #endif