From 4591e7cef8939d0d483c4dd4ea9b741e4f4bb4d0 Mon Sep 17 00:00:00 2001 From: iabdalkader Date: Sun, 28 Aug 2022 18:40:16 +0200 Subject: [PATCH] ports/stm32: Add support for LSM6DS3 I2C mode. --- src/omv/ports/stm32/modules/py_imu.c | 180 +++++++++++++++--------- src/omv/ports/stm32/stm32fxxx_hal_msp.c | 20 +++ 2 files changed, 132 insertions(+), 68 deletions(-) diff --git a/src/omv/ports/stm32/modules/py_imu.c b/src/omv/ports/stm32/modules/py_imu.c index 21581aba5..df4822c57 100644 --- a/src/omv/ports/stm32/modules/py_imu.c +++ b/src/omv/ports/stm32/modules/py_imu.c @@ -33,8 +33,8 @@ typedef union { } axis1bit16_t; -#define LSM_FUNC(f) lsm6ds3tr_c_##f -#define LSM_CONST(c) LSM6DS3TR_C_##c +#define LSM_FUNC(f) lsm6ds3tr_c_##f +#define LSM_CONST(c) LSM6DS3TR_C_##c #define lsm_from_fs8_to_mg lsm6ds3tr_c_from_fs8g_to_mg #define lsm_from_fs8_to_mg lsm6ds3tr_c_from_fs8g_to_mg @@ -63,30 +63,12 @@ typedef union { #define lsm_from_lsb_to_celsius lsm6dsox_from_lsb_to_celsius #else #error "imu chip variant is not defined." -#endif +#endif // IMU chip -STATIC int32_t platform_write(void *handle, uint8_t Reg, uint8_t *Bufp, - uint16_t len) -{ - HAL_GPIO_WritePin(IMU_SPI_SSEL_PORT, IMU_SPI_SSEL_PIN, GPIO_PIN_RESET); - HAL_SPI_Transmit(handle, &Reg, 1, HAL_MAX_DELAY); - HAL_SPI_Transmit(handle, Bufp, len, HAL_MAX_DELAY); - HAL_GPIO_WritePin(IMU_SPI_SSEL_PORT, IMU_SPI_SSEL_PIN, GPIO_PIN_SET); - return 0; -} +static bool imu_initialized = false; -STATIC int32_t platform_read(void *handle, uint8_t Reg, uint8_t *Bufp, - uint16_t len) -{ - Reg |= 0x80; - HAL_GPIO_WritePin(IMU_SPI_SSEL_PORT, IMU_SPI_SSEL_PIN, GPIO_PIN_RESET); - HAL_SPI_Transmit(handle, &Reg, 1, HAL_MAX_DELAY); - HAL_SPI_Receive(handle, Bufp, len, HAL_MAX_DELAY); - HAL_GPIO_WritePin(IMU_SPI_SSEL_PORT, IMU_SPI_SSEL_PIN, GPIO_PIN_SET); - return 0; -} - -STATIC SPI_HandleTypeDef SPIHandle = { +#if defined(IMU_SPI) +static SPI_HandleTypeDef imubus = { .Instance = IMU_SPI, .Init.Mode = SPI_MODE_MASTER, .Init.Direction = SPI_DIRECTION_2LINES, @@ -98,25 +80,86 @@ STATIC SPI_HandleTypeDef SPIHandle = { .Init.FirstBit = SPI_FIRSTBIT_MSB, }; -STATIC stmdev_ctx_t dev_ctx = { - .write_reg = platform_write, - .read_reg = platform_read, - .handle = &SPIHandle -}; - -STATIC bool test_not_ready() -{ - return HAL_SPI_GetState(&SPIHandle) != HAL_SPI_STATE_READY; +static void platform_init(void *imubus) { + HAL_SPI_DeInit(imubus); + HAL_SPI_Init(imubus); } -STATIC void error_on_not_ready() +static void platform_deinit(void *imubus) { + HAL_SPI_DeInit(imubus); + imu_initialized = false; +} + +static int32_t platform_write(void *imubus, uint8_t Reg, uint8_t *Bufp, uint16_t len) { - if (test_not_ready()) { + HAL_GPIO_WritePin(IMU_SPI_SSEL_PORT, IMU_SPI_SSEL_PIN, GPIO_PIN_RESET); + HAL_SPI_Transmit(imubus, &Reg, 1, HAL_MAX_DELAY); + HAL_SPI_Transmit(imubus, Bufp, len, HAL_MAX_DELAY); + HAL_GPIO_WritePin(IMU_SPI_SSEL_PORT, IMU_SPI_SSEL_PIN, GPIO_PIN_SET); + return 0; +} + +static int32_t platform_read(void *imubus, uint8_t Reg, uint8_t *Bufp, uint16_t len) +{ + Reg |= 0x80; + HAL_GPIO_WritePin(IMU_SPI_SSEL_PORT, IMU_SPI_SSEL_PIN, GPIO_PIN_RESET); + HAL_SPI_Transmit(imubus, &Reg, 1, HAL_MAX_DELAY); + HAL_SPI_Receive(imubus, Bufp, len, HAL_MAX_DELAY); + HAL_GPIO_WritePin(IMU_SPI_SSEL_PORT, IMU_SPI_SSEL_PIN, GPIO_PIN_SET); + return 0; +} +#elif defined(IMU_I2C) +static I2C_HandleTypeDef imubus = { + .Instance = IMU_I2C, + .Init.Timing = IMU_I2C_SPEED, + .Init.DualAddressMode = I2C_DUALADDRESS_DISABLED, + .Init.GeneralCallMode = I2C_GENERALCALL_DISABLED, + .Init.NoStretchMode = I2C_NOSTRETCH_DISABLED, + .Init.OwnAddress1 = 0xFE, + .Init.OwnAddress2 = 0xFE, + .Init.OwnAddress2Masks = 0, + .Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT, +}; + +static void platform_init(void *imubus) { + HAL_I2C_DeInit(imubus); + HAL_I2C_Init(imubus); +} + +static void platform_deinit(void *imubus) { + HAL_I2C_DeInit(imubus); + imu_initialized = false; +} + +static int32_t platform_write(void *imubus, uint8_t reg, uint8_t *bufp, uint16_t len) +{ + HAL_I2C_Mem_Write(imubus, LSM6DS3TR_C_I2C_ADD_L, reg, I2C_MEMADD_SIZE_8BIT, bufp, len, 1000); + return 0; +} + +static int32_t platform_read(void *imubus, 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); + return 0; +} +#else +#error "imu bus is not defined." +#endif + +static stmdev_ctx_t dev_ctx = { + .handle = &imubus, + .read_reg = platform_read, + .write_reg = platform_write, +}; + +static void error_on_not_ready() +{ + if (!imu_initialized) { mp_raise_msg(&mp_type_OSError, MP_ERROR_TEXT("IMU Not Ready!")); } } -STATIC mp_obj_t py_imu_tuple(float x, float y, float z) +static mp_obj_t py_imu_tuple(float x, float y, float z) { return mp_obj_new_tuple(3, (mp_obj_t [3]) {mp_obj_new_float(x), mp_obj_new_float(y), @@ -157,7 +200,7 @@ STATIC mp_obj_t py_imu_tuple(float x, float y, float z) #error "OMV_IMU_MOUNTING_Z_DIRECTION must be -1 or 1!" #endif -STATIC float py_imu_get_roll() +static float py_imu_get_roll() { axis3bit16_t data_raw_acceleration = {}; LSM_FUNC(acceleration_raw_get)(&dev_ctx, data_raw_acceleration.u8bit); @@ -181,7 +224,7 @@ STATIC float py_imu_get_roll() return fmodf((IM_RAD2DEG(fast_atan2f(-xr, yr)) + 180), 360); // rotate 180 } -STATIC float py_imu_get_pitch() +static float py_imu_get_pitch() { axis3bit16_t data_raw_acceleration = {}; LSM_FUNC(acceleration_raw_get)(&dev_ctx, data_raw_acceleration.u8bit); @@ -205,7 +248,7 @@ STATIC float py_imu_get_pitch() return IM_RAD2DEG(fast_atan2f(zr, -yr)); } -STATIC mp_obj_t py_imu_acceleration_mg() +static mp_obj_t py_imu_acceleration_mg() { error_on_not_ready(); @@ -215,9 +258,9 @@ STATIC mp_obj_t py_imu_acceleration_mg() lsm_from_fs8_to_mg(data_raw_acceleration.i16bit[1]), lsm_from_fs8_to_mg(data_raw_acceleration.i16bit[2])); } -STATIC MP_DEFINE_CONST_FUN_OBJ_0(py_imu_acceleration_mg_obj, py_imu_acceleration_mg); +static MP_DEFINE_CONST_FUN_OBJ_0(py_imu_acceleration_mg_obj, py_imu_acceleration_mg); -STATIC mp_obj_t py_imu_angular_rate_mdps() +static mp_obj_t py_imu_angular_rate_mdps() { error_on_not_ready(); @@ -227,9 +270,9 @@ STATIC mp_obj_t py_imu_angular_rate_mdps() lsm_from_fs2000_to_mdps(data_raw_angular_rate.i16bit[1]), lsm_from_fs2000_to_mdps(data_raw_angular_rate.i16bit[2])); } -STATIC MP_DEFINE_CONST_FUN_OBJ_0(py_imu_angular_rate_mdps_obj, py_imu_angular_rate_mdps); +static MP_DEFINE_CONST_FUN_OBJ_0(py_imu_angular_rate_mdps_obj, py_imu_angular_rate_mdps); -STATIC mp_obj_t py_imu_temperature_c() +static mp_obj_t py_imu_temperature_c() { error_on_not_ready(); @@ -237,25 +280,25 @@ STATIC mp_obj_t py_imu_temperature_c() LSM_FUNC(temperature_raw_get)(&dev_ctx, data_raw_temperature.u8bit); return mp_obj_new_float(LSM_FUNC(from_lsb_to_celsius)(data_raw_temperature.i16bit)); } -STATIC MP_DEFINE_CONST_FUN_OBJ_0(py_imu_temperature_c_obj, py_imu_temperature_c); +static MP_DEFINE_CONST_FUN_OBJ_0(py_imu_temperature_c_obj, py_imu_temperature_c); -STATIC mp_obj_t py_imu_roll() +static mp_obj_t py_imu_roll() { error_on_not_ready(); return mp_obj_new_float(py_imu_get_roll()); } -STATIC MP_DEFINE_CONST_FUN_OBJ_0(py_imu_roll_obj, py_imu_roll); +static MP_DEFINE_CONST_FUN_OBJ_0(py_imu_roll_obj, py_imu_roll); -STATIC mp_obj_t py_imu_pitch() +static mp_obj_t py_imu_pitch() { error_on_not_ready(); return mp_obj_new_float(py_imu_get_pitch()); } -STATIC MP_DEFINE_CONST_FUN_OBJ_0(py_imu_pitch_obj, py_imu_pitch); +static MP_DEFINE_CONST_FUN_OBJ_0(py_imu_pitch_obj, py_imu_pitch); -STATIC mp_obj_t py_imu_sleep(mp_obj_t enable) +static mp_obj_t py_imu_sleep(mp_obj_t enable) { error_on_not_ready(); @@ -264,9 +307,9 @@ STATIC mp_obj_t py_imu_sleep(mp_obj_t enable) LSM_FUNC(gy_data_rate_set)(&dev_ctx, en ? LSM_CONST(GY_ODR_OFF) : LSM_CONST(GY_ODR_52Hz)); return mp_const_none; } -STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_imu_sleep_obj, py_imu_sleep); +static MP_DEFINE_CONST_FUN_OBJ_1(py_imu_sleep_obj, py_imu_sleep); -STATIC mp_obj_t py_imu_write_reg(mp_obj_t addr, mp_obj_t val) +static mp_obj_t py_imu_write_reg(mp_obj_t addr, mp_obj_t val) { error_on_not_ready(); @@ -274,9 +317,9 @@ STATIC mp_obj_t py_imu_write_reg(mp_obj_t addr, mp_obj_t val) LSM_FUNC(write_reg)(&dev_ctx, mp_obj_get_int(addr), &v, sizeof(v)); return mp_const_none; } -STATIC MP_DEFINE_CONST_FUN_OBJ_2(py_imu_write_reg_obj, py_imu_write_reg); +static MP_DEFINE_CONST_FUN_OBJ_2(py_imu_write_reg_obj, py_imu_write_reg); -STATIC mp_obj_t py_imu_read_reg(mp_obj_t addr) +static mp_obj_t py_imu_read_reg(mp_obj_t addr) { error_on_not_ready(); @@ -284,9 +327,9 @@ STATIC mp_obj_t py_imu_read_reg(mp_obj_t addr) LSM_FUNC(read_reg)(&dev_ctx, mp_obj_get_int(addr), &v, sizeof(v)); return mp_obj_new_int(v); } -STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_imu_read_reg_obj, py_imu_read_reg); +static MP_DEFINE_CONST_FUN_OBJ_1(py_imu_read_reg_obj, py_imu_read_reg); -STATIC const mp_rom_map_elem_t globals_dict_table[] = { +static const mp_rom_map_elem_t globals_dict_table[] = { { MP_ROM_QSTR(MP_QSTR___name__), MP_OBJ_NEW_QSTR(MP_QSTR_imu) }, { MP_ROM_QSTR(MP_QSTR_acceleration_mg), MP_ROM_PTR(&py_imu_acceleration_mg_obj) }, { MP_ROM_QSTR(MP_QSTR_angular_rate_mdps), MP_ROM_PTR(&py_imu_angular_rate_mdps_obj) }, @@ -298,7 +341,7 @@ STATIC const mp_rom_map_elem_t globals_dict_table[] = { { MP_ROM_QSTR(MP_QSTR___read_reg), MP_ROM_PTR(&py_imu_read_reg_obj) }, }; -STATIC MP_DEFINE_CONST_DICT(globals_dict, globals_dict_table); +static MP_DEFINE_CONST_DICT(globals_dict, globals_dict_table); const mp_obj_module_t imu_module = { .base = { &mp_type_module }, @@ -307,9 +350,10 @@ const mp_obj_module_t imu_module = { void py_imu_init() { - HAL_SPI_Init(&SPIHandle); + uint8_t rst = 1; + uint8_t whoamI = 0; - uint8_t whoamI = 0, rst = 1; + platform_init(&imubus); // Try to read device id... for (int i = 0; (i < 10) && (whoamI != LSM_CONST(ID)); i++) { @@ -317,7 +361,7 @@ void py_imu_init() } if (whoamI != LSM_CONST(ID)) { - HAL_SPI_DeInit(&SPIHandle); + platform_deinit(&imubus); return; } @@ -328,7 +372,7 @@ void py_imu_init() } if (rst) { - HAL_SPI_DeInit(&SPIHandle); + platform_deinit(&imubus); return; } @@ -339,24 +383,24 @@ void py_imu_init() LSM_FUNC(xl_full_scale_set)(&dev_ctx, LSM_CONST(8g)); LSM_FUNC(gy_full_scale_set)(&dev_ctx, LSM_CONST(2000dps)); + + imu_initialized = true; } float py_imu_roll_rotation() { - if (test_not_ready()) { - return 0; // output when the camera is mounted upright + if (imu_initialized) { + return py_imu_get_roll(); } - - return py_imu_get_roll(); + return 0.0f; } float py_imu_pitch_rotation() { - if (test_not_ready()) { - return 0; // output when the camera is mounted upright + if (imu_initialized) { + return py_imu_get_pitch(); } - - return py_imu_get_pitch(); + return 0.0f; } MP_REGISTER_MODULE(MP_QSTR_imu, imu_module, MICROPY_PY_IMU); diff --git a/src/omv/ports/stm32/stm32fxxx_hal_msp.c b/src/omv/ports/stm32/stm32fxxx_hal_msp.c index b2cb16432..38439bc7b 100644 --- a/src/omv/ports/stm32/stm32fxxx_hal_msp.c +++ b/src/omv/ports/stm32/stm32fxxx_hal_msp.c @@ -305,6 +305,20 @@ void HAL_I2C_MspInit(I2C_HandleTypeDef *hi2c) GPIO_InitStructure.Pin = TOF_I2C_SDA_PIN; HAL_GPIO_Init(TOF_I2C_SDA_PORT, &GPIO_InitStructure); #endif + #if defined(IMU_I2C) + } else if (hi2c->Instance == IMU_I2C) { + /* Enable I2C clock */ + IMU_I2C_CLK_ENABLE(); + + GPIO_InitStructure.Speed = GPIO_SPEED_FREQ_VERY_HIGH; + GPIO_InitStructure.Alternate = IMU_I2C_AF; + + GPIO_InitStructure.Pin = IMU_I2C_SCL_PIN; + HAL_GPIO_Init(IMU_I2C_SCL_PORT, &GPIO_InitStructure); + + GPIO_InitStructure.Pin = IMU_I2C_SDA_PIN; + HAL_GPIO_Init(IMU_I2C_SDA_PORT, &GPIO_InitStructure); + #endif #if defined(ISC_I2C_ALT) } else if (hi2c->Instance == ISC_I2C_ALT) { /* Enable I2C clock */ @@ -340,6 +354,12 @@ void HAL_I2C_MspDeInit(I2C_HandleTypeDef *hi2c) TOF_I2C_RELEASE_RESET(); TOF_I2C_CLK_DISABLE(); #endif + #if defined(IMU_I2C) + } else if (hi2c->Instance == IMU_I2C) { + IMU_I2C_FORCE_RESET(); + IMU_I2C_RELEASE_RESET(); + IMU_I2C_CLK_DISABLE(); + #endif #if defined(ISC_I2C_ALT) } else if (hi2c->Instance == ISC_I2C_ALT) { ISC_I2C_ALT_FORCE_RESET();