feat(driver): autodetect IMU, add support for SC7U22 (#7191)

Co-authored-by: raphaelcoeffic <1050031+raphaelcoeffic@users.noreply.github.com>
Co-authored-by: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
This commit is contained in:
3djc
2026-03-25 16:19:03 +10:00
committed by GitHub
co-authored by raphaelcoeffic Claude Opus 4.6
parent 5ea531d391
commit 0ce5c8f3f2
59 changed files with 867 additions and 865 deletions
@@ -137,7 +137,7 @@ endif()
if(IMU)
add_definitions(-DIMU)
set(SRC ${SRC} gyro.cpp)
set(SRC ${SRC} gyro.cpp hal/imu.cpp)
endif()
if(AUX_SERIAL OR AUX2_SERIAL)
@@ -1,275 +0,0 @@
/*
* Copyright (C) EdgeTX
*
* Based on code named
* opentx - https://github.com/opentx/opentx
* th9x - http://code.google.com/p/th9x
* er9x - http://code.google.com/p/er9x
* gruvin9x - http://code.google.com/p/gruvin9x
*
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#include "gyro.h"
#include "hal.h"
#if defined(IMU_I2C_BUS) && defined(IMU_I2C_ADDRESS)
#include "hal/i2c_driver.h"
#include "stm32_i2c_driver.h"
#include "delays_driver.h"
#include <string.h>
#include <stdint.h>
/* COMMON VALUES FOR ACCEL-GYRO SENSORS */
#define LSM6DS_WHO_AM_I 0x0f
#define LSM6DS_WHO_AM_I_DEF 0x69
#define LSM6DS_AXIS_EN_MASK 0x38
#define LSM6DS_INT1_CTRL_ADDR 0x0d
#define LSM6DS_INT2_CTRL_ADDR 0x0e
#define LSM6DS_INT1_FULL 0x20
#define LSM6DS_INT1_FTH 0x08
#define LSM6DS_MD1_ADDR 0x5e
#define LSM6DS_ODR_LIST_NUM 5
#define LSM6DS_ODR_POWER_OFF_VAL 0x00
#define LSM6DS_ODR_26HZ_VAL 0x02
#define LSM6DS_ODR_52HZ_VAL 0x03
#define LSM6DS_ODR_104HZ_VAL 0x04
#define LSM6DS_ODR_208HZ_VAL 0x05
#define LSM6DS_ODR_416HZ_VAL 0x06
#define LSM6DS_FS_LIST_NUM 4
#define LSM6DS_BDU_ADDR 0x12
#define LSM6DS_BDU_MASK 0x40
#define LSM6DS_EN_BIT 0x01
#define LSM6DS_DIS_BIT 0x00
#define LSM6DS_FUNC_EN_ADDR 0x19
#define LSM6DS_FUNC_EN_MASK 0x04
#define LSM6DS_FUNC_CFG_ACCESS_ADDR 0x01
#define LSM6DS_FUNC_CFG_ACCESS_MASK 0x01
#define LSM6DS_FUNC_CFG_ACCESS_MASK2 0x04
#define LSM6DS_FUNC_CFG_REG2_MASK 0x80
#define LSM6DS_FUNC_CFG_START1_ADDR 0x62
#define LSM6DS_FUNC_CFG_START2_ADDR 0x63
#define LSM6DS_SELFTEST_ADDR 0x14
#define LSM6DS_SELFTEST_ACCEL_MASK 0x03
#define LSM6DS_SELFTEST_GYRO_MASK 0x0c
#define LSM6DS_SELF_TEST_DISABLED_VAL 0x00
#define LSM6DS_SELF_TEST_POS_SIGN_VAL 0x01
#define LSM6DS_SELF_TEST_NEG_ACCEL_SIGN_VAL 0x02
#define LSM6DS_SELF_TEST_NEG_GYRO_SIGN_VAL 0x03
#define LSM6DS_LIR_ADDR 0x58
#define LSM6DS_LIR_MASK 0x01
#define LSM6DS_TIMER_EN_ADDR 0x58
#define LSM6DS_TIMER_EN_MASK 0x80
#define LSM6DS_PEDOMETER_EN_ADDR 0x58
#define LSM6DS_PEDOMETER_EN_MASK 0x40
#define LSM6DS_INT2_ON_INT1_ADDR 0x13
#define LSM6DS_INT2_ON_INT1_MASK 0x20
#define LSM6DS_MIN_DURATION_MS 1638
#define LSM6DS_ROUNDING_ADDR 0x16
#define LSM6DS_ROUNDING_MASK 0x04
#define LSM6DS_FIFO_MODE_ADDR 0x0a
#define LSM6DS_FIFO_MODE_MASK 0x07
#define LSM6DS_FIFO_MODE_BYPASS 0x00
#define LSM6DS_FIFO_MODE_CONTINUOS 0x06
#define LSM6DS_FIFO_THRESHOLD_IRQ_MASK 0x08
#define LSM6DS_FIFO_ODR_ADDR 0x0a
#define LSM6DS_FIFO_ODR_MASK 0x78
#define LSM6DS_FIFO_ODR_MAX 0x07
#define LSM6DS_FIFO_ODR_MAX_HZ 800
#define LSM6DS_FIFO_ODR_OFF 0x00
#define LSM6DS_FIFO_CTRL3_ADDR 0x08
#define LSM6DS_FIFO_ACCEL_DECIMATOR_MASK 0x07
#define LSM6DS_FIFO_GYRO_DECIMATOR_MASK 0x38
#define LSM6DS_FIFO_CTRL4_ADDR 0x09
#define LSM6DS_FIFO_STEP_C_DECIMATOR_MASK 0x38
#define LSM6DS_FIFO_THR_L_ADDR 0x06
#define LSM6DS_FIFO_THR_H_ADDR 0x07
#define LSM6DS_FIFO_THR_H_MASK 0x0f
#define LSM6DS_FIFO_THR_IRQ_MASK 0x08
#define LSM6DS_FIFO_PEDO_E_ADDR 0x07
#define LSM6DS_FIFO_PEDO_E_MASK 0x80
#define LSM6DS_FIFO_STEP_C_FREQ 25
/* CUSTOM VALUES FOR ACCEL SENSOR */
#define LSM6DS_ACCEL_ODR_ADDR 0x10
#define LSM6DS_ACCEL_ODR_MASK 0xf0
#define LSM6DS_ACCEL_FS_ADDR 0x10
#define LSM6DS_ACCEL_FS_MASK 0x0c
#define LSM6DS_ACCEL_FS_2G_VAL 0x00
#define LSM6DS_ACCEL_FS_4G_VAL 0x02
#define LSM6DS_ACCEL_FS_8G_VAL 0x03
#define LSM6DS_ACCEL_FS_16G_VAL 0x01
#define LSM6DS_ACCEL_FS_2G_GAIN 61
#define LSM6DS_ACCEL_FS_4G_GAIN 122
#define LSM6DS_ACCEL_FS_8G_GAIN 244
#define LSM6DS_ACCEL_FS_16G_GAIN 488
#define LSM6DS_ACCEL_OUT_X_L_ADDR 0x28
#define LSM6DS_ACCEL_OUT_Y_L_ADDR 0x2a
#define LSM6DS_ACCEL_OUT_Z_L_ADDR 0x2c
#define LSM6DS_ACCEL_AXIS_EN_ADDR 0x18
#define LSM6DS_ACCEL_DRDY_IRQ_MASK 0x01
#define LSM6DS_ACCEL_STD 1
#define LSM6DS_ACCEL_STD_FROM_PD 2
/* CUSTOM VALUES FOR GYRO SENSOR */
#define LSM6DS_GYRO_ODR_ADDR 0x11
#define LSM6DS_GYRO_ODR_MASK 0xf0
#define LSM6DS_GYRO_FS_ADDR 0x11
#define LSM6DS_GYRO_FS_MASK 0x0c
#define LSM6DS_GYRO_FS_245_VAL 0x00
#define LSM6DS_GYRO_FS_500_VAL 0x01
#define LSM6DS_GYRO_FS_1000_VAL 0x02
#define LSM6DS_GYRO_FS_2000_VAL 0x03
#define LSM6DS_GYRO_FS_245_GAIN 8750
#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
#define LSM6DS_GYRO_AXIS_EN_ADDR 0x19
#define LSM6DS_GYRO_DRDY_IRQ_MASK 0x02
#define LSM6DS_GYRO_STD 6
#define LSM6DS_GYRO_STD_FROM_PD 2
#define LSM6DS_OUT_XYZ_SIZE 8
/* CUSTOM VALUES FOR SIGNIFICANT MOTION SENSOR */
#define LSM6DS_SIGN_MOTION_EN_ADDR 0x19
#define LSM6DS_SIGN_MOTION_EN_MASK 0x01
#define LSM6DS_SIGN_MOTION_DRDY_IRQ_MASK 0x40
/* CUSTOM VALUES FOR STEP DETECTOR SENSOR */
#define LSM6DS_STEP_DETECTOR_DRDY_IRQ_MASK 0x80
/* CUSTOM VALUES FOR STEP COUNTER SENSOR */
#define LSM6DS_STEP_COUNTER_DRDY_IRQ_MASK 0x80
#define LSM6DS_STEP_COUNTER_OUT_L_ADDR 0x4b
#define LSM6DS_STEP_COUNTER_OUT_SIZE 2
#define LSM6DS_STEP_COUNTER_RES_ADDR 0x19
#define LSM6DS_STEP_COUNTER_RES_MASK 0x06
#define LSM6DS_STEP_COUNTER_RES_ALL_EN 0x03
#define LSM6DS_STEP_COUNTER_RES_FUNC_EN 0x02
#define LSM6DS_STEP_COUNTER_DURATION_ADDR 0x15
/* CUSTOM VALUES FOR TILT SENSOR */
#define LSM6DS_TILT_EN_ADDR 0x58
#define LSM6DS_TILT_EN_MASK 0x20
#define LSM6DS_TILT_DRDY_IRQ_MASK 0x02
#define LSM6DS_ENABLE_AXIS 0x07
#define LSM6DS_FIFO_DIFF_L 0x3a
#define LSM6DS_FIFO_DIFF_MASK 0x0fff
#define LSM6DS_FIFO_DATA_OUT_L 0x3e
#define LSM6DS_FIFO_ELEMENT_LEN_BYTE 6
#define LSM6DS_FIFO_BYTE_FOR_CHANNEL 2
#define LSM6DS_FIFO_DATA_OVR_2REGS 0x4000
#define LSM6DS_FIFO_DATA_OVR 0x40
#define LSM6DS_SRC_FUNC_ADDR 0x53
#define LSM6DS_FIFO_DATA_AVL_ADDR 0x3b
#define LSM6DS_SRC_SIGN_MOTION_DATA_AVL 0x40
#define LSM6DS_SRC_STEP_DETECTOR_DATA_AVL 0x10
#define LSM6DS_SRC_TILT_DATA_AVL 0x20
#define LSM6DS_SRC_STEP_COUNTER_DATA_AVL 0x80
#define LSM6DS_FIFO_DATA_AVL 0x80
#define LSM6DS_RESET_ADDR 0x12
#define LSM6DS_RESET_MASK 0x01
#define LSM6DSLTR_ID 0x6A
#define LSM6DS33TR_ID 0x69
static const char configure[][2] = {
// ODR = 1000 (1.66 kHz (high performance)); FS_XL = 00 (+/-2 g full scale)
{LSM6DS_ACCEL_ODR_ADDR, 0x80},
// ODR = 1000 (1.66 kHz (high performance)); FS_XL = 00 (245 dps)
{LSM6DS_GYRO_ODR_ADDR, 0x80},
// IF_INC = 1 (automatically increment register address)
{LSM6DS_BDU_ADDR, 0x04}
};
#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)
{
uint8_t buffer[I2C_LSM6DS_MAX_WRITE];
if (len >= I2C_LSM6DS_MAX_WRITE - 1) return -1;
buffer[0] = reg;
memcpy(buffer + 1, data, len);
return stm32_i2c_master_tx(IMU_I2C_BUS, IMU_I2C_ADDRESS, buffer, len + 1, 100);
}
static int I2C_LSM6DS_WriteRegister(uint8_t reg, uint8_t value)
{
return I2C_LSM6DS_WriteRegister(reg, &value, 1);
}
static int I2C_LSM6DS_ReadRegister(uint8_t reg, uint8_t* data, uint8_t len)
{
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;
}
static int I2C_LSM6DS_ReadRegister(uint8_t reg)
{
uint8_t value = 0;
if (I2C_LSM6DS_ReadRegister(reg, &value, 1) < 0)
return -1;
return value;
}
int gyroInit()
{
if (i2c_init(IMU_I2C_BUS) < 0)
return -1;
// 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;
for (uint8_t i = 0; i < DIM(configure); i++) {
I2C_LSM6DS_WriteRegister(configure[i][0], configure[i][1]);
}
return 0;
}
int gyroRead(uint8_t buffer[IMU_BUFFER_LENGTH])
{
return I2C_LSM6DS_ReadRegister(LSM6DS_GYRO_OUT_X_L_ADDR, buffer, IMU_BUFFER_LENGTH);
}
#else
int gyroInit() { return -1; }
int gyroRead(uint8_t buffer[IMU_BUFFER_LENGTH]) { return -1; }
#endif
+1 -1
View File
@@ -291,7 +291,7 @@ add_library(board OBJECT EXCLUDE_FROM_ALL
${BOARD_COMMON_SRC}
targets/common/arm/stm32/delays_driver.cpp
targets/common/arm/stm32/heartbeat_driver.cpp
targets/common/arm/stm32/lsm6ds_driver.cpp
drivers/lsm6ds.cpp
targets/common/arm/stm32/mixer_scheduler_driver.cpp
targets/common/arm/stm32/module_timer_driver.cpp
targets/common/arm/stm32/sticks_pwm_driver.cpp
+21
View File
@@ -53,6 +53,10 @@
#include "csd203_sensor.h"
#endif
#if defined(IMU) && defined(IMU_I2C_BUS) && defined(IMU_I2C_ADDRESS)
#define HAS_IMU
#endif
HardwareOptions hardwareOptions;
bool boardBacklightOn = false;
@@ -150,6 +154,19 @@ uint16_t getSixPosAnalogValue(uint16_t adcValue)
}
#endif
#if defined(HAS_IMU)
#include "drivers/lsm6ds.h"
#include "stm32_i2c_driver.h"
static void gyroInit()
{
const etx_imu_t candidates[] = {
{ &imu_lsm6ds_driver, IMU_I2C_BUS, IMU_I2C_ADDRESS },
};
gyroStart(imuDetect(candidates, DIM(candidates)));
}
#endif
void boardInit()
{
delaysInit();
@@ -246,6 +263,10 @@ void boardInit()
ledGreen();
#endif
#endif
#if defined(HAS_IMU)
gyroInit();
#endif
}
#endif
-263
View File
@@ -1,263 +0,0 @@
/*
* Copyright (C) EdgeTX
*
* Based on code named
* opentx - https://github.com/opentx/opentx
* th9x - http://code.google.com/p/th9x
* er9x - http://code.google.com/p/er9x
* gruvin9x - http://code.google.com/p/gruvin9x
*
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#include "edgetx.h"
#include "i2c_driver.h"
#include "imu_lsm6ds33.h"
I2C_HandleTypeDef hi2c2;
sIMUsettings IMUsettings = {-1, -1.0, -1, -1, -1.0};
sIMUoutput IMUoutput = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
float fIMUtemp = 0.0;
bool I2C_LSM6DS33_ReadRegister(uint8_t reg, uint8_t * buf, uint8_t len)
{
if (HAL_I2C_Master_Transmit(&hi2c2, LSM6DS33_I2C_ADDR << 1, &reg, 1, 10000) != HAL_OK)
{
TRACE("I2C B2 ERROR: ReadRegister write reg. address failed");
return false;
}
if (HAL_I2C_Master_Receive(&hi2c2, LSM6DS33_I2C_ADDR << 1, buf, len, 10000) != HAL_OK)
{
TRACE("I2C B2 ERROR: ReadRegister read register failed");
return false;
}
return true;
}
bool I2C_LSM6DS33_WriteRegister(uint8_t reg, uint8_t * buf, uint8_t len)
{
uint8_t uAddrAndBuf[15];
uAddrAndBuf[0] = reg;
if (len > 0)
{
for (int i = 0;i < len;i++)
{
uAddrAndBuf[i + 1] = buf[i];
}
}
if (HAL_I2C_Master_Transmit(&hi2c2, LSM6DS33_I2C_ADDR << 1, uAddrAndBuf, len + 1, 10000) != HAL_OK)
{
TRACE("I2C B2 ERROR: WriteRegister failed");
return false;
}
return true;
}
bool IMUgetConf(void)
{
uint8_t ui8_regs[2] = {0};
if (!I2C_LSM6DS33_ReadRegister(LSM6DS33_CTRL1_XL_ADDR, ui8_regs, 2))
{
TRACE("ERROR: LSM6DS33 did not respond to I2C address");
return false;
}
switch ((ui8_regs[0] >> 4) & 0x0F) // CTRL1_XL
{
case LSM6DS33_ODR_0Hz: IMUsettings.linacc_odr = 0; TRACE("LSM6DS33 lin. acc. output turned off"); break;
case LSM6DS33_ODR_12_5Hz: IMUsettings.linacc_odr = 13; break;
case LSM6DS33_ODR_26Hz: IMUsettings.linacc_odr = 26; break;
case LSM6DS33_ODR_52Hz: IMUsettings.linacc_odr = 52; break;
case LSM6DS33_ODR_104Hz: IMUsettings.linacc_odr = 104; break;
case LSM6DS33_ODR_208Hz: IMUsettings.linacc_odr = 208; break;
case LSM6DS33_ODR_416Hz: IMUsettings.linacc_odr = 417; break;
case LSM6DS33_ODR_833Hz: IMUsettings.linacc_odr = 833; break;
case LSM6DS33_ODR_1_66kHz: IMUsettings.linacc_odr = 1667; break;
case LSM6DS33_ODR_3_33kHz: IMUsettings.linacc_odr = 3333; break;
case LSM6DS33_ODR_6_66kHz: IMUsettings.linacc_odr = 6667; break;
default: TRACE("ERROR: LSM6DS33 lin.acc. output data rate at illegal setting"); return false; break;
}
if (IMUsettings.linacc_odr > 0) TRACE("LSM6DS33 lin. acc. output at %i Hz", IMUsettings.linacc_odr);
switch ((ui8_regs[0] >> 2) & 0x03)
{
case LSM6DS33_LA_FS_2G: IMUsettings.linacc_mG = 0.061; break;
case LSM6DS33_LA_FS_4G: IMUsettings.linacc_mG = 0.122; break;
case LSM6DS33_LA_FS_8G: IMUsettings.linacc_mG = 0.244; break;
case LSM6DS33_LA_FS_16G: IMUsettings.linacc_mG = 0.488; break;
default: TRACE("ERROR: LSM6DS33 lin. acc full scale value at illegal setting"); return false; break;
}
TRACE("LSM6DS33 lin. acc. resolution %.2f G", IMUsettings.linacc_mG);
switch ((ui8_regs[0] >> 2) & 0x03)
{
case LSM6DS33_LA_AABW_400Hz: IMUsettings.linacc_aabw = 400; break;
case LSM6DS33_LA_AABW_200Hz: IMUsettings.linacc_aabw = 200; break;
case LSM6DS33_LA_AABW_100Hz: IMUsettings.linacc_aabw = 100; break;
case LSM6DS33_LA_AABW_50Hz: IMUsettings.linacc_aabw = 50; break;
default: TRACE("ERROR: LSM6DS33 lin. acc anti aliasing bandwidth at illegal setting"); return false; break;
}
TRACE("LSM6DS33 lin. acc. anti aliasing bandwidth at %i Hz", IMUsettings.linacc_aabw);
switch ((ui8_regs[1] >> 4) & 0x0F) // CTRL2_G
{
case LSM6DS33_ODR_0Hz: IMUsettings.gyro_odr = 0; TRACE("LSM6DS33 gyro output turned off"); break;
case LSM6DS33_ODR_12_5Hz: IMUsettings.gyro_odr = 13; break;
case LSM6DS33_ODR_26Hz: IMUsettings.gyro_odr = 26; break;
case LSM6DS33_ODR_52Hz: IMUsettings.gyro_odr = 52; break;
case LSM6DS33_ODR_104Hz: IMUsettings.gyro_odr = 104; break;
case LSM6DS33_ODR_208Hz: IMUsettings.gyro_odr = 208; break;
case LSM6DS33_ODR_416Hz: IMUsettings.gyro_odr = 417; break;
case LSM6DS33_ODR_833Hz: IMUsettings.gyro_odr = 833; break;
case LSM6DS33_ODR_1_66kHz: IMUsettings.gyro_odr = 1667; break;
default: TRACE("ERROR: LSM6DS33 gyro output data rate at illegal setting"); return false; break;
}
if (IMUsettings.gyro_odr > 0) TRACE("LSM6DS33 gyro output at %i Hz", IMUsettings.gyro_odr);
switch ((ui8_regs[1] >> 1) & 0x07)
{
case LSM6DS33_GY_FS_125dps: IMUsettings.gyro_mDPS = 4.375; break;
case LSM6DS33_GY_FS_250dps: IMUsettings.gyro_mDPS = 8.75; break;
case LSM6DS33_GY_FS_500dps: IMUsettings.gyro_mDPS = 17.5; break;
case LSM6DS33_GY_FS_1000dps: IMUsettings.gyro_mDPS = 35.0; break;
case LSM6DS33_GY_FS_2000dps: IMUsettings.gyro_mDPS = 70.0; break;
default: TRACE("ERROR: LSM6DS33 gyro full scale value at illegal setting"); return false; break;
}
TRACE("LSM6DS33 raw gyro resolution: %.2f milli-degrees/sec.", IMUsettings.gyro_mDPS);
return true;
}
void init_imu_i2c(void)
{
TRACE("I2C IMU Init");
hi2c2.Instance = I2C_B2;
hi2c2.Init.ClockSpeed = I2C_B2_CLK_RATE;
hi2c2.Init.DutyCycle = I2C_DUTYCYCLE_2;
hi2c2.Init.OwnAddress1 = 0;
hi2c2.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
hi2c2.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
hi2c2.Init.OwnAddress2 = 0;
hi2c2.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
hi2c2.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
if (HAL_I2C_Init(&hi2c2) != HAL_OK)
{
TRACE("I2C B2 ERROR: HAL_I2C_Init() failed");
}
// Configure Analogue filter
if (HAL_I2CEx_ConfigAnalogFilter(&hi2c2, I2C_ANALOGFILTER_ENABLE) != HAL_OK)
{
TRACE("I2C B2 ERROR: HAL_I2CEx_ConfigAnalogFilter() failed");
}
// Configure Digital filter
if (HAL_I2CEx_ConfigDigitalFilter(&hi2c2, 0) != HAL_OK)
{
TRACE("I2C B2 ERROR: HAL_I2CEx_ConfigDigitalFilter() failed");
}
}
bool imu_lsm6ds33_init(void)
{
uint8_t ui8_reg[2] = {0};
GPIO_SetBits(AUX_I2C_B2_PWR_GPIO, AUX_I2C_B2_PWR_GPIO_PIN); // Turn on AUX1 power
init_imu_i2c();
if (!I2C_LSM6DS33_ReadRegister(LSM6DS33_WHO_AM_I_ADDR, ui8_reg, 1))
{
TRACE("ERROR: LSM6DS33 did not respond to I2C address");
return false;
}
if (ui8_reg[0] != LSM6DS33_WHOAMI)
{
TRACE("LSM6DS33 ERROR: Product ID read failes");
return false;
}
TRACE("LSM6DS33 detected");
// Set IMU configuration
ui8_reg[0] = (LSM6DS33_ODR_104Hz << 4) | (LSM6DS33_LA_FS_4G << 2) | LSM6DS33_LA_AABW_100Hz; // Linear acceleration configuration
ui8_reg[1] = (LSM6DS33_ODR_104Hz << 4) | (LSM6DS33_GY_FS_125dps < 1); // Gyro configuration
if (!I2C_LSM6DS33_WriteRegister(LSM6DS33_CTRL1_XL_ADDR, ui8_reg, 2))
{
TRACE("ERROR: Failed to write LSM6DS33 configuration");
return false;
}
if (!IMUgetConf())
{
TRACE("ERROR: Failed ro read LSM6DS33 configuration");
return false;
}
return true;
}
bool imu_lsm6ds33_read()
{
uint8_t ui8_regs[14] = {0};
int16_t i16_rawTemperature = 0;
int16_t i16_rawGyroX = 0;
int16_t i16_rawGyroY = 0;
int16_t i16_rawGyroZ = 0;
int16_t i16_rawAccX = 0;
int16_t i16_rawAccY = 0;
int16_t i16_rawAccZ = 0;
// Check first if new data is available
if (!I2C_LSM6DS33_ReadRegister(LSM6DS33_STATUS_REG_ADDR, ui8_regs, 1))
{
TRACE("ERROR: LSM6DS33 did not respond to I2C address");
return false;
}
if ((ui8_regs[0] & 0x07) != 0x07)
{
TRACE("ERROR: LSM6DS33 has no new data available yet");
return false;
}
if (!I2C_LSM6DS33_ReadRegister(LSM6DS33_OUT_TEMP_L_ADDR, ui8_regs, 14))
{
TRACE("ERROR: LSM6DS33 did not respond to I2C address");
return false;
}
i16_rawTemperature = (ui8_regs[1] << 8) | ui8_regs[0];
i16_rawGyroX = (ui8_regs[3] << 8) | ui8_regs[2];
i16_rawGyroY = (ui8_regs[5] << 8) | ui8_regs[4];
i16_rawGyroZ = (ui8_regs[7] << 8) | ui8_regs[6];
i16_rawAccX = (ui8_regs[9] << 8) | ui8_regs[8];
i16_rawAccY = (ui8_regs[11] << 8) | ui8_regs[10];
i16_rawAccZ = (ui8_regs[13] << 8) | ui8_regs[12];
IMUoutput.fGyroXradps = i16_rawGyroX * IMUsettings.gyro_mDPS * M_PI/180.0 / 1000.0;
IMUoutput.fGyroYradps = i16_rawGyroY * IMUsettings.gyro_mDPS * M_PI/180.0 / 1000.0;
IMUoutput.fGyroZradps = i16_rawGyroZ * IMUsettings.gyro_mDPS * M_PI/180.0 / 1000.0;
IMUoutput.fAccX = i16_rawAccX * IMUsettings.linacc_mG * GRAVITY_EARTH / 1000.0;
IMUoutput.fAccY = i16_rawAccY * IMUsettings.linacc_mG * GRAVITY_EARTH / 1000.0;
IMUoutput.fAccZ = i16_rawAccZ * IMUsettings.linacc_mG * GRAVITY_EARTH / 1000.0;
IMUoutput.fTemperatureDegC = (float)i16_rawTemperature / 16.0 + 25.0;
/*TRACE("LSM6DS33: %.2f deg.C, GyroXYZ [rad/s]: %.2f, %.2f, %.2f, AccXYZ [m/s^2]: %.2f %.2f %.2f",
IMUoutput.fTemperatureDegC,
IMUoutput.fGyroXradps, IMUoutput.fGyroYradps, IMUoutput.fGyroZradps,
IMUoutput.fAccX, IMUoutput.fAccY, IMUoutput.fAccZ); */
return true;
}
-139
View File
@@ -1,139 +0,0 @@
/*
* Copyright (C) EdgeTX
*
* Based on code named
* opentx - https://github.com/opentx/opentx
* th9x - http://code.google.com/p/th9x
* er9x - http://code.google.com/p/er9x
* gruvin9x - http://code.google.com/p/gruvin9x
*
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#pragma once
#include "i2c_driver.h"
#define GRAVITY_EARTH 9.80665F // m/s^2
#define LSM6DS33_I2C_ADDR 0x6A
#define LSM6DS33_WHOAMI 0x69
#define LSM6DS33_TIMEOUT 3 // 3ms
// LSM6DS33 register map
#define LSM6DS33_FUNC_CFG_ACCESS_ADDR 0x01
#define LSM6DS33_FIFO_CTRL1_ADDR 0x06
#define LSM6DS33_FIFO_CTRL2_ADDR 0x07
#define LSM6DS33_FIFO_CTRL3_ADDR 0x08
#define LSM6DS33_FIFO_CTRL4_ADDR 0x09
#define LSM6DS33_FIFO_CTRL5_ADDR 0x0A
#define LSM6DS33_ORIENT_CFG_G_ADDR 0x0B
#define LSM6DS33_INT1_CTRL_ADDR 0x0D
#define LSM6DS33_INT2_CTRL_ADDR 0x0E
#define LSM6DS33_WHO_AM_I_ADDR 0x0F
#define LSM6DS33_CTRL1_XL_ADDR 0x10
#define LSM6DS33_CTRL2_G_ADDR 0x11
#define LSM6DS33_CTRL3_C_ADDR 0x12
#define LSM6DS33_CTRL4_C_ADDR 0x13
#define LSM6DS33_CTRL5_C_ADDR 0x14
#define LSM6DS33_CTRL6_C_ADDR 0x15
#define LSM6DS33_CTRL7_G_ADDR 0x16
#define LSM6DS33_CTRL8_XL_ADDR 0x17
#define LSM6DS33_CTRL9_XL_ADDR 0x18
#define LSM6DS33_CTRL10_C_ADDR 0x19
#define LSM6DS33_WAKE_UP_SRC_ADDR 0x1B
#define LSM6DS33_TAP_SRC_ADDR 0x1C
#define LSM6DS33_D6D_SRC_ADDR 0x1D
#define LSM6DS33_STATUS_REG_ADDR 0x1E
#define LSM6DS33_OUT_TEMP_L_ADDR 0x20
#define LSM6DS33_OUT_TEMP_H_ADDR 0x21
#define LSM6DS33_OUTX_L_G_ADDR 0x22
#define LSM6DS33_OUTX_H_G_ADDR 0x23
#define LSM6DS33_OUTY_L_G_ADDR 0x24
#define LSM6DS33_OUTY_H_G_ADDR 0x25
#define LSM6DS33_OUTZ_L_G_ADDR 0x26
#define LSM6DS33_OUTZ_H_G_ADDR 0x27
#define LSM6DS33_OUTX_L_XL_ADDR 0x28
#define LSM6DS33_OUTX_H_XL_ADDR 0x29
#define LSM6DS33_OUTY_L_XL_ADDR 0x2A
#define LSM6DS33_OUTY_H_XL_ADDR 0x2B
#define LSM6DS33_OUTZ_L_XL_ADDR 0x2C
#define LSM6DS33_OUTZ_H_XL_ADDR 0x2D
#define LSM6DS33_FIFO_STATUS1_ADDR 0x3A
#define LSM6DS33_FIFO_STATUS2_ADDR 0x3B
#define LSM6DS33_FIFO_STATUS3_ADDR 0x3C
#define LSM6DS33_FIFO_STATUS4_ADDR 0x3D
#define LSM6DS33_FIFO_DATA_OUT_L_ADDR 0x3E
#define LSM6DS33_FIFO_DATA_OUT_H_ADDR 0x3F
#define LSM6DS33_TIMESTAMP0_REG_ADDR 0x40
#define LSM6DS33_TIMESTAMP1_REG_ADDR 0x41
#define LSM6DS33_TIMESTAMP2_REG_ADDR 0x42
#define LSM6DS33_STEP_TIMESTAMP_L_ADDR 0x49
#define LSM6DS33_STEP_TIMESTAMP_H_ADDR 0x4A
#define LSM6DS33_STEP_COUNTER_L_ADDR 0x4B
#define LSM6DS33_STEP_COUNTER_H_ADDR 0x4C
#define LSM6DS33_FUNC_SRC_ADDR 0x53
#define LSM6DS33_TAP_CFG_ADDR 0x58
#define LSM6DS33_TAP_THS_6D_ADDR 0x59
#define LSM6DS33_INT_DUR2_ADDR 0x5A
#define LSM6DS33_WAKE_UP_THS_ADDR 0x5B
#define LSM6DS33_WAKE_UP_DUR_ADDR 0x5C
#define LSM6DS33_FREE_FALL_ADDR 0x5D
#define LSM6DS33_MD1_CFG_ADDR 0x5E
#define LSM6DS33_MD2_CFG_ADDR 0x5F
#define LSM6DS33_ODR_0Hz 0x00
#define LSM6DS33_ODR_12_5Hz 0x01
#define LSM6DS33_ODR_26Hz 0x02
#define LSM6DS33_ODR_52Hz 0x03
#define LSM6DS33_ODR_104Hz 0x04
#define LSM6DS33_ODR_208Hz 0x05
#define LSM6DS33_ODR_416Hz 0x06
#define LSM6DS33_ODR_833Hz 0x07
#define LSM6DS33_ODR_1_66kHz 0x08
#define LSM6DS33_ODR_3_33kHz 0x09
#define LSM6DS33_ODR_6_66kHz 0x0A
#define LSM6DS33_LA_FS_2G 0x00
#define LSM6DS33_LA_FS_16G 0x01
#define LSM6DS33_LA_FS_4G 0x02
#define LSM6DS33_LA_FS_8G 0x03
#define LSM6DS33_LA_AABW_400Hz 0x00
#define LSM6DS33_LA_AABW_200Hz 0x01
#define LSM6DS33_LA_AABW_100Hz 0x02
#define LSM6DS33_LA_AABW_50Hz 0x03
#define LSM6DS33_GY_FS_125dps 0x01
#define LSM6DS33_GY_FS_250dps 0x00
#define LSM6DS33_GY_FS_500dps 0x02
#define LSM6DS33_GY_FS_1000dps 0x04
#define LSM6DS33_GY_FS_2000dps 0x05
typedef struct {
int16_t linacc_odr;
float linacc_mG; // milli G per Bit
int16_t linacc_aabw;
int16_t gyro_odr;
float gyro_mDPS; // milli degrees per second per Bit
} sIMUsettings;
typedef struct {
float fTemperatureDegC; //°C
float fAccX, fAccY, fAccZ; // m/s^2
float fGyroXradps, fGyroYradps, fGyroZradps; // rad/s
} sIMUoutput;
extern sIMUoutput IMUoutput;
bool imu_lsm6ds33_init(void);
bool imu_lsm6ds33_read();
+1 -1
View File
@@ -146,7 +146,7 @@ add_library(board OBJECT EXCLUDE_FROM_ALL
${TARGET_SRC_DIR}/battery_driver.cpp
targets/common/arm/stm32/audio_dac_driver.cpp
targets/common/arm/stm32/heartbeat_driver.cpp
targets/common/arm/stm32/lsm6ds_driver.cpp
drivers/lsm6ds.cpp
targets/common/arm/stm32/mixer_scheduler_driver.cpp
targets/common/arm/stm32/module_timer_driver.cpp
targets/common/arm/stm32/stm32_pulse_driver.cpp
+12
View File
@@ -47,11 +47,13 @@
#include "sdcard.h"
#include "debug.h"
#include "keys.h"
#include "gyro.h"
#include "flysky_gimbal_driver.h"
#include "timers_driver.h"
#include "battery_driver.h"
#include "drivers/lsm6ds.h"
#include <string.h>
@@ -126,6 +128,14 @@ void boardBLInit()
flashRegisterDriver(QSPI_BASE, 8 * 1024 * 1024, &extflash_driver);
}
static void gyroInit()
{
const etx_imu_t candidates[] = {
{ &imu_lsm6ds_driver, IMU_I2C_BUS, IMU_I2C_ADDRESS },
};
gyroStart(imuDetect(candidates, DIM(candidates)));
}
bool pwrPressedDebounced()
{
static bool debouncedState = 0;
@@ -377,6 +387,8 @@ void boardInit()
#if defined(RTCLOCK)
rtcInit(); // RTC must be initialized before rambackupRestore() is called
#endif
gyroInit();
}
extern void rtcDisableBackupReg();
-1
View File
@@ -35,7 +35,6 @@ set(SIMU_DRIVERS
module_drivers.cpp
led_driver.cpp
backlight_driver.cpp
gyro_driver.cpp
bt_driver.cpp
timers_driver.cpp
abnormal_reboot.cpp
-25
View File
@@ -1,25 +0,0 @@
/*
* Copyright (C) EdgeTX
*
* Based on code named
* opentx - https://github.com/opentx/opentx
* th9x - http://code.google.com/p/th9x
* er9x - http://code.google.com/p/er9x
* gruvin9x - http://code.google.com/p/gruvin9x
*
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#include "gyro.h"
int gyroInit() { return -1; }
int gyroRead(uint8_t*) { return -1; }
+1 -1
View File
@@ -149,7 +149,7 @@ add_library(board OBJECT EXCLUDE_FROM_ALL
${TARGET_SRC_DIR}/touch_driver.cpp
${TARGET_SRC_DIR}/battery_driver.cpp
targets/common/arm/stm32/heartbeat_driver.cpp
targets/common/arm/stm32/lsm6ds_driver.cpp
drivers/lsm6ds.cpp
targets/common/arm/stm32/mixer_scheduler_driver.cpp
targets/common/arm/stm32/module_timer_driver.cpp
targets/common/arm/stm32/stm32_pulse_driver.cpp
+12
View File
@@ -48,11 +48,13 @@
#include "sdcard.h"
#include "debug.h"
#include "keys.h"
#include "gyro.h"
#include "flysky_gimbal_driver.h"
#include "timers_driver.h"
#include "battery_driver.h"
#include "drivers/lsm6ds.h"
#include "bitmapbuffer.h"
#include "colors.h"
@@ -173,6 +175,14 @@ void boardBLInit()
flashRegisterDriver(QSPI_BASE, 8 * 1024 * 1024, &extflash_driver);
}
static void gyroInit()
{
const etx_imu_t candidates[] = {
{ &imu_lsm6ds_driver, IMU_I2C_BUS, IMU_I2C_ADDRESS },
};
gyroStart(imuDetect(candidates, DIM(candidates)));
}
void boardInit()
{
// enable interrupts
@@ -263,6 +273,8 @@ void boardInit()
#if defined(RTCLOCK)
rtcInit(); // RTC must be initialized before rambackupRestore() is called
#endif
gyroInit();
}
extern void rtcDisableBackupReg();
@@ -99,7 +99,6 @@ add_library(board OBJECT EXCLUDE_FROM_ALL
${TARGET_SRC_DIR}/tp_gt911.cpp
${TARGET_SRC_DIR}/audio_driver.cpp
drivers/wm8904.cpp
targets/common/arm/stm32/lsm6ds_driver.cpp
targets/common/arm/stm32/mixer_scheduler_driver.cpp
targets/common/arm/stm32/stm32_switch_driver.cpp
targets/common/arm/stm32/stm32_i2s.cpp
@@ -40,6 +40,7 @@
#include "sdcard.h"
#include "touch.h"
#include "debug.h"
#include "gyro.h"
#include "timers_driver.h"
#include "tp_gt911.h"
-1
View File
@@ -149,7 +149,6 @@ add_library(board OBJECT EXCLUDE_FROM_ALL
${BOARD_SRC_DIR}/touch_driver.cpp
targets/common/arm/stm32/audio_dac_driver.cpp
targets/common/arm/stm32/heartbeat_driver.cpp
targets/common/arm/stm32/lsm6ds_driver.cpp
targets/common/arm/stm32/mixer_scheduler_driver.cpp
targets/common/arm/stm32/module_timer_driver.cpp
targets/common/arm/stm32/stm32_pulse_driver.cpp
+1 -1
View File
@@ -627,7 +627,7 @@ if(USB_CHARGER)
endif()
if(IMU)
target_sources(board PRIVATE targets/common/arm/stm32/lsm6ds_driver.cpp)
target_sources(board PRIVATE drivers/lsm6ds.cpp)
endif()
if(AUTOUPDATE)
+19
View File
@@ -125,6 +125,21 @@ void SDLEDpwrInit()
}
#endif
#if defined(IMU) && defined(IMU_I2C_BUS) && defined(IMU_I2C_BUS)
#include "drivers/lsm6ds.h"
#include "stm32_i2c_driver.h"
#define HAS_IMU
static void gyroInit()
{
const etx_imu_t candidates[] = {
{ &imu_lsm6ds_driver, IMU_I2C_BUS, IMU_I2C_ADDRESS },
};
gyroStart(imuDetect(candidates, DIM(candidates)));
}
#endif
void boardInit()
{
LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_SYSCFG);
@@ -277,6 +292,10 @@ void boardInit()
#if defined(RADIO_GX12)
gpio_init(HALL_SYNC, GPIO_OUT, GPIO_PIN_SPEED_LOW);
#endif
#if defined(HAS_IMU)
gyroInit();
#endif
}
#endif
+4 -3
View File
@@ -45,6 +45,7 @@ endif()
# IMU support
set(IMU ON)
add_definitions(-DIMU_ICM4207C)
add_definitions(-DIMU_SC7U22)
# for size report script
set(CPU_TYPE_FULL STM32H750xB)
@@ -144,6 +145,9 @@ set(BOARD_COMMON_SRC
${BOARD_SRC_DIR}/extflash_driver.cpp
${BOARD_SRC_DIR}/usb_charger_driver.cpp
${TARGET_SRC_DIR}/key_driver.cpp
drivers/pca95xx.cpp
drivers/icm42607C.cpp
drivers/sc7u22.cpp
targets/common/arm/stm32/abnormal_reboot.cpp
targets/common/arm/stm32/delays_driver.cpp
targets/common/arm/stm32/dma2d.cpp
@@ -152,7 +156,6 @@ set(BOARD_COMMON_SRC
targets/common/arm/stm32/rotary_encoder_driver.cpp
targets/common/arm/stm32/rtc_driver.cpp
targets/common/arm/stm32/watchdog_driver.cpp
drivers/pca95xx.cpp
)
if(BLUETOOTH)
@@ -186,8 +189,6 @@ add_library(board OBJECT EXCLUDE_FROM_ALL
${BOARD_COMMON_SRC}
${BOARD_SRC_DIR}/tp_gt911.cpp
${BOARD_SRC_DIR}/audio_driver.cpp
drivers/icm42607C.cpp
drivers/imu_filter.cpp
drivers/tas2505.cpp
targets/common/arm/stm32/heartbeat_driver.cpp
targets/common/arm/stm32/mixer_scheduler_driver.cpp
+1 -1
View File
@@ -382,8 +382,8 @@ TIM17: ROTARY_ENCODER_TIMER
// IMU
#define IMU_I2C_BUS I2C_Bus_1
#define IMU_I2C_ADDRESS 0x6A
#define IMU_INT_GPIO GPIO_PIN(GPIOG, 13) // PG.13
// IMU_INT_EXTI IRQ
#if !defined(USE_EXTI15_10_IRQ)
#define USE_EXTI15_10_IRQ
+3 -2
View File
@@ -36,6 +36,7 @@ set(INTERNAL_GPS_BAUDRATE "9600" CACHE STRING "Baud rate for internal GPS")
# IMU support
set(IMU ON)
add_definitions(-DIMU_ICM4207C)
add_definitions(-DIMU_SC7U22)
# for size report script
set(CPU_TYPE_FULL STM32H750xB)
@@ -172,9 +173,9 @@ add_library(board OBJECT EXCLUDE_FROM_ALL
${BOARD_SRC_DIR}/tp_gt911.cpp
${BOARD_SRC_DIR}/audio_driver.cpp
${BOARD_SRC_DIR}/luminosity_sensor.cpp
drivers/icm42607C.cpp
drivers/imu_filter.cpp
drivers/tas2505.cpp
drivers/icm42607C.cpp
drivers/sc7u22.cpp
targets/common/arm/stm32/heartbeat_driver.cpp
targets/common/arm/stm32/mixer_scheduler_driver.cpp
targets/common/arm/stm32/module_timer_driver.cpp
+1 -2
View File
@@ -572,8 +572,7 @@ USART6: INTMODULE_USART
#endif
#define IMU_I2C_BUS I2C_Bus_2
#define IMU_I2C_ADDRESS 0x6A
#define IMU_INT_GPIO GPIO_PIN(GPIOG, 13) // PG.13
#define IMU_INT_GPIO GPIO_PIN(GPIOG, 13) // PG.13
#if !defined(USE_EXTI15_10_IRQ)
#define USE_EXTI15_10_IRQ
#define EXTI15_10_IRQ_Priority 6