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 #include "drivers/pwm_output.h"
  46
  47 #include "sensors/gyro.h"
  48 #include "pg/gyrodev.h"
  49
  50 // Allows frequency to be set from the compile line EXTRA_FLAGS by adding e.g.
  51 // -D'ICM426XX_CLOCK=12000000'. If using the configurator this simply becomes
  52 // ICM426XX_CLOCK=12000000 in the custom settings text box.
  53 #ifndef ICM426XX_CLOCK
  54 // Default: 24 MHz max SPI frequency
  55 #define ICM426XX_MAX_SPI_CLK_HZ 24000000
  56 #else
  57 // Use the supplied value
  58 #define ICM426XX_MAX_SPI_CLK_HZ ICM426XX_CLOCK
  59 #endif
  60
  61 #define ICM426XX_CLKIN_FREQ                         32000
  62
  63 #define ICM426XX_RA_REG_BANK_SEL                    0x76
  64 #define ICM426XX_BANK_SELECT0                       0x00
  65 #define ICM426XX_BANK_SELECT1                       0x01
  66 #define ICM426XX_BANK_SELECT2                       0x02
  67 #define ICM426XX_BANK_SELECT3                       0x03
  68 #define ICM426XX_BANK_SELECT4                       0x04
  69
  70 // Fix for stalls in gyro output. See https://github.com/ArduPilot/ardupilot/pull/25332
  71 #define ICM426XX_INTF_CONFIG1                       0x4D
  72 #define ICM426XX_INTF_CONFIG1_AFSR_MASK             0xC0
  73 #define ICM426XX_INTF_CONFIG1_AFSR_DISABLE          0x40
  74
  75 #define ICM426XX_RA_PWR_MGMT0                       0x4E  // User Bank 0
  76 #define ICM426XX_PWR_MGMT0_ACCEL_MODE_LN            (3 << 0)
  77 #define ICM426XX_PWR_MGMT0_GYRO_MODE_LN             (3 << 2)
  78 #define ICM426XX_PWR_MGMT0_GYRO_ACCEL_MODE_OFF      ((0 << 0) | (0 << 2))
  79 #define ICM426XX_PWR_MGMT0_TEMP_DISABLE_OFF         (0 << 5)
  80 #define ICM426XX_PWR_MGMT0_TEMP_DISABLE_ON          (1 << 5)
  81
  82 #define ICM426XX_RA_GYRO_CONFIG0                    0x4F
  83 #define ICM426XX_RA_ACCEL_CONFIG0                   0x50
  84
  85 // --- Registers for gyro and acc Anti-Alias Filter ---------
  86 #define ICM426XX_RA_GYRO_CONFIG_STATIC3             0x0C  // User Bank 1
  87 #define ICM426XX_RA_GYRO_CONFIG_STATIC4             0x0D  // User Bank 1
  88 #define ICM426XX_RA_GYRO_CONFIG_STATIC5             0x0E  // User Bank 1
  89 #define ICM426XX_RA_ACCEL_CONFIG_STATIC2            0x03  // User Bank 2
  90 #define ICM426XX_RA_ACCEL_CONFIG_STATIC3            0x04  // User Bank 2
  91 #define ICM426XX_RA_ACCEL_CONFIG_STATIC4            0x05  // User Bank 2
  92 // --- Register & setting for gyro and acc UI Filter --------
  93 #define ICM426XX_RA_GYRO_ACCEL_CONFIG0              0x52  // User Bank 0
  94 #define ICM426XX_ACCEL_UI_FILT_BW_LOW_LATENCY       (15 << 4)
  95 #define ICM426XX_GYRO_UI_FILT_BW_LOW_LATENCY        (15 << 0)
  96 // ----------------------------------------------------------
  97
  98 #define ICM426XX_RA_GYRO_DATA_X1                    0x25  // User Bank 0
  99 #define ICM426XX_RA_ACCEL_DATA_X1                   0x1F  // User Bank 0
 100
 101 #define ICM426XX_RA_INT_CONFIG                      0x14  // User Bank 0
 102 #define ICM426XX_INT1_MODE_PULSED                   (0 << 2)
 103 #define ICM426XX_INT1_MODE_LATCHED                  (1 << 2)
 104 #define ICM426XX_INT1_DRIVE_CIRCUIT_OD              (0 << 1)
 105 #define ICM426XX_INT1_DRIVE_CIRCUIT_PP              (1 << 1)
 106 #define ICM426XX_INT1_POLARITY_ACTIVE_LOW           (0 << 0)
 107 #define ICM426XX_INT1_POLARITY_ACTIVE_HIGH          (1 << 0)
 108
 109 #define ICM426XX_RA_INT_CONFIG0                     0x63  // User Bank 0
 110 #define ICM426XX_UI_DRDY_INT_CLEAR_ON_SBR           ((0 << 5) || (0 << 4))
 111 #define ICM426XX_UI_DRDY_INT_CLEAR_ON_SBR_DUPLICATE ((0 << 5) || (0 << 4)) // duplicate settings in datasheet, Rev 1.2.
 112 #define ICM426XX_UI_DRDY_INT_CLEAR_ON_F1BR          ((1 << 5) || (0 << 4))
 113 #define ICM426XX_UI_DRDY_INT_CLEAR_ON_SBR_AND_F1BR  ((1 << 5) || (1 << 4))
 114
 115 #define ICM426XX_RA_INT_CONFIG1                     0x64   // User Bank 0
 116 #define ICM426XX_INT_ASYNC_RESET_BIT                4
 117 #define ICM426XX_INT_TDEASSERT_DISABLE_BIT          5
 118 #define ICM426XX_INT_TDEASSERT_ENABLED              (0 << ICM426XX_INT_TDEASSERT_DISABLE_BIT)
 119 #define ICM426XX_INT_TDEASSERT_DISABLED             (1 << ICM426XX_INT_TDEASSERT_DISABLE_BIT)
 120 #define ICM426XX_INT_TPULSE_DURATION_BIT            6
 121 #define ICM426XX_INT_TPULSE_DURATION_100            (0 << ICM426XX_INT_TPULSE_DURATION_BIT)
 122 #define ICM426XX_INT_TPULSE_DURATION_8              (1 << ICM426XX_INT_TPULSE_DURATION_BIT)
 123
 124 #define ICM426XX_RA_INT_SOURCE0                     0x65  // User Bank 0
 125 #define ICM426XX_UI_DRDY_INT1_EN_DISABLED           (0 << 3)
 126 #define ICM426XX_UI_DRDY_INT1_EN_ENABLED            (1 << 3)
 127
 128 // specific to CLKIN configuration
 129 #define ICM426XX_INTF_CONFIG5                       0x7B  // User Bank 1
 130 #define ICM426XX_INTF_CONFIG1_CLKIN                 (1 << 2)
 131 #define ICM426XX_INTF_CONFIG5_PIN9_FUNCTION_MASK    (3 << 1)   // PIN9 mode config
 132 #define ICM426XX_INTF_CONFIG5_PIN9_FUNCTION_CLKIN   (2 << 1)   // PIN9 as CLKIN
 133
 134 typedef enum {
 135     ODR_CONFIG_8K = 0,
 136     ODR_CONFIG_4K,
 137     ODR_CONFIG_2K,
 138     ODR_CONFIG_1K,
 139     ODR_CONFIG_COUNT
 140 } odrConfig_e;
 141
 142 typedef enum {
 143     AAF_CONFIG_258HZ = 0,
 144     AAF_CONFIG_536HZ,
 145     AAF_CONFIG_997HZ,
 146     AAF_CONFIG_1962HZ,
 147     AAF_CONFIG_COUNT
 148 } aafConfig_e;
 149
 150 typedef struct aafConfig_s {
 151     uint8_t delt;
 152     uint16_t deltSqr;
 153     uint8_t bitshift;
 154 } aafConfig_t;
 155
 156 // Possible output data rates (ODRs)
 157 static uint8_t odrLUT[ODR_CONFIG_COUNT] = {  // see GYRO_ODR in section 5.6
 158     [ODR_CONFIG_8K] = 3,
 159     [ODR_CONFIG_4K] = 4,
 160     [ODR_CONFIG_2K] = 5,
 161     [ODR_CONFIG_1K] = 6,
 162 };
 163
 164 // Possible gyro Anti-Alias Filter (AAF) cutoffs for ICM-42688P
 165 static aafConfig_t aafLUT42688[AAF_CONFIG_COUNT] = {  // see table in section 5.3
 166     [AAF_CONFIG_258HZ]  = {  6,   36, 10 },
 167     [AAF_CONFIG_536HZ]  = { 12,  144,  8 },
 168     [AAF_CONFIG_997HZ]  = { 21,  440,  6 },
 169     [AAF_CONFIG_1962HZ] = { 37, 1376,  4 },
 170 };
 171
 172 // Possible gyro Anti-Alias Filter (AAF) cutoffs for ICM-42688P
 173 // actual cutoff differs slightly from those of the 42688P
 174 static aafConfig_t aafLUT42605[AAF_CONFIG_COUNT] = {  // see table in section 5.3
 175     [AAF_CONFIG_258HZ]  = { 21,  440,  6 }, // actually 249 Hz
 176     [AAF_CONFIG_536HZ]  = { 39, 1536,  4 }, // actually 524 Hz
 177     [AAF_CONFIG_997HZ]  = { 63, 3968,  3 }, // actually 995 Hz
 178     [AAF_CONFIG_1962HZ] = { 63, 3968,  3 }, // 995 Hz is the max cutoff on the 42605
 179 };
 180
 181 static void setUserBank(const extDevice_t *dev, const uint8_t user_bank)
 182 {
 183     spiWriteReg(dev, ICM426XX_RA_REG_BANK_SEL, user_bank & 7);
 184 }
 185
 186 #if defined(USE_GYRO_CLKIN)
 187 static pwmOutputPort_t pwmGyroClk = {0};
 188
 189 static bool initExternalClock(const extDevice_t *dev)
 190 {
 191     int cfg;
 192     if (&gyro.gyroSensor1.gyroDev.dev == dev) {
 193         cfg = 0;
 194     } else if (&gyro.gyroSensor2.gyroDev.dev == dev) {
 195         cfg = 1;
 196     } else {
 197         // only gyroSensor<n> device supported
 198         return false;
 199     }
 200     const ioTag_t tag = gyroDeviceConfig(cfg)->clkIn;
 201     const IO_t io = IOGetByTag(tag);
 202     if (pwmGyroClk.enabled) {
 203        // pwm is already taken, but test for shared clkIn pin
 204        return pwmGyroClk.io == io;
 205     }
 206
 207     const timerHardware_t *timer = timerAllocate(tag, OWNER_GYRO_CLKIN, RESOURCE_INDEX(cfg));
 208     if (!timer) {
 209         // Error handling: failed to allocate timer
 210         return false;
 211     }
 212
 213     pwmGyroClk.io = io;
 214     pwmGyroClk.enabled = true;
 215
 216     IOInit(io, OWNER_GYRO_CLKIN, RESOURCE_INDEX(cfg));
 217     IOConfigGPIOAF(io, IOCFG_AF_PP, timer->alternateFunction);
 218
 219     const uint32_t clock = timerClock(timer->tim);  // Get the timer clock frequency
 220     const uint16_t period = clock / ICM426XX_CLKIN_FREQ;
 221
 222     // Calculate duty cycle value for 50%
 223     const uint16_t value = period / 2;
 224
 225     // Configure PWM output
 226     pwmOutConfig(&pwmGyroClk.channel, timer, clock, period - 1, value - 1, 0);
 227
 228     // Set CCR value
 229     *pwmGyroClk.channel.ccr = value - 1;
 230
 231     return true;
 232 }
 233
 234 static void icm426xxEnableExternalClock(const extDevice_t *dev)
 235 {
 236     if (initExternalClock(dev)) {
 237         // Switch to Bank 1 and set bits 2:1 in INTF_CONFIG5 (0x7B) to enable CLKIN on PIN9
 238         setUserBank(dev, ICM426XX_BANK_SELECT1);
 239         uint8_t intf_config5 = spiReadRegMsk(dev, ICM426XX_INTF_CONFIG5);
 240         intf_config5 = (intf_config5 & ~ICM426XX_INTF_CONFIG5_PIN9_FUNCTION_MASK) | ICM426XX_INTF_CONFIG5_PIN9_FUNCTION_CLKIN;  // Clear & set bits 2:1 to 0b10 for CLKIN
 241         spiWriteReg(dev, ICM426XX_INTF_CONFIG5, intf_config5);
 242
 243         // Switch to Bank 0 and set bit 2 in RTC_MODE (0x4D) to enable external CLK signal
 244         setUserBank(dev, ICM426XX_BANK_SELECT0);
 245         uint8_t rtc_mode = spiReadRegMsk(dev, ICM426XX_INTF_CONFIG1);
 246         rtc_mode |= ICM426XX_INTF_CONFIG1_CLKIN; // Enable external CLK signal
 247         spiWriteReg(dev, ICM426XX_INTF_CONFIG1, rtc_mode);
 248     }
 249 }
 250 #endif
 251
 252 uint8_t icm426xxSpiDetect(const extDevice_t *dev)
 253 {
 254     spiWriteReg(dev, ICM426XX_RA_PWR_MGMT0, 0x00);
 255
 256 #if defined(USE_GYRO_CLKIN)
 257     icm426xxEnableExternalClock(dev);
 258 #endif
 259
 260     uint8_t icmDetected = MPU_NONE;
 261     uint8_t attemptsRemaining = 20;
 262     do {
 263         delay(150);
 264         const uint8_t whoAmI = spiReadRegMsk(dev, MPU_RA_WHO_AM_I);
 265         switch (whoAmI) {
 266         case ICM42605_WHO_AM_I_CONST:
 267             icmDetected = ICM_42605_SPI;
 268             break;
 269         case ICM42688P_WHO_AM_I_CONST:
 270             icmDetected = ICM_42688P_SPI;
 271             break;
 272         default:
 273             icmDetected = MPU_NONE;
 274             break;
 275         }
 276         if (icmDetected != MPU_NONE) {
 277             break;
 278         }
 279         if (!attemptsRemaining) {
 280             return MPU_NONE;
 281         }
 282     } while (attemptsRemaining--);
 283
 284     return icmDetected;
 285 }
 286
 287 void icm426xxAccInit(accDev_t *acc)
 288 {
 289     acc->acc_1G = 512 * 4;
 290 }
 291
 292 bool icm426xxSpiAccDetect(accDev_t *acc)
 293 {
 294     switch (acc->mpuDetectionResult.sensor) {
 295     case ICM_42605_SPI:
 296         break;
 297     case ICM_42688P_SPI:
 298         break;
 299     default:
 300         return false;
 301     }
 302
 303     acc->initFn = icm426xxAccInit;
 304     acc->readFn = mpuAccReadSPI;
 305
 306     return true;
 307 }
 308
 309 static aafConfig_t getGyroAafConfig(const mpuSensor_e, const aafConfig_e);
 310
 311 static void turnGyroAccOff(const extDevice_t *dev)
 312 {
 313     spiWriteReg(dev, ICM426XX_RA_PWR_MGMT0, ICM426XX_PWR_MGMT0_GYRO_ACCEL_MODE_OFF);
 314 }
 315
 316 // Turn on gyro and acc on in Low Noise mode
 317 static void turnGyroAccOn(const extDevice_t *dev)
 318 {
 319     spiWriteReg(dev, ICM426XX_RA_PWR_MGMT0, ICM426XX_PWR_MGMT0_TEMP_DISABLE_OFF | ICM426XX_PWR_MGMT0_ACCEL_MODE_LN | ICM426XX_PWR_MGMT0_GYRO_MODE_LN);
 320     delay(1);
 321 }
 322
 323 void icm426xxGyroInit(gyroDev_t *gyro)
 324 {
 325     const extDevice_t *dev = &gyro->dev;
 326
 327     spiSetClkDivisor(dev, spiCalculateDivider(ICM426XX_MAX_SPI_CLK_HZ));
 328
 329     mpuGyroInit(gyro);
 330     gyro->accDataReg = ICM426XX_RA_ACCEL_DATA_X1;
 331     gyro->gyroDataReg = ICM426XX_RA_GYRO_DATA_X1;
 332
 333     // Turn off ACC and GYRO so they can be configured
 334     // See section 12.9 in ICM-42688-P datasheet v1.7
 335     setUserBank(dev, ICM426XX_BANK_SELECT0);
 336     turnGyroAccOff(dev);
 337
 338     // Configure gyro Anti-Alias Filter (see section 5.3 "ANTI-ALIAS FILTER")
 339     const mpuSensor_e gyroModel = gyro->mpuDetectionResult.sensor;
 340     aafConfig_t aafConfig = getGyroAafConfig(gyroModel, gyroConfig()->gyro_hardware_lpf);
 341     setUserBank(dev, ICM426XX_BANK_SELECT1);
 342     spiWriteReg(dev, ICM426XX_RA_GYRO_CONFIG_STATIC3, aafConfig.delt);
 343     spiWriteReg(dev, ICM426XX_RA_GYRO_CONFIG_STATIC4, aafConfig.deltSqr & 0xFF);
 344     spiWriteReg(dev, ICM426XX_RA_GYRO_CONFIG_STATIC5, (aafConfig.deltSqr >> 8) | (aafConfig.bitshift << 4));
 345
 346     // Configure acc Anti-Alias Filter for 1kHz sample rate (see tasks.c)
 347     aafConfig = getGyroAafConfig(gyroModel, AAF_CONFIG_258HZ);
 348     setUserBank(dev, ICM426XX_BANK_SELECT2);
 349     spiWriteReg(dev, ICM426XX_RA_ACCEL_CONFIG_STATIC2, aafConfig.delt << 1);
 350     spiWriteReg(dev, ICM426XX_RA_ACCEL_CONFIG_STATIC3, aafConfig.deltSqr & 0xFF);
 351     spiWriteReg(dev, ICM426XX_RA_ACCEL_CONFIG_STATIC4, (aafConfig.deltSqr >> 8) | (aafConfig.bitshift << 4));
 352
 353     // Configure gyro and acc UI Filters
 354     setUserBank(dev, ICM426XX_BANK_SELECT0);
 355     spiWriteReg(dev, ICM426XX_RA_GYRO_ACCEL_CONFIG0, ICM426XX_ACCEL_UI_FILT_BW_LOW_LATENCY | ICM426XX_GYRO_UI_FILT_BW_LOW_LATENCY);
 356
 357     // Configure interrupt pin
 358     spiWriteReg(dev, ICM426XX_RA_INT_CONFIG, ICM426XX_INT1_MODE_PULSED | ICM426XX_INT1_DRIVE_CIRCUIT_PP | ICM426XX_INT1_POLARITY_ACTIVE_HIGH);
 359     spiWriteReg(dev, ICM426XX_RA_INT_CONFIG0, ICM426XX_UI_DRDY_INT_CLEAR_ON_SBR);
 360
 361     spiWriteReg(dev, ICM426XX_RA_INT_SOURCE0, ICM426XX_UI_DRDY_INT1_EN_ENABLED);
 362
 363     uint8_t intConfig1Value = spiReadRegMsk(dev, ICM426XX_RA_INT_CONFIG1);
 364     // Datasheet says: "User should change setting to 0 from default setting of 1, for proper INT1 and INT2 pin operation"
 365     intConfig1Value &= ~(1 << ICM426XX_INT_ASYNC_RESET_BIT);
 366     intConfig1Value |= (ICM426XX_INT_TPULSE_DURATION_8 | ICM426XX_INT_TDEASSERT_DISABLED);
 367
 368     spiWriteReg(dev, ICM426XX_RA_INT_CONFIG1, intConfig1Value);
 369
 370     // Disable AFSR to prevent stalls in gyro output
 371     uint8_t intfConfig1Value = spiReadRegMsk(dev, ICM426XX_INTF_CONFIG1);
 372     intfConfig1Value &= ~ICM426XX_INTF_CONFIG1_AFSR_MASK;
 373     intfConfig1Value |= ICM426XX_INTF_CONFIG1_AFSR_DISABLE;
 374     spiWriteReg(dev, ICM426XX_INTF_CONFIG1, intfConfig1Value);
 375
 376     // Turn on gyro and acc on again so ODR and FSR can be configured
 377     turnGyroAccOn(dev);
 378
 379     // Get desired output data rate
 380     uint8_t odrConfig;
 381     const unsigned decim = llog2(gyro->mpuDividerDrops + 1);
 382     if (gyro->gyroRateKHz && decim < ODR_CONFIG_COUNT) {
 383         odrConfig = odrLUT[decim];
 384     } else {
 385         odrConfig = odrLUT[ODR_CONFIG_1K];
 386         gyro->gyroRateKHz = GYRO_RATE_1_kHz;
 387     }
 388
 389     STATIC_ASSERT(INV_FSR_2000DPS == 3, "INV_FSR_2000DPS must be 3 to generate correct value");
 390     spiWriteReg(dev, ICM426XX_RA_GYRO_CONFIG0, (3 - INV_FSR_2000DPS) << 5 | (odrConfig & 0x0F));
 391     delay(15);
 392
 393     STATIC_ASSERT(INV_FSR_16G == 3, "INV_FSR_16G must be 3 to generate correct value");
 394     spiWriteReg(dev, ICM426XX_RA_ACCEL_CONFIG0, (3 - INV_FSR_16G) << 5 | (odrConfig & 0x0F));
 395     delay(15);
 396 }
 397
 398 bool icm426xxSpiGyroDetect(gyroDev_t *gyro)
 399 {
 400     switch (gyro->mpuDetectionResult.sensor) {
 401     case ICM_42605_SPI:
 402         break;
 403     case ICM_42688P_SPI:
 404         break;
 405     default:
 406         return false;
 407     }
 408
 409     gyro->initFn = icm426xxGyroInit;
 410     gyro->readFn = mpuGyroReadSPI;
 411
 412     gyro->scale = GYRO_SCALE_2000DPS;
 413
 414     return true;
 415 }
 416
 417 static aafConfig_t getGyroAafConfig(const mpuSensor_e gyroModel, const aafConfig_e config)
 418 {
 419     switch (gyroModel){
 420     case ICM_42605_SPI:
 421         switch (config) {
 422         case GYRO_HARDWARE_LPF_NORMAL:
 423             return aafLUT42605[AAF_CONFIG_258HZ];
 424         case GYRO_HARDWARE_LPF_OPTION_1:
 425             return aafLUT42605[AAF_CONFIG_536HZ];
 426         case GYRO_HARDWARE_LPF_OPTION_2:
 427             return aafLUT42605[AAF_CONFIG_997HZ];
 428         default:
 429             return aafLUT42605[AAF_CONFIG_258HZ];
 430         }
 431
 432     case ICM_42688P_SPI:
 433     default:
 434         switch (config) {
 435         case GYRO_HARDWARE_LPF_NORMAL:
 436             return aafLUT42688[AAF_CONFIG_258HZ];
 437         case GYRO_HARDWARE_LPF_OPTION_1:
 438             return aafLUT42688[AAF_CONFIG_536HZ];
 439         case GYRO_HARDWARE_LPF_OPTION_2:
 440             return aafLUT42688[AAF_CONFIG_997HZ];
 441 #ifdef USE_GYRO_DLPF_EXPERIMENTAL
 442         case GYRO_HARDWARE_LPF_EXPERIMENTAL:
 443             return aafLUT42688[AAF_CONFIG_1962HZ];
 444 #endif
 445         default:
 446             return aafLUT42688[AAF_CONFIG_258HZ];
 447         }
 448     }
 449 }
 450
 451 #endif // USE_GYRO_SPI_ICM42605 || USE_GYRO_SPI_ICM42688P