WiP: support for LSM6DSxx
This commit is contained in:
committed by
Raphael Coeffic
parent
df83a82285
commit
0b8d93ca88
@@ -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
|
||||
|
||||
@@ -224,9 +224,6 @@ void boardInit()
|
||||
if (!adcInit(&ADC_DRIVER))
|
||||
TRACE("adcInit failed");
|
||||
|
||||
#if defined(IMU_LSM6DS33)
|
||||
imu_lsm6ds33_init();
|
||||
#endif
|
||||
|
||||
init2MhzTimer();
|
||||
init1msTimer();
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user