Merge pull request #706 from kwagyeman/kwabena/just_imu_driver

Add IMU driver
This commit is contained in:
Ibrahim Abd Elkader 2020-02-11 14:31:24 +02:00 committed by GitHub
commit 913fd49649
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
12 changed files with 16686 additions and 0 deletions

View File

@ -52,6 +52,7 @@ MICROPY_DIR=micropython
OMV_DIR=omv
LEPTON_DIR=lepton
MLX_DIR=mlx
ST_DIR=st
TENSORFLOW_DIR=libtf
WINC1500_DIR=winc1500
BOOTLDR_DIR=bootloader
@ -126,6 +127,7 @@ OMV_CFLAGS += -I$(TOP_DIR)/$(OMV_DIR)/img/
OMV_CFLAGS += -I$(OMV_BOARD_CONFIG_DIR)
OMV_CFLAGS += -I$(TOP_DIR)/$(LEPTON_DIR)/include/
OMV_CFLAGS += -I$(TOP_DIR)/$(MLX_DIR)/include/
OMV_CFLAGS += -I$(TOP_DIR)/$(ST_DIR)/include/
OMV_CFLAGS += -I$(TOP_DIR)/$(TENSORFLOW_DIR)/$(CPU)/
OMV_CFLAGS += -I$(TOP_DIR)/$(WINC1500_DIR)/include/
@ -169,6 +171,7 @@ endif
FIRM_OBJ += $(wildcard $(BUILD)/$(STHAL_DIR)/src/*.o)
FIRM_OBJ += $(wildcard $(BUILD)/$(LEPTON_DIR)/src/*.o)
FIRM_OBJ += $(wildcard $(BUILD)/$(MLX_DIR)/src/*.o)
FIRM_OBJ += $(wildcard $(BUILD)/$(ST_DIR)/src/*.o)
ifeq ($(MICROPY_PY_WINC1500), 1)
FIRM_OBJ += $(wildcard $(BUILD)/$(WINC1500_DIR)/src/*.o)
endif
@ -273,6 +276,7 @@ FIRM_OBJ += $(addprefix $(BUILD)/$(OMV_DIR)/py/, \
py_cpufreq.o \
py_nn.o \
py_tf.o \
py_imu.o \
)
@ -532,6 +536,7 @@ UVC_OBJ += $(addprefix $(BUILD)/$(OMV_DIR)/img/,\
UVC_OBJ += $(wildcard $(BUILD)/$(LEPTON_DIR)/src/*.o)
UVC_OBJ += $(wildcard $(BUILD)/$(MLX_DIR)/src/*.o)
UVC_OBJ += $(wildcard $(BUILD)/$(ST_DIR)/src/*.o)
###################################################
#Export Variables
@ -567,6 +572,7 @@ FIRMWARE_OBJS: | $(BUILD) $(FW_DIR)
$(MAKE) -C $(MICROPY_DIR)/ports/stm32 BUILD=$(BUILD)/$(MICROPY_DIR) BOARD=$(TARGET) DEBUG=$(DEBUG) QSTR_DEFS="$(OMV_QSTR_DEFS)"
$(MAKE) -C $(LEPTON_DIR) BUILD=$(BUILD)/$(LEPTON_DIR) CFLAGS="$(CFLAGS) -MMD"
$(MAKE) -C $(MLX_DIR) BUILD=$(BUILD)/$(MLX_DIR) CFLAGS="$(CFLAGS) -MMD"
$(MAKE) -C $(ST_DIR) BUILD=$(BUILD)/$(ST_DIR) CFLAGS="$(CFLAGS) -MMD"
ifeq ($(MICROPY_PY_WINC1500), 1)
$(MAKE) -C $(WINC1500_DIR) BUILD=$(BUILD)/$(WINC1500_DIR) CFLAGS="$(CFLAGS) -MMD"
endif

View File

@ -107,6 +107,7 @@ SRCS += $(addprefix py/, \
py_cpufreq.c \
py_nn.c \
py_tf.c \
py_imu.c \
)
OBJS = $(addprefix $(BUILD)/, $(SRCS:.c=.o))

View File

@ -49,6 +49,9 @@
#define OMV_ENABLE_MT9V034 (1)
#define OMV_ENABLE_LEPTON (1)
// Enable IMU sensor
#define OMV_ENABLE_IMU (1)
// Enable WiFi debug
#define OMV_ENABLE_WIFIDBG (1)
@ -241,4 +244,28 @@
#define LEPTON_SPI_MOSI_PORT (GPIOB)
#define LEPTON_SPI_SSEL_PORT (GPIOA)
// The IMU sensor is on the same SPI bus pins as the camera module interface
// SPI bus. While the buses overlap both devices will never be in-use at once.
#define IMU_SPI (SPI1)
#define IMU_SPI_AF (GPIO_AF5_SPI1)
// SPI1/2/3 clock source is PLL2 (160MHz/16 == 10MHz).
#define IMU_SPI_PRESCALER (SPI_BAUDRATEPRESCALER_16)
#define IMU_SPI_RESET() __HAL_RCC_SPI1_FORCE_RESET()
#define IMU_SPI_RELEASE() __HAL_RCC_SPI1_RELEASE_RESET()
#define IMU_SPI_CLK_ENABLE() __HAL_RCC_SPI1_CLK_ENABLE()
#define IMU_SPI_CLK_DISABLE() __HAL_RCC_SPI1_CLK_DISABLE()
#define IMU_SPI_SCLK_PIN (GPIO_PIN_3)
#define IMU_SPI_MISO_PIN (GPIO_PIN_4)
#define IMU_SPI_MOSI_PIN (GPIO_PIN_5)
#define IMU_SPI_SSEL_PIN (GPIO_PIN_15)
#define IMU_SPI_SCLK_PORT (GPIOB)
#define IMU_SPI_MISO_PORT (GPIOB)
#define IMU_SPI_MOSI_PORT (GPIOB)
#define IMU_SPI_SSEL_PORT (GPIOA)
#endif //__OMV_BOARDCONFIG_H__

300
src/omv/py/py_imu.c Normal file
View File

@ -0,0 +1,300 @@
/*
* This file is part of the OpenMV project.
*
* Copyright (c) 2013-2020 Ibrahim Abdelkader <iabdalkader@openmv.io>
* Copyright (c) 2013-2020 Kwabena W. Agyeman <kwagyeman@openmv.io>
*
* This work is licensed under the MIT license, see the file LICENSE for details.
*
* IMU Python module.
*/
#include STM32_HAL_H
#include "lsm6ds3tr_c_reg.h"
#include "omv_boardconfig.h"
#include "py_helper.h"
#include "py_imu.h"
#if defined(OMV_ENABLE_IMU)
typedef union {
int16_t i16bit[3];
uint8_t u8bit[6];
} axis3bit16_t;
typedef union {
int16_t i16bit;
uint8_t u8bit[2];
} axis1bit16_t;
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 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 = {
.Instance = IMU_SPI,
.Init.Mode = SPI_MODE_MASTER,
.Init.Direction = SPI_DIRECTION_2LINES,
.Init.DataSize = SPI_DATASIZE_8BIT,
.Init.CLKPolarity = SPI_POLARITY_HIGH,
.Init.CLKPhase = SPI_PHASE_2EDGE,
.Init.NSS = SPI_NSS_SOFT,
.Init.BaudRatePrescaler = IMU_SPI_PRESCALER,
.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 error_on_not_ready()
{
if (test_not_ready()) nlr_raise(mp_obj_new_exception_msg(&mp_type_OSError, "IMU Not Ready!"));
}
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),
mp_obj_new_float(z)});
}
// For when the camera board is lying on a table face up.
// X points to the right of the camera sensor
// Y points down below the camera sensor
// Z points in the reverse direction of the camera sensor
// Thus (https://www.nxp.com/docs/en/application-note/AN3461.pdf):
//
// Roll = atan2(Y, Z)
// Pitch = atan2(-X, sqrt(Y^2, + Z^2)) -> assume Y=0 -> atan2(-X, Z)
// For when the camera board is standing right-side up.
// X points to the right of the camera sensor (still X)
// Y points down below the camera sensor (now Z)
// Z points in the reverse direction of the camera sensor (now -Y)
// So:
//
// Roll = atan2(-X, sqrt(Z^2, + Y^2)) -> assume Z=0 -> atan2(-X, Y)
// Pitch = atan2(Z, -Y)
STATIC float py_imu_get_roll()
{
axis3bit16_t data_raw_acceleration = {};
lsm6ds3tr_c_acceleration_raw_get(&dev_ctx, data_raw_acceleration.u8bit);
float x = lsm6ds3tr_c_from_fs8g_to_mg(data_raw_acceleration.i16bit[0]);
float y = lsm6ds3tr_c_from_fs8g_to_mg(data_raw_acceleration.i16bit[1]);
return fmodf((IM_RAD2DEG(fast_atan2f(-x, y)) + 180), 360); // rotate 180
}
STATIC float py_imu_get_pitch()
{
axis3bit16_t data_raw_acceleration = {};
lsm6ds3tr_c_acceleration_raw_get(&dev_ctx, data_raw_acceleration.u8bit);
float y = lsm6ds3tr_c_from_fs8g_to_mg(data_raw_acceleration.i16bit[1]);
float z = lsm6ds3tr_c_from_fs8g_to_mg(data_raw_acceleration.i16bit[2]);
return IM_RAD2DEG(fast_atan2f(z, -y));
}
STATIC mp_obj_t py_imu_acceleration_mg()
{
error_on_not_ready();
axis3bit16_t data_raw_acceleration = {};
lsm6ds3tr_c_acceleration_raw_get(&dev_ctx, data_raw_acceleration.u8bit);
return py_imu_tuple(lsm6ds3tr_c_from_fs8g_to_mg(data_raw_acceleration.i16bit[0]),
lsm6ds3tr_c_from_fs8g_to_mg(data_raw_acceleration.i16bit[1]),
lsm6ds3tr_c_from_fs8g_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_obj_t py_imu_angular_rate_mdps()
{
error_on_not_ready();
axis3bit16_t data_raw_angular_rate = {};
lsm6ds3tr_c_angular_rate_raw_get(&dev_ctx, data_raw_angular_rate.u8bit);
return py_imu_tuple(lsm6ds3tr_c_from_fs2000dps_to_mdps(data_raw_angular_rate.i16bit[0]),
lsm6ds3tr_c_from_fs2000dps_to_mdps(data_raw_angular_rate.i16bit[1]),
lsm6ds3tr_c_from_fs2000dps_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_obj_t py_imu_temperature_c()
{
error_on_not_ready();
axis1bit16_t data_raw_temperature = {};
lsm6ds3tr_c_temperature_raw_get(&dev_ctx, data_raw_temperature.u8bit);
return mp_obj_new_float(lsm6ds3tr_c_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_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_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_obj_t py_imu_sleep(mp_obj_t enable)
{
error_on_not_ready();
bool en = mp_obj_get_int(enable);
lsm6ds3tr_c_xl_data_rate_set(&dev_ctx, en ? LSM6DS3TR_C_XL_ODR_OFF : LSM6DS3TR_C_XL_ODR_52Hz);
lsm6ds3tr_c_gy_data_rate_set(&dev_ctx, en ? LSM6DS3TR_C_GY_ODR_OFF : LSM6DS3TR_C_GY_ODR_52Hz);
return mp_const_none;
}
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)
{
error_on_not_ready();
uint8_t v = mp_obj_get_int(val);
lsm6ds3tr_c_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_obj_t py_imu_read_reg(mp_obj_t addr)
{
error_on_not_ready();
uint8_t v;
lsm6ds3tr_c_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);
#endif
STATIC const mp_rom_map_elem_t globals_dict_table[] = {
{ MP_ROM_QSTR(MP_QSTR___name__), MP_OBJ_NEW_QSTR(MP_QSTR_imu) },
#if defined(OMV_ENABLE_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) },
{ MP_ROM_QSTR(MP_QSTR_temperature_c), MP_ROM_PTR(&py_imu_temperature_c_obj) },
{ MP_ROM_QSTR(MP_QSTR_roll), MP_ROM_PTR(&py_imu_roll_obj) },
{ MP_ROM_QSTR(MP_QSTR_pitch), MP_ROM_PTR(&py_imu_pitch_obj) },
{ MP_ROM_QSTR(MP_QSTR_sleep), MP_ROM_PTR(&py_imu_sleep_obj) },
{ MP_ROM_QSTR(MP_QSTR___write_reg), MP_ROM_PTR(&py_imu_write_reg_obj) },
{ MP_ROM_QSTR(MP_QSTR___read_reg), MP_ROM_PTR(&py_imu_read_reg_obj) },
#else
{ MP_ROM_QSTR(MP_QSTR_acceleration_mg), MP_ROM_PTR(&py_func_unavailable_obj) },
{ MP_ROM_QSTR(MP_QSTR_angular_rate_mdps), MP_ROM_PTR(&py_func_unavailable_obj) },
{ MP_ROM_QSTR(MP_QSTR_temperature_c), MP_ROM_PTR(&py_func_unavailable_obj) },
{ MP_ROM_QSTR(MP_QSTR_roll), MP_ROM_PTR(&py_func_unavailable_obj) },
{ MP_ROM_QSTR(MP_QSTR_pitch), MP_ROM_PTR(&py_func_unavailable_obj) },
{ MP_ROM_QSTR(MP_QSTR_sleep), MP_ROM_PTR(&py_func_unavailable_obj) },
{ MP_ROM_QSTR(MP_QSTR___write_reg), MP_ROM_PTR(&py_func_unavailable_obj) },
{ MP_ROM_QSTR(MP_QSTR___read_reg), MP_ROM_PTR(&py_func_unavailable_obj) },
#endif
};
STATIC MP_DEFINE_CONST_DICT(globals_dict, globals_dict_table);
const mp_obj_module_t imu_module = {
.base = { &mp_type_module },
.globals = (mp_obj_t) &globals_dict
};
void py_imu_init()
{
#if defined(OMV_ENABLE_IMU)
HAL_SPI_Init(&SPIHandle);
uint8_t whoamI = 0, rst = 1;
// Try to read device id...
for (int i = 0; (i < 10) && (whoamI != LSM6DS3TR_C_ID); i++) {
lsm6ds3tr_c_device_id_get(&dev_ctx, &whoamI);
}
if (whoamI != LSM6DS3TR_C_ID) {
HAL_SPI_DeInit(&SPIHandle);
return;
}
lsm6ds3tr_c_reset_set(&dev_ctx, PROPERTY_ENABLE);
for (int i = 0; (i < 10000) && rst; i++) {
lsm6ds3tr_c_reset_get(&dev_ctx, &rst);
}
if (rst) {
HAL_SPI_DeInit(&SPIHandle);
return;
}
lsm6ds3tr_c_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);
lsm6ds3tr_c_xl_data_rate_set(&dev_ctx, LSM6DS3TR_C_XL_ODR_52Hz);
lsm6ds3tr_c_gy_data_rate_set(&dev_ctx, LSM6DS3TR_C_GY_ODR_52Hz);
lsm6ds3tr_c_xl_full_scale_set(&dev_ctx, LSM6DS3TR_C_8g);
lsm6ds3tr_c_gy_full_scale_set(&dev_ctx, LSM6DS3TR_C_2000dps);
#endif
}
float py_imu_roll_rotation()
{
#if defined(OMV_ENABLE_IMU)
if (test_not_ready()) {
return 0; // output when the camera is mounted upright
}
return py_imu_get_roll();
#else
return 0; // output when the camera is mounted upright
#endif
}
float py_imu_pitch_rotation()
{
#if defined(OMV_ENABLE_IMU)
if (test_not_ready()) {
return 0; // output when the camera is mounted upright
}
return py_imu_get_pitch();
#else
return 0; // output when the camera is mounted upright
#endif
}

16
src/omv/py/py_imu.h Normal file
View File

@ -0,0 +1,16 @@
/*
* This file is part of the OpenMV project.
*
* Copyright (c) 2013-2020 Ibrahim Abdelkader <iabdalkader@openmv.io>
* Copyright (c) 2013-2020 Kwabena W. Agyeman <kwagyeman@openmv.io>
*
* This work is licensed under the MIT license, see the file LICENSE for details.
*
* IMU Python module.
*/
#ifndef __PY_IMU_H__
#define __PY_IMU_H__
void py_imu_init();
float py_imu_roll_rotation(); // in degrees
float py_imu_pitch_rotation(); // in degrees
#endif // __PY_IMU_H__

View File

@ -1215,3 +1215,11 @@ Q(output)
// Segment
// duplicate Q(segment)
// duplicate Q(roi)
// IMU Module
Q(imu)
Q(acceleration_mg)
Q(angular_rate_mdps)
Q(temperature_c)
Q(roll)
Q(pitch)

View File

@ -216,6 +216,32 @@ void HAL_DCMI_MspInit(DCMI_HandleTypeDef* hdcmi)
void HAL_SPI_MspInit(SPI_HandleTypeDef *hspi)
{
#if defined(IMU_SPI)
if (hspi->Instance == IMU_SPI) {
IMU_SPI_CLK_ENABLE();
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.Pull = GPIO_PULLUP;
GPIO_InitStructure.Mode = GPIO_MODE_AF_PP;
GPIO_InitStructure.Alternate = IMU_SPI_AF;
GPIO_InitStructure.Speed = GPIO_SPEED_FREQ_LOW;
GPIO_InitStructure.Pin = IMU_SPI_SCLK_PIN;
HAL_GPIO_Init(IMU_SPI_SCLK_PORT, &GPIO_InitStructure);
GPIO_InitStructure.Pin = IMU_SPI_MISO_PIN;
HAL_GPIO_Init(IMU_SPI_MISO_PORT, &GPIO_InitStructure);
GPIO_InitStructure.Pin = IMU_SPI_MOSI_PIN;
HAL_GPIO_Init(IMU_SPI_MOSI_PORT, &GPIO_InitStructure);
GPIO_InitStructure.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStructure.Pin = IMU_SPI_SSEL_PIN;
HAL_GPIO_Init(IMU_SPI_SSEL_PORT, &GPIO_InitStructure);
}
#endif
#if defined(LEPTON_SPI)
if (hspi->Instance == LEPTON_SPI) {
LEPTON_SPI_CLK_ENABLE();

25
src/st/Makefile Normal file
View File

@ -0,0 +1,25 @@
# This file is part of the OpenMV project.
#
# Copyright (c) 2013-2020 Ibrahim Abdelkader <iabdalkader@openmv.io>
# Copyright (c) 2013-2020 Kwabena W. Agyeman <kwagyeman@openmv.io>
#
# This work is licensed under the MIT license, see the file LICENSE for details.
#
# ST Makefile
SRCS = $(wildcard src/*.c)
OBJS = $(addprefix $(BUILD)/, $(SRCS:.c=.o))
OBJ_DIRS = $(sort $(dir $(OBJS)))
all: | $(OBJ_DIRS) $(OBJS)
$(OBJ_DIRS):
$(MKDIR) -p $@
$(BUILD)/%.o : %.c
$(ECHO) "CC $<"
$(CC) $(CFLAGS) -c -o $@ $<
$(BUILD)/%.o : %.s
$(ECHO) "AS $<"
$(AS) $(AFLAGS) $< -o $@
-include $(OBJS:%.o=%.d)

1669
src/st/include/lsm6ds3_reg.h Normal file

File diff suppressed because it is too large Load Diff

File diff suppressed because it is too large Load Diff

5910
src/st/src/lsm6ds3_reg.c Normal file

File diff suppressed because it is too large Load Diff

6805
src/st/src/lsm6ds3tr_c_reg.c Normal file

File diff suppressed because it is too large Load Diff