511 lines
20 KiB
C
511 lines
20 KiB
C
//LIS2DH12驱动
|
||
#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
|
||
|
||
#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 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;
|
||
|
||
|
||
u8 gsensor_alarm;
|
||
// u8 xtell_uart_Gsensor_static[5]={0xAA,0x55,0x0F,0x55,0xAA}; //发现用户长时间静止时，串口发数据给AI芯片
|
||
|
||
|
||
// 假设当前设置的满量程范围
|
||
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)
|
||
#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 //使能温度传感器
|
||
#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)
|
||
{
|
||
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;
|
||
}
|
||
/***传感器校验 读 读IIC寄存器 自身地址的值*/
|
||
char LIS2DH12_Check()
|
||
{
|
||
u8 reg_value = 0;
|
||
SL_MEMS_i2cRead(LIS2DH12_R_ADDR, LIS2DH12_WHO_AM_I, 1, ®_value);
|
||
xlog("xtell LIS2DH12_Check: reg WHO_AM_I value = %x H\n", reg_value);
|
||
if (reg_value == 0x33) {
|
||
return 0x01;
|
||
} else {
|
||
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)
|
||
{
|
||
u8 fifo_src = 0;
|
||
u8 samples_available = 0;
|
||
u8 data[192]; //32*6
|
||
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;
|
||
// 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位数据
|
||
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);
|
||
}
|
||
}
|
||
// 判断传感器是否长期静止的函数
|
||
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;
|
||
// 计算每个轴的均值
|
||
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;
|
||
// 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;
|
||
|
||
// 计算每个轴的方差
|
||
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);
|
||
|
||
// 打印均值和方差，用于调试
|
||
// xlog("Variance X: %.2f, Y: %.2f, Z: %.2f\n", variance_x, variance_y, variance_z);
|
||
|
||
// 判断方差是否在阈值范围内
|
||
//非静止状态
|
||
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);
|
||
}
|
||
|
||
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轴加速度的结构体
|
||
*/
|
||
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;
|
||
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));
|
||
}
|
||
}
|
||
|
||
|
||
void xtell_i2c_test() {
|
||
|
||
// 获取LIS2DH12三轴加速度
|
||
LIS2DH12_read_data(current_data);
|
||
|
||
//判断是否静止, is_sensor_stable会更新全局变量gsensor_xtell
|
||
sensor_is_stable = is_sensor_stable(current_data, SAMPLE_COUNT);
|
||
|
||
axis_info_xtell current_accel_mss = get_current_accel_mss();
|
||
axis_info_xtell linear_accel;
|
||
|
||
// 低通滤波器，把运动产生的加速度和地球重力产生的加速度分开
|
||
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;
|
||
|
||
// 积分得到速度
|
||
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;
|
||
|
||
// 静止
|
||
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 += 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;
|
||
|
||
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");
|
||
|
||
}
|