diff --git a/src/sensor/imu/ICM45686.c b/src/sensor/imu/ICM45686.c index e8a757d8..c9c6194a 100644 --- a/src/sensor/imu/ICM45686.c +++ b/src/sensor/imu/ICM45686.c @@ -439,6 +439,148 @@ int icm45_ext_passthrough(bool passthrough) return 0; } +int icm45_ext_setup() { + int err = 0; + err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_IMU, ICM45686_IOC_PAD_SCENARIO_AUX_OVRD, 0x17); // AUX1_MODE_OVRD, AUX1 in I2CM Master, AUX1_ENABLE_OVRD, AUX1 enabled + sensor_interface_ext_configure(&sensor_ext_icm45686); + return err; +} + +int icm45_bank_write(const uint8_t bank, const uint8_t reg, const uint8_t *buf, uint32_t num_bytes) +{ + if (num_bytes == 0) + { + LOG_ERR("Invalid bank write size"); + return -1; + } + + int err = 0; + uint8_t ireg_buf[3]; + ireg_buf[0] = bank; + ireg_buf[1] = reg; + ireg_buf[2] = buf[0]; + err |= ssi_burst_write(SENSOR_INTERFACE_DEV_IMU, ICM45686_IREG_ADDR_15_8, ireg_buf, 3); + k_usleep(4); + + for (uint32_t i = 1; i < num_bytes; i++) + { + err |= ssi_reg_write_byte(SENSOR_INTERFACE_DEV_IMU, ICM45686_IREG_DATA, buf[i]); + k_usleep(4); + } + + return err; +} + +int icm45_bank_write_byte(const uint8_t bank, const uint8_t reg, const uint8_t value) +{ + return icm45_bank_write(bank, reg, &value, 1); +} + +int icm45_bank_read(const uint8_t bank, const uint8_t reg, uint8_t *buf, uint32_t num_bytes) +{ + if (num_bytes == 0) + { + LOG_ERR("Invalid bank read size"); + return -1; + } + + int err = 0; + uint8_t ireg_buf[2]; + ireg_buf[0] = bank; + ireg_buf[1] = reg; + err |= ssi_burst_write(SENSOR_INTERFACE_DEV_IMU, ICM45686_IREG_ADDR_15_8, ireg_buf, 2); + k_usleep(4); + + for (uint32_t i = 0; i < num_bytes; i++) + { + err |= ssi_reg_read_byte(SENSOR_INTERFACE_DEV_IMU, ICM45686_IREG_DATA, &buf[i]); + k_usleep(4); + } + return err; +} + +int icm45_bank_read_byte(const uint8_t bank, const uint8_t reg, uint8_t *value) { + return icm45_bank_read(bank, reg, value, 1); +} + +int icm45_ext_write(const uint8_t addr, const uint8_t *buf, uint32_t num_bytes) +{ + if (num_bytes > 6) + { + LOG_ERR("Unsupported write"); + return -1; + } + + int err = 0; + + err |= icm45_bank_write(ICM45686_IPREG_TOP1, ICM45686_I2CM_WR_DATA_0, buf, num_bytes); + err |= icm45_bank_write_byte(ICM45686_IPREG_TOP1, ICM45686_I2CM_COMMAND_0, 0x80 + num_bytes); // Last transaction, channel 0, write num_bytes bytes + err |= icm45_bank_write_byte(ICM45686_IPREG_TOP1, ICM45686_I2CM_CONTROL, 0x01); // No restarts, fast mode, start transaction + + uint8_t last_status = 0; + err |= icm45_bank_read_byte(ICM45686_IPREG_TOP1, ICM45686_I2CM_STATUS, &last_status); + while (last_status & 0x01) // I2CM busy + { + err |= icm45_bank_read_byte(ICM45686_IPREG_TOP1, ICM45686_I2CM_STATUS, &last_status); + } + + if (last_status != 0x02) // Not (just) "done" + { + LOG_ERR("I2CM error: %02x", last_status); + return -1; + } + + uint8_t dev_status; + err |= icm45_bank_read_byte(ICM45686_IPREG_TOP1, ICM45686_I2CM_EXT_DEV_STATUS, &dev_status); + + if (dev_status & 0x01) { + // Maybe log the nack? + return -1; + } + + return err; +} + +int icm45_ext_write_read(const uint8_t addr, const void *write_buf, size_t num_write, void *read_buf, size_t num_read) { + if (num_write != 1 || num_read < 1 || num_read > 15) + { + LOG_ERR("Unsupported write_read"); + return -1; + } + + int err = 0; + + uint8_t dev_profile_data[2] = {((const uint8_t *)write_buf)[0], addr}; + err |= icm45_bank_write(ICM45686_IPREG_TOP1, ICM45686_DEV_PROFILE_0, dev_profile_data, 2); + err |= icm45_bank_write_byte(ICM45686_IPREG_TOP1, ICM45686_I2CM_COMMAND_0, 0x90 + num_read); // Last transaction, channel 0, read num_read bytes with register specified + err |= icm45_bank_write_byte(ICM45686_IPREG_TOP1, ICM45686_I2CM_CONTROL, 0x01); // No restarts, fast mode, start transaction + + uint8_t last_status = 0; + err |= icm45_bank_read_byte(ICM45686_IPREG_TOP1, ICM45686_I2CM_STATUS, &last_status); + while (last_status & 0x01) // I2CM busy + { + err |= icm45_bank_read_byte(ICM45686_IPREG_TOP1, ICM45686_I2CM_STATUS, &last_status); + } + + if (last_status != 0x02) // Not (just) "done" + { + LOG_ERR("I2CM error: %02x", last_status); + return -1; + } + + uint8_t dev_status; + err |= icm45_bank_read_byte(ICM45686_IPREG_TOP1, ICM45686_I2CM_EXT_DEV_STATUS, &dev_status); + + if (dev_status & 0x01) { + // Maybe log the nack? + return -1; + } + + err |= icm45_bank_read(ICM45686_IPREG_TOP1, ICM45686_I2CM_RD_DATA_0, read_buf, num_read); + + return err; +} + const sensor_imu_t sensor_imu_icm45686 = { *icm45_init, *icm45_shutdown, @@ -454,7 +596,13 @@ const sensor_imu_t sensor_imu_icm45686 = { *icm45_setup_DRDY, *icm45_setup_WOM, - - *imu_none_ext_setup, + + *icm45_ext_setup, *icm45_ext_passthrough }; + +const sensor_ext_ssi_t sensor_ext_icm45686 = { + *icm45_ext_write, + *icm45_ext_write_read, + 15 +}; diff --git a/src/sensor/imu/ICM45686.h b/src/sensor/imu/ICM45686.h index d4f85cf5..abc4a740 100644 --- a/src/sensor/imu/ICM45686.h +++ b/src/sensor/imu/ICM45686.h @@ -53,6 +53,15 @@ // User Bank IPREG_TOP1 #define ICM45686_IPREG_TOP1 0xA2 // MSB +#define ICM45686_I2CM_COMMAND_0 0x06 +#define ICM45686_DEV_PROFILE_0 0x0e +#define ICM45686_DEV_PROFILE_1 0x0f +#define ICM45686_I2CM_CONTROL 0x16 +#define ICM45686_I2CM_STATUS 0x18 +#define ICM45686_I2CM_EXT_DEV_STATUS 0x1a +#define ICM45686_I2CM_RD_DATA_0 0x1b +#define ICM45686_I2CM_WR_DATA_0 0x33 + #define ICM45686_SMC_CONTROL_0 0x58 #define ICM45686_SREG_CTRL 0x67 diff --git a/src/sensor/interface.c b/src/sensor/interface.c index 59274df5..f3292cd8 100644 --- a/src/sensor/interface.c +++ b/src/sensor/interface.c @@ -143,6 +143,9 @@ int ssi_write(enum sensor_interface_dev dev, const uint8_t *buf, uint32_t num_by #endif case SENSOR_INTERFACE_SPEC_I2C: return i2c_write_dt(sensor_interface_dev_i2c[dev], buf, num_bytes); + case SENSOR_INTERFACE_SPEC_EXT: + if (ext_ssi != NULL) + return ext_ssi->ext_write(ext_addr, buf, num_bytes); default: return -1; } @@ -179,9 +182,6 @@ int ssi_read(enum sensor_interface_dev dev, uint8_t *buf, uint32_t num_bytes) #endif case SENSOR_INTERFACE_SPEC_I2C: return i2c_read_dt(sensor_interface_dev_i2c[dev], buf, num_bytes); - case SENSOR_INTERFACE_SPEC_EXT: - if (ext_ssi != NULL) - return ext_ssi->ext_write(ext_addr, buf, num_bytes); default: return -1; }