src/main/drivers/accgyro/accgyro_icm42605.c

   1 /*
   2  * This file is part of INAV.
   3  *
   4  * INAV is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * INAV is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <stdlib.h>
  21
  22 #include "platform.h"
  23
  24 #include "build/debug.h"
  25
  26 #include "common/axis.h"
  27 #include "common/maths.h"
  28 #include "common/utils.h"
  29 #include "common/log.h"
  30
  31 #include "drivers/system.h"
  32 #include "drivers/time.h"
  33
  34 #include "drivers/sensor.h"
  35 #include "drivers/accgyro/accgyro.h"
  36 #include "drivers/accgyro/accgyro_mpu.h"
  37 #include "drivers/accgyro/accgyro_icm42605.h"
  38
  39 #if defined(USE_IMU_ICM42605)
  40
  41 #define ICM42605_RA_PWR_MGMT0                       0x4E
  42
  43 #define ICM42605_PWR_MGMT0_ACCEL_MODE_LN            (3 << 0)
  44 #define ICM42605_PWR_MGMT0_GYRO_MODE_LN             (3 << 2)
  45 #define ICM42605_PWR_MGMT0_TEMP_DISABLE_OFF         (0 << 5)
  46 #define ICM42605_PWR_MGMT0_TEMP_DISABLE_ON          (1 << 5)
  47
  48 #define ICM426XX_RA_REG_BANK_SEL                    0x76
  49 #define ICM426XX_BANK_SELECT0                       0x00
  50 #define ICM426XX_BANK_SELECT1                       0x01
  51 #define ICM426XX_BANK_SELECT2                       0x02
  52 #define ICM426XX_BANK_SELECT3                       0x03
  53 #define ICM426XX_BANK_SELECT4                       0x04
  54
  55 #define ICM42605_RA_GYRO_CONFIG0                    0x4F
  56 #define ICM42605_RA_ACCEL_CONFIG0                   0x50
  57
  58 #define ICM42605_RA_GYRO_ACCEL_CONFIG0              0x52
  59
  60 #define ICM42605_ACCEL_UI_FILT_BW_LOW_LATENCY       (15 << 4)
  61 #define ICM42605_GYRO_UI_FILT_BW_LOW_LATENCY        (15 << 0)
  62
  63 #define ICM42605_RA_GYRO_DATA_X1                    0x25
  64 #define ICM42605_RA_ACCEL_DATA_X1                   0x1F
  65
  66 #define ICM42605_RA_INT_CONFIG                      0x14
  67 #define ICM42605_INT1_MODE_PULSED                   (0 << 2)
  68 #define ICM42605_INT1_MODE_LATCHED                  (1 << 2)
  69 #define ICM42605_INT1_DRIVE_CIRCUIT_OD              (0 << 1)
  70 #define ICM42605_INT1_DRIVE_CIRCUIT_PP              (1 << 1)
  71 #define ICM42605_INT1_POLARITY_ACTIVE_LOW           (0 << 0)
  72 #define ICM42605_INT1_POLARITY_ACTIVE_HIGH          (1 << 0)
  73
  74 #define ICM42605_RA_INT_CONFIG0                     0x63
  75 #define ICM42605_UI_DRDY_INT_CLEAR_ON_SBR           ((0 << 5) || (0 << 4))
  76 #define ICM42605_UI_DRDY_INT_CLEAR_ON_SBR_DUPLICATE ((0 << 5) || (0 << 4)) // duplicate settings in datasheet, Rev 1.2.
  77 #define ICM42605_UI_DRDY_INT_CLEAR_ON_F1BR          ((1 << 5) || (0 << 4))
  78 #define ICM42605_UI_DRDY_INT_CLEAR_ON_SBR_AND_F1BR  ((1 << 5) || (1 << 4))
  79
  80 #define ICM42605_RA_INT_CONFIG1                     0x64
  81 #define ICM42605_INT_ASYNC_RESET_BIT                4
  82 #define ICM42605_INT_TDEASSERT_DISABLE_BIT          5
  83 #define ICM42605_INT_TDEASSERT_ENABLED              (0 << ICM42605_INT_TDEASSERT_DISABLE_BIT)
  84 #define ICM42605_INT_TDEASSERT_DISABLED             (1 << ICM42605_INT_TDEASSERT_DISABLE_BIT)
  85 #define ICM42605_INT_TPULSE_DURATION_BIT            6
  86 #define ICM42605_INT_TPULSE_DURATION_100            (0 << ICM42605_INT_TPULSE_DURATION_BIT)
  87 #define ICM42605_INT_TPULSE_DURATION_8              (1 << ICM42605_INT_TPULSE_DURATION_BIT)
  88
  89 #define ICM42605_RA_INT_SOURCE0                     0x65
  90 #define ICM42605_UI_DRDY_INT1_EN_DISABLED           (0 << 3)
  91 #define ICM42605_UI_DRDY_INT1_EN_ENABLED            (1 << 3)
  92
  93 #define ICM42605_INTF_CONFIG1                       0x4D
  94 #define ICM42605_INTF_CONFIG1_AFSR_MASK             0xC0
  95 #define ICM42605_INTF_CONFIG1_AFSR_DISABLE          0x40
  96
  97 // --- Registers for gyro and acc Anti-Alias Filter ---------
  98 #define ICM426XX_RA_GYRO_CONFIG_STATIC3             0x0C  // User Bank 1
  99 #define ICM426XX_RA_GYRO_CONFIG_STATIC4             0x0D  // User Bank 1
 100 #define ICM426XX_RA_GYRO_CONFIG_STATIC5             0x0E  // User Bank 1
 101 #define ICM426XX_RA_ACCEL_CONFIG_STATIC2            0x03  // User Bank 2
 102 #define ICM426XX_RA_ACCEL_CONFIG_STATIC3            0x04  // User Bank 2
 103 #define ICM426XX_RA_ACCEL_CONFIG_STATIC4            0x05  // User Bank 2
 104
 105 static bool is42688P = false;
 106
 107 typedef struct aafConfig_s {
 108     uint16_t freq;
 109     uint8_t delt;
 110     uint16_t deltSqr;
 111     uint8_t bitshift;
 112 } aafConfig_t;
 113
 114 // Possible gyro Anti-Alias Filter (AAF) cutoffs for ICM-42688P
 115 static aafConfig_t aafLUT42688[] = {  // see table in section 5.3 https://invensense.tdk.com/wp-content/uploads/2020/04/ds-000347_icm-42688-p-datasheet.pdf
 116     // freq, delt, deltSqr, bitshift
 117     { 42,  1,    1, 15 },
 118     { 84,  2,    4, 13 },
 119     {126,  3,    9, 12 },
 120     {170,  4,   16, 11 },
 121     {213,  5,   25, 10 },
 122     {258,  6,   36, 10 },
 123     {303,  7,   49,  9 },
 124     {536, 12,  144,  8 },
 125     {997, 21,  440,  6 },
 126     {1962, 37, 1376,  4 },
 127     { 0, 0, 0, 0}, // 42HZ
 128 };
 129
 130 // Possible gyro Anti-Alias Filter (AAF) cutoffs for ICM-42688P
 131 static aafConfig_t aafLUT42605[] = {  // see table in section 5.3 https://invensense.tdk.com/wp-content/uploads/2022/09/DS-000292-ICM-42605-v1.7.pdf
 132     // freq, delt, deltSqr, bitshift
 133     {  10,  1,    1, 15 },
 134     {  21,  2,    4, 13 },
 135     {  32,  3,    9, 12 },
 136     {  42,  4,   16, 11 },
 137     {  99,  9,   81,  9 },
 138     { 171, 15,  224,  7 },
 139     { 184, 16,  256,  7 },
 140     { 196, 17,  288,  7 },
 141     { 249, 21,  440,  6 },
 142     { 524, 39, 1536,  4 },
 143     { 995, 63, 3968,  3 },
 144     {   0,  0,    0,  0 }
 145 };
 146
 147 static const aafConfig_t *getGyroAafConfig(bool is42688, const uint16_t desiredLpf);
 148
 149 static void setUserBank(const busDevice_t *dev, const uint8_t user_bank)
 150 {
 151     busWrite(dev, ICM426XX_RA_REG_BANK_SEL, user_bank & 7);
 152 }
 153
 154 static void icm42605AccInit(accDev_t *acc)
 155 {
 156     acc->acc_1G = 512 * 4;
 157 }
 158
 159 static bool icm42605AccRead(accDev_t *acc)
 160 {
 161     uint8_t data[6];
 162
 163     const bool ack = busReadBuf(acc->busDev, ICM42605_RA_ACCEL_DATA_X1, data, 6);
 164     if (!ack) {
 165         return false;
 166     }
 167
 168     acc->ADCRaw[X] = (float) int16_val_big_endian(data, 0);
 169     acc->ADCRaw[Y] = (float) int16_val_big_endian(data, 1);
 170     acc->ADCRaw[Z] = (float) int16_val_big_endian(data, 2);
 171
 172     return true;
 173 }
 174
 175 bool icm42605AccDetect(accDev_t *acc)
 176 {
 177     acc->busDev = busDeviceOpen(BUSTYPE_ANY, DEVHW_ICM42605, acc->imuSensorToUse);
 178     if (acc->busDev == NULL) {
 179         return false;
 180     }
 181
 182     mpuContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev);
 183     if (ctx->chipMagicNumber != 0x4265) {
 184         return false;
 185     }
 186
 187     acc->initFn = icm42605AccInit;
 188     acc->readFn = icm42605AccRead;
 189     acc->accAlign = acc->busDev->param;
 190
 191     return true;
 192 }
 193
 194 static const gyroFilterAndRateConfig_t icm42605GyroConfigs[] = {
 195     /*                            DLPF  ODR */
 196     { GYRO_LPF_256HZ,   8000,   { 6,    3  } }, /* 400 Hz LPF */
 197     { GYRO_LPF_256HZ,   4000,   { 5,    4  } }, /* 250 Hz LPF */
 198     { GYRO_LPF_256HZ,   2000,   { 3,    5  } }, /* 250 Hz LPF */
 199     { GYRO_LPF_256HZ,   1000,   { 1,    6  } }, /* 250 Hz LPF */
 200     { GYRO_LPF_256HZ,    500,   { 0,    15 } }, /* 250 Hz LPF */
 201 };
 202
 203 static void icm42605AccAndGyroInit(gyroDev_t *gyro)
 204 {
 205     busDevice_t * dev = gyro->busDev;
 206     const gyroFilterAndRateConfig_t * config = chooseGyroConfig(gyro->lpf, 1000000 / gyro->requestedSampleIntervalUs,
 207                                                                 &icm42605GyroConfigs[0], ARRAYLEN(icm42605GyroConfigs));
 208     gyro->sampleRateIntervalUs = 1000000 / config->gyroRateHz;
 209
 210     busSetSpeed(dev, BUS_SPEED_INITIALIZATION);
 211
 212     setUserBank(dev, ICM426XX_BANK_SELECT0);
 213     busWrite(dev, ICM42605_RA_PWR_MGMT0, ICM42605_PWR_MGMT0_TEMP_DISABLE_OFF | ICM42605_PWR_MGMT0_ACCEL_MODE_LN | ICM42605_PWR_MGMT0_GYRO_MODE_LN);
 214     delay(15);
 215
 216     /* ODR and dynamic range */
 217     busWrite(dev, ICM42605_RA_GYRO_CONFIG0, (0x00) << 5 | (config->gyroConfigValues[1] & 0x0F));    /* 2000 deg/s */
 218     delay(15);
 219
 220     busWrite(dev, ICM42605_RA_ACCEL_CONFIG0, (0x00) << 5 | (config->gyroConfigValues[1] & 0x0F));    /* 16 G deg/s */
 221     delay(15);
 222
 223     /* LPF bandwidth */
 224     // low latency, same as BF
 225     busWrite(dev, ICM42605_RA_GYRO_ACCEL_CONFIG0, ICM42605_ACCEL_UI_FILT_BW_LOW_LATENCY | ICM42605_GYRO_UI_FILT_BW_LOW_LATENCY);
 226     delay(15);
 227
 228     if (gyro->lpf != GYRO_LPF_NONE) {
 229         // Configure gyro Anti-Alias Filter (see section 5.3 "ANTI-ALIAS FILTER")
 230         const aafConfig_t *aafConfig = getGyroAafConfig(is42688P, gyro->lpf);
 231
 232         setUserBank(dev, ICM426XX_BANK_SELECT1);
 233         busWrite(dev, ICM426XX_RA_GYRO_CONFIG_STATIC3, aafConfig->delt);
 234         busWrite(dev, ICM426XX_RA_GYRO_CONFIG_STATIC4, aafConfig->deltSqr & 0xFF);
 235         busWrite(dev, ICM426XX_RA_GYRO_CONFIG_STATIC5, (aafConfig->deltSqr >> 8) | (aafConfig->bitshift << 4));
 236
 237         aafConfig = getGyroAafConfig(is42688P, 256);  // This was hard coded on BF
 238         setUserBank(dev, ICM426XX_BANK_SELECT2);
 239         busWrite(dev, ICM426XX_RA_ACCEL_CONFIG_STATIC2, aafConfig->delt << 1);
 240         busWrite(dev, ICM426XX_RA_ACCEL_CONFIG_STATIC3, aafConfig->deltSqr & 0xFF);
 241         busWrite(dev, ICM426XX_RA_ACCEL_CONFIG_STATIC4, (aafConfig->deltSqr >> 8) | (aafConfig->bitshift << 4));
 242     }
 243
 244     setUserBank(dev, ICM426XX_BANK_SELECT0);
 245     busWrite(dev, ICM42605_RA_INT_CONFIG, ICM42605_INT1_MODE_PULSED | ICM42605_INT1_DRIVE_CIRCUIT_PP | ICM42605_INT1_POLARITY_ACTIVE_HIGH);
 246     delay(15);
 247
 248     busWrite(dev, ICM42605_RA_INT_CONFIG0, ICM42605_UI_DRDY_INT_CLEAR_ON_SBR);
 249     delay(100);
 250
 251     uint8_t intConfig1Value;
 252
 253     busWrite(dev, ICM42605_RA_INT_SOURCE0, ICM42605_UI_DRDY_INT1_EN_ENABLED);
 254
 255     // Datasheet says: "User should change setting to 0 from default setting of 1, for proper INT1 and INT2 pin operation"
 256     busRead(dev, ICM42605_RA_INT_CONFIG1, &intConfig1Value);
 257
 258     intConfig1Value &= ~(1 << ICM42605_INT_ASYNC_RESET_BIT);
 259     intConfig1Value |= (ICM42605_INT_TPULSE_DURATION_8 | ICM42605_INT_TDEASSERT_DISABLED);
 260
 261     busWrite(dev, ICM42605_RA_INT_CONFIG1, intConfig1Value);
 262     delay(15);
 263
 264     //Disable AFSR as in BF and Ardupilot
 265     uint8_t intfConfig1Value;
 266     busRead(dev, ICM42605_INTF_CONFIG1, &intfConfig1Value);
 267     intfConfig1Value &= ~ICM42605_INTF_CONFIG1_AFSR_MASK;
 268     intfConfig1Value |= ICM42605_INTF_CONFIG1_AFSR_DISABLE;
 269     busWrite(dev, ICM42605_INTF_CONFIG1, intfConfig1Value);
 270
 271     delay(15);
 272
 273     busSetSpeed(dev, BUS_SPEED_FAST);
 274 }
 275
 276 static bool icm42605DeviceDetect(busDevice_t * dev)
 277 {
 278     uint8_t tmp;
 279     uint8_t attemptsRemaining = 5;
 280
 281     busSetSpeed(dev, BUS_SPEED_INITIALIZATION);
 282
 283     busWrite(dev, ICM42605_RA_PWR_MGMT0, 0x00);
 284
 285     do {
 286         delay(150);
 287
 288         busRead(dev, MPU_RA_WHO_AM_I, &tmp);
 289
 290         switch (tmp) {
 291             /* ICM42605 and ICM42688P share the register structure*/
 292             case ICM42605_WHO_AM_I_CONST:
 293                 is42688P = false;
 294                 return true;
 295             case ICM42688P_WHO_AM_I_CONST:
 296                 is42688P = true;
 297                 return true;
 298
 299             default:
 300                 // Retry detection
 301                 break;
 302         }
 303     } while (attemptsRemaining--);
 304
 305     return false;
 306 }
 307
 308 static bool icm42605GyroRead(gyroDev_t *gyro)
 309 {
 310     uint8_t data[6];
 311
 312     const bool ack = busReadBuf(gyro->busDev, ICM42605_RA_GYRO_DATA_X1, data, 6);
 313     if (!ack) {
 314         return false;
 315     }
 316
 317     gyro->gyroADCRaw[X] = (float) int16_val_big_endian(data, 0);
 318     gyro->gyroADCRaw[Y] = (float) int16_val_big_endian(data, 1);
 319     gyro->gyroADCRaw[Z] = (float) int16_val_big_endian(data, 2);
 320
 321     return true;
 322 }
 323
 324 bool icm42605GyroDetect(gyroDev_t *gyro)
 325 {
 326     gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_ICM42605, gyro->imuSensorToUse, OWNER_MPU);
 327     if (gyro->busDev == NULL) {
 328         return false;
 329     }
 330
 331     if (!icm42605DeviceDetect(gyro->busDev)) {
 332         busDeviceDeInit(gyro->busDev);
 333         return false;
 334     }
 335
 336     // Magic number for ACC detection to indicate that we have detected icm42605 gyro
 337     mpuContextData_t * ctx = busDeviceGetScratchpadMemory(gyro->busDev);
 338     ctx->chipMagicNumber = 0x4265;
 339
 340     gyro->initFn = icm42605AccAndGyroInit;
 341     gyro->readFn = icm42605GyroRead;
 342     gyro->intStatusFn = gyroCheckDataReady;
 343     gyro->temperatureFn = NULL;
 344     gyro->scale = 1.0f / 16.4f;     // 16.4 dps/lsb scalefactor
 345     gyro->gyroAlign = gyro->busDev->param;
 346
 347     return true;
 348 }
 349
 350 static uint16_t getAafFreq(const uint8_t gyroLpf)
 351 {
 352     switch (gyroLpf) {
 353         default:
 354         case GYRO_LPF_256HZ:
 355             return 256;
 356         case GYRO_LPF_188HZ:
 357             return 188;
 358         case GYRO_LPF_98HZ:
 359             return 98;
 360         case GYRO_LPF_42HZ:
 361             return 42;
 362         case GYRO_LPF_20HZ:
 363             return 20;
 364         case GYRO_LPF_10HZ:
 365             return 10;
 366         case GYRO_LPF_5HZ:
 367             return 5;
 368         case GYRO_LPF_NONE:
 369             return 0;
 370     }
 371 }
 372
 373 static const aafConfig_t *getGyroAafConfig(bool is42688, const uint16_t desiredLpf)
 374 {
 375     uint16_t desiredFreq = getAafFreq(desiredLpf);
 376     const aafConfig_t *aafConfigs = NULL;
 377     if (is42688) {
 378         aafConfigs = aafLUT42688;
 379     } else {
 380         aafConfigs = aafLUT42605;
 381     }
 382     int i;
 383     int8_t selectedFreq = aafConfigs[0].freq;
 384     const aafConfig_t * candidate = &aafConfigs[0];
 385
 386     // Choose closest supported LPF value
 387     for (i = 1; aafConfigs[i].freq != 0; i++) {
 388         if (ABS(desiredFreq - aafConfigs[i].freq) < ABS(desiredFreq - selectedFreq)) {
 389             selectedFreq = aafConfigs[i].freq;
 390             candidate = &aafConfigs[i];
 391         }
 392     }
 393
 394     LOG_VERBOSE(GYRO, "ICM426%s AAF CONFIG { %d, %d } -> { %d }; delt: %d deltSqr: %d, shift: %d",
 395                 (is42688P ? "88" : "05"),
 396                 desiredLpf, desiredFreq,
 397                 candidate->freq,
 398                 candidate->delt, candidate->deltSqr, candidate->bitshift);
 399
 400     return candidate;
 401 }
 402
 403 #endif