mirror of
https://github.com/openmv/openmv.git
synced 2025-09-26 23:09:13 +08:00
modules/py_imu: Fix I2C mode.
This commit is contained in:
parent
fb3958a86e
commit
59a1db0088
@ -111,9 +111,9 @@ static bool imu_initialized = false;
|
|||||||
#define IMU_SPI_BUS_TIMEOUT (5000)
|
#define IMU_SPI_BUS_TIMEOUT (5000)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
static omv_spi_t imubus;
|
static omv_spi_t imu_bus;
|
||||||
|
|
||||||
static void platform_init(void *imubus) {
|
static void platform_init(void *imu_bus) {
|
||||||
omv_spi_config_t spi_config;
|
omv_spi_config_t spi_config;
|
||||||
omv_spi_default_config(&spi_config, OMV_IMU_SPI_ID);
|
omv_spi_default_config(&spi_config, OMV_IMU_SPI_ID);
|
||||||
|
|
||||||
@ -122,16 +122,16 @@ static void platform_init(void *imubus) {
|
|||||||
spi_config.clk_pha = OMV_SPI_CPHA_2EDGE;
|
spi_config.clk_pha = OMV_SPI_CPHA_2EDGE;
|
||||||
spi_config.nss_enable = false; // Soft NSS
|
spi_config.nss_enable = false; // Soft NSS
|
||||||
|
|
||||||
omv_spi_init(imubus, &spi_config);
|
omv_spi_init(imu_bus, &spi_config);
|
||||||
}
|
}
|
||||||
|
|
||||||
static void platform_deinit(void *imubus) {
|
static void platform_deinit(void *imu_bus) {
|
||||||
omv_spi_deinit(imubus);
|
omv_spi_deinit(imu_bus);
|
||||||
imu_initialized = false;
|
imu_initialized = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
static int32_t platform_write(void *imubus, uint8_t Reg, const uint8_t *Bufp, uint16_t len) {
|
static int32_t platform_write(void *imu_bus, uint8_t Reg, const uint8_t *Bufp, uint16_t len) {
|
||||||
omv_spi_t *spi_bus = imubus;
|
omv_spi_t *spi_bus = imu_bus;
|
||||||
|
|
||||||
omv_spi_transfer_t spi_xfer = {
|
omv_spi_transfer_t spi_xfer = {
|
||||||
.timeout = IMU_SPI_BUS_TIMEOUT,
|
.timeout = IMU_SPI_BUS_TIMEOUT,
|
||||||
@ -155,8 +155,8 @@ static int32_t platform_write(void *imubus, uint8_t Reg, const uint8_t *Bufp, ui
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
static int32_t platform_read(void *imubus, uint8_t Reg, uint8_t *Bufp, uint16_t len) {
|
static int32_t platform_read(void *imu_bus, uint8_t Reg, uint8_t *Bufp, uint16_t len) {
|
||||||
omv_spi_t *spi_bus = imubus;
|
omv_spi_t *spi_bus = imu_bus;
|
||||||
|
|
||||||
Reg |= 0x80;
|
Reg |= 0x80;
|
||||||
omv_spi_transfer_t spi_xfer = {
|
omv_spi_transfer_t spi_xfer = {
|
||||||
@ -180,34 +180,37 @@ static int32_t platform_read(void *imubus, uint8_t Reg, uint8_t *Bufp, uint16_t
|
|||||||
omv_gpio_write(spi_bus->cs, 1);
|
omv_gpio_write(spi_bus->cs, 1);
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
#elif defined(IMU_I2C)
|
#elif defined(OMV_IMU_I2C_ID)
|
||||||
static omv_i2c_t imu_bus = {};
|
static omv_i2c_t imu_bus = {};
|
||||||
|
|
||||||
static void platform_init(void *imubus) {
|
static void platform_init(void *imu_bus) {
|
||||||
omv_i2c_init(&imu_bus, OMV_IMU_I2C_ID, OMV_I2C_SPEED_FULL);
|
omv_i2c_init(imu_bus, OMV_IMU_I2C_ID, OMV_IMU_I2C_SPEED);
|
||||||
}
|
}
|
||||||
|
|
||||||
static void platform_deinit(void *imubus) {
|
static void platform_deinit(void *imu_bus) {
|
||||||
omv_i2c_deinit(&imu_bus);
|
omv_i2c_deinit(imu_bus);
|
||||||
imu_initialized = false;
|
imu_initialized = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
static int32_t platform_write(void *imubus, uint8_t reg, const uint8_t *bufp, uint16_t len) {
|
static int32_t platform_write(void *imu_bus, uint8_t reg, const uint8_t *bufp, uint16_t len) {
|
||||||
if (omv_i2c_write_bytes(&imu_bus, LSM6DS3TR_C_I2C_ADD_L, (uint8_t *) ®, 1, OMV_I2C_XFER_SUSPEND) != 0) {
|
omv_i2c_t *i2c_bus = imu_bus;
|
||||||
|
|
||||||
|
if (omv_i2c_write_bytes(i2c_bus, LSM6DSM_I2C_ADD_L, (uint8_t *) ®, 1, OMV_I2C_XFER_SUSPEND) != 0) {
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
if (omv_i2c_write_bytes(&imu_bus, LSM6DS3TR_C_I2C_ADD_L, (uint8_t *) bufp, len, OMV_I2C_XFER_NO_FLAGS) != 0) {
|
if (omv_i2c_write_bytes(i2c_bus, LSM6DSM_I2C_ADD_L, (uint8_t *) bufp, len, OMV_I2C_XFER_NO_FLAGS) != 0) {
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
static int32_t platform_read(void *imubus, uint8_t reg, uint8_t *bufp, uint16_t len) {
|
static int32_t platform_read(void *imu_bus, uint8_t reg, uint8_t *bufp, uint16_t len) {
|
||||||
HAL_I2C_Mem_Read(imubus, LSM6DS3TR_C_I2C_ADD_L, reg, I2C_MEMADD_SIZE_8BIT, bufp, len, 1000);
|
omv_i2c_t *i2c_bus = imu_bus;
|
||||||
if (omv_i2c_write_bytes(&imu_bus, LSM6DS3TR_C_I2C_ADD_L, (uint8_t *) ®, 1, OMV_I2C_XFER_NO_STOP) != 0) {
|
|
||||||
|
if (omv_i2c_write_bytes(i2c_bus, LSM6DSM_I2C_ADD_L, (uint8_t *) ®, 1, OMV_I2C_XFER_NO_STOP) != 0) {
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
if (omv_i2c_read_bytes(&imu_bus, LSM6DS3TR_C_I2C_ADD_L, bufp, len, OMV_I2C_XFER_NO_FLAGS) != 0) {
|
if (omv_i2c_read_bytes(i2c_bus, LSM6DSM_I2C_ADD_L, bufp, len, OMV_I2C_XFER_NO_FLAGS) != 0) {
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
return 0;
|
return 0;
|
||||||
@ -217,7 +220,7 @@ static int32_t platform_read(void *imubus, uint8_t reg, uint8_t *bufp, uint16_t
|
|||||||
#endif
|
#endif
|
||||||
|
|
||||||
static stmdev_ctx_t dev_ctx = {
|
static stmdev_ctx_t dev_ctx = {
|
||||||
.handle = &imubus,
|
.handle = &imu_bus,
|
||||||
.read_reg = platform_read,
|
.read_reg = platform_read,
|
||||||
.write_reg = platform_write,
|
.write_reg = platform_write,
|
||||||
};
|
};
|
||||||
@ -411,7 +414,7 @@ void py_imu_init() {
|
|||||||
uint8_t rst = 1;
|
uint8_t rst = 1;
|
||||||
uint8_t whoamI = 0;
|
uint8_t whoamI = 0;
|
||||||
|
|
||||||
platform_init(&imubus);
|
platform_init(&imu_bus);
|
||||||
|
|
||||||
// Try to read device id...
|
// Try to read device id...
|
||||||
for (int i = 0; (i < 10) && (whoamI != LSM_CONST(ID)); i++) {
|
for (int i = 0; (i < 10) && (whoamI != LSM_CONST(ID)); i++) {
|
||||||
@ -420,7 +423,7 @@ void py_imu_init() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
if (whoamI != LSM_CONST(ID)) {
|
if (whoamI != LSM_CONST(ID)) {
|
||||||
platform_deinit(&imubus);
|
platform_deinit(&imu_bus);
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -431,7 +434,7 @@ void py_imu_init() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
if (rst) {
|
if (rst) {
|
||||||
platform_deinit(&imubus);
|
platform_deinit(&imu_bus);
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user