Merge pull request #1709 from openmv/lsm6ds3_i2c_mode

ports/stm32: Add support for LSM6DS3 I2C mode.
This commit is contained in:
Ibrahim Abdelkader 2022-08-28 19:33:22 +02:00 committed by GitHub
commit 17d356fa3d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 132 additions and 68 deletions

View File

@ -63,30 +63,12 @@ typedef union {
#define lsm_from_lsb_to_celsius lsm6dsox_from_lsb_to_celsius #define lsm_from_lsb_to_celsius lsm6dsox_from_lsb_to_celsius
#else #else
#error "imu chip variant is not defined." #error "imu chip variant is not defined."
#endif #endif // IMU chip
STATIC int32_t platform_write(void *handle, uint8_t Reg, uint8_t *Bufp, static bool imu_initialized = false;
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 int32_t platform_read(void *handle, uint8_t Reg, uint8_t *Bufp, #if defined(IMU_SPI)
uint16_t len) static SPI_HandleTypeDef imubus = {
{
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 = {
.Instance = IMU_SPI, .Instance = IMU_SPI,
.Init.Mode = SPI_MODE_MASTER, .Init.Mode = SPI_MODE_MASTER,
.Init.Direction = SPI_DIRECTION_2LINES, .Init.Direction = SPI_DIRECTION_2LINES,
@ -98,25 +80,86 @@ STATIC SPI_HandleTypeDef SPIHandle = {
.Init.FirstBit = SPI_FIRSTBIT_MSB, .Init.FirstBit = SPI_FIRSTBIT_MSB,
}; };
STATIC stmdev_ctx_t dev_ctx = { static void platform_init(void *imubus) {
.write_reg = platform_write, HAL_SPI_DeInit(imubus);
.read_reg = platform_read, HAL_SPI_Init(imubus);
.handle = &SPIHandle
};
STATIC bool test_not_ready()
{
return HAL_SPI_GetState(&SPIHandle) != HAL_SPI_STATE_READY;
} }
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!")); 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), return mp_obj_new_tuple(3, (mp_obj_t [3]) {mp_obj_new_float(x),
mp_obj_new_float(y), 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!" #error "OMV_IMU_MOUNTING_Z_DIRECTION must be -1 or 1!"
#endif #endif
STATIC float py_imu_get_roll() static float py_imu_get_roll()
{ {
axis3bit16_t data_raw_acceleration = {}; axis3bit16_t data_raw_acceleration = {};
LSM_FUNC(acceleration_raw_get)(&dev_ctx, data_raw_acceleration.u8bit); 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 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 = {}; axis3bit16_t data_raw_acceleration = {};
LSM_FUNC(acceleration_raw_get)(&dev_ctx, data_raw_acceleration.u8bit); 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)); 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(); 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[1]),
lsm_from_fs8_to_mg(data_raw_acceleration.i16bit[2])); 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(); 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[1]),
lsm_from_fs2000_to_mdps(data_raw_angular_rate.i16bit[2])); 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(); 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); 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)); 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(); error_on_not_ready();
return mp_obj_new_float(py_imu_get_roll()); 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(); error_on_not_ready();
return mp_obj_new_float(py_imu_get_pitch()); 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(); 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)); LSM_FUNC(gy_data_rate_set)(&dev_ctx, en ? LSM_CONST(GY_ODR_OFF) : LSM_CONST(GY_ODR_52Hz));
return mp_const_none; 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(); 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)); LSM_FUNC(write_reg)(&dev_ctx, mp_obj_get_int(addr), &v, sizeof(v));
return mp_const_none; 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(); 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)); LSM_FUNC(read_reg)(&dev_ctx, mp_obj_get_int(addr), &v, sizeof(v));
return mp_obj_new_int(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___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_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) }, { 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) }, { 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 = { const mp_obj_module_t imu_module = {
.base = { &mp_type_module }, .base = { &mp_type_module },
@ -307,9 +350,10 @@ const mp_obj_module_t imu_module = {
void py_imu_init() 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... // 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++) {
@ -317,7 +361,7 @@ void py_imu_init()
} }
if (whoamI != LSM_CONST(ID)) { if (whoamI != LSM_CONST(ID)) {
HAL_SPI_DeInit(&SPIHandle); platform_deinit(&imubus);
return; return;
} }
@ -328,7 +372,7 @@ void py_imu_init()
} }
if (rst) { if (rst) {
HAL_SPI_DeInit(&SPIHandle); platform_deinit(&imubus);
return; return;
} }
@ -339,25 +383,25 @@ void py_imu_init()
LSM_FUNC(xl_full_scale_set)(&dev_ctx, LSM_CONST(8g)); LSM_FUNC(xl_full_scale_set)(&dev_ctx, LSM_CONST(8g));
LSM_FUNC(gy_full_scale_set)(&dev_ctx, LSM_CONST(2000dps)); LSM_FUNC(gy_full_scale_set)(&dev_ctx, LSM_CONST(2000dps));
imu_initialized = true;
} }
float py_imu_roll_rotation() float py_imu_roll_rotation()
{ {
if (test_not_ready()) { if (imu_initialized) {
return 0; // output when the camera is mounted upright
}
return py_imu_get_roll(); return py_imu_get_roll();
} }
return 0.0f;
}
float py_imu_pitch_rotation() float py_imu_pitch_rotation()
{ {
if (test_not_ready()) { if (imu_initialized) {
return 0; // output when the camera is mounted upright
}
return py_imu_get_pitch(); return py_imu_get_pitch();
} }
return 0.0f;
}
MP_REGISTER_MODULE(MP_QSTR_imu, imu_module, MICROPY_PY_IMU); MP_REGISTER_MODULE(MP_QSTR_imu, imu_module, MICROPY_PY_IMU);
#endif // MICROPY_PY_IMU #endif // MICROPY_PY_IMU

View File

@ -305,6 +305,20 @@ void HAL_I2C_MspInit(I2C_HandleTypeDef *hi2c)
GPIO_InitStructure.Pin = TOF_I2C_SDA_PIN; GPIO_InitStructure.Pin = TOF_I2C_SDA_PIN;
HAL_GPIO_Init(TOF_I2C_SDA_PORT, &GPIO_InitStructure); HAL_GPIO_Init(TOF_I2C_SDA_PORT, &GPIO_InitStructure);
#endif #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) #if defined(ISC_I2C_ALT)
} else if (hi2c->Instance == ISC_I2C_ALT) { } else if (hi2c->Instance == ISC_I2C_ALT) {
/* Enable I2C clock */ /* Enable I2C clock */
@ -340,6 +354,12 @@ void HAL_I2C_MspDeInit(I2C_HandleTypeDef *hi2c)
TOF_I2C_RELEASE_RESET(); TOF_I2C_RELEASE_RESET();
TOF_I2C_CLK_DISABLE(); TOF_I2C_CLK_DISABLE();
#endif #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) #if defined(ISC_I2C_ALT)
} else if (hi2c->Instance == ISC_I2C_ALT) { } else if (hi2c->Instance == ISC_I2C_ALT) {
ISC_I2C_ALT_FORCE_RESET(); ISC_I2C_ALT_FORCE_RESET();