257 lines
9.9 KiB
C
257 lines
9.9 KiB
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, ®_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);
|
||
}
|