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

@ -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);

View File

@ -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();