//LIS2DH12驱动 #include "gSensor/gSensor_manage.h" #include "app_config.h" #include "math.h" #include "LIS2DH12.h" #include "colorful_lights/colorful_lights.h" #include // 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"); }