lmx/99_7018_lmx
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

cun

Browse Source
This commit is contained in:
lmx
2025-10-30 11:33:38 +08:00
parent 7567ddc088
commit c21ac0ab82
34 changed files with 208045 additions and 207243 deletions
Download Patch File Download Diff File Expand all files Collapse all files

518
apps/earphone/xtell_Sensor/LIS2DH12.c
View File

@ -1,10 +1,10 @@
//LIS2DH12驱动
// 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> // For memcpy
#include <string.h> // 用于 memcpy
#define ENABLE_XLOG 1
#ifdef xlog
@ -16,269 +16,138 @@
#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 SAMPLE_COUNT 25 // 定义静止状态检测所需的样本数量
#define SAMPLE_COUNT 6 // 定义静止状态检测所需的样本数量
// #define THRESHOLD 2.00f // 定义静止状态检测的阈值，单位g
#define THRESHOLD 10.00f // 定义静止状态检测的阈值， 三轴传感器数据的方差
#define STATIC_MAX_TIME 60*5*5 //传感器静止最大时间，单位 200ms ，超过这个值就发数据通知AI询问用户是否需要帮助
#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]; // 全局变量存储当前数据
// Global variables for motion data
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, 0.0f};// 存储估算的重力方向
static bool sensor_is_stable = false;
// --- 原有业务逻辑宏定义 ---
#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;
// u8 xtell_uart_Gsensor_static[5]={0xAA,0x55,0x0F,0x55,0xAA}; //发现用户长时间静止时，串口发数据给AI芯片
axis_info_xtell gsensor_xtell; // 存储is_sensor_stable计算出的平均值
// 假设当前设置的满量程范围
const scale_t CURRENT_SCALE = SCALE_8G;
#define LIS2DH12_FROM_FS_8g_HR_TO_mg(lsb) (float)((int16_t)lsb>>4) * 4.0f
axis_info_xtell gsensor_xtell;
/***使用驱动前请根据实际接线情况配置（7bit）IIC地址******/
/** LIS2DH12 的SDO 脚接地： 0x30****************/
/** LIS2DH12 的SDO 脚接电源： 0x31****************/
// #define LIS2DH12_W_ADDR (0x31U << 1 | 0x0)
// #define LIS2DH12_R_ADDR (0x31U << 1 | 0x1)
// --- I2C地址定义 ---
#define LIS2DH12_W_ADDR 0x32
#define LIS2DH12_R_ADDR 0x33
/**IIC 寄存器地址宏定义***************/
#define LIS2DH12_OUT_TEMP_L 0x0C //温度数据低位
#define LIS2DH12_OUT_TEMP_H 0x0D //温度数据高位
#define LIS2DH12_WHO_AM_I 0x0F //读IIC寄存器 自身地址的值，正确应该是0x33
#define LIS2DH12_CTRL_RGE0 0x1E //设置SDO 内部上/下拉
#define LIS2DH12_TEMP_CFG_REG 0x1F //使能温度传感器
// --- IIC 寄存器地址宏定义 ---
#define LIS2DH12_WHO_AM_I 0x0F
#define LIS2DH12_CTRL_REG1 0x20
#define LIS2DH12_CTRL_REG2 0x21
#define LIS2DH12_CTRL_REG3 0x22
#define LIS2DH12_CTRL_REG4 0x23
#define LIS2DH12_CTRL_REG5 0x24
#define LIS2DH12_CTRL_REG6 0x25
#define LIS2DH12_REFERENCE 0x26
#define LIS2DH12_STATUS_REG 0x27
#define LIS2DH12_OUT_X_L 0x28
#define LIS2DH12_OUT_X_H 0x29
#define LIS2DH12_OUT_Y_L 0x2A
#define LIS2DH12_OUT_Y_H 0x2B
#define LIS2DH12_OUT_Z_L 0x2C
#define LIS2DH12_OUT_Z_H 0x2D
#define LIS2DH12_FIFO_CTRL_REG 0x2E
#define LIS2DH12_SRC_REG 0x2F
#define LIS2DH12_INT1_CFG 0x30
#define LIS2DH12_INT1_SRC 0x31
#define LIS2DH12_INT1_THS 0x32
#define LIS2DH12_INT1_DURATION 0x33
#define LIS2DH12_INT2_CFG 0x34
#define LIS2DH12_INT2_SRC 0x35
#define LIS2DH12_INT2_THS 0x36
#define LIS2DH12_INT2_DURATION 0x37
#define LIS2DH12_CLICK_CFG 0x38
#define LIS2DH12_CLICK_SRC 0x39
#define LIS2DH12_CLICK_THS 0x3A
#define LIS2DH12_TIME_LIMIT 0x3B
#define LIS2DH12_TIME_LATENCY 0x3C
#define LIS2DH12_TIME_WINDOW 0x3D
#define LIS2DH12_ACT_THS 0x3E
#define LIS2DH12_ACT_DUR 0x3F
u32 SL_MEMS_i2cRead(u8 addr, u8 reg, u8 len, u8 *buf)
{
// --- 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)
{
u8 SL_MEMS_i2cWrite(u8 addr, u8 reg, u8 data) {
gravity_sensor_command(addr, reg, data);
return 0;
}
/***传感器校验 读 读IIC寄存器 自身地址的值*/
char LIS2DH12_Check()
{
// 检查传感器ID，确认设备是否正常连接
char LIS2DH12_Check() {
u8 reg_value = 0;
SL_MEMS_i2cRead(LIS2DH12_R_ADDR, LIS2DH12_WHO_AM_I, 1, &reg_value);
xlog("xtell LIS2DH12_Check: reg WHO_AM_I value = %x H\n", reg_value);
if (reg_value == 0x33) {
return 0x01;
} else {
return 0x00;
}
return 0x00;
}
/*
传感器配置 初始化
*/
u8 LIS2DH12_Config(void)
{
u8 Check_Flag = 0;
u8 write_num = 0, i = 0;
// u8 ODR = 0x77; //数据速率400HZ， 正常模式（低功耗-> 0x7F）
u8 ODR = 0x37; //速率25hz,
u8 HP = 0x03; //开启高通滤波 HPCF位对应的表格没找到
// u8 click_int = 0x04; //将Click中断映射到INT1 OVERRUN
u8 range = 0xA0; //8g量程，块使能，正常模式(10位数据)
u8 fifo_en = 0x40; //使能FIFO模式
u8 fifo_mode = 0x80;//0x40; //流模式
// u8 click_mode = 0x15; //单击3轴触发
Check_Flag = LIS2DH12_Check();
if (Check_Flag == 1) {
// SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_TEMP_CFG_REG, 0xC0); //开启温度传感器
SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_CTRL_REG1, ODR);
SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_CTRL_REG2, HP);
// SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_CTRL_REG3, click_int);// 中断的使能，默认都是禁用
SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_CTRL_REG4, range); //
SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_CTRL_REG5, fifo_en);
SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_FIFO_CTRL_REG, fifo_mode);
/*
SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_CLICK_CFG, click_mode);//单Z轴
SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_CLICK_THS, LIS2DH12_CLICK_TH);//62.6mg(4g)*10
SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_TIME_LIMIT, LIS2DH12_CLICK_WINDOWS);
SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_TIME_LATENCY, LIS2DH12_CLICK_LATENCY);
*/
// SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_CTRL_REG6, LIS2DH12_INT_LEVEL);
xlog(" LIS2DH12 i2c check ok\n");
return 0;
} else {
xlog(" LIS2DH12 i2c check fail\n");
return -1;
}
}
u8 LIS2DH12_disable(void)
{
u8 Check_Flag = LIS2DH12_Check();
if (Check_Flag == 1) {
u8 ODR = 0x00;
SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_CTRL_REG1, ODR);
xlog("check ok,LIS2DH12_disable\n");
return 0;
} else {
xlog("check fail\n");
return -1;
}
}
void LIS2DH12_read_data_old(axis_info_t *sl_accel)
{
u8 data[6];
u8 fifo_len = 0;
u8 STATUS_REG_data =0;
SL_MEMS_i2cRead(LIS2DH12_R_ADDR, LIS2DH12_SRC_REG, 1, &fifo_len);
SL_MEMS_i2cRead(LIS2DH12_R_ADDR, LIS2DH12_STATUS_REG, 1, &STATUS_REG_data);
xlog("STATUS_REG vlau = %xH\n",STATUS_REG_data);
xlog("FIFO vlau = %xH\n",fifo_len);
if ((fifo_len & 0x40) == 0x40) { //FIFO 溢出标志
fifo_len = 32;
xlog("FIFO is full\n");
} else {
fifo_len = fifo_len & 0x1f;
}
for (u8 i = 0; i < fifo_len; i++) {
SL_MEMS_i2cRead(LIS2DH12_R_ADDR, LIS2DH12_OUT_X_L, 6, data);
sl_accel[i].x = (short)((data[1] << 8) | data[0]);
// sl_accel[i].x = sl_accel[i].x >> 4;
sl_accel[i].y = (short)((data[3] << 8) | data[2]);
// sl_accel[i].y = sl_accel[i].y >> 4;
sl_accel[i].z = (short)((data[5] << 8) | data[4]);
// sl_accel[i].z = sl_accel[i].z >> 4;
// xlog("group:%2d,sl_accel_x:%5d, sl_accel_y:%5d, sl_accel_z:%5d\n", i, sl_accel[i].x, sl_accel[i].y, sl_accel[i].z);
}
xlog("group:%2d,sl_accel_x:%5d, sl_accel_y:%5d, sl_accel_z:%5d\n", sl_accel[fifo_len -1].x, sl_accel[fifo_len -1].y, sl_accel[fifo_len -1].z);
}
//8.1 小李
// 获取三轴传感器的x,y,z加速度值
/**
* @brief 获取三轴传感器的x,y,z加速度值
*
* @param axis_info_t 参数是axis_info_t类型结构体变量，用于存放x,y,z的加速度值
* @return void 此函数无返回值
*/
extern u16 High_g;
void LIS2DH12_read_data(axis_info_t *sl_accel)
{
// 从传感器FIFO读取一批原始数据
void LIS2DH12_read_data(axis_info_t *sl_accel) {
u8 fifo_src = 0;
u8 samples_available = 0;
u8 data[192]; //32*6
u8 data[192];
s16 raw_x,raw_y,raw_z;
// print_register_values(); // 打印寄存器值
// 读取FIFO状态
SL_MEMS_i2cRead(LIS2DH12_R_ADDR, LIS2DH12_SRC_REG, 1, &fifo_src);
// xlog("FIFO_SRC_REG: 0x%02X\n", fifo_src);
// 检查FIFO是否为空
// if (fifo_src & 0x20) {
// xlog("FIFO is empty\n");
// LIS2DH12_reinit_fifo(); //重新启用FIFO
// 尝试重新初始化FIFO
// SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_CTRL_REG5, 0x40); // 使能FIFO
// SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_FIFO_CTRL_REG, 0x40); // FIFO模式
// return;
// }
samples_available = fifo_src & 0x1F;
if (samples_available == 0) return;
// 获取可用样本数
samples_available = fifo_src & 0x1F;
// xlog("Available samples: %d\n", samples_available);
// 一次性读取所有FIFO数据
SL_MEMS_i2cRead(LIS2DH12_R_ADDR, LIS2DH12_OUT_X_L | 0x80, samples_available * 6, data);
// if (SL_MEMS_i2cRead(LIS2DH12_R_ADDR, LIS2DH12_OUT_X_L | 0x80, samples_available * 6, data) != 0) {
// xlog("Error reading FIFO data\n");
// return;
// }
// 解析数据
for (u8 i = 0; i < samples_available; i++) {
raw_x = (int16_t)((data[i * 6 + 1] << 8) | data[i * 6]) >> 6; //传感器输出的是10位数据
// 数据处理方式与 +/-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;
raw_x =convert_10bit_2s_complement(raw_x);
raw_y =convert_10bit_2s_complement(raw_y);
raw_z =convert_10bit_2s_complement(raw_z);
sl_accel[i].x = raw_x;
sl_accel[i].y = raw_y;
sl_accel[i].z = raw_z;
// if (raw_z > 0) {
// // High_g = raw_z * 0.16f;
// High_g = (u8)((float)raw_z * 0.16f);
// } else {
// // High_g = -1.0f * raw_z * 0.16f;
// High_g = (u8)((-1.0f * (float)raw_z) * 0.16f);
// }
// xlog("###High_g == %d\n",High_g);
// xlog("\nSample %d: X=%d, Y=%d, Z=%d\n", i, sl_accel[i].x, sl_accel[i].y, sl_accel[i].z); //打印从寄存器读到的数据
// gsensor_xtell.x = raw_x;
// gsensor_xtell.y = raw_y;
// gsensor_xtell.z = raw_z;
// sl_accel[i].x = raw_x;
// sl_accel[i].y = raw_y;
// sl_accel[i].z = raw_z;
// sl_accel[i].x = (float)raw_x *0.16f; //实际的加速度值 8g 量程
// sl_accel[i].y = (float)raw_y *0.16f;
// sl_accel[i].z = (float)raw_z *0.16f;
// xlog("Sample %d: X=%.2f g, Y=%.2f g, Z=%.2f g\n", i, sl_accel[i].x, sl_accel[i].y, sl_accel[i].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;
@ -287,12 +156,12 @@ bool is_sensor_stable(axis_info_t *accel_data, int sample_count) {
mean_x /= sample_count;
mean_y /= sample_count;
mean_z /= sample_count;
// xlog("Mean X: %.2f, Y: %.2f, Z: %.2f\n", mean_x, mean_y, mean_z); //平均数
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);
@ -302,209 +171,86 @@ bool is_sensor_stable(axis_info_t *accel_data, int sample_count) {
variance_y /= (sample_count - 1);
variance_z /= (sample_count - 1);
// 打印均值和方差，用于调试
// xlog("Variance X: %.2f, Y: %.2f, Z: %.2f\n", variance_x, variance_y, variance_z);
// 判断方差是否在阈值范围内
//非静止状态
// 3. 如果方差大于阈值，则认为在运动
if (variance_x > THRESHOLD || variance_y > THRESHOLD || variance_z > THRESHOLD) {
gsensor_static_flag = 0;
gsensor_alarm =0;
// xlog("**********Sensor is not stable\n");
//超过预设值休眠后重新有数据刷新
if(dormancy_flag){
dormancy_flag = 0;
set_rgb(dormancy_ago_moedl);
dormancy_ago_moedl = 0;
}
return false;
}else { //静止状态
gsensor_static_flag++;
if(gsensor_static_flag >= STATIC_MAX_TIME){
gsensor_alarm =0xFF;
// xlog("@@@Do you need help?@@@");
// gsensor_static_flag =0;
}
#if 0 //0表示测试 1表示正式代码
if(gsensor_static_flag >= DORMANCY_MAX_TIME){ //静止时间超过预设值就关闭灯效，有数据刷新再打开之前灯效
if(dormancy_ago_moedl != RGB_model){ //休眠时灯效和当前灯效 不一致就关闭灯效
dormancy_ago_moedl = RGB_model;
dormancy_flag =1;
colorful_lights_function().update_display_mode(COLORFUL_LIGHTS_DISPLAY_NULL); //关闭灯效
// xlog("dormancy close LED\n");
}
}
#endif
return true; // 传感器处于静止状态
}
}
void print_register_values(){
u8 ctrl_reg1, ctrl_reg3, ctrl_reg5, fifo_ctrl;
SL_MEMS_i2cRead(LIS2DH12_R_ADDR, LIS2DH12_CTRL_REG1, 1, &ctrl_reg1);
SL_MEMS_i2cRead(LIS2DH12_R_ADDR, LIS2DH12_CTRL_REG3, 1, &ctrl_reg3);
SL_MEMS_i2cRead(LIS2DH12_R_ADDR, LIS2DH12_CTRL_REG5, 1, &ctrl_reg5);
SL_MEMS_i2cRead(LIS2DH12_R_ADDR, LIS2DH12_FIFO_CTRL_REG, 1, &fifo_ctrl);
// xlog("CTRL_REG1: 0x%02X, CTRL_REG3: 0x%02X, CTRL_REG5: 0x%02X, FIFO_CTRL_REG: 0x%02X\n", ctrl_reg1, ctrl_reg3, ctrl_reg5, fifo_ctrl);
return true;
}
void LIS2DH12_reinit_fifo() {
SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_CTRL_REG5, 0x00); // 禁用FIFO
SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_FIFO_CTRL_REG, 0x00); // 禁用FIFO模式
delay_2ms(1);
SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_CTRL_REG5, 0x40); // 重新使能FIFO
SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_FIFO_CTRL_REG, 0x40); // 重新设置FIFO模式
}
// 转换函数
float convert_to_g_old(int16_t raw_value, scale_t scale) {
// 将 16 位有符号整数转换为浮点数
float sensitivity;
switch(scale) {
case SCALE_2G:
sensitivity = 1.0f / 16384.0f; // ±2g 范围下的灵敏度
break;
case SCALE_4G:
sensitivity = 1.0f / 8192.0f; // ±4g 范围下的灵敏度
break;
case SCALE_8G:
sensitivity = 1.0f / 4096.0f; // ±8g 范围下的灵敏度
break;
case SCALE_16G:
sensitivity = 1.0f / 2048.0f; // ±16g 范围下的灵敏度
break;
default:
sensitivity = 1.0f / 16384.0f; // 默认使用 ±2g 范围
}
return (float)raw_value * sensitivity * scale / 2.0f;
}
//10位2补码转换
int16_t convert_10bit_2s_complement(int16_t data) {
if (data & 0x200) { // 判断最高位（符号位）是否为1，符号位为1表示2补码时负数
// 如果2补码是负数，把2补码转成原码
data = ~(data - 1); // 先减1，再按位取反
data &= 0x3FF; // 保留低10位
data = -data; // 加上负号
}
return data;
}
// 将原始数据转换为实际加速度值的函数
float convert_to_g(int16_t raw_value) {
float sensitivity = 0.016f; // 16 mg/digit
// switch(CURRENT_SCALE) {
// case SCALE_2G:
// sensitivity = 0.001f; // 4 mg/digit
// break;
// case SCALE_4G:
// sensitivity = 0.008f; // 8 mg/digit
// break;
// case SCALE_8G:
// sensitivity = 0.016f; // 16 mg/digit
// break;
// case SCALE_16G:
// sensitivity = 0.048f; // 48 mg/digit
// break;
// default:
// sensitivity = 0.004f; // Default to ±2g range
// }
if (raw_value & 0x200) { // 判断最高位（符号位）是否为1，符号位为1表示2补码时负数
// 如果2补码是负数，把2补码转成原码
raw_value = ~(raw_value - 1); // 先减1，再按位取反
raw_value &= 0x3FF; // 保留低10位
raw_value = -raw_value; // 加上负号
}
// xlog("raw value= %d",raw_value);
// Convert to g
return (float)raw_value * sensitivity;
}
// 读取传感器数据的定时器回调函数
// 在全局变量区域添加：
axis_info_t last_accel_data = {0.0f, 0.0f, 0.0f}; // 用于存储上一时刻的加速度
// --- New functions for motion tracking ---
/**
* @brief 获取当前加速度值，单位 m/s^2
* @return axis_info_xtell 包含x,y,z轴加速度的结构体
*/
// 获取当前的总加速度（包含重力），单位 m/s^2
axis_info_xtell get_current_accel_mss(void) {
axis_info_xtell accel_mss;
// 1 LSb = 62 mg @ FS = 8 g
// 8g量程: 16 mg/LSB == 0.016 g/LSB
//g：ADC / (灵敏度LSB/g) = ADC * X g/LSB
//实际加速度 = g * 9.8 m/s^2
accel_mss.x = (float)gsensor_xtell.x * 0.016f * GRAVITY_EARTH;
accel_mss.y = (float)gsensor_xtell.y * 0.016f * GRAVITY_EARTH;
accel_mss.z = (float)gsensor_xtell.z * 0.016f * GRAVITY_EARTH;
// 灵敏度 @ +/-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;
}
/**
* @brief 获取计算出的运动数据（速度和距离）
* @param data 指向 motion_data_t 结构体的指针，用于存放结果
*/
// 获取计算好的运动数据（速度和距离）
void get_motion_data(motion_data_t *data) {
if (data) {
// Use a critical section or disable interrupts if this can be called from another thread
memcpy(data, &motion_data, sizeof(motion_data_t));
}
}
// 获取移除重力后的线性加速度
axis_info_xtell get_linear_accel_mss(void) {
return linear_accel_global;
}
void xtell_i2c_test() {
// 获取LIS2DH12三轴加速度
// 核心计算任务，由定时器周期性调用
void xtell_i2c_test() {
// 1. 读取一批最新的传感器数据
LIS2DH12_read_data(current_data);
//判断是否静止, is_sensor_stable会更新全局变量gsensor_xtell
// 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();
axis_info_xtell linear_accel;
// 低通滤波器，把运动产生的加速度和地球重力产生的加速度分开
// 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;
// 总加速度中减去重力
linear_accel.x = current_accel_mss.x - gravity_vector.x;
linear_accel.y = current_accel_mss.y - gravity_vector.y;
linear_accel.z = current_accel_mss.z - gravity_vector.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;
// 积分得到速度
motion_data.velocity.x += linear_accel.x * SAMPLING_PERIOD_S;
motion_data.velocity.y += linear_accel.y * SAMPLING_PERIOD_S;
motion_data.velocity.z += linear_accel.z * SAMPLING_PERIOD_S;
// 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) {
xlog("stop now\n");
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("================================================================\n");
xlog("Total Distance Magnitude: %.2f m\n", total_distance_magnitude);
xlog("================================================================\n");
// xlog("Total distance traveled: %.2f m\n", total_distance_magnitude);
}