// 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 // 用于 memcpy ////////////////////////////////////////////////////////////////////////////////////////////////// //START -- 宏定义 #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 // --- 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 //END -- 宏定义 ////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////// //START -- 变量定义 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计算出的平均值 //END -- 变量定义 ////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////// //START -- 函数定义 //END -- 函数定义 ////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////// //实现 // --- 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); }