ICM45686 Driver.cpp

//////////////////////////// Bank read methods
uint8_t AP_InertialSensor_Invensensev3::register_read_bank_icm456xy(uint16_t bank_addr, uint16_t reg)
{
    // combine addr
    uint16_t addr = bank_addr + reg;

    uint8_t send[] = {INV3REG_456_IREG_ADDRH, (uint8_t)(addr >> 8), (uint8_t)(addr & 0xFF)};

    // set indirect register address
    dev->transfer(send, sizeof(send), nullptr, 0);

    // try reading IREG_DATA on ready
    for (uint8_t i=0; i<10; i++) {
        if (register_read(INV3REG_456_REG_MISC2) & 0x01) {
            break;
        }
        // minimum wait time-gap between IREG access is 4us
        hal.scheduler->delay_microseconds(10);
    }

    // read the data
    return register_read(INV3REG_456_IREG_DATA);
}

void AP_InertialSensor_Invensensev3::register_write_bank_icm456xy(uint16_t bank_addr, uint16_t reg, uint8_t val)
{
    // combine addr
    uint16_t addr = bank_addr + reg;

    uint8_t send[] = {INV3REG_456_IREG_ADDRH, (uint8_t)(addr >> 8), (uint8_t)(addr & 0xFF), val};

    // set indirect register address
    dev->transfer(send, sizeof(send), nullptr, 0);

    //check if IREG_DATA ready, we can return immediately if so
    if (register_read(INV3REG_456_REG_MISC2) & 0x01) {
        return;
    }
    // minimum wait time-gap between IREG access is 4us
    hal.scheduler->delay_microseconds(10);
}



////////////////////////////////////////////////////  Hardware Initialisation
        // do soft reset
        register_write(INV3REG_456_REG_MISC2, 0x02);
        hal.scheduler->delay_microseconds(1000);
        // check if reset done
        if (!(register_read(INV3REG_456_INT1_STATUS0) & 0x80)) {
            // failed to reset
            return false;
        }
        // turn off aux1
        register_write(INV3REG_456_PWR_MGMT_AUX1, 0x3);

        // gyro and accel in low-noise mode
        register_write(INV3REG_456_PWR_MGMT0, 0x0f);

#ifdef ICM45686_CLKIN
        /*************************CLKIN setting*************************/
        // override INT2 pad as CLKIN, AUX1 disabled
        register_write(INV3REG_456_IOC_PAD_SCENARIO_OVRD, (0x1 << 2)| 0x2 , true);

        // disable AUX1
        register_write(INV3REG_456_IOC_PAD_SCENARIO_AUX_OVRD, (0x1<<1U), true);

        // enable RTC MODE
        register_write(INV3REG_456_RTC_CONFIG, (0x1<<5U));
#endif
        /*************************CLKIN setting*************************/
        // disable STC
        uint8_t reg = register_read_bank_icm456xy(INV3BANK_456_IPREG_TOP1_ADDR, 0x68);  // I3C_STC_MODE b2
        register_write_bank_icm456xy(INV3BANK_456_IPREG_TOP1_ADDR, 0x68, reg & ~0x04);



//////////////////////////// ODR Initialisation
    // Disable FIFO first
    register_write(INV3REG_456_FIFO_CONFIG3, 0x00);
    register_write(INV3REG_456_FIFO_CONFIG0, 0x07);

    // setup gyro for 1.6-6.4kHz, 4000dps range
    register_write(INV3REG_456_GYRO_CONFIG0, (0x0 << 4) | odr_config); // GYRO_UI_FS_SEL b4-7, GYRO_ODR b0-3

    // setup accel for 1.6-6.4kHz, 32g range
    register_write(INV3REG_456_ACCEL_CONFIG0, (0x0 << 4) | odr_config); // ACCEL_UI_FS_SEL b4-6, ACCEL_ODR b0-3

    // enable timestamps on FIFO data 
    // SMC_CONTROL_0
    uint8_t reg = register_read_bank_icm456xy(INV3BANK_456_IPREG_TOP1_ADDR, 0x58);
#ifdef ICM45686_CLKIN
    reg |= (0x1<<4U); // ACCEL_LP_CLK_SEL
#endif
    register_write_bank_icm456xy(INV3BANK_456_IPREG_TOP1_ADDR, 0x58, reg | 0x01);

    // enable FIFO for each sensor
    register_write(INV3REG_456_FIFO_CONFIG3, 0x06, true);

    // FIFO stop-on-full, disable bypass and 2K FIFO
    register_write(INV3REG_456_FIFO_CONFIG0, 0x87, true);

    // enable Interpolator and Anti Aliasing Filter on Gyro
    reg = register_read_bank_icm456xy(INV3BANK_456_IPREG_SYS1_ADDR, 0xA6);  // GYRO_SRC_CTRL b5-6
    register_write_bank_icm456xy(INV3BANK_456_IPREG_SYS1_ADDR, 0xA6, (reg & ~(0x3 << 5)) | (0x2 << 5));

    // enable Interpolator and Anti Aliasing Filter on accel
    reg = register_read_bank_icm456xy(INV3BANK_456_IPREG_SYS2_ADDR, 0x7B); // ACCEL_SRC_CTRL b0-1
    register_write_bank_icm456xy(INV3BANK_456_IPREG_SYS2_ADDR, 0x7B, (reg & ~0x3) | 0x2);

    // enable FIFO
    register_write(INV3REG_456_FIFO_CONFIG3, 0x07, true);