mirror of
https://github.com/openmv/openmv.git
synced 2025-11-04 14:49:50 +08:00
Merge pull request #1709 from openmv/lsm6ds3_i2c_mode
ports/stm32: Add support for LSM6DS3 I2C mode.
This commit is contained in:
commit
17d356fa3d
@ -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);
|
||||
|
||||
@ -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();
|
||||
|
||||
Loading…
Reference in New Issue
Block a user