piotr/edgetx-gx12
1
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

WiP: support for LSM6DSxx

Browse Source
This commit is contained in:
Raphael Coeffic
2022-06-25 16:09:04 +02:00
committed by Raphael Coeffic
parent df83a82285
commit 0b8d93ca88
44 changed files with 356 additions and 429 deletions
Download Patch File Download Diff File Expand all files Collapse all files
radio/src/targets/common/arm/stm32/CMakeLists.txt
+18 -33
View File
@@ -26,8 +26,7 @@ set(STM32USB_SRC
STM32_USB_Device_Library/Core/src/usbd_req.c
)
set(FIRMWARE_TARGET_SRC
${FIRMWARE_TARGET_SRC}
set(FIRMWARE_TARGET_SRC ${FIRMWARE_TARGET_SRC}
../common/arm/stm32/rtc_driver.cpp
../common/arm/stm32/delays_driver.cpp
../common/arm/stm32/usb_bsp.c
@@ -41,6 +40,14 @@ set(FIRMWARE_TARGET_SRC
../common/arm/stm32/stm32_i2c_driver.cpp
)
if(IMU)
add_definitions(-DIMU)
set(FIRMWARE_SRC ${FIRMWARE_SRC}
targets/common/arm/stm32/lsm6ds_driver.cpp
)
set(SRC ${SRC} gyro.cpp)
endif()
if(AUX_SERIAL OR AUX2_SERIAL)
set(FIRMWARE_TARGET_SRC ${FIRMWARE_TARGET_SRC}
../common/arm/stm32/aux_serial_driver.cpp
@@ -54,20 +61,17 @@ if(AUX_SERIAL OR AUX2_SERIAL)
endif()
if(DEBUG OR CLI OR USB_SERIAL)
set(STM32USB_SRC
${STM32USB_SRC}
set(STM32USB_SRC ${STM32USB_SRC}
STM32_USB_Device_Library/Class/cdc/src/usbd_cdc_core.c
)
set(FIRMWARE_TARGET_SRC
${FIRMWARE_TARGET_SRC}
set(FIRMWARE_TARGET_SRC ${FIRMWARE_TARGET_SRC}
../common/arm/stm32/usbd_cdc.cpp
)
add_definitions(-DUSB_SERIAL)
message("-- Adding support for USB serial")
endif()
set(STM32USB_SRC
${STM32USB_SRC}
set(STM32USB_SRC ${STM32USB_SRC}
STM32_USB_Device_Library/Class/msc/src/usbd_msc_data.c
STM32_USB_Device_Library/Class/msc/src/usbd_msc_scsi.c
STM32_USB_Device_Library/Class/msc/src/usbd_msc_bot.c
@@ -75,37 +79,18 @@ set(STM32USB_SRC
)
if(GVARS)
set(GUI_SRC
${GUI_SRC}
${GVAR_SCREEN}
)
set(GUI_SRC ${GUI_SRC} ${GVAR_SCREEN})
endif()
set(FIRMWARE_SRC
${FIRMWARE_SRC}
syscalls.c
)
set(FIRMWARE_SRC ${FIRMWARE_SRC} syscalls.c)
foreach(FILE ${STM32LIB_SRC})
set(FIRMWARE_SRC
${FIRMWARE_SRC}
${STM32LIB_DIR}/${FILE}
)
set(FIRMWARE_SRC ${FIRMWARE_SRC} ${STM32LIB_DIR}/${FILE})
endforeach()
foreach(FILE ${STM32USB_SRC})
set(FIRMWARE_SRC
${FIRMWARE_SRC}
${STM32USB_DIR}/${FILE}
)
set(FIRMWARE_SRC ${FIRMWARE_SRC} ${STM32USB_DIR}/${FILE})
endforeach()
set(TARGET_SRC
${TARGET_SRC}
../common/arm/stm32/cpu_id.cpp
)
set(SRC
${SRC}
io/bootloader_flash.cpp
)
set(TARGET_SRC ${TARGET_SRC} ../common/arm/stm32/cpu_id.cpp)
set(SRC ${SRC} io/bootloader_flash.cpp)
radio/src/targets/common/arm/stm32/lsm6ds_driver.cpp
+44 -137
View File
@@ -19,8 +19,12 @@
* GNU General Public License for more details.
*/
#include "opentx.h"
#include "stm32_i2c_driver.h"
#include "delays_driver.h"
#include "hal.h"
#include "gyro.h"
#include <string.h>
/* COMMON VALUES FOR ACCEL-GYRO SENSORS */
#define LSM6DS_WHO_AM_I 0x0f
@@ -126,6 +130,10 @@
#define LSM6DS_GYRO_FS_500_GAIN 17500
#define LSM6DS_GYRO_FS_1000_GAIN 35000
#define LSM6DS_GYRO_FS_2000_GAIN 70000
#define LSM6DS_STATUS_REG_ADDR 0x1E
#define LSM6DS_OUT_TEMP_L_ADDR 0x20
#define LSM6DS_OUT_TEMP_H_ADDR 0x21
#define LSM6DS_GYRO_OUT_X_L_ADDR 0x22
#define LSM6DS_GYRO_OUT_Y_L_ADDR 0x24
#define LSM6DS_GYRO_OUT_Z_L_ADDR 0x26
@@ -179,7 +187,6 @@
#define LSM6DS_RESET_ADDR 0x12
#define LSM6DS_RESET_MASK 0x01
#define LSM6DS_ADDRESS 0xD6
#define LSM6DSLTR_ID 0x6A
#define LSM6DS33TR_ID 0x69
@@ -192,170 +199,70 @@ static const char configure[][2] = {
{LSM6DS_INT2_CTRL_ADDR, 0x3},
};
static void i2c2Init()
#define I2C_TIMEOUT_MAX 10000
#define I2C_LSM6DS_MAX_WRITE 32
static int I2C_LSM6DS_WriteRegister(uint8_t reg, uint8_t* data, uint8_t len)
{
I2C_DeInit(I2C_B2);
uint8_t buffer[I2C_LSM6DS_MAX_WRITE];
I2C_InitTypeDef I2C_InitStructure;
I2C_InitStructure.I2C_ClockSpeed = I2C_B2_SPEED;
I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2;
I2C_InitStructure.I2C_OwnAddress1 = 0x00;
I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
I2C_Init(I2C_B2, &I2C_InitStructure);
I2C_Cmd(I2C_B2, ENABLE);
if (len >= I2C_LSM6DS_MAX_WRITE - 1) return -1;
GPIO_PinAFConfig(I2C_B2_SCL_GPIO, I2C_B2_SCL_GPIO_PinSource, I2C_B2_GPIO_AF);
GPIO_PinAFConfig(I2C_B2_SDA_GPIO, I2C_B2_SDA_GPIO_PinSource, I2C_B2_GPIO_AF);
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Pin = I2C_B2_SCL_GPIO_PIN;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_Init(I2C_B2_SCL_GPIO, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = I2C_B2_SDA_GPIO_PIN;
GPIO_Init(I2C_B2_SDA_GPIO, &GPIO_InitStructure);
buffer[0] = reg;
memcpy(buffer + 1, data, len);
return stm32_i2c_master_tx(IMU_I2C_BUS, IMU_I2C_ADDRESS, buffer, len + 1, 100);
}
#define I2C_TIMEOUT_MAX 10000
bool I2C2_WaitEvent(uint32_t event)
static int I2C_LSM6DS_WriteRegister(uint8_t reg, uint8_t value)
{
uint32_t timeout = I2C_TIMEOUT_MAX;
while (!I2C_CheckEvent(I2C_B2, event)) {
if ((timeout--) == 0) return false;
}
return true;
return I2C_LSM6DS_WriteRegister(reg, &value, 1);
}
bool I2C2_WaitEventCleared(uint32_t event)
static int I2C_LSM6DS_ReadRegister(uint8_t reg, uint8_t* data, uint8_t len)
{
uint32_t timeout = I2C_TIMEOUT_MAX;
while (I2C_CheckEvent(I2C_B2, event)) {
if ((timeout--) == 0) {
TRACE("I2C Timeout!");
return false;
}
}
return true;
if (stm32_i2c_master_tx(IMU_I2C_BUS, IMU_I2C_ADDRESS, &reg, 1, 10) < 0)
return -1;
uint8_t value = 0;
if (stm32_i2c_master_rx(IMU_I2C_BUS, IMU_I2C_ADDRESS, data, len, 100) < 0)
return -1;
return value;
}
int setGyroRegister(uint8_t address, uint8_t value)
static int I2C_LSM6DS_ReadRegister(uint8_t reg)
{
if (!I2C2_WaitEventCleared(I2C_FLAG_BUSY))
uint8_t value = 0;
if (I2C_LSM6DS_ReadRegister(reg, &value, 1) < 0)
return -1;
I2C_GenerateSTART(I2C_B2, ENABLE);
if (!I2C2_WaitEvent(I2C_EVENT_MASTER_MODE_SELECT))
return -1;
I2C_Send7bitAddress(I2C_B2, LSM6DS_ADDRESS, I2C_Direction_Transmitter);
if (!I2C2_WaitEvent(I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED))
return -1;
I2C_SendData(I2C_B2, address);
if (!I2C2_WaitEvent(I2C_EVENT_MASTER_BYTE_TRANSMITTING))
return -1;
I2C_SendData(I2C_B2, value);
if (!I2C2_WaitEvent(I2C_EVENT_MASTER_BYTE_TRANSMITTED))
return -1;
I2C_GenerateSTOP(I2C_B2, ENABLE);
return 0;
}
int readGyroRegister(uint8_t address)
{
if (!I2C2_WaitEventCleared(I2C_FLAG_BUSY))
return -1;
I2C_GenerateSTART(I2C_B2, ENABLE);
if (!I2C2_WaitEvent(I2C_EVENT_MASTER_MODE_SELECT))
return -1;
I2C_Send7bitAddress(I2C_B2, LSM6DS_ADDRESS, I2C_Direction_Transmitter);
if (!I2C2_WaitEvent(I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED))
return -1;
I2C_SendData(I2C_B2, address);
if (!I2C2_WaitEvent(I2C_EVENT_MASTER_BYTE_TRANSMITTED))
return -1;
I2C_GenerateSTART(I2C_B2, ENABLE);
if (!I2C2_WaitEvent(I2C_EVENT_MASTER_MODE_SELECT))
return -1;
I2C_Send7bitAddress(I2C_B2, LSM6DS_ADDRESS, I2C_Direction_Receiver);
I2C_AcknowledgeConfig(I2C_B2, DISABLE);
if (!I2C2_WaitEvent(I2C_EVENT_MASTER_BYTE_RECEIVED))
return -1;
uint8_t value = I2C_ReceiveData(I2C_B2);
I2C_GenerateSTOP(I2C_B2, ENABLE);
return value;
}
int gyroInit()
{
i2c2Init();
uint8_t id = readGyroRegister(LSM6DS_WHO_AM_I);
if (id != LSM6DSLTR_ID && id != LSM6DS33TR_ID) { // LSM6DS33TR works with LSM6DSLTR code for our use
if (stm32_i2c_init(IMU_I2C_BUS, IMU_I2C_CLK_RATE) < 0)
return -1;
}
setGyroRegister(0x12, 0x05);
// LSM6DS33TR works with LSM6DSLTR code for our use
uint8_t id = I2C_LSM6DS_ReadRegister(LSM6DS_WHO_AM_I);
if (id != LSM6DSLTR_ID && id != LSM6DS33TR_ID)
return -1;
I2C_LSM6DS_WriteRegister(0x12, 0x05);
delay_ms(1);
setGyroRegister(0x12, 0x04);
I2C_LSM6DS_WriteRegister(0x12, 0x04);
delay_ms(1);
for (uint8_t i = 0; i < DIM(configure); i++) {
setGyroRegister(configure[i][0], configure[i][1]);
I2C_LSM6DS_WriteRegister(configure[i][0], configure[i][1]);
}
return 0;
}
int gyroRead(uint8_t buffer[GYRO_BUFFER_LENGTH])
int gyroRead(uint8_t buffer[IMU_BUFFER_LENGTH])
{
if (!I2C2_WaitEventCleared(I2C_FLAG_BUSY))
return -1;
I2C_GenerateSTART(I2C_B2, ENABLE);
if (!I2C2_WaitEvent(I2C_EVENT_MASTER_MODE_SELECT))
return -1;
I2C_Send7bitAddress(I2C_B2, LSM6DS_ADDRESS, I2C_Direction_Transmitter);
if (!I2C2_WaitEvent(I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED))
return -1;
I2C_SendData(I2C_B2, LSM6DS_GYRO_OUT_X_L_ADDR);
if (!I2C2_WaitEvent(I2C_EVENT_MASTER_BYTE_TRANSMITTED))
return -1;
I2C_GenerateSTART(I2C_B2, ENABLE);
if (!I2C2_WaitEvent(I2C_EVENT_MASTER_MODE_SELECT))
return -1;
I2C_Send7bitAddress(I2C_B2, LSM6DS_ADDRESS, I2C_Direction_Receiver);
I2C_AcknowledgeConfig(I2C_B2, ENABLE);
for (uint8_t i=0; i<GYRO_BUFFER_LENGTH; i++) {
if (i == GYRO_BUFFER_LENGTH - 1)
I2C_AcknowledgeConfig(I2C_B2, DISABLE);
if (!I2C2_WaitEvent(I2C_EVENT_MASTER_BYTE_RECEIVED))
return -1;
buffer[i] = I2C_ReceiveData(I2C_B2);
}
I2C_GenerateSTOP(I2C_B2, ENABLE);
return 0;
return I2C_LSM6DS_ReadRegister(LSM6DS_GYRO_OUT_X_L_ADDR, buffer, IMU_BUFFER_LENGTH);
}
radio/src/targets/horus/CMakeLists.txt
+3 -2
View File
@@ -56,6 +56,9 @@ set(ENABLE_SERIAL_PASSTHROUGH ON CACHE BOOL "Enable serial passthrough")
set(CLI ON CACHE BOOL "Enable CLI")
set(AUX_SERIAL ON)
# IMU support
set(IMU ON)
if (PCB STREQUAL X10)
set(PCBREV "STD" CACHE STRING "PCB Revision")
add_definitions(-DSOFTWARE_VOLUME)
@@ -214,7 +217,6 @@ endif()
if(IMU_LSM6DS33)
add_definitions(-DIMU_LSM6DS33)
set(AUX_SERIAL OFF)
set(IMU_LSM6DS33_DRIVER imu_lsm6ds33.cpp)
endif()
if(NOT INTERNAL_MODULE_PXX1)
@@ -274,7 +276,6 @@ set(FIRMWARE_TARGET_SRC
${FIRMWARE_TARGET_SRC}
${LCD_DRIVER}
${FLYSKY_HALL_STICKS_DRIVER}
${IMU_LSM6DS33_DRIVER}
${TOUCH_DRIVER}
board.cpp
backlight_driver.cpp
radio/src/targets/horus/board.cpp
-3
View File
@@ -224,9 +224,6 @@ void boardInit()
if (!adcInit(&ADC_DRIVER))
TRACE("adcInit failed");
#if defined(IMU_LSM6DS33)
imu_lsm6ds33_init();
#endif
init2MhzTimer();
init1msTimer();
radio/src/targets/horus/board.h
-3
View File
@@ -38,9 +38,6 @@
#include "tp_gt911.h"
#endif
#if defined(IMU_LSM6DS33)
#include "imu_lsm6ds33.h"
#endif
PACK(typedef struct {
uint8_t pcbrev:2;
radio/src/targets/horus/hal.h
+11 -1
View File
@@ -745,11 +745,21 @@
#define I2C_B2_SCL_GPIO_PIN LL_GPIO_PIN_10 // PB.10
#define I2C_B2_SDA_GPIO_PIN LL_GPIO_PIN_11 // PB.11
#define I2C_B2_GPIO_AF LL_GPIO_AF_4 // I2C2
#define I2C_B2_CLK_RATE 100000
#define AUX_I2C_B2_PWR_GPIO GPIOA
#define AUX_I2C_B2_PWR_GPIO_PIN GPIO_Pin_15 // PA.15
#endif
#if defined(IMU)
#if defined(PCBX12S)
#define IMU_I2C_BUS I2C_Bus_1
#define IMU_I2C_ADDRESS 0x6A
#elif defined(IMU_LSM6DS33)
#define IMU_I2C_BUS I2C_Bus_2
#define IMU_I2C_ADDRESS 0x6A
#endif
#define IMU_I2C_CLK_RATE 400000
#endif
// Haptic
#define HAPTIC_PWM
#if defined(PCBX12S)
radio/src/targets/horus/old_imu_driver.cpp
+40
View File
@@ -0,0 +1,40 @@
#define OUTX_L_G 0x22
#define ALPHA 0.98
#define DT 0.01
#define SCALE_FACT_ACC 0.005
#define SCALE_FACT_IMU 0.0078 // 250deg/s / 32000
float roll=0;
float pitch=0;
void imuCompute()
{
uint8_t gyr_acc_raw[12];
int8_t err = I2C_readBytes(IMU_I2C_ADDRESS, OUTX_L_G, 12, gyr_acc_raw);
int16_t gx = (int16_t)(gyr_acc_raw[1] << 8 | gyr_acc_raw[0]);
int16_t gy = (int16_t)(gyr_acc_raw[3] << 8 | gyr_acc_raw[2]);
int16_t gz = (int16_t)(gyr_acc_raw[5] << 8 | gyr_acc_raw[4]);
int16_t ax = (int16_t)(gyr_acc_raw[7] << 8 | gyr_acc_raw[6]);
int16_t ay = (int16_t)(gyr_acc_raw[9] << 8 | gyr_acc_raw[8]);
int16_t az = (int16_t)(gyr_acc_raw[11] << 8 | gyr_acc_raw[10]);
// integrate gyro
roll -= gx * SCALE_FACT_IMU * DT;
pitch += gy * SCALE_FACT_IMU * DT;
int32_t magn = abs(ax) + abs(ay) + abs(az);
if (magn > 8192 && magn < 32768) {
if (az < 0) az = -az;
float rollAcc = atan2f((float)ay,(float)az) * 57.3 /* 180/PI */;
float pitchAcc = atan2f((float)ax,(float)az) * 57.3 /* 180/PI */;
roll = roll * ALPHA + rollAcc * (1.0-ALPHA);
pitch = pitch * ALPHA + pitchAcc * (1.0-ALPHA);
}
}
radio/src/targets/taranis/CMakeLists.txt
+1 -13
View File
@@ -387,7 +387,7 @@ elseif(PCB STREQUAL XLITES)
set(STATUS_LEDS YES)
set(PXX2 ON)
set(BLUETOOTH YES)
set(GYRO YES)
set(IMU YES)
add_definitions(-DMANUFACTURER_FRSKY)
endif()
@@ -395,18 +395,6 @@ if(NOT PCB STREQUAL PCBXLITE)
add_definitions(-DHARDWARE_TRAINER_JACK)
endif()
if(GYRO)
add_definitions(-DGYRO)
set(FIRMWARE_SRC
${FIRMWARE_SRC}
targets/common/arm/stm32/lsm6ds_driver.cpp
)
set(SRC
${SRC}
gyro.cpp
)
endif()
if(INTERNAL_MODULE_SERIAL)
if("${PCBREV}" STREQUAL 2019 OR "${PCBREV}" STREQUAL ACCESS)
radio/src/targets/taranis/board.cpp
+1 -7
View File
@@ -115,7 +115,6 @@ void boardInit()
TRAINER_RCC_AHB1Periph |
TRAINER_MODULE_RCC_AHB1Periph |
BT_RCC_AHB1Periph |
I2C_B2_RCC_AHB1Periph |
USB_CHARGER_RCC_AHB1Periph,
ENABLE);
@@ -134,8 +133,7 @@ void boardInit()
INTMODULE_RCC_APB1Periph |
TRAINER_MODULE_RCC_APB1Periph |
MIXER_SCHEDULER_TIMER_RCC_APB1Periph |
BT_RCC_APB1Periph |
I2C_B2_RCC_APB1Periph,
BT_RCC_APB1Periph,
ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG |
@@ -281,10 +279,6 @@ void boardInit()
initHeadphoneTrainerSwitch();
#if defined(GYRO)
gyroInit();
#endif
#if defined(RTCLOCK) && !defined(COPROCESSOR)
rtcInit(); // RTC must be initialized before rambackupRestore() is called
#endif
radio/src/targets/taranis/board.h
-10
View File
@@ -973,16 +973,6 @@ extern Fifo<uint8_t, TELEMETRY_FIFO_SIZE> telemetryFifo;
#define INTMODULE_FIFO_SIZE 128
// Gyro driver
#define GYRO_VALUES_COUNT 6
#define GYRO_BUFFER_LENGTH (GYRO_VALUES_COUNT * sizeof(int16_t))
int gyroInit();
int gyroRead(uint8_t buffer[GYRO_BUFFER_LENGTH]);
#define GYRO_MAX_DEFAULT 30
#define GYRO_MAX_RANGE 60
#define GYRO_OFFSET_MIN -30
#define GYRO_OFFSET_MAX 10
#if defined (RADIO_TX12)
#define BATTERY_DIVIDER 22830
#elif defined (RADIO_T8)
radio/src/targets/taranis/hal.h
+4 -5
View File
@@ -2154,8 +2154,6 @@
// Second I2C Bus: IMU
#if defined(PCBXLITES)
#define I2C_B2_RCC_AHB1Periph (RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOC)
#define I2C_B2_RCC_APB1Periph RCC_APB1Periph_I2C3
#define I2C_B2 I2C3
#define I2C_B2_SCL_GPIO GPIOA
#define I2C_B2_SCL_GPIO_PIN LL_GPIO_PIN_8 // PA.08
@@ -2163,9 +2161,10 @@
#define I2C_B2_SDA_GPIO_PIN LL_GPIO_PIN_9 // PC.09
#define I2C_B2_GPIO_AF LL_GPIO_AF_4
#define I2C_B2_SPEED 400000
#else
#define I2C_B2_RCC_AHB1Periph 0
#define I2C_B2_RCC_APB1Periph 0
#define IMU_I2C_BUS I2C_Bus_2
#define IMU_I2C_ADDRESS 0x6B
#define IMU_I2C_CLK_RATE I2C_B2_SPEED
#endif
// SD - SPI2