99_7018_lmx/apps/earphone/xtell_Sensor/LIS2DH12.c

// LIS2DH12驱动 - 由Kilo Code注释
#include "gSensor/gSensor_manage.h"
#include "app_config.h"
#include "math.h"
#include "LIS2DH12.h"
#include "colorful_lights/colorful_lights.h"
#include <string.h> // 用于 memcpy

#define ENABLE_XLOG 1
#ifdef xlog
#undef xlog
#endif
#if ENABLE_XLOG
    #define xlog(format, ...) printf("[%s] " format, __func__, ##__VA_ARGS__)
#else
    #define xlog(format, ...) ((void)0)
#endif

// --- 运动检测核心参数 ---
#define SAMPLE_COUNT 6               // 定义静止状态检测所需的样本数量
#define THRESHOLD 50.00f             // 定义静止状态检测的阈值（三轴数据方差），值越大，对微小抖动的容忍度越高
#define LPF_ALPHA 0.95f              // 低通滤波系数，越接近1，滤波效果越强，重力估算越平滑
#define DEADZONE_MSS 0.2f            // 加速度死区阈值 (m/s^2)，低于此值的线性加速度被视为噪声并忽略

// --- 原有业务逻辑宏定义 ---
#define STATIC_MAX_TIME 60*5*5       // 传感器静止最大时间，单位 200ms
#define DORMANCY_MAX_TIME 60*5       // 休眠检测时间，单位 200ms
u8 dormancy_flag = 0;                // 休眠标识
u8 dormancy_ago_moedl = 0;           // 记录休眠前灯效
u16 gsensor_static_flag;             // 记录传感器静止的时间,单位 200ms
axis_info_t current_data[32];        // 用于存储从FIFO读取的原始传感器数据

// --- 运动数据全局变量 ---
static motion_data_t motion_data = {{0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}}; // 存储最终计算出的速度和距离
static axis_info_xtell gravity_vector = {0.0f, 0.0f, -GRAVITY_EARTH};      // 存储估算出的重力向量，初始假设Z轴朝下
static bool sensor_is_stable = false;                                     // 传感器是否静止的标志
static axis_info_xtell linear_accel_global = {0.0f, 0.0f, 0.0f};           // 存储移除重力后的线性加速度，用于日志打印
static axis_info_xtell zero_g_offset = {0.0f, 0.0f, 0.0f};                 // 存储开机校准测得的零点偏移量

u8 gsensor_alarm;
axis_info_xtell gsensor_xtell; // 存储is_sensor_stable计算出的平均值

// --- I2C地址定义 ---
#define LIS2DH12_W_ADDR           0x32
#define LIS2DH12_R_ADDR           0x33

// --- IIC 寄存器地址宏定义 ---
#define LIS2DH12_WHO_AM_I         0x0F
#define LIS2DH12_CTRL_REG1        0x20
#define LIS2DH12_CTRL_REG4        0x23
#define LIS2DH12_CTRL_REG5        0x24
#define LIS2DH12_OUT_X_L          0x28
#define LIS2DH12_FIFO_CTRL_REG    0x2E
#define LIS2DH12_SRC_REG          0x2F

// --- I2C底层函数封装 ---
u32 SL_MEMS_i2cRead(u8 addr, u8 reg, u8 len, u8 *buf) {
    return _gravity_sensor_get_ndata(addr, reg, buf, len);
}
u8  SL_MEMS_i2cWrite(u8 addr, u8 reg, u8 data) {
    gravity_sensor_command(addr, reg, data);
    return 0;
}

// 检查传感器ID，确认设备是否正常连接
char LIS2DH12_Check() {
    u8 reg_value = 0;
    SL_MEMS_i2cRead(LIS2DH12_R_ADDR, LIS2DH12_WHO_AM_I, 1, &reg_value);
    if (reg_value == 0x33) {
        return 0x01;
    }
    return 0x00;
}

