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:
co-authored by
raphaelcoeffic
Claude Opus 4.6
parent
5ea531d391
commit
0ce5c8f3f2
@@ -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",
|
||||
};
|
||||
|
||||
@@ -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*);
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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;
|
||||
@@ -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",
|
||||
};
|
||||
@@ -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)
|
||||
Reference in New Issue
Block a user