#include <zephyr/kernel.h>
#include <zephyr/device.h>
#include <zephyr/devicetree.h>
#include <zephyr/drivers/i2c.h>
#include <zephyr/logging/log.h>
#include <stdio.h>
#include <math.h>
#include "GVL.h"
#include "data_preprocess.h"
#define LOG_MODULE_NAME LSM6DSOWTR_thread
LOG_MODULE_REGISTER(LOG_MODULE_NAME);
// LSM6DSOWTR 寄存器定义
#define LSM6DSOWTR_REG_WHO_AM_I 0x0F
#define LSM6DSOWTR_REG_CTRL1_XL 0x10
#define LSM6DSOWTR_REG_CTRL2_G 0x11
#define LSM6DSOWTR_REG_CTRL3_C 0x12
#define LSM6DSOWTR_REG_OUTX_L_G 0x22
#define LSM6DSOWTR_REG_OUTX_H_G 0x23
#define LSM6DSOWTR_REG_OUTY_L_G 0x24
#define LSM6DSOWTR_REG_OUTY_H_G 0x25
#define LSM6DSOWTR_REG_OUTZ_L_G 0x26
#define LSM6DSOWTR_REG_OUTZ_H_G 0x27
#define LSM6DSOWTR_REG_OUTX_L_XL 0x28
#define LSM6DSOWTR_REG_OUTX_H_XL 0x29
#define LSM6DSOWTR_REG_OUTY_L_XL 0x2A
#define LSM6DSOWTR_REG_OUTY_H_XL 0x2B
#define LSM6DSOWTR_REG_OUTZ_L_XL 0x2C
#define LSM6DSOWTR_REG_OUTZ_H_XL 0x2D
// 配置参数
#define LSM6DSOWTR_WHO_AM_I_VAL 0x6C
#define LSM6DSOWTR_ODR_XL_104HZ 0x40 // 加速度计 104Hz
#define LSM6DSOWTR_FS_XL_2G 0x00 // ±2g 量程
#define LSM6DSOWTR_ODR_G_104HZ 0x40 // 陀螺仪 104Hz
#define LSM6DSOWTR_FS_G_245DPS 0x00 // ±245dps 量程
#define LSM6DSOWTR_REG_STATUS_REG 0x1E
#define ACCEL_SENSITIVITY_2G 0.000061f
#define GYRO_SENSITIVITY_245DPS 0.00875f // 8.75 mdps/LSB
// 压力传感器LPS28DFW 寄存器定义
// LPS28DFW 寄存器定义
#define LPS28DFW_WHO_AM_I 0x0F
#define LPS28DFW_CTRL_REG1 0x10
#define LPS28DFW_CTRL_REG2 0x11
#define LPS28DFW_CTRL_REG3 0x12
#define LPS28DFW_PRESS_OUT_XL 0x28
#define LPS28DFW_PRESS_OUT_L 0x29
#define LPS28DFW_PRESS_OUT_H 0x2A
#define LPS28DFW_TEMP_OUT_L 0x2B
#define LPS28DFW_TEMP_OUT_H 0x2C
#define LPS28DFW_INTERRUPT_CFG 0x0B
#define LPS28DFW_THS_P_L 0x0C
#define LPS28DFW_THS_P_H 0x0D
// 配置值
#define LPS28DFW_WHO_AM_I_VAL 0xB4
#define LPS28DFW_ODR_50HZ 0x50 // 50Hz输出数据率
#define LPS28DFW_LPF_ENABLE 0x08 // 启用低通滤波器
#define LPS28DFW_BDU_ENABLE 0x08 // 启用块数据更新
#define LPS28DFW_WATER_MODE 0x40 // 水压测量模式
#define SAMPLE_INTERVAL_MS 100
//MS5837压力传感器
#define MS5837_CMD_RESET 0x1E
#define MS5837_CMD_CONVERT_D1_OSR1024 0x44
#define MS5837_CMD_CONVERT_D2_OSR1024 0x54
#define MS5837_CMD_ADC_READ 0x00
#define MS5837_PROM_READ_BASE 0xA0
struct ms5837_calib {
uint16_t factory; // PROM word 0 (厂商数据)
uint16_t c1; // 压力灵敏度
uint16_t c2; // 压力偏移
uint16_t c3; // 温度系数-压力灵敏度
uint16_t c4; // 温度系数-压力偏移
uint16_t c5; // 参考温度
uint16_t c6; // 温度系数
uint16_t crc; // CRC校验
};
// 等待数据就绪
static bool wait_for_data_ready(const struct i2c_dt_spec *dev_spec)
{
uint8_t status;
int retries = 10;
while (retries-- > 0) {
if (i2c_reg_read_byte_dt(dev_spec, LSM6DSOWTR_REG_STATUS_REG, &status) == 0) {
// 检查加速度计和陀螺仪数据就绪位 (bit0 & bit1)
if ((status & 0x03) == 0x03) return true;
}
k_msleep(1);
}
return false;
}
static int init_LPS28DFW_water(const struct i2c_dt_spec *dev_spec)
{
const int max_retries = 5;
uint8_t whoami;
int ret;
int retries = 0;
LOG_INF("Starting LPS28DFW initialization...");
LOG_DBG("Verifying I2C connection to device at 0x%02X", dev_spec->addr);
do {
ret = i2c_reg_read_byte_dt(dev_spec, LPS28DFW_WHO_AM_I, &whoami);
if (ret == 0 && whoami == LPS28DFW_WHO_AM_I_VAL) break;
if (ret != 0) {
LOG_WRN("I2C read error: %d (attempt %d)", ret, retries+1);
} else {
LOG_WRN("Unexpected WHO_AM_I: 0x%02X (expected 0x%02X) attempt %d",
whoami, LPS28DFW_WHO_AM_I_VAL, retries+1);
}
i2c_recover_bus(dev_spec->bus);
k_msleep(10);
} while (++retries < max_retries);
if (retries >= max_retries) {
LOG_ERR("Device detection failed after %d attempts", max_retries);
return -ENODEV;
}
uint8_t ctrl_reset=0x04;
ret = i2c_reg_write_byte_dt(dev_spec, LPS28DFW_CTRL_REG2, ctrl_reset);
k_msleep(10);
if (ret) {
LOG_ERR("ctrl_reg2 reset failed: %d", ret);
return ret;
}
uint8_t ctrl_reset_value;
ret = i2c_reg_read_byte_dt(dev_spec, LPS28DFW_CTRL_REG2, &ctrl_reset_value);
if (ret == 0) {
// 检查关键位
const uint8_t critical_bits_mask_reset = 0x04;
if (ctrl_reset_value == critical_bits_mask_reset) {
LOG_DBG("CTRL_REG2 reset verified: 0x%02X", ctrl_reset_value);
} else{
LOG_WRN("CTRL_REG2 reset failed: read 0x%02X", ctrl_reset_value);
}
}
uint8_t ctrl_reg1=0x50;
ret = i2c_reg_write_byte_dt(dev_spec, LPS28DFW_CTRL_REG1, ctrl_reg1);
if (ret) {
LOG_ERR("ctrl_reg1 set failed: %d", ret);
return ret;
}
uint8_t ctrl_reg2 =0x18;
ret = i2c_reg_write_byte_dt(dev_spec, LPS28DFW_CTRL_REG2, ctrl_reg2);
if (ret) {
LOG_ERR("ctrl_reg1 set failed: %d", ret);
return ret;
}
uint8_t reg_value1;
uint8_t reg_value2;
uint8_t reg_value3;
ret = i2c_reg_read_byte_dt(dev_spec, LPS28DFW_CTRL_REG1, ®_value1);
if (ret == 0) {
// 检查关键位
const uint8_t critical_bits_mask1 = 0x50;
if (reg_value1 == critical_bits_mask1) {
LOG_DBG("CTRL_REG1 verified: 0x%02X", reg_value1);
} else{
LOG_WRN("CTRL_REG1 failed: read 0x%02X", reg_value1);
}
}
ret = i2c_reg_read_byte_dt(dev_spec, LPS28DFW_CTRL_REG2, ®_value2);
if (ret == 0) {
// 检查关键位
const uint8_t critical_bits_mask2 = 0x18;
if (reg_value2 == critical_bits_mask2) {
LOG_DBG("CTRL_REG2 verified: 0x%02X", reg_value2);
} else{
LOG_WRN("CTRL_REG2 failed: read 0x%02X", reg_value2);
}
}
uint8_t ctrl_reg3 =0x01;
ret = i2c_reg_write_byte_dt(dev_spec, LPS28DFW_CTRL_REG3, ctrl_reg3);
if (ret) {
LOG_ERR("ctrl_reg3 set failed: %d", ret);
return ret;
}
ret = i2c_reg_read_byte_dt(dev_spec, LPS28DFW_CTRL_REG3, ®_value3);
if (ret == 0) {
// 检查关键位
const uint8_t critical_bits_mask3 = 0x01;
if (reg_value3 == critical_bits_mask3) {
LOG_DBG("CTRL_REG3 verified: 0x%02X", reg_value3);
} else{
LOG_WRN("CTRL_REG3 failed: read 0x%02X", reg_value3);
}
}
LOG_INF("LPS28DFW initialized successfully");
return 0;
}
// 读取原始压力数据 (24位有符号整数)
static int32_t read_raw_pressure(const struct i2c_dt_spec *dev_spec)
{
uint8_t press_data[3];
int ret;
ret = i2c_burst_read_dt(dev_spec, LPS28DFW_PRESS_OUT_XL, press_data, sizeof(press_data));
if (ret != 0) {
LOG_ERR("Pressure read failed: %d", ret);
return 0;
}
int32_t raw_pressure = ((int32_t)press_data[2] << 16) |
((int32_t)press_data[1] << 8) |
press_data[0];
if (raw_pressure & 0x00800000) {
raw_pressure |= 0xFF000000;
}
return raw_pressure;
}
// 读取原始温度数据 (16位有符号整数)
static int16_t read_raw_temperature(const struct i2c_dt_spec *dev_spec)
{
uint8_t temp_data[2];
int ret;
ret = i2c_burst_read_dt(dev_spec, LPS28DFW_TEMP_OUT_L, temp_data, sizeof(temp_data));
if (ret != 0) {
LOG_ERR("Temperature read failed: %d", ret);
return 0;
}
// 组合16位有符号整数 (小端格式)
return (int16_t)((temp_data[1] << 8) | temp_data[0]);
}
// 读取带温度补偿的水压
static double read_compensated_water_pressure(const struct i2c_dt_spec *dev_spec)
{
// 读取原始压力值
int32_t raw_pressure = read_raw_pressure(dev_spec);
// 读取原始温度值
int16_t raw_temp = read_raw_temperature(dev_spec);
// 转换为摄氏度 (100 LSB/°C)
double temperature = (double)raw_temp / 100.0f;
// 转换为hPa
double pressure_hPa = (double)raw_pressure / 4096.0f;
// 温度补偿因子 (根据数据手册的典型特性)
// 实际应用中应根据传感器校准数据调整
double comp_factor = 1.0f + (0.0001f * temperature);
// 应用温度补偿
pressure_hPa *= comp_factor;
// 转换为米水柱
return pressure_hPa*100.0;
}
// 读取水深 (单位: 米)
static double read_water_depth(const struct i2c_dt_spec *dev_spec)
{
// 读取补偿后的水压
double pressure_mH2O = read_compensated_water_pressure(dev_spec);
// 水深 = 水压值 (忽略大气压影响)
return pressure_mH2O;
}
static double read_compensated_water_depth(const struct i2c_dt_spec *dev_spec, double atmospheric_pressure)
{
// 读取补偿后的水压（单位：米水柱）
//double absolute_pressure_mH2O = read_compensated_water_pressure(dev_spec);
double raw_press = read_raw_pressure(dev_spec) / 0.4096f;
double raw_temp = read_raw_temperature(dev_spec) / 100.0f;
// double absolute_pressure_Pa = read_compensated_water_pressure(dev_spec);
// 转换为Pa (1 mH2O = 9806.65 Pa)
//double temp_depth = (absolute_pressure_Pa - atmospheric_pressure) / (1000.0 * 9.80665);
//double temp_depth = (sensor_data_PWM.pressure/10.0f - atmospheric_pressure/10.0f) / (1000.0 * 9.80665);
double temp_depth = (raw_press/10.0f - atmospheric_pressure/10.0f) / (1000.0 * 9.80665);
LOG_INF("raw temp:%.2f raw pressure: %.2f atmospheric_pressure: %.2f kPa (%.4f mH2O)",
raw_temp,raw_press/10.f,atmospheric_pressure/10.0f, temp_depth);
// 水深 = (绝对压力 - 大气压) / (水密度 * 重力加速度)
//return (absolute_pressure_Pa - atmospheric_pressure) / (1000.0f * 9.80665f);
return (raw_press/10.0f - atmospheric_pressure/10.0f) / (1000.0f * 9.80665f);
}
// 初始化 LSM6DS0 传感器
static int init_lsm6dsowtr(const struct i2c_dt_spec *dev_spec)
{
uint8_t whoami;
int ret;
LOG_INF("Initializing LSM6DSOWTR sensor...");
// 读取 WHO_AM_I 寄存器
ret = i2c_reg_read_byte_dt(dev_spec, LSM6DSOWTR_REG_WHO_AM_I, &whoami);
if (ret != 0) {
LOG_ERR("Failed to read WHO_AM_I: %d", ret);
return ret;
}
if (whoami != LSM6DSOWTR_WHO_AM_I_VAL) {
LOG_ERR("Unexpected WHO_AM_I: 0x%02X (expected 0x%02X)",
whoami, LSM6DSOWTR_WHO_AM_I_VAL);
return -ENODEV;
}
LOG_INF("LSM6DSOWTR detected (WHO_AM_I=0x%02X)", whoami);
// 配置加速度计
uint8_t ctrl1_xl = LSM6DSOWTR_ODR_XL_104HZ | LSM6DSOWTR_FS_XL_2G;
ret = i2c_reg_write_byte_dt(dev_spec, LSM6DSOWTR_REG_CTRL1_XL, ctrl1_xl);
if (ret != 0) {
LOG_ERR("Failed to write CTRL1_XL: %d", ret);
return ret;
}
// 配置陀螺仪
uint8_t ctrl2_g = LSM6DSOWTR_ODR_G_104HZ | LSM6DSOWTR_FS_G_245DPS;
ret = i2c_reg_write_byte_dt(dev_spec, LSM6DSOWTR_REG_CTRL2_G, ctrl2_g);
if (ret != 0) {
LOG_ERR("Failed to write CTRL2_G: %d", ret);
return ret;
}
uint8_t reg_val;
ret = i2c_reg_read_byte_dt(dev_spec, LSM6DSOWTR_REG_CTRL1_XL, ®_val);
if (ret != 0 || reg_val != ctrl1_xl) {
LOG_ERR("CTRL1_XL verification failed: wrote 0x%02X, read 0x%02X",
ctrl1_xl, reg_val);
return -EIO;
}
ret = i2c_reg_read_byte_dt(dev_spec, LSM6DSOWTR_REG_CTRL2_G, ®_val);
if (ret != 0 || reg_val != ctrl2_g) {
LOG_ERR("CTRL2_G verification failed: wrote 0x%02X, read 0x%02X",
ctrl2_g, reg_val);
return -EIO;
}
// 配置控制寄存器3
ret = i2c_reg_write_byte_dt(dev_spec, LSM6DSOWTR_REG_CTRL3_C, 0x04); // BDU=1 (块数据更新)
if (ret != 0) {
LOG_ERR("Failed to write CTRL3_C: %d", ret);
return ret;
}
LOG_INF("LSM6DSOWTR initialized successfully");
return 0;
}
// 读取加速度计数据
static void read_accelerometer(const struct i2c_dt_spec *dev_spec,
double *x, double *y, double *z)
{
if (!wait_for_data_ready(dev_spec)) {
LOG_WRN("Accel data not ready");
*x = *y = *z = NAN;
return;
}
uint8_t data[6];
int ret = i2c_burst_read_dt(dev_spec, LSM6DSOWTR_REG_OUTX_L_XL, data, sizeof(data));
if (ret != 0) {
LOG_ERR("Accelerometer read failed: %d", ret);
*x = *y = *z = NAN;
return;
}
// 组合16位数据（小端格式）
int16_t raw_x = (int16_t)((data[1] << 8) | data[0]);
int16_t raw_y = (int16_t)((data[3] << 8) | data[2]);
int16_t raw_z = (int16_t)((data[5] << 8) | data[4]);
// 转换为 g (重力加速度) - ±2g 量程，灵敏度 0.061 mg/LSB
*x = raw_x * ACCEL_SENSITIVITY_2G;
*y = raw_y * ACCEL_SENSITIVITY_2G;
*z = raw_z * ACCEL_SENSITIVITY_2G;
}
// 读取陀螺仪数据
static void read_gyroscope(const struct i2c_dt_spec *dev_spec,
double *x, double *y, double *z)
{
if (!wait_for_data_ready(dev_spec)) {
LOG_WRN("Gyro data not ready");
*x = *y = *z = NAN;
return;
}
uint8_t data[6];
int ret = i2c_burst_read_dt(dev_spec, LSM6DSOWTR_REG_OUTX_L_G, data, sizeof(data));
if (ret != 0) {
LOG_ERR("Gyroscope read failed: %d", ret);
*x = *y = *z = NAN;
return;
}
// 组合16位数据（小端格式）
int16_t raw_x = (int16_t)((data[1] << 8) | data[0]);
int16_t raw_y = (int16_t)((data[3] << 8) | data[2]);
int16_t raw_z = (int16_t)((data[5] << 8) | data[4]);
// 转换为 dps (度/秒) - ±245dps 量程，灵敏度 8.75 mdps/LSB
*x = raw_x * GYRO_SENSITIVITY_245DPS;
*y = raw_y * GYRO_SENSITIVITY_245DPS;
*z = raw_z * GYRO_SENSITIVITY_245DPS;
}
// 将浮点值转换为定点表示（用于存储）
static int32_t sensor_value_to_int(double value, int scale_factor)
{
return (int32_t)(value * scale_factor);
}
// 主传感器线程
void lsm6dsowtr_thread(void)
{
LOG_INF("LSM6DSOWTR sensor thread started");
// 获取六轴传感器设备规范
static const struct i2c_dt_spec lsm6dsowtr_dev = I2C_DT_SPEC_GET(DT_NODELABEL(lsm6dsowtr));
if (!device_is_ready(lsm6dsowtr_dev.bus)) {
LOG_ERR("I2C bus not ready: %s", lsm6dsowtr_dev.bus->name);
return;
}
// 获取压力传感器设备规范
static const struct i2c_dt_spec pressure_dev = I2C_DT_SPEC_GET(DT_NODELABEL(mysensor));
if (!device_is_ready(pressure_dev.bus)) {
LOG_ERR("I2C bus %s is not ready!", pressure_dev.bus->name);
return;
}
// 初始化传感器
int init_result;
// 初始化压力传感器
init_result = init_LPS28DFW_water(&pressure_dev);
if (init_result != 0) {
LOG_ERR("Pressure sensor initialization failed: %d", init_result);
} else {
LOG_INF("Pressure sensor initialized successfully");
}
// struct ms5837_calib calib_data;
// int ms5837_init_ret = init_MS5837(&pressure_dev, &calib_data);
// if (ms5837_init_ret != 0) {
// LOG_ERR("MS5837 initialization failed: %d", ms5837_init_ret);
// // 非致命错误，继续运行其他传感器
// } else {
// LOG_INF("MS5837 initialized successfully");
// }
// 初始化六轴传感器
init_result = init_lsm6dsowtr(&lsm6dsowtr_dev);
if (init_result != 0) {
LOG_ERR("LSM6DSOWTR initialization failed: %d", init_result);
} else {
LOG_INF("LSM6DSOWTR initialized successfully");
}
LOG_INF("Starting sensor data collection...");
// 主循环变量
int64_t last_sample_time = k_uptime_get();
int64_t last_batch_time = last_sample_time;
static int datacount = 0;
static double press_a = 0;
static double depth_comp = 0;
static int depth_count = 0;
while (1) {
double press = read_raw_pressure(&pressure_dev);
double temp = read_raw_temperature(&pressure_dev);
double compensated_depth;
if(datacount < 20){
press_a = press_a + press/4096.0f;
datacount++;
// LOG_INF("press_a: %.4f Pa,datacount:%d", press_a,datacount);
} else{
press_a = press_a/20.0f;
// LOG_INF("press_a: %.4f Pa,datacount:%d", press_a,datacount);
press_a = press_a*10000;
// sensor_data_PWM.pressure = press_a*10000;
compensated_depth = read_compensated_water_depth(&pressure_dev, press_a);
if(depth_count < 20){
depth_comp = depth_comp + compensated_depth;
depth_count++;
// LOG_INF("press_a: %.4f Pa,datacount:%d", press_a,datacount);
} else{
depth_comp = depth_comp/20.0f;
LOG_INF("depth_comp: %.4f Pa,depth_count:%d", depth_comp,depth_count);
depth_count = 0;
depth_comp = 0;
}
datacount = 0;
press_a = 0;
}
// if(depth_count < 20){
// depth_comp = depth_comp + compensated_depth/4096.0f;
// datacount++;
// // LOG_INF("press_a: %.4f Pa,datacount:%d", press_a,datacount);
// } else{
// press_a = press_a/20.0f;
// // LOG_INF("press_a: %.4f Pa,datacount:%d", press_a,datacount);
// press_a = press_a*10000;
// compensated_depth = read_compensated_water_depth(&pressure_dev, press_a);
// datacount = 0;
// press_a = 0;
// }
double accel_x, accel_y, accel_z;
double gyro_x, gyro_y, gyro_z;
read_accelerometer(&lsm6dsowtr_dev, &accel_x, &accel_y, &accel_z);
read_gyroscope(&lsm6dsowtr_dev, &gyro_x, &gyro_y, &gyro_z);
//float magnitude = calculate_acceleration_magnitude(accel_x, accel_y, accel_z);
// LOG_INF("Accel: X=%.3f g, Y=%.3f g, Z=%.3f g",
// accel_x, accel_y, accel_z);
// LOG_INF("Gyro: X=%.2f dps, Y=%.2f dps, Z=%.2f dps",
// gyro_x, gyro_y, gyro_z);
// 获取当前时间戳
int64_t current_time = k_uptime_get();
// 准备数据点
// sensor_data_point_t data_point = {
// .timestamp = current_time,
// .pressure = sensor_value_to_int(press, 1000), // kPa * 1000 = milli-kPa
// .temp = sensor_value_to_int(temp, 100), // °C * 100 = centi-°C
// .acc_x = sensor_value_to_int(accel_x, 1000), // g * 1000 = milli-g
// .acc_y = sensor_value_to_int(accel_y, 1000),
// .acc_z = sensor_value_to_int(accel_z, 1000),
// .gyro_x = sensor_value_to_int(gyro_x, 1000), // dps * 1000 = milli-dps
// .gyro_y = sensor_value_to_int(gyro_y, 1000),
// .gyro_z = sensor_value_to_int(gyro_z, 1000)
// };
sensor_data_point_t data_point = {
.timestamp = current_time,
.pressure = sensor_value_to_int(press/4096.0f, 10000), // kPa * 1000 = milli-kPa
.temp = sensor_value_to_int(temp, 100), // °C * 100 = centi-°C
.acc_x = sensor_value_to_int(accel_x, 1000), // g * 1000 = milli-g
.acc_y = sensor_value_to_int(accel_y, 1000),
.acc_z = sensor_value_to_int(accel_z, 1000),
.gyro_x = sensor_value_to_int(gyro_x, 1000), // dps * 1000 = milli-dps
.gyro_y = sensor_value_to_int(gyro_y, 1000),
.gyro_z = sensor_value_to_int(gyro_z, 1000)
};
// LOG_INF("data filter");
data_point = filter_sensor_data(&data_point);
atmospheric_pressure = data_point.pressure;
//double compensated_depth = read_compensated_water_depth(&pressure_dev, atmospheric_pressure);
// double compensated_depth = read_compensated_water_depth(&pressure_dev, press/4096.0f);
// LOG_INF("Compensated Depth: %.4f m",compensated_depth);
// // 安全地添加到批量缓冲区
// k_mutex_lock(&data_mutex, K_FOREVER);
// // 检查当前批次缓冲区是否有效
// if (sensor_batch.count == 0) {
// // 新批次，记录起始时间戳
// sensor_batch.start_timestamp = current_time;
// }
// // 添加数据点到批次
// if (sensor_batch.count < BATCH_SIZE) {
// sensor_batch.data[sensor_batch.count] = data_point;
// sensor_batch.count++;
// // 检查批次是否已满
// if (sensor_batch.count >= BATCH_SIZE) {
// // 批次已满，标记为就绪
// sensor_batch.ready = true;
// LOG_DBG("Batch filled (%d points)", BATCH_SIZE);
// }
// } else {
// LOG_WRN("Batch buffer full, discarding data point");
// }
// // 检查是否超时（即使批次未满）
// if ((current_time - sensor_batch.start_timestamp) >= (MAX_BATCH_TIMEOUT * 1000)) {
// if (sensor_batch.count > 0) {
// sensor_batch.ready = true;
// LOG_DBG("Batch timeout with %d points", sensor_batch.count);
// }
// }
// // 如果批次就绪，通知BLE线程
// if (sensor_batch.ready) {
// k_sem_give(&batch_ready_sem);
// LOG_DBG("Notified BLE thread of data ready");
// }
// k_mutex_unlock(&data_mutex);
// 计算下一个采样点的时间
int64_t elapsed = k_uptime_get() - last_sample_time;
int32_t sleep_time = SAMPLE_INTERVAL_MS - elapsed;
if (sleep_time > 0) {
k_msleep(sleep_time);
} else {
LOG_WRN("Sampling behind schedule by %d ms", -sleep_time);
}
last_sample_time = k_uptime_get();
}
}
// 定义传感器线程
K_THREAD_DEFINE(lsm6dsowtr_thread_id, 4096, lsm6dsowtr_thread, NULL, NULL, NULL, 7, 0, 0);对原始压力值进行中值滤波
最新发布
意识到自己需要付出额外的努力才能赶上他人，本文探讨了如何提高自身能力、制定有效策略并保持持续进步的方法。
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