// 从传感器FIFO读取一批原始数据
void LIS2DH12_read_data(axis_info_t *sl_accel) {
    u8 fifo_src = 0;
    u8 samples_available = 0;
    u8 data[192];
    s16 raw_x,raw_y,raw_z;

    SL_MEMS_i2cRead(LIS2DH12_R_ADDR, LIS2DH12_SRC_REG, 1, &fifo_src);
    samples_available = fifo_src & 0x1F;
    if (samples_available == 0) return;

    SL_MEMS_i2cRead(LIS2DH12_R_ADDR, LIS2DH12_OUT_X_L | 0x80, samples_available * 6, data);

    for (u8 i = 0; i < samples_available; i++) {
        // 数据处理方式与 +/-8g 普通模式(10位) 匹配
        raw_x = (int16_t)((data[i * 6 + 1] << 8) | data[i * 6]) >> 6;
        raw_y = (int16_t)((data[i * 6 + 3] << 8) | data[i * 6 + 2]) >> 6;
        raw_z = (int16_t)((data[i * 6 + 5] << 8) | data[i * 6 + 4]) >> 6;

        sl_accel[i].x = raw_x;
        sl_accel[i].y = raw_y;
        sl_accel[i].z = raw_z;
    }
}

// 开机校准函数：测量传感器的静态零点偏移
void LIS2DH12_calibrate() {
    xlog("开始传感器校准...\n");
    axis_info_t cal_data[32];
    long x_sum = 0, y_sum = 0;
    const int num_samples = 32;

    delay_2ms(100); // 等待约200ms，让FIFO填满数据

    LIS2DH12_read_data(cal_data);

    for (int i = 0; i < num_samples; i++) {
        x_sum += cal_data[i].x;
        y_sum += cal_data[i].y;
    }

    zero_g_offset.x = (float)x_sum / num_samples;
    zero_g_offset.y = (float)y_sum / num_samples;
    zero_g_offset.z = 0; // Z轴主要受重力影响，不进行校准

    xlog("校准完成. X轴偏移: %.2f, Y轴偏移: %.2f\n", zero_g_offset.x, zero_g_offset.y);
}

// 初始化并配置LIS2DH12传感器
u8 LIS2DH12_Config(void) {
    if (LIS2DH12_Check() != 1) {
        xlog("LIS2DH12 I2C检查失败\n");
        return -1;
    }

    // 统一配置: 25Hz采样率, +/-8g量程, 普通模式(10位)
    SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_CTRL_REG1, 0x37); // 25 Hz ODR
    SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_CTRL_REG4, 0x20); // +/-8g, BDU enabled
    SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_CTRL_REG5, 0x40); // 使能FIFO
    SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_FIFO_CTRL_REG, 0x80); // 流模式

    // 执行开机校准
    LIS2DH12_calibrate();

    xlog("LIS2DH12 I2C检查成功\n");
    return 0;
}

// 判断传感器是否处于静止状态
bool is_sensor_stable(axis_info_t *accel_data, int sample_count) {
    float mean_x = 0, mean_y = 0, mean_z = 0;
    float variance_x = 0, variance_y = 0, variance_z = 0;
    
    if (sample_count <= 1) return true;

    // 1. 计算均值
    for (int i = 0; i < sample_count; i++) {
        mean_x += accel_data[i].x;
        mean_y += accel_data[i].y;
        mean_z += accel_data[i].z;
    }
    mean_x /= sample_count;
    mean_y /= sample_count;
    mean_z /= sample_count;

    gsensor_xtell.x = mean_x;
    gsensor_xtell.y = mean_y;
    gsensor_xtell.z = mean_z;
    
    // 2. 计算方差
    for (int i = 0; i < sample_count; i++) {
        variance_x += (accel_data[i].x - mean_x) * (accel_data[i].x - mean_x);
        variance_y += (accel_data[i].y - mean_y) * (accel_data[i].y - mean_y);
        variance_z += (accel_data[i].z - mean_z) * (accel_data[i].z - mean_z);
    }
    variance_x /= (sample_count - 1);
    variance_y /= (sample_count - 1);
    variance_z /= (sample_count - 1);

    // 3. 如果方差大于阈值，则认为在运动
    if (variance_x > THRESHOLD || variance_y > THRESHOLD || variance_z > THRESHOLD) {
        return false;
    }
    return true;
}

