src/main/drivers/accgyro/accgyro_spi_icm426xx.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 #if defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P)
  32
  33 #include "common/axis.h"
  34 #include "common/utils.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_icm426xx.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 #include "sensors/gyro.h"
  47
  48 // 24 MHz max SPI frequency
  49 #define ICM426XX_MAX_SPI_CLK_HZ 24000000
  50
  51 #define ICM426XX_RA_REG_BANK_SEL                    0x76
  52 #define ICM426XX_BANK_SELECT0                       0x00
  53 #define ICM426XX_BANK_SELECT1                       0x01
  54 #define ICM426XX_BANK_SELECT2                       0x02
  55 #define ICM426XX_BANK_SELECT3                       0x03
  56 #define ICM426XX_BANK_SELECT4                       0x04
  57
  58 #define ICM426XX_RA_PWR_MGMT0                       0x4E  // User Bank 0
  59 #define ICM426XX_PWR_MGMT0_ACCEL_MODE_LN            (3 << 0)
  60 #define ICM426XX_PWR_MGMT0_GYRO_MODE_LN             (3 << 2)
  61 #define ICM426XX_PWR_MGMT0_GYRO_ACCEL_MODE_OFF      ((0 << 0) | (0 << 2))
  62 #define ICM426XX_PWR_MGMT0_TEMP_DISABLE_OFF         (0 << 5)
  63 #define ICM426XX_PWR_MGMT0_TEMP_DISABLE_ON          (1 << 5)
  64
  65 #define ICM426XX_RA_GYRO_CONFIG0                    0x4F
  66 #define ICM426XX_RA_ACCEL_CONFIG0                   0x50
  67
  68 // --- Registers for gyro and acc Anti-Alias Filter ---------
  69 #define ICM426XX_RA_GYRO_CONFIG_STATIC3             0x0C  // User Bank 1
  70 #define ICM426XX_RA_GYRO_CONFIG_STATIC4             0x0D  // User Bank 1
  71 #define ICM426XX_RA_GYRO_CONFIG_STATIC5             0x0E  // User Bank 1
  72 #define ICM426XX_RA_ACCEL_CONFIG_STATIC2            0x03  // User Bank 2
  73 #define ICM426XX_RA_ACCEL_CONFIG_STATIC3            0x04  // User Bank 2
  74 #define ICM426XX_RA_ACCEL_CONFIG_STATIC4            0x05  // User Bank 2
  75 // --- Register & setting for gyro and acc UI Filter --------
  76 #define ICM426XX_RA_GYRO_ACCEL_CONFIG0              0x52  // User Bank 0
  77 #define ICM426XX_ACCEL_UI_FILT_BW_LOW_LATENCY       (15 << 4)
  78 #define ICM426XX_GYRO_UI_FILT_BW_LOW_LATENCY        (15 << 0)
  79 // ----------------------------------------------------------
  80
  81 #define ICM426XX_RA_GYRO_DATA_X1                    0x25  // User Bank 0
  82 #define ICM426XX_RA_ACCEL_DATA_X1                   0x1F  // User Bank 0
  83
  84 #define ICM426XX_RA_INT_CONFIG                      0x14  // User Bank 0
  85 #define ICM426XX_INT1_MODE_PULSED                   (0 << 2)
  86 #define ICM426XX_INT1_MODE_LATCHED                  (1 << 2)
  87 #define ICM426XX_INT1_DRIVE_CIRCUIT_OD              (0 << 1)
  88 #define ICM426XX_INT1_DRIVE_CIRCUIT_PP              (1 << 1)
  89 #define ICM426XX_INT1_POLARITY_ACTIVE_LOW           (0 << 0)
  90 #define ICM426XX_INT1_POLARITY_ACTIVE_HIGH          (1 << 0)
  91
  92 #define ICM426XX_RA_INT_CONFIG0                     0x63  // User Bank 0
  93 #define ICM426XX_UI_DRDY_INT_CLEAR_ON_SBR           ((0 << 5) || (0 << 4))
  94 #define ICM426XX_UI_DRDY_INT_CLEAR_ON_SBR_DUPLICATE ((0 << 5) || (0 << 4)) // duplicate settings in datasheet, Rev 1.2.
  95 #define ICM426XX_UI_DRDY_INT_CLEAR_ON_F1BR          ((1 << 5) || (0 << 4))
  96 #define ICM426XX_UI_DRDY_INT_CLEAR_ON_SBR_AND_F1BR  ((1 << 5) || (1 << 4))
  97
  98 #define ICM426XX_RA_INT_CONFIG1                     0x64   // User Bank 0
  99 #define ICM426XX_INT_ASYNC_RESET_BIT                4
 100 #define ICM426XX_INT_TDEASSERT_DISABLE_BIT          5
 101 #define ICM426XX_INT_TDEASSERT_ENABLED              (0 << ICM426XX_INT_TDEASSERT_DISABLE_BIT)
 102 #define ICM426XX_INT_TDEASSERT_DISABLED             (1 << ICM426XX_INT_TDEASSERT_DISABLE_BIT)
 103 #define ICM426XX_INT_TPULSE_DURATION_BIT            6
 104 #define ICM426XX_INT_TPULSE_DURATION_100            (0 << ICM426XX_INT_TPULSE_DURATION_BIT)
 105 #define ICM426XX_INT_TPULSE_DURATION_8              (1 << ICM426XX_INT_TPULSE_DURATION_BIT)
 106
 107 #define ICM426XX_RA_INT_SOURCE0                     0x65  // User Bank 0
 108 #define ICM426XX_UI_DRDY_INT1_EN_DISABLED           (0 << 3)
 109 #define ICM426XX_UI_DRDY_INT1_EN_ENABLED            (1 << 3)
 110
 111 typedef enum {
 112     ODR_CONFIG_8K = 0,
 113     ODR_CONFIG_4K,
 114     ODR_CONFIG_2K,
 115     ODR_CONFIG_1K,
 116     ODR_CONFIG_COUNT
 117 } odrConfig_e;
 118
 119 typedef enum {
 120     AAF_CONFIG_258HZ = 0,
 121     AAF_CONFIG_536HZ,
 122     AAF_CONFIG_997HZ,
 123     AAF_CONFIG_1962HZ,
 124     AAF_CONFIG_COUNT
 125 } aafConfig_e;
 126
 127 typedef struct aafConfig_s {
 128     uint8_t delt;
 129     uint16_t deltSqr;
 130     uint8_t bitshift;
 131 } aafConfig_t;
 132
 133 // Possible output data rates (ODRs)
 134 static uint8_t odrLUT[ODR_CONFIG_COUNT] = {  // see GYRO_ODR in section 5.6
 135     [ODR_CONFIG_8K] = 3,
 136     [ODR_CONFIG_4K] = 4,
 137     [ODR_CONFIG_2K] = 5,
 138     [ODR_CONFIG_1K] = 6,
 139 };
 140
 141 // Possible gyro Anti-Alias Filter (AAF) cutoffs for ICM-42688P
 142 static aafConfig_t aafLUT42688[AAF_CONFIG_COUNT] = {  // see table in section 5.3
 143     [AAF_CONFIG_258HZ]  = {  6,   36, 10 },
 144     [AAF_CONFIG_536HZ]  = { 12,  144,  8 },
 145     [AAF_CONFIG_997HZ]  = { 21,  440,  6 },
 146     [AAF_CONFIG_1962HZ] = { 37, 1376,  4 },
 147 };
 148
 149 // Possible gyro Anti-Alias Filter (AAF) cutoffs for ICM-42688P
 150 // actual cutoff differs slightly from those of the 42688P
 151 static aafConfig_t aafLUT42605[AAF_CONFIG_COUNT] = {  // see table in section 5.3
 152     [AAF_CONFIG_258HZ]  = { 21,  440,  6 }, // actually 249 Hz
 153     [AAF_CONFIG_536HZ]  = { 39, 1536,  4 }, // actually 524 Hz
 154     [AAF_CONFIG_997HZ]  = { 63, 3968,  3 }, // actually 995 Hz
 155     [AAF_CONFIG_1962HZ] = { 63, 3968,  3 }, // 995 Hz is the max cutoff on the 42605
 156 };
 157
 158 uint8_t icm426xxSpiDetect(const extDevice_t *dev)
 159 {
 160     spiWriteReg(dev, ICM426XX_RA_PWR_MGMT0, 0x00);
 161
 162     uint8_t icmDetected = MPU_NONE;
 163     uint8_t attemptsRemaining = 20;
 164     do {
 165         delay(150);
 166         const uint8_t whoAmI = spiReadRegMsk(dev, MPU_RA_WHO_AM_I);
 167         switch (whoAmI) {
 168         case ICM42605_WHO_AM_I_CONST:
 169             icmDetected = ICM_42605_SPI;
 170             break;
 171         case ICM42688P_WHO_AM_I_CONST:
 172             icmDetected = ICM_42688P_SPI;
 173             break;
 174         default:
 175             icmDetected = MPU_NONE;
 176             break;
 177         }
 178         if (icmDetected != MPU_NONE) {
 179             break;
 180         }
 181         if (!attemptsRemaining) {
 182             return MPU_NONE;
 183         }
 184     } while (attemptsRemaining--);
 185
 186     return icmDetected;
 187 }
 188
 189 void icm426xxAccInit(accDev_t *acc)
 190 {
 191     acc->acc_1G = 512 * 4;
 192 }
 193
 194 bool icm426xxSpiAccDetect(accDev_t *acc)
 195 {
 196     switch (acc->mpuDetectionResult.sensor) {
 197     case ICM_42605_SPI:
 198         break;
 199     case ICM_42688P_SPI:
 200         break;
 201     default:
 202         return false;
 203     }
 204
 205     acc->initFn = icm426xxAccInit;
 206     acc->readFn = mpuAccReadSPI;
 207
 208     return true;
 209 }
 210
 211 static aafConfig_t getGyroAafConfig(const mpuSensor_e, const aafConfig_e);
 212
 213 static void turnGyroAccOff(const extDevice_t *dev)
 214 {
 215     spiWriteReg(dev, ICM426XX_RA_PWR_MGMT0, ICM426XX_PWR_MGMT0_GYRO_ACCEL_MODE_OFF);
 216 }
 217
 218 // Turn on gyro and acc on in Low Noise mode
 219 static void turnGyroAccOn(const extDevice_t *dev)
 220 {
 221     spiWriteReg(dev, ICM426XX_RA_PWR_MGMT0, ICM426XX_PWR_MGMT0_TEMP_DISABLE_OFF | ICM426XX_PWR_MGMT0_ACCEL_MODE_LN | ICM426XX_PWR_MGMT0_GYRO_MODE_LN);
 222     delay(1);
 223 }
 224
 225 static void setUserBank(const extDevice_t *dev, const uint8_t user_bank)
 226 {
 227     spiWriteReg(dev, ICM426XX_RA_REG_BANK_SEL, user_bank & 7);
 228 }
 229
 230 void icm426xxGyroInit(gyroDev_t *gyro)
 231 {
 232     const extDevice_t *dev = &gyro->dev;
 233
 234     spiSetClkDivisor(dev, spiCalculateDivider(ICM426XX_MAX_SPI_CLK_HZ));
 235
 236     mpuGyroInit(gyro);
 237     gyro->accDataReg = ICM426XX_RA_ACCEL_DATA_X1;
 238     gyro->gyroDataReg = ICM426XX_RA_GYRO_DATA_X1;
 239
 240     // Turn off ACC and GYRO so they can be configured
 241     // See section 12.9 in ICM-42688-P datasheet v1.7
 242     setUserBank(dev, ICM426XX_BANK_SELECT0);
 243     turnGyroAccOff(dev);
 244
 245     // Configure gyro Anti-Alias Filter (see section 5.3 "ANTI-ALIAS FILTER")
 246     const mpuSensor_e gyroModel = gyro->mpuDetectionResult.sensor;
 247     aafConfig_t aafConfig = getGyroAafConfig(gyroModel, gyroConfig()->gyro_hardware_lpf);
 248     setUserBank(dev, ICM426XX_BANK_SELECT1);
 249     spiWriteReg(dev, ICM426XX_RA_GYRO_CONFIG_STATIC3, aafConfig.delt);
 250     spiWriteReg(dev, ICM426XX_RA_GYRO_CONFIG_STATIC4, aafConfig.deltSqr & 0xFF);
 251     spiWriteReg(dev, ICM426XX_RA_GYRO_CONFIG_STATIC5, (aafConfig.deltSqr >> 8) | (aafConfig.bitshift << 4));
 252
 253     // Configure acc Anti-Alias Filter for 1kHz sample rate (see tasks.c)
 254     aafConfig = getGyroAafConfig(gyroModel, AAF_CONFIG_258HZ);
 255     setUserBank(dev, ICM426XX_BANK_SELECT2);
 256     spiWriteReg(dev, ICM426XX_RA_ACCEL_CONFIG_STATIC2, aafConfig.delt << 1);
 257     spiWriteReg(dev, ICM426XX_RA_ACCEL_CONFIG_STATIC3, aafConfig.deltSqr & 0xFF);
 258     spiWriteReg(dev, ICM426XX_RA_ACCEL_CONFIG_STATIC4, (aafConfig.deltSqr >> 8) | (aafConfig.bitshift << 4));
 259
 260     // Configure gyro and acc UI Filters
 261     setUserBank(dev, ICM426XX_BANK_SELECT0);
 262     spiWriteReg(dev, ICM426XX_RA_GYRO_ACCEL_CONFIG0, ICM426XX_ACCEL_UI_FILT_BW_LOW_LATENCY | ICM426XX_GYRO_UI_FILT_BW_LOW_LATENCY);
 263
 264     // Configure interrupt pin
 265     spiWriteReg(dev, ICM426XX_RA_INT_CONFIG, ICM426XX_INT1_MODE_PULSED | ICM426XX_INT1_DRIVE_CIRCUIT_PP | ICM426XX_INT1_POLARITY_ACTIVE_HIGH);
 266     spiWriteReg(dev, ICM426XX_RA_INT_CONFIG0, ICM426XX_UI_DRDY_INT_CLEAR_ON_SBR);
 267
 268     spiWriteReg(dev, ICM426XX_RA_INT_SOURCE0, ICM426XX_UI_DRDY_INT1_EN_ENABLED);
 269
 270     uint8_t intConfig1Value = spiReadRegMsk(dev, ICM426XX_RA_INT_CONFIG1);
 271     // Datasheet says: "User should change setting to 0 from default setting of 1, for proper INT1 and INT2 pin operation"
 272     intConfig1Value &= ~(1 << ICM426XX_INT_ASYNC_RESET_BIT);
 273     intConfig1Value |= (ICM426XX_INT_TPULSE_DURATION_8 | ICM426XX_INT_TDEASSERT_DISABLED);
 274
 275     spiWriteReg(dev, ICM426XX_RA_INT_CONFIG1, intConfig1Value);
 276
 277     // Turn on gyro and acc on again so ODR and FSR can be configured
 278     turnGyroAccOn(dev);
 279
 280     // Get desired output data rate
 281     uint8_t odrConfig;
 282     const unsigned decim = llog2(gyro->mpuDividerDrops + 1);
 283     if (gyro->gyroRateKHz && decim < ODR_CONFIG_COUNT) {
 284         odrConfig = odrLUT[decim];
 285     } else {
 286         odrConfig = odrLUT[ODR_CONFIG_1K];
 287         gyro->gyroRateKHz = GYRO_RATE_1_kHz;
 288     }
 289
 290     STATIC_ASSERT(INV_FSR_2000DPS == 3, "INV_FSR_2000DPS must be 3 to generate correct value");
 291     spiWriteReg(dev, ICM426XX_RA_GYRO_CONFIG0, (3 - INV_FSR_2000DPS) << 5 | (odrConfig & 0x0F));
 292     delay(15);
 293
 294     STATIC_ASSERT(INV_FSR_16G == 3, "INV_FSR_16G must be 3 to generate correct value");
 295     spiWriteReg(dev, ICM426XX_RA_ACCEL_CONFIG0, (3 - INV_FSR_16G) << 5 | (odrConfig & 0x0F));
 296     delay(15);
 297 }
 298
 299 bool icm426xxSpiGyroDetect(gyroDev_t *gyro)
 300 {
 301     switch (gyro->mpuDetectionResult.sensor) {
 302     case ICM_42605_SPI:
 303         break;
 304     case ICM_42688P_SPI:
 305         break;
 306     default:
 307         return false;
 308     }
 309
 310     gyro->initFn = icm426xxGyroInit;
 311     gyro->readFn = mpuGyroReadSPI;
 312
 313     gyro->scale = GYRO_SCALE_2000DPS;
 314
 315     return true;
 316 }
 317
 318 static aafConfig_t getGyroAafConfig(const mpuSensor_e gyroModel, const aafConfig_e config)
 319 {
 320     switch (gyroModel){
 321     case ICM_42605_SPI:
 322         switch (config) {
 323         case GYRO_HARDWARE_LPF_NORMAL:
 324             return aafLUT42605[AAF_CONFIG_258HZ];
 325         case GYRO_HARDWARE_LPF_OPTION_1:
 326             return aafLUT42605[AAF_CONFIG_536HZ];
 327         case GYRO_HARDWARE_LPF_OPTION_2:
 328             return aafLUT42605[AAF_CONFIG_997HZ];
 329         default:
 330             return aafLUT42605[AAF_CONFIG_258HZ];
 331         }
 332
 333     case ICM_42688P_SPI:
 334     default:
 335         switch (config) {
 336         case GYRO_HARDWARE_LPF_NORMAL:
 337             return aafLUT42688[AAF_CONFIG_258HZ];
 338         case GYRO_HARDWARE_LPF_OPTION_1:
 339             return aafLUT42688[AAF_CONFIG_536HZ];
 340         case GYRO_HARDWARE_LPF_OPTION_2:
 341             return aafLUT42688[AAF_CONFIG_997HZ];
 342 #ifdef USE_GYRO_DLPF_EXPERIMENTAL
 343         case GYRO_HARDWARE_LPF_EXPERIMENTAL:
 344             return aafLUT42688[AAF_CONFIG_1962HZ];
 345 #endif
 346         default:
 347             return aafLUT42688[AAF_CONFIG_258HZ];
 348         }
 349     }
 350 }
 351
 352 #endif // USE_GYRO_SPI_ICM42605 || USE_GYRO_SPI_ICM42688P