WiP: support for LSM6DSxx

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