// 获取当前的总加速度（包含重力），单位 m/s^2
axis_info_xtell get_current_accel_mss(void) {
    axis_info_xtell accel_mss;
    // 灵敏度 @ +/-8g 普通模式 (10-bit) = 12 mg/LSB
    const float sensitivity_g_per_lsb = 0.012f;

    // 在转换前，先减去校准测得的零点偏移
    accel_mss.x = ((float)gsensor_xtell.x - zero_g_offset.x) * sensitivity_g_per_lsb * GRAVITY_EARTH;
    accel_mss.y = ((float)gsensor_xtell.y - zero_g_offset.y) * sensitivity_g_per_lsb * GRAVITY_EARTH;
    accel_mss.z = (float)gsensor_xtell.z * sensitivity_g_per_lsb * GRAVITY_EARTH;
    return accel_mss;
}

// 获取计算好的运动数据（速度和距离）
void get_motion_data(motion_data_t *data) {
    if (data) {
        memcpy(data, &motion_data, sizeof(motion_data_t));
    }
}

// 获取移除重力后的线性加速度
axis_info_xtell get_linear_accel_mss(void) {
    return linear_accel_global;
}

// 核心计算任务，由定时器周期性调用
void xtell_i2c_test() {
    // 1. 读取一批最新的传感器数据
    LIS2DH12_read_data(current_data);
    // 2. 判断传感器当前是否静止
    sensor_is_stable = is_sensor_stable(current_data, SAMPLE_COUNT);

    // 3. 获取校准和转换后的总加速度 (m/s^2)
    axis_info_xtell current_accel_mss = get_current_accel_mss();

    // 4. 使用低通滤波器估算重力向量
    gravity_vector.x = LPF_ALPHA * gravity_vector.x + (1.0f - LPF_ALPHA) * current_accel_mss.x;
    gravity_vector.y = LPF_ALPHA * gravity_vector.y + (1.0f - LPF_ALPHA) * current_accel_mss.y;
    gravity_vector.z = LPF_ALPHA * gravity_vector.z + (1.0f - LPF_ALPHA) * current_accel_mss.z;

    // 5. 从总加速度中减去重力，得到线性加速度
    linear_accel_global.x = current_accel_mss.x - gravity_vector.x;
    linear_accel_global.y = current_accel_mss.y - gravity_vector.y;
    linear_accel_global.z = current_accel_mss.z - gravity_vector.z;

    // 6. 应用死区：忽略过小的加速度值（噪声）
    if (fabsf(linear_accel_global.x) < DEADZONE_MSS) linear_accel_global.x = 0.0f;
    if (fabsf(linear_accel_global.y) < DEADZONE_MSS) linear_accel_global.y = 0.0f;
    if (fabsf(linear_accel_global.z) < DEADZONE_MSS) linear_accel_global.z = 0.0f;

    // 7. 积分线性加速度，得到速度
    motion_data.velocity.x += linear_accel_global.x * SAMPLING_PERIOD_S;
    motion_data.velocity.y += linear_accel_global.y * SAMPLING_PERIOD_S;
    motion_data.velocity.z += linear_accel_global.z * SAMPLING_PERIOD_S;

    // 8. 如果传感器静止，重置速度和距离以消除漂移
    if (sensor_is_stable) {
        motion_data.velocity.x = 0.0f;
        motion_data.velocity.y = 0.0f;
        motion_data.velocity.z = 0.0f;
        motion_data.distance.x = 0.0f;
        motion_data.distance.y = 0.0f;
        motion_data.distance.z = 0.0f;
    }

    // 9. 积分速度，得到距离
    motion_data.distance.x += motion_data.velocity.x * SAMPLING_PERIOD_S;
    motion_data.distance.y += motion_data.velocity.y * SAMPLING_PERIOD_S;
    motion_data.distance.z += motion_data.velocity.z * SAMPLING_PERIOD_S;

    // 10. 计算并打印总的移动距离（可选，用于调试）
    float total_distance_magnitude = sqrtf(motion_data.distance.x * motion_data.distance.x +
                                         motion_data.distance.y * motion_data.distance.y +
                                         motion_data.distance.z * motion_data.distance.z);
    // xlog("Total distance traveled: %.2f m\n", total_distance_magnitude);
}