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
+54 -50
View File
@@ -1,5 +1,5 @@
/*
* Copyright (C) EdgeTX
* Copyright (C) EdgeTX
*
* Based on code named
* opentx - https://github.com/opentx/opentx
@@ -24,33 +24,36 @@
with code for generic ICM4267XX. All register have different addresses
*/
#include "edgetx.h"
#include "hal/i2c_driver.h"
#include "stm32_i2c_driver.h"
#include "hal/imu.h"
#include "hal/gpio.h"
#include "delays_driver.h"
#include "icm42607C.h"
#include "imu_filter.h"
#include "stm32_gpio.h"
#include "inactivity_timer.h"
#include "hal.h"
#include "debug.h"
constexpr uint32_t I2C_TIMEOUT = 5; //ms
static etx_i2c_bus_t s_i2c_bus;
static uint16_t s_i2c_addr;
int16_t getGyroTemperature()
static int16_t __attribute__((unused)) get42607Temperature()
{
uint8_t reg = TEMP_DATA_X0_REG;
uint8_t buf[2] = {0};
if (stm32_i2c_read(IMU_I2C_BUS, ICM426xx_I2C_ADDR, reg, 1, buf, 2, I2C_TIMEOUT) < 0) {
if (i2c_read(s_i2c_bus, s_i2c_addr, TEMP_DATA_X0_REG, 1, buf, 2) < 0) {
TRACE("ICM426xx ERROR: TEMP_DATA_X0_REG i2c read error");
return -1;
}
return (((buf[0] << 8) | buf[1]) / 128) + 25;
int16_t raw = (int16_t)((buf[0] << 8) | buf[1]);
return (int16_t)(raw / 128 + 25);
}
int write_cmd(uint8_t reg, uint8_t val)
static int write_cmd(uint8_t reg, uint8_t val)
{
return i2c_write(IMU_I2C_BUS, ICM426xx_I2C_ADDR, reg, 1, &val, 1);
return i2c_write(s_i2c_bus, s_i2c_addr, reg, 1, &val, 1);
}
#if defined(IMU_INT_GPIO)
@@ -60,30 +63,35 @@ static void imu_exti_isr(void)
}
#endif
#if defined(IMU_INT_GPIO)
static int write_mreg1(uint8_t reg, uint8_t val) {
if (write_cmd(BLK_SEL_W_REG, 0x00) < 0) return -1;
if (write_cmd(BLK_SEL_W_REG, 0x00) < 0) return -1;
if (write_cmd(MADDR_W_REG, reg) < 0) return -1;
if (write_cmd(M_W_REG, val) < 0) return -1;
delay_us(10); //42627-pdf p12“Accessing MREG1, MREG2 And MREG3 Registers
delay_us(10); // small delay as recommended by datasheet
return 0;
}
#endif
int gyroInit(void)
static int gyro42607Init(etx_i2c_bus_t bus, uint16_t addr)
{
TRACE("ICM426xx I2C Init");
s_i2c_bus = bus;
s_i2c_addr = addr;
if (i2c_init(IMU_I2C_BUS) < 0) {
TRACE("ICM426xx I2C Init at address 0x%x", addr);
if (i2c_init(s_i2c_bus) < 0) {
TRACE("ICM426xx ERROR: i2c_init bus error");
return -1;
}
if (i2c_dev_ready(IMU_I2C_BUS, ICM426xx_I2C_ADDR) < 0) {
if (i2c_dev_ready(s_i2c_bus, s_i2c_addr) < 0) {
TRACE("ICM426xx device init error");
return -1;
}
uint8_t data = 0;
if (stm32_i2c_read(IMU_I2C_BUS, ICM426xx_I2C_ADDR, 0x75, 1, &data, 1, I2C_TIMEOUT) < 0) {
if (i2c_read(s_i2c_bus, s_i2c_addr, WHO_AM_I_REG, 1, &data, 1) < 0) {
TRACE("ICM426xx ERROR: i2c read error");
return -1;
}
@@ -105,14 +113,14 @@ int gyroInit(void)
}
delay_ms(10);
// Set gyro to 2000 dps, 1kHz ODR
// Configure gyro: 2000 dps, 1 kHz ODR
if(write_cmd(GYRO_CONFIG0_REG, GYRO_ODR_1KHZ) < 0) {
TRACE("ICM426xx ERROR: GYRO_CONFIG0_REG error");
return -1;
}
delay_ms(1);
// Set accel to ±4g, 1kHz ODR
// Configure accelerometer: ±4g, 1 kHz ODR
if (write_cmd(ACCEL_CONFIG0_REG, ACCEL_ODR_1KHZ) < 0) {
TRACE("ICM426xx ERROR: ACCEL_CONFIG0_REG error");
return -1;
@@ -127,58 +135,54 @@ int gyroInit(void)
delay_ms(1);
#if defined(IMU_INT_GPIO)
// CCEL_WOM_X_THR:0-255(0-1g),0x10--16mg
// Configure Wake-on-Motion thresholds via MREG1 interface
// Values chosen to approximate activity detection behaviour used elsewhere
if (write_mreg1(ACCEL_WOM_X_THR_REG, 0xFE) < 0) return -1;
if (write_mreg1(ACCEL_WOM_Y_THR_REG, 0xFE) < 0) return -1;
if (write_mreg1(ACCEL_WOM_Z_THR_REG, 0xFE) < 0) return -1;
delay_ms(1);
// config WoM
// Configure WoM mode (OR across axes)
if (write_cmd(WOM_CONFIG_REG, 0x03) < 0) return -1; // OR mode
delay_ms(1);
// enable INT1
// Enable INT1 source bits for XYZ
if (write_cmd(INT_SOURCE1_REG, 0x07) < 0) return -1; // XYZ interrupt
delay_ms(1);
// config INT1 pin
// Ensure INT1 is configured to default behaviour
if (write_cmd(INT_CONFIG_REG, 0x00) < 0) return -1;
gpio_init_int(IMU_INT_GPIO, GPIO_IN_PU, GPIO_FALLING, imu_exti_isr);
#endif
TRACE("ICM426xx succeeded");
return 0;
}
int gyroRead(uint8_t buffer[IMU_BUFFER_LENGTH])
static int gyro42607Read(etx_imu_data_t* data)
{
IMU_RawData_t imu_raw;
IMU_FilteredData_t *filtered;
uint8_t buf[6];
uint8_t reg = GYRO_DATA_X0_REG;
if (stm32_i2c_read(IMU_I2C_BUS, ICM426xx_I2C_ADDR, reg, 1, buf, 6, I2C_TIMEOUT) < 0) {
TRACE("ICM426xx ERROR: i2c read error");
if (i2c_read(s_i2c_bus, s_i2c_addr, GYRO_DATA_X0_REG, 1, buf, 6) < 0) {
TRACE("ICM426xx ERROR: gyro read error");
return -1;
}
imu_raw.gyro_x = -((int16_t)(buf[0] << 8) | buf[1]); // x
imu_raw.gyro_y = -((int16_t)(buf[2] << 8) | buf[3]); // y
imu_raw.gyro_z = ((int16_t)(buf[4] << 8) | buf[5]); // z
data->gyro_x = -((int16_t)(buf[0] << 8) | buf[1]);
data->gyro_y = -((int16_t)(buf[2] << 8) | buf[3]);
data->gyro_z = ((int16_t)(buf[4] << 8) | buf[5]);
reg = ACCEL_DATA_X0_REG;
if (stm32_i2c_read(IMU_I2C_BUS, ICM426xx_I2C_ADDR, reg, 1, buf, 6, I2C_TIMEOUT) < 0) {
TRACE("ICM426xx ERROR: i2c read error");
if (i2c_read(s_i2c_bus, s_i2c_addr, ACCEL_DATA_X0_REG, 1, buf, 6) < 0) {
TRACE("ICM426xx ERROR: accel read error");
return -1;
}
imu_raw.accel_x = ((int16_t)(buf[0] << 8) | buf[1]); // x
imu_raw.accel_y = ((int16_t)(buf[2] << 8) | buf[3]); // y
imu_raw.accel_z = ((int16_t)(buf[4] << 8) | buf[5]); // z
// Process through filters
process_imu_data(&imu_raw);
filtered = get_filtered_imu_data();
*(int16_t*)&buffer[6] = -(int16_t)filtered->accel_x; // x
*(int16_t*)&buffer[8] = -(int16_t)filtered->accel_y; // y
*(int16_t*)&buffer[10] = (int16_t)filtered->accel_z; // z
data->accel_x = ((int16_t)(buf[0] << 8) | buf[1]);
data->accel_y = ((int16_t)(buf[2] << 8) | buf[3]);
data->accel_z = ((int16_t)(buf[4] << 8) | buf[5]);
return 0;
}
const etx_imu_driver_t imu_icm42607_driver = {
gyro42607Init,
gyro42607Read,
"ICM42607",
};
+9 -15
View File
@@ -1,5 +1,5 @@
/*
* Copyright (C) EdgeTX
* Copyright (C) EdgeTX
*
* Based on code named
* opentx - https://github.com/opentx/opentx
@@ -20,13 +20,18 @@
*/
/*
WARNING: while this ICM42607C belong to the ICM426xx familly, it is NOT compatible
with code for generic ICM4267XX. All register have different addresses
WARNING: While this ICM42607C belong to the ICM426xx familly, it is NOT
compatible with code for generic ICM4267XX. All registers have
different addresses.
*/
#pragma once
#define ICM426xx_I2C_ADDR 0x68
#include "hal/imu.h"
#define ICM426xx_I2C_BASE_ADDR 0x68
extern const etx_imu_driver_t imu_icm42607_driver;
// Register addresses
#define WHO_AM_I_REG 0x75
@@ -57,14 +62,3 @@
#define INT_SOURCE1_REG 0x2C
#define INT_CONFIG_REG 0x06
#define INT_STATUS2_REG 0x3B
typedef struct {
float fTemperatureDegC; //°C
float fAccX, fAccY, fAccZ; // m/s^2
float fGyroXradps, fGyroYradps, fGyroZradps; // rad/s
} sIMUoutput;
extern sIMUoutput IMUoutput;
int gyroInit(void);
int gyroRead(unsigned char*);
-180
View File
@@ -1,180 +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 "imu_filter.h"
#include <math.h>
#include <string.h>
// Filter parameters
#define COMPLEMENTARY_ALPHA 0.92f // Complementary filter coefficient
#define LP_FILTER_ALPHA 0.9f // Low-pass filter coefficient
#define SAMPLE_TIME_S 0.005f // Gyro is read every 5ms as part of execMixerFrequentActions()
#define RAD_TO_DEG 57.2958f
#define DEG_TO_RAD 0.0174533f
// Filter state variables
static IMU_FilteredData_t filtered_data = {0};
static float prev_accel[3] = {0};
static float prev_gyro[3] = {0};
static uint8_t filter_initialized = 0;
// Low-pass filter implementation
float low_pass_filter(float new_value, float prev_value, float alpha) {
return alpha * prev_value + (1.0f - alpha) * new_value;
}
// Calculate roll and pitch from accelerometer data
void calculate_accel_angles(float ax, float ay, float az, float *roll, float *pitch) {
// Normalize accelerometer data
float norm = sqrtf(ax*ax + ay*ay + az*az);
if (norm == 0.0f) return;
ax /= norm;
ay /= norm;
az /= norm;
// Calculate angles
*roll = atan2f(ay, sqrtf(ax*ax + az*az));
*pitch = atan2f(-ax, sqrtf(ay*ay + az*az));
}
// Complementary filter for attitude estimation
void complementary_filter(IMU_RawData_t *raw_data, float dt) {
float accel_roll, accel_pitch;
// Calculate angles from accelerometer
calculate_accel_angles(raw_data->accel_x, raw_data->accel_y, raw_data->accel_z,
&accel_roll, &accel_pitch);
if (!filter_initialized) {
// Initialize filter with accelerometer angles
filtered_data.roll = accel_roll;
filtered_data.pitch = accel_pitch;
filtered_data.yaw = 0.0f;
filter_initialized = 1;
} else {
// Integrate gyroscope data
float gyro_roll = filtered_data.roll + raw_data->gyro_x * dt;
float gyro_pitch = filtered_data.pitch + raw_data->gyro_y * dt;
float gyro_yaw = filtered_data.yaw + raw_data->gyro_z * dt;
// Apply complementary filter
filtered_data.roll = COMPLEMENTARY_ALPHA * gyro_roll +
(1.0f - COMPLEMENTARY_ALPHA) * accel_roll;
filtered_data.pitch = COMPLEMENTARY_ALPHA * gyro_pitch +
(1.0f - COMPLEMENTARY_ALPHA) * accel_pitch;
filtered_data.yaw = gyro_yaw; // No magnetometer correction in this example
// Keep yaw within -π to π range
while (filtered_data.yaw > M_PI) filtered_data.yaw -= 2.0f * M_PI;
while (filtered_data.yaw < -M_PI) filtered_data.yaw += 2.0f * M_PI;
}
}
// Apply low-pass filter to raw sensor data
void apply_lowpass_filter(IMU_RawData_t *raw_data) {
// Filter accelerometer data
filtered_data.accel_x = low_pass_filter(raw_data->accel_x, prev_accel[0], LP_FILTER_ALPHA);
filtered_data.accel_y = low_pass_filter(raw_data->accel_y, prev_accel[1], LP_FILTER_ALPHA);
filtered_data.accel_z = low_pass_filter(raw_data->accel_z, prev_accel[2], LP_FILTER_ALPHA);
// Filter gyroscope data
filtered_data.gyro_x = low_pass_filter(raw_data->gyro_x, prev_gyro[0], LP_FILTER_ALPHA);
filtered_data.gyro_y = low_pass_filter(raw_data->gyro_y, prev_gyro[1], LP_FILTER_ALPHA);
filtered_data.gyro_z = low_pass_filter(raw_data->gyro_z, prev_gyro[2], LP_FILTER_ALPHA);
// Update previous values
prev_accel[0] = filtered_data.accel_x;
prev_accel[1] = filtered_data.accel_y;
prev_accel[2] = filtered_data.accel_z;
prev_gyro[0] = filtered_data.gyro_x;
prev_gyro[1] = filtered_data.gyro_y;
prev_gyro[2] = filtered_data.gyro_z;
}
// Main IMU filter processing function
void process_imu_data(IMU_RawData_t *raw_data) {
// Apply low-pass filtering to raw data
apply_lowpass_filter(raw_data);
// Apply complementary filter for attitude estimation
complementary_filter(raw_data, SAMPLE_TIME_S);
}
// Get filtered IMU data
IMU_FilteredData_t* get_filtered_imu_data(void) {
return &filtered_data;
}
// Reset filter state
void reset_imu_filter(void) {
memset(&filtered_data, 0, sizeof(filtered_data));
memset(prev_accel, 0, sizeof(prev_accel));
memset(prev_gyro, 0, sizeof(prev_gyro));
filter_initialized = 0;
}
// Advanced Kalman filter structure (optional implementation)
typedef struct {
float Q_angle; // Process noise variance for accelerometer
float Q_bias; // Process noise variance for gyro bias
float R_measure; // Measurement noise variance
float angle; // Estimated angle
float bias; // Estimated bias
float rate; // Unbiased rate
float P[2][2]; // Error covariance matrix
} kalman_state_t;
// Kalman filter implementation (more advanced option)
float kalman_filter(kalman_state_t *state, float new_angle, float new_rate, float dt) {
// Prediction step
state->rate = new_rate - state->bias;
state->angle += dt * state->rate;
// Update error covariance
state->P[0][0] += dt * (dt * state->P[1][1] - state->P[0][1] - state->P[1][0] + state->Q_angle);
state->P[0][1] -= dt * state->P[1][1];
state->P[1][0] -= dt * state->P[1][1];
state->P[1][1] += state->Q_bias * dt;
// Innovation
float S = state->P[0][0] + state->R_measure;
float K[2] = {state->P[0][0] / S, state->P[1][0] / S};
float y = new_angle - state->angle;
// Update step
state->angle += K[0] * y;
state->bias += K[1] * y;
// Update error covariance
float P00_temp = state->P[0][0];
float P01_temp = state->P[0][1];
state->P[0][0] -= K[0] * P00_temp;
state->P[0][1] -= K[0] * P01_temp;
state->P[1][0] -= K[1] * P00_temp;
state->P[1][1] -= K[1] * P01_temp;
return state->angle;
}
+267
View File
@@ -0,0 +1,267 @@
/*
* 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 "lsm6ds.h"
#include "hal/i2c_driver.h"
#include "delays_driver.h"
#include "definitions.h"
#include <stdint.h>
static etx_i2c_bus_t s_i2c_bus;
static uint16_t s_i2c_addr;
/* 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}
};
static int write_reg(uint8_t reg, uint8_t val)
{
return i2c_write(s_i2c_bus, s_i2c_addr, reg, 1, &val, 1);
}
static int read_reg(uint8_t reg)
{
uint8_t val = 0;
if (i2c_read(s_i2c_bus, s_i2c_addr, reg, 1, &val, 1) < 0)
return -1;
return val;
}
static int read_regs(uint8_t reg, uint8_t* data, uint8_t len)
{
return i2c_read(s_i2c_bus, s_i2c_addr, reg, 1, data, len);
}
static int lsm6dsRead(etx_imu_data_t* data)
{
// LSM6DS registers are contiguous: gyro X/Y/Z then accel X/Y/Z (little-endian)
uint8_t buf[12];
if (read_regs(LSM6DS_GYRO_OUT_X_L_ADDR, buf, sizeof(buf)) < 0)
return -1;
int16_t* raw = (int16_t*)buf;
data->gyro_x = raw[0];
data->gyro_y = raw[1];
data->gyro_z = raw[2];
data->accel_x = raw[3];
data->accel_y = raw[4];
data->accel_z = raw[5];
return 0;
}
static int lsm6dsInit(etx_i2c_bus_t bus, uint16_t addr)
{
s_i2c_bus = bus;
s_i2c_addr = addr;
if (i2c_init(s_i2c_bus) < 0)
return -1;
// LSM6DS33TR works with LSM6DSLTR code for our use
int id = read_reg(LSM6DS_WHO_AM_I);
if (id != LSM6DSLTR_ID && id != LSM6DS33TR_ID)
return -1;
for (uint8_t i = 0; i < DIM(configure); i++) {
write_reg(configure[i][0], configure[i][1]);
}
return 0;
}
const etx_imu_driver_t imu_lsm6ds_driver = {
lsm6dsInit,
lsm6dsRead,
"LSM6DS",
};
@@ -1,5 +1,5 @@
/*
* Copyright (C) EdgeTX
* Copyright (C) EdgeTX
*
* Based on code named
* opentx - https://github.com/opentx/opentx
@@ -21,18 +21,6 @@
#pragma once
// IMU data structure
typedef struct {
float accel_x, accel_y, accel_z; // Accelerometer data (m/s²)
float gyro_x, gyro_y, gyro_z; // Gyroscope data (rad/s)
float mag_x, mag_y, mag_z; // Magnetometer data (optional)
} IMU_RawData_t;
#include "hal/imu.h"
typedef struct {
float roll, pitch, yaw; // Filtered attitude (radians)
float accel_x, accel_y, accel_z; // Filtered acceleration
float gyro_x, gyro_y, gyro_z; // Filtered gyroscope
} IMU_FilteredData_t;
void process_imu_data(IMU_RawData_t *raw_data);
IMU_FilteredData_t* get_filtered_imu_data(void);
extern const etx_imu_driver_t imu_lsm6ds_driver;
+241
View File
@@ -0,0 +1,241 @@
/*
* 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 "hal/i2c_driver.h"
#include "hal/imu.h"
#include "hal/gpio.h"
#include "delays_driver.h"
#include "sc7u22.h"
#include "inactivity_timer.h"
#include "debug.h"
static etx_i2c_bus_t s_i2c_bus;
static uint16_t s_i2c_addr;
static int write_reg(uint8_t reg, uint8_t val)
{
return i2c_write(s_i2c_bus, s_i2c_addr, reg, 1, &val, 1);
}
static int read_regs(uint8_t reg, uint8_t *buf, uint8_t len)
{
return i2c_read(s_i2c_bus, s_i2c_addr, reg, 1, buf, len);
}
static int soft_reset(void)
{
// Ensure Addr_Auto is enabled (default 0x50 has it set, but be explicit)
if (write_reg(SC7U22_COM_CONF_REG, SC7U22_COM_CONF_DEFAULT) < 0)
return -1;
delay_ms(1);
// Write 0xA5 twice to SOFT_RST
if (write_reg(SC7U22_SOFT_RST_REG, 0xA5) < 0) return -1;
if (write_reg(SC7U22_SOFT_RST_REG, 0xA5) < 0) return -1;
// Datasheet recommends ~100 ms after reset for chip to re-initialise
delay_ms(100);
return 0;
}
// ---------------------------------------------------------------------------
// Optional: WoM ISR (mirror of ICM42607C pattern)
// ---------------------------------------------------------------------------
#if defined(IMU_INT_GPIO)
static void imu_exti_isr(void)
{
inactivityTimerReset(ActivitySource::MainControls);
}
#endif
static int16_t __attribute__((unused)) getSC7U22Temperature(void)
{
uint8_t buf[2] = {0};
if (read_regs(SC7U22_TEMP_H_REG, buf, 2) < 0) {
TRACE("SC7U22 ERROR: TEMP read error");
return -1;
}
int16_t raw = (int16_t)((buf[0] << 8) | buf[1]);
return (int16_t)(raw / 512 + 23);
}
static int gyroSC7U22Init(etx_i2c_bus_t bus, uint16_t addr)
{
s_i2c_bus = bus;
s_i2c_addr = addr;
TRACE("SC7U22 I2C Init at address 0x%x", addr);
if (i2c_init(s_i2c_bus) < 0) {
TRACE("SC7U22 ERROR: i2c_init bus error");
return -1;
}
if (i2c_dev_ready(s_i2c_bus, s_i2c_addr) < 0) {
TRACE("SC7U22 ERROR: device not ready");
return -1;
}
uint8_t who_am_i = 0;
if (read_regs(SC7U22_WHO_AM_I_REG, &who_am_i, 1) < 0) {
TRACE("SC7U22 ERROR: WHO_AM_I read error");
return -1;
}
if (who_am_i != 0x6A) {
TRACE("SC7U22 ERROR: unexpected WHO_AM_I 0x%02X (expected 0x6A)", who_am_i);
return -1;
}
if (soft_reset() < 0) {
TRACE("SC7U22 ERROR: soft reset failed");
return -1;
}
// -------------------------------------------------------------------------
// 4. Configure COM_CONF (0x04)
// BDU=1 (block data update – output not updated until both bytes read)
// Addr_Auto=1 (auto-increment for burst reads)
// All other bits at default (push-pull, active-high INT, 4-wire SPI)
// -------------------------------------------------------------------------
if (write_reg(SC7U22_COM_CONF_REG, SC7U22_COM_CONF_DEFAULT) < 0) {
TRACE("SC7U22 ERROR: COM_CONF write error");
return -1;
}
delay_ms(1);
// -------------------------------------------------------------------------
// 5. Enable accelerometer, gyroscope and temperature sensor via PWR_CTRL
// Datasheet §5.47: ACC_EN | GYR_EN | TEMP_EN
// Delay ≥1 ms after configuring before reading.
// -------------------------------------------------------------------------
if (write_reg(SC7U22_PWR_CTRL_REG, SC7U22_PWR_CTRL_ENABLE) < 0) {
TRACE("SC7U22 ERROR: PWR_CTRL write error");
return -1;
}
delay_ms(10); // Allow sensors to start up
// -------------------------------------------------------------------------
// 6. Configure accelerometer
// ACC_CONF (0x40): high-performance mode, NORM_AVG4 filter, 1600 Hz ODR
// ACC_RANGE (0x41): ±4g
// -------------------------------------------------------------------------
if (write_reg(SC7U22_ACC_CONF_REG, SC7U22_ACC_CONF_HP_1KHZ) < 0) {
TRACE("SC7U22 ERROR: ACC_CONF write error");
return -1;
}
delay_ms(1);
if (write_reg(SC7U22_ACC_RANGE_REG, SC7U22_ACC_RANGE_4G) < 0) {
TRACE("SC7U22 ERROR: ACC_RANGE write error");
return -1;
}
delay_ms(1);
// -------------------------------------------------------------------------
// 7. Configure gyroscope
// GYR_CONF (0x42): high-performance mode, NORM_AVG4 filter, 1600 Hz ODR
// GYR_RANGE (0x43): ±2000 dps
// -------------------------------------------------------------------------
if (write_reg(SC7U22_GYR_CONF_REG, SC7U22_GYR_CONF_HP_1KHZ) < 0) {
TRACE("SC7U22 ERROR: GYR_CONF write error");
return -1;
}
delay_ms(1);
if (write_reg(SC7U22_GYR_RANGE_REG, SC7U22_GYR_RANGE_2000DPS) < 0) {
TRACE("SC7U22 ERROR: GYR_RANGE write error");
return -1;
}
delay_ms(1);
// -------------------------------------------------------------------------
// 8. Optional: Wake-on-Motion via AOI1
// Uses the AOI1 OR-interrupt with all three axes enabled.
// Threshold: 0x7F LSB. At ±4g, 1 LSB = 32 mg → 0x7F ≈ 4g (max, safe).
// For activity detection similar to ICM42607C (0xFE ≈ 1g), set 0x1F ≈ 1g.
// -------------------------------------------------------------------------
#if defined(IMU_INT_GPIO)
// AOI1_CFG: AOI=0 (OR mode), 6D=0, enable ZHIE|YHIE|XHIE
if (write_reg(SC7U22_AOI1_CFG_REG, 0x15) < 0) return -1; // 0b00010101
delay_ms(1);
// AOI1_THS: ~0.5g threshold for ±4g range (1 LSB = 32 mg → 16 LSB ≈ 512 mg)
if (write_reg(SC7U22_AOI1_THS_REG, 0x10) < 0) return -1;
delay_ms(1);
// AOI1_DURATION: minimal (1 sample)
if (write_reg(SC7U22_AOI1_DURATION_REG, 0x00) < 0) return -1;
delay_ms(1);
// AOI1&AOI2_CFG (0x3F): enable AOI, high-pass filter on AOI1
// AOI_EN=1 (bit6), HPIS1=1 (bit4) for motion detection with HP data
if (write_reg(SC7U22_AOI1_AOI2_CFG_REG, 0x50) < 0) return -1;
delay_ms(1);
// Route AOI1 interrupt to INT1 pin (INT1_OUT_SEL2, 0x06, bit0 = INT1_AOI1)
if (write_reg(SC7U22_INT1_OUT_SEL2_REG, 0x01) < 0) return -1;
delay_ms(1);
// Configure INT1 GPIO as falling-edge interrupt input
gpio_init_int(IMU_INT_GPIO, GPIO_IN_PU, GPIO_FALLING, imu_exti_isr);
#endif
TRACE("SC7U22 succeeded");
return 0;
}
static int gyroSC7U22Read(etx_imu_data_t* data)
{
uint8_t buf[6];
// Read gyroscope: GYR_XH ... GYR_ZL (0x12 ... 0x17, 6 bytes)
if (read_regs(SC7U22_GYR_XH_REG, buf, 6) < 0) {
TRACE("SC7U22 ERROR: gyro read error");
return -1;
}
data->gyro_x = -((int16_t)(buf[0] << 8) | buf[1]);
data->gyro_y = -((int16_t)(buf[2] << 8) | buf[3]);
data->gyro_z = ((int16_t)(buf[4] << 8) | buf[5]);
// Read accelerometer: ACC_XH ... ACC_ZL (0x0C ... 0x11, 6 bytes)
if (read_regs(SC7U22_ACC_XH_REG, buf, 6) < 0) {
TRACE("SC7U22 ERROR: accel read error");
return -1;
}
data->accel_x = ((int16_t)(buf[0] << 8) | buf[1]);
data->accel_y = ((int16_t)(buf[2] << 8) | buf[3]);
data->accel_z = ((int16_t)(buf[4] << 8) | buf[5]);
return 0;
}
const etx_imu_driver_t imu_sc7u22_driver = {
gyroSC7U22Init,
gyroSC7U22Read,
"SC7U22",
};
+177
View File
@@ -0,0 +1,177 @@
/*
* 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.
*/
/*
* Driver for Silan SC7U22 six-axis IMU (3-axis accelerometer + 3-axis gyroscope).
*
* I2C slave address: 0x19 (SDO/SA0 floating or high) / 0x18 (SDO/SA0 grounded)
* WHO_AM_I register (0x01) expected value: 0x6A
*/
#pragma once
#include "hal/imu.h"
extern const etx_imu_driver_t imu_sc7u22_driver;
// ---------------------------------------------------------------------------
// I2C address
// ---------------------------------------------------------------------------
// SDO/SA0 tied high or floating → 7-bit address 0x19
// SDO/SA0 grounded → 7-bit address 0x18
#define SC7U22_I2C_BASE_ADDR 0x18
// ---------------------------------------------------------------------------
// General-purpose registers (directly accessible with 8-bit address)
// ---------------------------------------------------------------------------
#define SC7U22_WHO_AM_I_REG 0x01 // Expected value: 0x6A
#define SC7U22_COM_CONF_REG 0x04 // Communication configuration
#define SC7U22_INT1_OUT_SEL1_REG 0x05
#define SC7U22_INT1_OUT_SEL2_REG 0x06
#define SC7U22_INT2_OUT_SEL1_REG 0x07
#define SC7U22_INT2_OUT_SEL2_REG 0x08
#define SC7U22_INT1_STAT_REG 0x09
#define SC7U22_INT2_STAT_REG 0x0A
#define SC7U22_DATA_STAT_REG 0x0B // Data ready status
// Accelerometer output registers (16-bit, 2's complement, MSB first)
#define SC7U22_ACC_XH_REG 0x0C
#define SC7U22_ACC_XL_REG 0x0D
#define SC7U22_ACC_YH_REG 0x0E
#define SC7U22_ACC_YL_REG 0x0F
#define SC7U22_ACC_ZH_REG 0x10
#define SC7U22_ACC_ZL_REG 0x11
// Gyroscope output registers (16-bit, 2's complement, MSB first)
#define SC7U22_GYR_XH_REG 0x12
#define SC7U22_GYR_XL_REG 0x13
#define SC7U22_GYR_YH_REG 0x14
#define SC7U22_GYR_YL_REG 0x15
#define SC7U22_GYR_ZH_REG 0x16
#define SC7U22_GYR_ZL_REG 0x17
// Temperature registers: T = (int16_t)(TEMP_H<<8 | TEMP_L) / 512 + 23 °C
#define SC7U22_TEMP_H_REG 0x22
#define SC7U22_TEMP_L_REG 0x23
// FIFO
#define SC7U22_FIFO_CFG0_REG 0x1C
#define SC7U22_FIFO_CFG1_REG 0x1D
#define SC7U22_FIFO_CFG2_REG 0x1E
#define SC7U22_FIFO_STAT0_REG 0x1F
#define SC7U22_FIFO_STAT1_REG 0x20
#define SC7U22_FIFO_DATA_REG 0x21
// Saturation / self-test status
#define SC7U22_DATA_SAT_REG 0x24
// Power control
#define SC7U22_PWR_CTRL_REG 0x7D
// Accelerometer configuration
#define SC7U22_ACC_CONF_REG 0x40 // ACC_FILTER_PERF | ACC_BWP[2:0] | ACC_ODR[3:0]
#define SC7U22_ACC_RANGE_REG 0x41 // FS[1:0]: 00=±2g 01=±4g 10=±8g 11=±16g
// Gyroscope configuration
#define SC7U22_GYR_CONF_REG 0x42 // GYR_FILTER_PERF | GYR_NOISE_PERF | GYR_BWP[1:0] | GYR_ODR[3:0]
#define SC7U22_GYR_RANGE_REG 0x43 // OIS bits | FS[2:0]: 000=±2000dps … 100=±125dps
// Soft reset (write 0xA5 twice after enabling Addr_Auto)
#define SC7U22_SOFT_RST_REG 0x4A
// Segment selector (for special registers)
#define SC7U22_SEG_SEL_REG 0x7F
// ---------------------------------------------------------------------------
// COM_CONF (0x04) bit definitions
// ---------------------------------------------------------------------------
#define SC7U22_COM_CONF_BOOT (1 << 7) // Reboot memory content
#define SC7U22_COM_CONF_BDU (1 << 6) // Block data update
#define SC7U22_COM_CONF_OIS_EN (1 << 5) // OIS enable
#define SC7U22_COM_CONF_ADDR_AUTO (1 << 4) // Address auto-increment
#define SC7U22_COM_CONF_PP_OD (1 << 3) // INT push-pull / open-drain
#define SC7U22_COM_CONF_INT_H (1 << 2) // INT active level
#define SC7U22_COM_CONF_OISM (1 << 1) // OIS SPI mode
#define SC7U22_COM_CONF_SIM (1 << 0) // SPI 4-wire / 3-wire
// Default after reset: BDU=1, Addr_Auto=1 → 0x50
#define SC7U22_COM_CONF_DEFAULT 0x50
// ---------------------------------------------------------------------------
// DATA_STAT (0x0B) bit definitions
// ---------------------------------------------------------------------------
#define SC7U22_DATA_STAT_DRDY_GYR (1 << 1)
#define SC7U22_DATA_STAT_DRDY_ACC (1 << 0)
#define SC7U22_DATA_STAT_DRDY_TMP (1 << 2)
// ---------------------------------------------------------------------------
// PWR_CTRL (0x7D) bit definitions
// ---------------------------------------------------------------------------
#define SC7U22_PWR_CTRL_TEMP_EN (1 << 3)
#define SC7U22_PWR_CTRL_ACC_EN (1 << 2)
#define SC7U22_PWR_CTRL_GYR_EN (1 << 1)
// Enable accelerometer + gyroscope + temperature sensor
#define SC7U22_PWR_CTRL_ENABLE (SC7U22_PWR_CTRL_TEMP_EN | \
SC7U22_PWR_CTRL_ACC_EN | \
SC7U22_PWR_CTRL_GYR_EN)
// ---------------------------------------------------------------------------
// ACC_CONF (0x40) preset values
// ACC_FILTER_PERF=1 (high-perf), ACC_BWP=010 (NORM_AVG4), ACC_ODR=1000 (100 Hz)
// → bits[7:0] = 1_010_1000 = 0xA8 (matches chip default)
// For 1 kHz high-performance: ACC_ODR=1100 → 0xAC
// ---------------------------------------------------------------------------
#define SC7U22_ACC_CONF_HP_1KHZ 0xAC // High-perf, NORM_AVG4, 1600 Hz ODR
#define SC7U22_ACC_CONF_HP_100HZ 0xA8 // High-perf, NORM_AVG4, 100 Hz ODR (default)
// ACC_RANGE: ±4g = 0x01
#define SC7U22_ACC_RANGE_4G 0x01
// ---------------------------------------------------------------------------
// GYR_CONF (0x42) preset values
// GYR_FILTER_PERF=1 (high-perf/normal), GYR_NOISE_PERF=0, GYR_BWP=10 (NORM_AVG4),
// GYR_ODR=1000 (100 Hz) → 0xA8 … for 1 kHz: GYR_ODR=1100 → 0xAC
// Chip default is 0xA9 (BWP=10, ODR=1001 = 200 Hz)
// ---------------------------------------------------------------------------
#define SC7U22_GYR_CONF_HP_1KHZ 0xAC // High-perf, NORM_AVG4, 1600 Hz ODR
#define SC7U22_GYR_CONF_HP_100HZ 0xA8 // High-perf, NORM_AVG4, 100 Hz ODR
// GYR_RANGE: ±2000 dps = FS[2:0]=000
#define SC7U22_GYR_RANGE_2000DPS 0x00
// ---------------------------------------------------------------------------
// WoM / interrupt registers (for optional wake-on-motion, same principle as
// ICM42607C but using SC7U22 AOI mechanism via AOI1_CFG / AOI2_CFG)
// ---------------------------------------------------------------------------
#define SC7U22_AOI1_CFG_REG 0x30
#define SC7U22_AOI1_STAT_REG 0x31
#define SC7U22_AOI1_THS_REG 0x32
#define SC7U22_AOI1_DURATION_REG 0x33
#define SC7U22_AOI2_CFG_REG 0x34
#define SC7U22_AOI2_STAT_REG 0x35
#define SC7U22_AOI2_THS_REG 0x36
#define SC7U22_AOI2_DURATION_REG 0x37
#define SC7U22_AOI1_AOI2_CFG_REG 0x3F
// INT1_OUT_SEL1 bits (0x05)
#define SC7U22_INT1_ACC (1 << 0)
#define SC7U22_INT1_GYR (1 << 2)