diff --git a/Makefile b/Makefile index e537e7c..17e2ca1 100644 --- a/Makefile +++ b/Makefile @@ -252,6 +252,7 @@ INCLUDES := \ -Iapps/earphone/xtell_Sensor/ano \ -Iapps/earphone/xtell_Sensor/sensor/ \ -Iapps/earphone/xtell_Sensor/sensor/ \ + -Iapps/earphone/xtell_Sensor/sensor/ \ -I$(SYS_INC_DIR) \ @@ -626,6 +627,7 @@ c_SRC_FILES := \ apps/earphone/xtell_Sensor/ano/ano_protocol.c \ apps/earphone/xtell_Sensor/sensor/MMC56.c \ apps/earphone/xtell_Sensor/sensor/BMP280.c \ + apps/earphone/xtell_Sensor/sensor/AK8963.c \ # 需要编译的 .S 文件 diff --git a/apps/common/device/gSensor/gSensor_manage.c b/apps/common/device/gSensor/gSensor_manage.c index 53ab6b6..7849604 100644 --- a/apps/common/device/gSensor/gSensor_manage.c +++ b/apps/common/device/gSensor/gSensor_manage.c @@ -314,7 +314,7 @@ u8 _gravity_sensor_get_ndata(u8 r_chip_id, u8 register_address, u8 *buf, u8 data iic_start(gSensor_info->iic_hdl); if (0 == iic_tx_byte(gSensor_info->iic_hdl, r_chip_id - 1)) { - xlog("\n gsen iic rd err 0\n"); + xlog("I2C NACK on writing ADDR: 0x%X\n", r_chip_id - 1); read_len = 0; strcpy(&sen_log_buffer_1, "gsen iic rd err 0\n"); goto __gdend; @@ -323,7 +323,8 @@ u8 _gravity_sensor_get_ndata(u8 r_chip_id, u8 register_address, u8 *buf, u8 data delay(gSensor_info->iic_delay); if (0 == iic_tx_byte(gSensor_info->iic_hdl, register_address)) { - xlog("\n gsen iic rd err 1\n"); + xlog("I2C NACK on register ADDR: 0x%X\n", register_address); + // xlog("\n gsen iic rd err 1\n"); read_len = 0; strcpy(&sen_log_buffer_2, "gsen iic rd err 1\n"); goto __gdend; diff --git a/apps/earphone/board/br28/board_jl701n_demo.c b/apps/earphone/board/br28/board_jl701n_demo.c index 3602cfb..c2d2c6d 100644 --- a/apps/earphone/board/br28/board_jl701n_demo.c +++ b/apps/earphone/board/br28/board_jl701n_demo.c @@ -526,7 +526,7 @@ const struct hw_iic_config hw_iic_cfg[] = { .baudrate = TCFG_HW_I2C0_CLK, //IIC通讯波特率 .hdrive = 0, //是否打开IO口强驱 .io_filter = 1, //是否打开滤波器（去纹波） - .io_pu = 1, //是否打开上拉电阻，如果外部电路没有焊接上拉电阻需要置1 + .io_pu = 0, //是否打开上拉电阻，如果外部电路没有焊接上拉电阻需要置1 }, }; diff --git a/apps/earphone/xtell_Sensor/A_hide/calculate/skiing_tracker.c b/apps/earphone/xtell_Sensor/A_hide/calculate/skiing_tracker.c new file mode 100644 index 0000000..a45d496 --- /dev/null +++ b/apps/earphone/xtell_Sensor/A_hide/calculate/skiing_tracker.c @@ -0,0 +1,651 @@ +/* + +*/ +#include "skiing_tracker.h" +#include "../sensor/SC7U22.h" +#include +#include + +#define G_ACCELERATION 9.81f +#define DEG_TO_RAD (3.14159265f / 180.0f) + +#define ENABLE_XLOG 1 +#ifdef xlog +#undef xlog +#endif +#if ENABLE_XLOG + #define xlog(format, ...) printf("[XT:%s] " format, __func__, ##__VA_ARGS__) +#else + #define xlog(format, ...) ((void)0) +#endif + +// --- 静止检测 --- +//两个判断是否静止的必要条件：动态零速更新(ZUPT)阈值 +// 加速方差阈值，提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新 +#define STOP_ACC_VARIANCE_THRESHOLD 0.2f +// 陀螺仪方差阈值 +#define STOP_GYR_VARIANCE_THRESHOLD 5.0f +// 静止时候的陀螺仪模长 +#define STOP_GYR_MAG_THRESHOLD 15 +// --- --- --- + +// --- 启动滑雪阈值 --- +// 加速度模长与重力的差值大于此值，认为开始运动；降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动 +#define START_ACC_MAG_THRESHOLD 1.0f //0.5、1 +// 陀螺仪方差阈值，以允许启动瞬间的正常抖动，但仍能过滤掉混乱的、非滑雪的晃动。 +#define START_GYR_VARIANCE_THRESHOLD 15.0f +// --- --- --- + +// --- 滑雪过程 --- +//加速度 模长(不含重力），低于此值视为 在做匀速运动 +#define SKIING_ACC_MAG_THRESHOLD 0.5f +//陀螺仪 模长，高于此值视为 摔倒了 +#define FALLEN_GRY_MAG_THRESHOLD 2000.0f //未确定 +// --- --- --- + +// --- 原地旋转抖动 --- +// 加速度 方差 阈值。此值比 静止检测 阈值更宽松， +#define WOBBLE_ACC_VARIANCE_THRESHOLD 0.5f +// 加速度 模长 阈值 +#define WOBBLE_ACC_MAG_THRESHOLD 1.0f +// 角速度 总模长 大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动 +#define ROTATION_GYR_MAG_THRESHOLD 30.0f +// --- --- --- + +// --- 滑雪转弯动 --- +// 加速度 方差 阈值，大于此值，滑雪过程可能发生了急转弯 +#define WHEEL_ACC_VARIANCE_THRESHOLD 7.0f +// 角速度 总模长 大于此值(度/秒)，认为滑雪过程中进行急转弯 +#define WHEEL_GYR_MAG_THRESHOLD 500.0f // +// --- --- --- + +// --- 跳跃 --- +// 加速度模长低于此值(g)，认为进入失重状态(IN_AIR) +#define AIRBORNE_ACC_MAG_LOW_THRESHOLD 0.4f +// 加速度模长高于此值(g)，认为发生落地冲击(LANDING) +#define LANDING_ACC_MAG_HIGH_THRESHOLD 3.5f +// 起跳加速度阈值(g)，用于进入TAKING_OFF状态 +#define TAKEOFF_ACC_MAG_HIGH_THRESHOLD 1.8f +// 进入空中状态确认计数：需要连续3个采样点加速度低于阈值才判断为起跳 +#define AIRBORNE_CONFIRM_COUNT 3 +// 落地状态确认计数：加速度恢复到1g附近并持续2个采样点(20ms)则认为已落地 +#define GROUNDED_CONFIRM_COUNT 2 +// 最大滞空时间(秒)，超过此时间强制认为已落地，防止状态锁死 +#define MAX_TIME_IN_AIR 12.5f +// --- --- --- + +// --- 用于消除积分漂移的滤波器和阈值 --- +// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。 +// alpha = RC / (RC + dt)，参考RC电路而来，fc ≈ (1 - alpha) / (2 * π * dt) +#define HPF_ALPHA 0.999f +//0.995f： 0.08 Hz 的信号 +//0.999f： 0.0159 Hz +// --- --- --- + +// --- 低通滤波器 --- +// 低通滤波器系数 (alpha)。alpha 越小，滤波效果越强（更平滑），但延迟越大。 +// alpha 推荐范围 0.7 ~ 0.95。可以从 0.85 开始尝试。 +#define LPF_ALPHA 0.7f + +// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。 +// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。 +//参考：0.2f ~ 0.4f +#define ACC_DEAD_ZONE_THRESHOLD 0.05f + +// --- 模拟摩擦力，进行速度衰减 --- +#define SPEED_ATTENUATION 1.0f //暂不模拟 +BLE_KS_send_data_t KS_data; +static float quaternion_data[4]; +#ifdef XTELL_TEST + +debug_t debug1; +debug_t debug2; +#endif + +static skiing_tracker_t my_skiing_tracker; +////////////////////////////////////////////////////////////////////////////////////////////////// +//实现 + +void clear_speed(void){ + my_skiing_tracker.state = STATIC; + memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity)); + my_skiing_tracker.speed = 0; +} + +void start_detection(void){ + my_skiing_tracker.state = STATIC; + memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity)); + my_skiing_tracker.distance = 0; + my_skiing_tracker.speed = 0; +} + +void stop_detection(void){ + my_skiing_tracker.state = STOP_DETECTION; + memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity)); + my_skiing_tracker.speed = 0; +} + +/** + * @brief 初始化滑雪追踪器 + * + * @param tracker + */ +void skiing_tracker_init(skiing_tracker_t *tracker) +{ + if (!tracker) { + return; + } + // 使用memset一次性清零整个结构体，包括新增的缓冲区 + memset(tracker, 0, sizeof(skiing_tracker_t)); + tracker->state = STATIC; +} + +/** + * @brief 当检测到落地时，计算空中的水平飞行距离并累加到总距离 + */ +static void calculate_air_distance(skiing_tracker_t *tracker) { + float horizontal_speed_on_takeoff = sqrtf( + tracker->initial_velocity_on_takeoff[0] * tracker->initial_velocity_on_takeoff[0] + + tracker->initial_velocity_on_takeoff[1] * tracker->initial_velocity_on_takeoff[1] + ); + float distance_in_air = horizontal_speed_on_takeoff * tracker->time_in_air; + tracker->distance += distance_in_air; +} + + + +/** + * @brief 使用四元数直接从设备坐标系的加速度中移除重力分量 + * @details 这种方法比使用欧拉角更精确、更稳定，且避免了万向节死锁。 + * @param acc_device 输入：设备坐标系下的原始加速度 [x, y, z], 单位 m/s^2 + * @param q 输入：表示姿态的四元数 [w, x, y, z] + * @param acc_linear_device 输出：设备坐标系下移除重力后的线性加速度 [x, y, z] + */ +void q_remove_gravity_with_quaternion(const float *acc_device, const float *q, float *acc_linear_device) +{ + // 从四元数计算重力在设备坐标系下的投影 + // G_device = R_transpose * G_world + // G_world = [0, 0, g] + // R_transpose 的第三列即为重力投影方向 + float gx = 2.0f * (q[1] * q[3] - q[0] * q[2]); + float gy = 2.0f * (q[0] * q[1] + q[2] * q[3]); + float gz = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]; + + // 从原始加速度中减去重力分量 + acc_linear_device[0] = acc_device[0] - gx * G_ACCELERATION; + acc_linear_device[1] = acc_device[1] - gy * G_ACCELERATION; + acc_linear_device[2] = acc_device[2] - gz * G_ACCELERATION; +} + +/** + * @brief 使用四元数将设备坐标系的线性加速度转换到世界坐标系 + * @details 同样，此方法比使用欧拉角更优。 + * @param acc_linear_device 输入：设备坐标系下的线性加速度 [x, y, z] + * @param q 输入：表示姿态的四元数 [w, x, y, z] + * @param acc_linear_world 输出：世界坐标系下的线性加速度 [x, y, z] + */ +void q_transform_to_world_with_quaternion(const float *acc_linear_device, const float *q, float *acc_linear_world) +{ + // 这是 R_device_to_world * acc_linear_device 的展开形式 + acc_linear_world[0] = (1.0f - 2.0f*q[2]*q[2] - 2.0f*q[3]*q[3]) * acc_linear_device[0] + + (2.0f*q[1]*q[2] - 2.0f*q[0]*q[3]) * acc_linear_device[1] + + (2.0f*q[1]*q[3] + 2.0f*q[0]*q[2]) * acc_linear_device[2]; + + acc_linear_world[1] = (2.0f*q[1]*q[2] + 2.0f*q[0]*q[3]) * acc_linear_device[0] + + (1.0f - 2.0f*q[1]*q[1] - 2.0f*q[3]*q[3]) * acc_linear_device[1] + + (2.0f*q[2]*q[3] - 2.0f*q[0]*q[1]) * acc_linear_device[2]; + + acc_linear_world[2] = (2.0f*q[1]*q[3] - 2.0f*q[0]*q[2]) * acc_linear_device[0] + + (2.0f*q[2]*q[3] + 2.0f*q[0]*q[1]) * acc_linear_device[1] + + (1.0f - 2.0f*q[1]*q[1] - 2.0f*q[2]*q[2]) * acc_linear_device[2]; + // acc_linear_world[2] -= G_ACCELERATION; +} + + + +/** + * @brief 计算缓冲区内三轴数据的方差之和 + * + * @param buffer 传进来的三轴数据：陀螺仪/加速度 + * @return float 返回方差和 + */ +static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3]) +{ + float mean[3] = {0}; + float variance[3] = {0}; + + // 计算均值 + for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) { + mean[0] += buffer[i][0]; + mean[1] += buffer[i][1]; + mean[2] += buffer[i][2]; + } + mean[0] /= VARIANCE_BUFFER_SIZE; + mean[1] /= VARIANCE_BUFFER_SIZE; + mean[2] /= VARIANCE_BUFFER_SIZE; + + // 计算方差 + for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) { + variance[0] += (buffer[i][0] - mean[0]) * (buffer[i][0] - mean[0]); + variance[1] += (buffer[i][1] - mean[1]) * (buffer[i][1] - mean[1]); + variance[2] += (buffer[i][2] - mean[2]) * (buffer[i][2] - mean[2]); + } + variance[0] /= VARIANCE_BUFFER_SIZE; + variance[1] /= VARIANCE_BUFFER_SIZE; + variance[2] /= VARIANCE_BUFFER_SIZE; + + // 返回三轴方差之和，作为一个综合的稳定度指标 + return variance[0] + variance[1] + variance[2]; +} + +/** + * @brief 摩擦力模拟，进行速度衰减 + * + * @param tracker + */ +void forece_of_friction(skiing_tracker_t *tracker){ + // 增加速度衰减，模拟摩擦力 + tracker->velocity[0] *= SPEED_ATTENUATION; + tracker->velocity[1] *= SPEED_ATTENUATION; + tracker->velocity[2] = 0; // 垂直速度强制归零 +} + +/** + * @brief 状态机更新 + * + * @param tracker 传入同步修改后传出 + * @param acc_device_ms2 三轴加速度，m/s^2 + * @param gyr_dps 三轴陀螺仪，dps + */ +static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps) +{ + // 缓冲区未填满时，不进行状态判断，默认为静止 + if (!tracker->buffer_filled) { + tracker->state = STATIC; + return; + } + + // --- 计算关键指标 --- + float acc_variance = calculate_variance(tracker->acc_buffer); // 计算加速度方差 + float gyr_variance = calculate_variance(tracker->gyr_buffer); // 计算陀螺仪方差 + float gyr_magnitude = sqrtf(gyr_dps[0]*gyr_dps[0] + gyr_dps[1]*gyr_dps[1] + gyr_dps[2]*gyr_dps[2]); //dps + float acc_magnitude = sqrtf(acc_device_ms2[0]*acc_device_ms2[0] + acc_device_ms2[1]*acc_device_ms2[1] + acc_device_ms2[2]*acc_device_ms2[2]); //m/s^s + float acc_magnitude_g = acc_magnitude / G_ACCELERATION; // 转换为g单位，用于跳跃判断 + + #ifdef XTELL_TEST + debug1.acc_variance =acc_variance; + debug1.gyr_variance =gyr_variance; + debug1.gyr_magnitude=gyr_magnitude; + debug1.acc_magnitude=fabsf(acc_magnitude - G_ACCELERATION); + #endif + + + + // --- 状态机逻辑 (核心修改区域) --- + + #if 0 //暂时不考虑空中 + // 1. 空中/落地状态的后续处理 + if (tracker->state == IN_AIR) { + // A. 检测巨大冲击 -> 落地 + if (acc_magnitude_g > LANDING_ACC_MAG_HIGH_THRESHOLD) { + tracker->state = LANDING; + // B. 检测超时 -> 强制落地 (安全机制) + } else if (tracker->time_in_air > MAX_TIME_IN_AIR) { + tracker->state = LANDING; + // C. 检测恢复正常重力 (平缓落地) + } else if (acc_magnitude_g > 0.8f && acc_magnitude_g < 1.5f) { + tracker->grounded_entry_counter++; + if (tracker->grounded_entry_counter >= GROUNDED_CONFIRM_COUNT) { + tracker->state = LANDING; + } + } else { + tracker->grounded_entry_counter = 0; + } + return; // 在空中或刚切换到落地，结束本次状态判断 + } + + // 2. 严格的 "起跳->空中" 状态转换逻辑 + // 只有当处于滑行状态时，才去检测起跳意图 + if (tracker->state == NO_CONSTANT_SPEED || tracker->state == CONSTANT_SPEED || tracker->state == WHEEL) { + if (acc_magnitude_g > TAKEOFF_ACC_MAG_HIGH_THRESHOLD) { + tracker->state = TAKING_OFF; + tracker->airborne_entry_counter = 0; // 准备检测失重 + return; + } + } + // 只有在TAKING_OFF状态下，才去检测是否进入失重 + if (tracker->state == TAKING_OFF) { + if (acc_magnitude_g < AIRBORNE_ACC_MAG_LOW_THRESHOLD) { + tracker->airborne_entry_counter++; + if (tracker->airborne_entry_counter >= AIRBORNE_CONFIRM_COUNT) { + memcpy(tracker->initial_velocity_on_takeoff, tracker->velocity, sizeof(tracker->velocity)); + tracker->time_in_air = 0; + tracker->state = IN_AIR; + tracker->airborne_entry_counter = 0; + tracker->grounded_entry_counter = 0; + return; + } + } else { + // 如果在起跳冲击后一段时间内没有失重，说明只是一个颠簸，恢复滑行 + // 可以加一个小的超时计数器，这里为了简单先直接恢复 + tracker->state = NO_CONSTANT_SPEED; + } + return; // 无论是否切换，都结束本次判断 + } + #endif + + + // --- 静止判断 --- + if (acc_variance < STOP_ACC_VARIANCE_THRESHOLD && gyr_variance < STOP_GYR_VARIANCE_THRESHOLD && gyr_magnitude < STOP_GYR_MAG_THRESHOLD) { + tracker->state = STATIC; + return; + } + + // --- 地面状态切换逻辑 --- + switch (tracker->state) { + case LANDING: + tracker->state = STATIC; + break; + case STATIC: + // 优先判断是否进入 WOBBLE 状态 + // 条件：陀螺仪活动剧烈，但整体加速度变化不大（说明是原地转或晃） + if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) < WOBBLE_ACC_MAG_THRESHOLD) { + tracker->state = WOBBLE; + } + // 只有在陀螺仪和加速度都满足“前进”特征时，才启动 + else if (gyr_variance > START_GYR_VARIANCE_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) { + tracker->state = NO_CONSTANT_SPEED; + } + break; + + case WOBBLE: + // 从 WOBBLE 状态启动的条件应该和从 STATIC 启动一样严格 + if (gyr_variance < START_GYR_VARIANCE_THRESHOLD * 2 && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) { + tracker->state = NO_CONSTANT_SPEED; + } + // 如果陀螺仪活动减弱，则可能恢复静止 + else if (gyr_magnitude < ROTATION_GYR_MAG_THRESHOLD * 0.8f) { // 增加迟滞，避免抖动 + // 不直接跳回STATIC，而是依赖下一轮的全局静止判断 + } + break; + case NO_CONSTANT_SPEED: //非匀速状态 + //暂时不考虑摔倒 + // if (gyr_magnitude > FALLEN_GRY_MAG_THRESHOLD) { + // tracker->state = FALLEN; //摔倒 + // } else + if (gyr_magnitude > WHEEL_GYR_MAG_THRESHOLD && acc_variance > WHEEL_ACC_VARIANCE_THRESHOLD) { + tracker->state = WHEEL; //转弯 + } else if (fabsf(acc_magnitude - G_ACCELERATION) < SKIING_ACC_MAG_THRESHOLD) { + tracker->state = CONSTANT_SPEED; //匀速 + } + break; + + case CONSTANT_SPEED: //匀速状态 + if (fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) { + tracker->state = NO_CONSTANT_SPEED; + } + //TODO：可以添加进入转弯或摔倒的判断 + break; + + case WHEEL: + // 从转弯状态，检查转弯是否结束 + // 如果角速度和加速度方差都降下来了，就回到普通滑行状态 + if (gyr_magnitude < WHEEL_GYR_MAG_THRESHOLD * 0.8f && acc_variance < WHEEL_ACC_VARIANCE_THRESHOLD * 0.8f) { // 乘以一个滞后系数避免抖动 + tracker->state = NO_CONSTANT_SPEED; + } + break; + + case FALLEN: + // TODO：回到 STATIC + break; + } + + +} + + +/** + * @brief 主更新函数 + * + * @param tracker + * @param acc_g 三轴加速度，g + * @param gyr_dps 三轴陀螺仪，dps + * @param angle 欧若拉角 + * @param dt 采样时间间隔，会用来积分求速度 + */ +void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt) +{ + if (!tracker || !acc_g || !gyr_dps || !angle || dt <= 0) { + return; + } + if(my_skiing_tracker.state == STOP_DETECTION) + return; + + // --- 数据预处理和缓冲 --- + float acc_device_ms2[3]; + acc_device_ms2[0] = acc_g[0] * G_ACCELERATION; + acc_device_ms2[1] = acc_g[1] * G_ACCELERATION; + acc_device_ms2[2] = acc_g[2] * G_ACCELERATION; + + // 将最新数据存入缓冲区 + memcpy(tracker->acc_buffer[tracker->buffer_index], acc_device_ms2, sizeof(acc_device_ms2)); + memcpy(tracker->gyr_buffer[tracker->buffer_index], gyr_dps, 3 * sizeof(float)); + + tracker->buffer_index++; + if (tracker->buffer_index >= VARIANCE_BUFFER_SIZE) { + tracker->buffer_index = 0; + tracker->buffer_filled = 1; // 标记缓冲区已满 + } + + // --- 更新状态机 --- + update_state_machine(tracker, acc_device_ms2, gyr_dps); + + // --- 根据状态执行不同的计算逻辑 --- + switch (tracker->state) { + case TAKING_OFF: + tracker->speed = 0.0f; + break; + case IN_AIR: + // 在空中时，只累加滞空时间 + tracker->time_in_air += dt; + break; + case LANDING: + // 刚落地，计算空中距离 + calculate_air_distance(tracker); + // 清理速度和滤波器状态，为恢复地面追踪做准备 + memset(tracker->velocity, 0, sizeof(tracker->velocity)); + tracker->speed = 0; + memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev)); + memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered)); + memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态 + break; + case WHEEL: + case NO_CONSTANT_SPEED: + float linear_acc_device[3]; + float linear_acc_world[3]; + // 在设备坐标系下，移除重力，得到线性加速度 + q_remove_gravity_with_quaternion(acc_device_ms2, quaternion_data, linear_acc_device); + + // 将设备坐标系下的线性加速度，旋转到世界坐标系 + q_transform_to_world_with_quaternion(linear_acc_device, quaternion_data, linear_acc_world); + // 将最终用于积分的加速度存入 tracker 结构体 + memcpy(tracker->acc_no_g, linear_acc_world, sizeof(linear_acc_world)); + + float acc_world_temp[3]; // 临时变量存储当前周期的加速度 + for (int i = 0; i < 2; i++) { // 只处理水平方向的 x 和 y 轴 + + // --- 核心修改：颠倒滤波器顺序为 HPF -> LPF --- + + // 1. 高通滤波 (HPF) 先行: 消除因姿态误差导致的重力泄漏（直流偏置） + // HPF的瞬态响应会产生尖峰，这是正常的。 + tracker->acc_world_filtered[i] = HPF_ALPHA * (tracker->acc_world_filtered[i] + tracker->acc_no_g[i] - tracker->acc_world_unfiltered_prev[i]); + tracker->acc_world_unfiltered_prev[i] = tracker->acc_no_g[i]; + + // 2. 低通滤波 (LPF) 殿后: 平滑掉HPF产生的尖峰和传感器自身的高频振动噪声。 + // 这里使用 tracker->acc_world_filtered[i] 作为LPF的输入。 + tracker->acc_world_lpf[i] = (1.0f - LPF_ALPHA) * tracker->acc_world_filtered[i] + LPF_ALPHA * tracker->acc_world_lpf[i]; + + // 将最终处理完的加速度值存入临时变量 + acc_world_temp[i] = tracker->acc_world_lpf[i]; + } + + // 计算处理后加速度的水平模长 + float acc_horizontal_mag = sqrtf(acc_world_temp[0] * acc_world_temp[0] + + acc_world_temp[1] * acc_world_temp[1]); + #if XTELL_TEST + debug2.acc_magnitude = acc_horizontal_mag; + #endif + // 应用死区，并积分 + if (acc_horizontal_mag > ACC_DEAD_ZONE_THRESHOLD) { + tracker->velocity[0] += acc_world_temp[0] * dt; + tracker->velocity[1] += acc_world_temp[1] * dt; + } + + // 更新速度和距离 + tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] + + tracker->velocity[1] * tracker->velocity[1]); + tracker->distance += tracker->speed * dt; + break; + case CONSTANT_SPEED: + //保持上次的速度不变。只更新距离 + tracker->distance += tracker->speed * dt; + break; + case STATIC: + case WOBBLE: + // 速度清零，抑制漂移 + memset(tracker->velocity, 0, sizeof(tracker->velocity)); + tracker->speed = 0.0f; + memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev)); + memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered)); + memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态 + #if XTELL_TEST + debug2.acc_magnitude = 0; + #endif + break; + case FALLEN: + // TODO + break; + default: + break; + } + + + + + #if 1 + float linear_acc_device[3]; + float linear_acc_world[3]; + float tmp_world_acc[3]; + // 在设备坐标系下，移除重力，得到线性加速度 + q_remove_gravity_with_quaternion(acc_device_ms2, quaternion_data, linear_acc_device); + + // 将设备坐标系下的线性加速度，旋转到世界坐标系 + q_transform_to_world_with_quaternion(linear_acc_device, quaternion_data, tmp_world_acc); + + + float all_world_mag = sqrtf(tmp_world_acc[0] * tmp_world_acc[0] + + tmp_world_acc[1] * tmp_world_acc[1] + + tmp_world_acc[2] * tmp_world_acc[2]); + + static int count = 0; + if(count > 100){ + xlog("===original(g): x %.2f, y %.2f, z %.2f===\n",acc_g[0],acc_g[1],acc_g[2]); + xlog("===world(m/s^2) no g: x %.2f, y %.2f, z %.2f, all %.2f===\n",tmp_world_acc[0],tmp_world_acc[1],tmp_world_acc[2],all_world_mag); //去掉重力加速度 + xlog("===gyr(dps) : x %.2f, y %.2f, z %.2f===\n",gyr_dps[0],gyr_dps[1],gyr_dps[2]); //angle + xlog("===angle : x %.2f, y %.2f, z %.2f,===\n",angle[0],angle[1],angle[2]); + count = 0; + + } + count++; + #endif + + +} + + + +/** + * @brief 滑雪数据计算 + * + * @param acc_data_buf 传入的三轴加速度数据 + * @param gyr_data_buf 传入的三轴陀螺仪数据 + * @param angle_data 传入的欧若拉角数据 + * @return BLE_send_data_t 要发送给蓝牙的数据 + */ +BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data, float* quaternion) { + + static int initialized = 0; + static float acc_data_g[3]; + static float gyr_data_dps[3]; + + if(quaternion != NULL){ + memcpy(quaternion_data, quaternion, 4 * sizeof(float)); + } + + // const float delta_time = DELTA_TIME+0.01f; + // const float delta_time = DELTA_TIME + 0.005f; + const float delta_time = DELTA_TIME; + BLE_send_data_t BLE_send_data; + + if (!initialized) { + skiing_tracker_init(&my_skiing_tracker); + initialized = 1; + printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n"); + } + + + #if ACC_RANGE==2 + // 加速度 LSB to g + acc_data_g[0] = (float)acc_data_buf[0] / 16384.0f; + acc_data_g[1] = (float)acc_data_buf[1] / 16384.0f; + acc_data_g[2] = (float)acc_data_buf[2] / 16384.0f; + #endif + + #if ACC_RANGE==4 + // 加速度 LSB to g + acc_data_g[0] = (float)acc_data_buf[0] / 8192.0f; + acc_data_g[1] = (float)acc_data_buf[1] / 8192.0f; + acc_data_g[2] = (float)acc_data_buf[2] / 8192.0f; + #endif + + #if ACC_RANGE==8 + //±8g 4096 + acc_data_g[0] = (float)acc_data_buf[0] / 4096.0f; //ax + acc_data_g[1] = (float)acc_data_buf[1] / 4096.0f; //ay + acc_data_g[2] = (float)acc_data_buf[2] / 4096.0f; //az + #endif + + #if ACC_RANGE==16 + //±16g 2048 + acc_data_g[0] = (float)acc_data_buf[0] / 2048.0f; //ax + acc_data_g[1] = (float)acc_data_buf[1] / 2048.0f; //ay + acc_data_g[2] = (float)acc_data_buf[2] / 2048.0f; //az + #endif + + // 陀螺仪 LSB to dps (度/秒) + // ±2000dps量程下，转换系数约为 0.061 + gyr_data_dps[0] = (float)gyr_data_buf[0] * 0.061f; + gyr_data_dps[1] = (float)gyr_data_buf[1] * 0.061f; + gyr_data_dps[2] = (float)gyr_data_buf[2] * 0.061f; + + skiing_tracker_update(&my_skiing_tracker, acc_data_g, gyr_data_dps, angle_data, delta_time); + + BLE_send_data.skiing_state = my_skiing_tracker.state; + for (int i = 0; i < 3; i++) { + #ifdef XTELL_TEST + BLE_send_data.acc_data[i] = (short)(acc_data_g[i] * 9.8f) * 100; //cm/^s2 + BLE_send_data.gyr_data[i] = (short)gyr_data_dps[i]; //dps + BLE_send_data.angle_data[i] = angle_data[i]; + #else + BLE_send_data.acc_data[i] = (short)acc_data_buf[i]; //原始adc数据 + BLE_send_data.gyr_data[i] = (short)gyr_data_buf[i]; //原始adc数据 + BLE_send_data.angle_data[i] = angle_data[i]; + #endif + } + BLE_send_data.speed_cms = (int)(my_skiing_tracker.speed * 100); + BLE_send_data.distance_cm = (int)(my_skiing_tracker.distance * 100); + // printf("Calculate the time interval =============== end\n"); + + return BLE_send_data; +} + diff --git a/apps/earphone/xtell_Sensor/A_hide/calculate/skiing_tracker.h b/apps/earphone/xtell_Sensor/A_hide/calculate/skiing_tracker.h new file mode 100644 index 0000000..750180d --- /dev/null +++ b/apps/earphone/xtell_Sensor/A_hide/calculate/skiing_tracker.h @@ -0,0 +1,88 @@ +#ifndef SKIING_TRACKER_H +#define SKIING_TRACKER_H + +#include "../xtell.h" +// 定义滑雪者可能的状态 +typedef enum { + STATIC, // 静止或动态稳定：0 + NO_CONSTANT_SPEED, // 正在滑雪，非匀速：1 + CONSTANT_SPEED, // 正在滑雪，匀速：2 + WOBBLE, // 正在原地旋转：3 + WHEEL, // 转弯：4 + FALLEN, // 已摔倒：5 + TAKING_OFF, // 起跳冲击阶段：6 + IN_AIR, // 空中失重阶段：7 + LANDING, // 落地冲击阶段：8 + STOP_DETECTION, // 停止检测：9 + UNKNOWN // 未知状态：10 +} skiing_state_t; + +#define VARIANCE_BUFFER_SIZE 5 // 用于计算方差的数据窗口大小 (5个样本 @ 100Hz = 50ms)，减小延迟，提高实时性 +#define DELTA_TIME 0.01f + + +// 追踪器数据结构体 +typedef struct { + // 公开数据 + float velocity[3]; // 当前速度 (x, y, z)，单位: m/s + float distance; // 总滑行距离，单位: m + float speed; // 当前速率 (标量)，单位: m/s + skiing_state_t state; // 当前滑雪状态 + + // 内部计算使用的私有成员 + float acc_no_g[3]; // 去掉重力分量后的加速度 + + // 用于空中距离计算 + float time_in_air; // 滞空时间计时器 + float initial_velocity_on_takeoff[3]; // 起跳瞬间的速度向量 + int airborne_entry_counter; // 进入空中状态的确认计数器 + int grounded_entry_counter; // 落地确认计数器 + + // --- 内部计算使用的私有成员 --- + // 用于动态零速更新和旋转检测的缓冲区 + float acc_buffer[VARIANCE_BUFFER_SIZE][3]; // 加速度数据窗口 + float gyr_buffer[VARIANCE_BUFFER_SIZE][3]; // 角速度数据窗口 + int buffer_index; // 缓冲区当前索引 + int buffer_filled; // 缓冲区是否已填满的标志 + + // 用于高通滤波器（巴特沃斯一阶滤波器）的私有成员，以消除加速度的直流偏置 + float acc_world_filtered[3]; //过滤过的 + float acc_world_unfiltered_prev[3]; //上一次没过滤的 + + float acc_world_lpf[3]; // 经过低通滤波后的世界坐标系加速度 +} skiing_tracker_t; + +//ble发送的数据 +typedef struct{ //__attribute__((packed)){ //该结构体取消内存对齐 + char sensor_state; + char skiing_state; + int speed_cms; //求出的速度，cm/s + int distance_cm; //求出的距离，cm + short acc_data[3]; //三轴加速度, g + short gyr_data[3]; //三轴陀螺仪, dps + float angle_data[3]; //欧若拉角 +}BLE_send_data_t; + +typedef struct{ + int acc_KS[3]; //卡尔曼后，LSB转换后的 三轴加速度数据（cm/s^2） + int gyr_KS_dps[3]; //卡尔曼后，LSB to dps 三轴陀螺仪数据 + int angle_KS[3]; //卡尔曼后，计算得到的欧若拉角数据 +}BLE_KS_send_data_t; + +#ifdef XTELL_TEST +typedef struct{ + float acc_variance; //三轴加速度方差之和 + float gyr_variance; //三轴陀螺仪方差之和 + float acc_magnitude; //三轴加速度模长 + float gyr_magnitude; //三轴陀螺仪模长 +}debug_t; +#endif +/** + * @brief 初始化滑雪追踪器 + * + * @param tracker 指向 skiing_tracker_t 结构体的指针 + */ +void skiing_tracker_init(skiing_tracker_t *tracker); + +BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data, float* quaternion); +#endif // SKIING_TRACKER_H \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c b/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c index 922558c..2e46e37 100644 --- a/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c +++ b/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c @@ -1,14 +1,11 @@ /* - + 使用四元数求角度和去掉重力分量 */ #include "skiing_tracker.h" #include "../sensor/SC7U22.h" #include #include -#define G_ACCELERATION 9.81f -#define DEG_TO_RAD (3.14159265f / 180.0f) - #define ENABLE_XLOG 1 #ifdef xlog #undef xlog @@ -19,83 +16,13 @@ #define xlog(format, ...) ((void)0) #endif -// --- 静止检测 --- -//两个判断是否静止的必要条件：动态零速更新(ZUPT)阈值 -// 加速方差阈值，提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新 -#define STOP_ACC_VARIANCE_THRESHOLD 0.2f -// 陀螺仪方差阈值 -#define STOP_GYR_VARIANCE_THRESHOLD 5.0f -// 静止时候的陀螺仪模长 -#define STOP_GYR_MAG_THRESHOLD 15 -// --- --- --- -// --- 启动滑雪阈值 --- -// 加速度模长与重力的差值大于此值，认为开始运动；降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动 -#define START_ACC_MAG_THRESHOLD 1.0f //0.5、1 -// 陀螺仪方差阈值，以允许启动瞬间的正常抖动，但仍能过滤掉混乱的、非滑雪的晃动。 -#define START_GYR_VARIANCE_THRESHOLD 15.0f -// --- --- --- +#define G_ACCELERATION 9.81f +#define DEG_TO_RAD (3.14159265f / 180.0f) -// --- 滑雪过程 --- -//加速度 模长(不含重力），低于此值视为 在做匀速运动 -#define SKIING_ACC_MAG_THRESHOLD 0.5f -//陀螺仪 模长，高于此值视为 摔倒了 -#define FALLEN_GRY_MAG_THRESHOLD 2000.0f //未确定 -// --- --- --- - -// --- 原地旋转抖动 --- -// 加速度 方差 阈值。此值比 静止检测 阈值更宽松， -#define WOBBLE_ACC_VARIANCE_THRESHOLD 0.5f -// 加速度 模长 阈值 -#define WOBBLE_ACC_MAG_THRESHOLD 1.0f -// 角速度 总模长 大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动 -#define ROTATION_GYR_MAG_THRESHOLD 30.0f -// --- --- --- - -// --- 滑雪转弯动 --- -// 加速度 方差 阈值，大于此值，滑雪过程可能发生了急转弯 -#define WHEEL_ACC_VARIANCE_THRESHOLD 7.0f -// 角速度 总模长 大于此值(度/秒)，认为滑雪过程中进行急转弯 -#define WHEEL_GYR_MAG_THRESHOLD 500.0f // -// --- --- --- - -// --- 跳跃 --- -// 加速度模长低于此值(g)，认为进入失重状态(IN_AIR) -#define AIRBORNE_ACC_MAG_LOW_THRESHOLD 0.4f -// 加速度模长高于此值(g)，认为发生落地冲击(LANDING) -#define LANDING_ACC_MAG_HIGH_THRESHOLD 3.5f -// 起跳加速度阈值(g)，用于进入TAKING_OFF状态 -#define TAKEOFF_ACC_MAG_HIGH_THRESHOLD 1.8f -// 进入空中状态确认计数：需要连续3个采样点加速度低于阈值才判断为起跳 -#define AIRBORNE_CONFIRM_COUNT 3 -// 落地状态确认计数：加速度恢复到1g附近并持续2个采样点(20ms)则认为已落地 -#define GROUNDED_CONFIRM_COUNT 2 -// 最大滞空时间(秒)，超过此时间强制认为已落地，防止状态锁死 -#define MAX_TIME_IN_AIR 12.5f -// --- --- --- - -// --- 用于消除积分漂移的滤波器和阈值 --- -// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。 -// alpha = RC / (RC + dt)，参考RC电路而来，fc ≈ (1 - alpha) / (2 * π * dt) -#define HPF_ALPHA 0.999f -//0.995f： 0.08 Hz 的信号 -//0.999f： 0.0159 Hz -// --- --- --- - -// --- 低通滤波器 --- -// 低通滤波器系数 (alpha)。alpha 越小，滤波效果越强（更平滑），但延迟越大。 -// alpha 推荐范围 0.7 ~ 0.95。可以从 0.85 开始尝试。 -#define LPF_ALPHA 0.7f - -// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。 -// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。 -//参考：0.2f ~ 0.4f -#define ACC_DEAD_ZONE_THRESHOLD 0.05f - -// --- 模拟摩擦力，进行速度衰减 --- -#define SPEED_ATTENUATION 1.0f //暂不模拟 BLE_KS_send_data_t KS_data; static float quaternion_data[4]; + #ifdef XTELL_TEST debug_t debug1; @@ -126,7 +53,7 @@ void stop_detection(void){ } /** - * @brief 初始化滑雪追踪器 + * @brief 初始化 * * @param tracker */ @@ -140,18 +67,6 @@ void skiing_tracker_init(skiing_tracker_t *tracker) tracker->state = STATIC; } -/** - * @brief 当检测到落地时，计算空中的水平飞行距离并累加到总距离 - */ -static void calculate_air_distance(skiing_tracker_t *tracker) { - float horizontal_speed_on_takeoff = sqrtf( - tracker->initial_velocity_on_takeoff[0] * tracker->initial_velocity_on_takeoff[0] + - tracker->initial_velocity_on_takeoff[1] * tracker->initial_velocity_on_takeoff[1] - ); - float distance_in_air = horizontal_speed_on_takeoff * tracker->time_in_air; - tracker->distance += distance_in_air; -} - /** @@ -178,7 +93,7 @@ void q_remove_gravity_with_quaternion(const float *acc_device, const float *q, f } /** - * @brief 使用四元数将设备坐标系的线性加速度转换到世界坐标系 + * @brief 使用四元数将设备坐标系的线性加速度转换到世界坐标系,并且移除重力分量 * @details 同样，此方法比使用欧拉角更优。 * @param acc_linear_device 输入：设备坐标系下的线性加速度 [x, y, z] * @param q 输入：表示姿态的四元数 [w, x, y, z] @@ -202,208 +117,6 @@ void q_transform_to_world_with_quaternion(const float *acc_linear_device, const } - -/** - * @brief 计算缓冲区内三轴数据的方差之和 - * - * @param buffer 传进来的三轴数据：陀螺仪/加速度 - * @return float 返回方差和 - */ -static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3]) -{ - float mean[3] = {0}; - float variance[3] = {0}; - - // 计算均值 - for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) { - mean[0] += buffer[i][0]; - mean[1] += buffer[i][1]; - mean[2] += buffer[i][2]; - } - mean[0] /= VARIANCE_BUFFER_SIZE; - mean[1] /= VARIANCE_BUFFER_SIZE; - mean[2] /= VARIANCE_BUFFER_SIZE; - - // 计算方差 - for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) { - variance[0] += (buffer[i][0] - mean[0]) * (buffer[i][0] - mean[0]); - variance[1] += (buffer[i][1] - mean[1]) * (buffer[i][1] - mean[1]); - variance[2] += (buffer[i][2] - mean[2]) * (buffer[i][2] - mean[2]); - } - variance[0] /= VARIANCE_BUFFER_SIZE; - variance[1] /= VARIANCE_BUFFER_SIZE; - variance[2] /= VARIANCE_BUFFER_SIZE; - - // 返回三轴方差之和，作为一个综合的稳定度指标 - return variance[0] + variance[1] + variance[2]; -} - -/** - * @brief 摩擦力模拟，进行速度衰减 - * - * @param tracker - */ -void forece_of_friction(skiing_tracker_t *tracker){ - // 增加速度衰减，模拟摩擦力 - tracker->velocity[0] *= SPEED_ATTENUATION; - tracker->velocity[1] *= SPEED_ATTENUATION; - tracker->velocity[2] = 0; // 垂直速度强制归零 -} - -/** - * @brief 状态机更新 - * - * @param tracker 传入同步修改后传出 - * @param acc_device_ms2 三轴加速度，m/s^2 - * @param gyr_dps 三轴陀螺仪，dps - */ -static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps) -{ - // 缓冲区未填满时，不进行状态判断，默认为静止 - if (!tracker->buffer_filled) { - tracker->state = STATIC; - return; - } - - // --- 计算关键指标 --- - float acc_variance = calculate_variance(tracker->acc_buffer); // 计算加速度方差 - float gyr_variance = calculate_variance(tracker->gyr_buffer); // 计算陀螺仪方差 - float gyr_magnitude = sqrtf(gyr_dps[0]*gyr_dps[0] + gyr_dps[1]*gyr_dps[1] + gyr_dps[2]*gyr_dps[2]); //dps - float acc_magnitude = sqrtf(acc_device_ms2[0]*acc_device_ms2[0] + acc_device_ms2[1]*acc_device_ms2[1] + acc_device_ms2[2]*acc_device_ms2[2]); //m/s^s - float acc_magnitude_g = acc_magnitude / G_ACCELERATION; // 转换为g单位，用于跳跃判断 - - #ifdef XTELL_TEST - debug1.acc_variance =acc_variance; - debug1.gyr_variance =gyr_variance; - debug1.gyr_magnitude=gyr_magnitude; - debug1.acc_magnitude=fabsf(acc_magnitude - G_ACCELERATION); - #endif - - - - // --- 状态机逻辑 (核心修改区域) --- - - #if 0 //暂时不考虑空中 - // 1. 空中/落地状态的后续处理 - if (tracker->state == IN_AIR) { - // A. 检测巨大冲击 -> 落地 - if (acc_magnitude_g > LANDING_ACC_MAG_HIGH_THRESHOLD) { - tracker->state = LANDING; - // B. 检测超时 -> 强制落地 (安全机制) - } else if (tracker->time_in_air > MAX_TIME_IN_AIR) { - tracker->state = LANDING; - // C. 检测恢复正常重力 (平缓落地) - } else if (acc_magnitude_g > 0.8f && acc_magnitude_g < 1.5f) { - tracker->grounded_entry_counter++; - if (tracker->grounded_entry_counter >= GROUNDED_CONFIRM_COUNT) { - tracker->state = LANDING; - } - } else { - tracker->grounded_entry_counter = 0; - } - return; // 在空中或刚切换到落地，结束本次状态判断 - } - - // 2. 严格的 "起跳->空中" 状态转换逻辑 - // 只有当处于滑行状态时，才去检测起跳意图 - if (tracker->state == NO_CONSTANT_SPEED || tracker->state == CONSTANT_SPEED || tracker->state == WHEEL) { - if (acc_magnitude_g > TAKEOFF_ACC_MAG_HIGH_THRESHOLD) { - tracker->state = TAKING_OFF; - tracker->airborne_entry_counter = 0; // 准备检测失重 - return; - } - } - // 只有在TAKING_OFF状态下，才去检测是否进入失重 - if (tracker->state == TAKING_OFF) { - if (acc_magnitude_g < AIRBORNE_ACC_MAG_LOW_THRESHOLD) { - tracker->airborne_entry_counter++; - if (tracker->airborne_entry_counter >= AIRBORNE_CONFIRM_COUNT) { - memcpy(tracker->initial_velocity_on_takeoff, tracker->velocity, sizeof(tracker->velocity)); - tracker->time_in_air = 0; - tracker->state = IN_AIR; - tracker->airborne_entry_counter = 0; - tracker->grounded_entry_counter = 0; - return; - } - } else { - // 如果在起跳冲击后一段时间内没有失重，说明只是一个颠簸，恢复滑行 - // 可以加一个小的超时计数器，这里为了简单先直接恢复 - tracker->state = NO_CONSTANT_SPEED; - } - return; // 无论是否切换，都结束本次判断 - } - #endif - - - // --- 静止判断 --- - if (acc_variance < STOP_ACC_VARIANCE_THRESHOLD && gyr_variance < STOP_GYR_VARIANCE_THRESHOLD && gyr_magnitude < STOP_GYR_MAG_THRESHOLD) { - tracker->state = STATIC; - return; - } - - // --- 地面状态切换逻辑 --- - switch (tracker->state) { - case LANDING: - tracker->state = STATIC; - break; - case STATIC: - // 优先判断是否进入 WOBBLE 状态 - // 条件：陀螺仪活动剧烈，但整体加速度变化不大（说明是原地转或晃） - if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) < WOBBLE_ACC_MAG_THRESHOLD) { - tracker->state = WOBBLE; - } - // 只有在陀螺仪和加速度都满足“前进”特征时，才启动 - else if (gyr_variance > START_GYR_VARIANCE_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) { - tracker->state = NO_CONSTANT_SPEED; - } - break; - - case WOBBLE: - // 从 WOBBLE 状态启动的条件应该和从 STATIC 启动一样严格 - if (gyr_variance < START_GYR_VARIANCE_THRESHOLD * 2 && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) { - tracker->state = NO_CONSTANT_SPEED; - } - // 如果陀螺仪活动减弱，则可能恢复静止 - else if (gyr_magnitude < ROTATION_GYR_MAG_THRESHOLD * 0.8f) { // 增加迟滞，避免抖动 - // 不直接跳回STATIC，而是依赖下一轮的全局静止判断 - } - break; - case NO_CONSTANT_SPEED: //非匀速状态 - //暂时不考虑摔倒 - // if (gyr_magnitude > FALLEN_GRY_MAG_THRESHOLD) { - // tracker->state = FALLEN; //摔倒 - // } else - if (gyr_magnitude > WHEEL_GYR_MAG_THRESHOLD && acc_variance > WHEEL_ACC_VARIANCE_THRESHOLD) { - tracker->state = WHEEL; //转弯 - } else if (fabsf(acc_magnitude - G_ACCELERATION) < SKIING_ACC_MAG_THRESHOLD) { - tracker->state = CONSTANT_SPEED; //匀速 - } - break; - - case CONSTANT_SPEED: //匀速状态 - if (fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) { - tracker->state = NO_CONSTANT_SPEED; - } - //TODO：可以添加进入转弯或摔倒的判断 - break; - - case WHEEL: - // 从转弯状态，检查转弯是否结束 - // 如果角速度和加速度方差都降下来了，就回到普通滑行状态 - if (gyr_magnitude < WHEEL_GYR_MAG_THRESHOLD * 0.8f && acc_variance < WHEEL_ACC_VARIANCE_THRESHOLD * 0.8f) { // 乘以一个滞后系数避免抖动 - tracker->state = NO_CONSTANT_SPEED; - } - break; - - case FALLEN: - // TODO：回到 STATIC - break; - } - - -} - - /** * @brief 主更新函数 * @@ -427,139 +140,46 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d acc_device_ms2[1] = acc_g[1] * G_ACCELERATION; acc_device_ms2[2] = acc_g[2] * G_ACCELERATION; - // 将最新数据存入缓冲区 - memcpy(tracker->acc_buffer[tracker->buffer_index], acc_device_ms2, sizeof(acc_device_ms2)); - memcpy(tracker->gyr_buffer[tracker->buffer_index], gyr_dps, 3 * sizeof(float)); - - tracker->buffer_index++; - if (tracker->buffer_index >= VARIANCE_BUFFER_SIZE) { - tracker->buffer_index = 0; - tracker->buffer_filled = 1; // 标记缓冲区已满 - } - - // --- 更新状态机 --- - update_state_machine(tracker, acc_device_ms2, gyr_dps); - - // --- 根据状态执行不同的计算逻辑 --- - switch (tracker->state) { - case TAKING_OFF: - tracker->speed = 0.0f; - break; - case IN_AIR: - // 在空中时，只累加滞空时间 - tracker->time_in_air += dt; - break; - case LANDING: - // 刚落地，计算空中距离 - calculate_air_distance(tracker); - // 清理速度和滤波器状态，为恢复地面追踪做准备 - memset(tracker->velocity, 0, sizeof(tracker->velocity)); - tracker->speed = 0; - memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev)); - memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered)); - memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态 - break; - case WHEEL: - case NO_CONSTANT_SPEED: - float linear_acc_device[3]; - float linear_acc_world[3]; - // 在设备坐标系下，移除重力，得到线性加速度 - q_remove_gravity_with_quaternion(acc_device_ms2, quaternion_data, linear_acc_device); - - // 将设备坐标系下的线性加速度，旋转到世界坐标系 - q_transform_to_world_with_quaternion(linear_acc_device, quaternion_data, linear_acc_world); - // 将最终用于积分的加速度存入 tracker 结构体 - memcpy(tracker->acc_no_g, linear_acc_world, sizeof(linear_acc_world)); - - float acc_world_temp[3]; // 临时变量存储当前周期的加速度 - for (int i = 0; i < 2; i++) { // 只处理水平方向的 x 和 y 轴 - - // --- 核心修改：颠倒滤波器顺序为 HPF -> LPF --- - - // 1. 高通滤波 (HPF) 先行: 消除因姿态误差导致的重力泄漏（直流偏置） - // HPF的瞬态响应会产生尖峰，这是正常的。 - tracker->acc_world_filtered[i] = HPF_ALPHA * (tracker->acc_world_filtered[i] + tracker->acc_no_g[i] - tracker->acc_world_unfiltered_prev[i]); - tracker->acc_world_unfiltered_prev[i] = tracker->acc_no_g[i]; - - // 2. 低通滤波 (LPF) 殿后: 平滑掉HPF产生的尖峰和传感器自身的高频振动噪声。 - // 这里使用 tracker->acc_world_filtered[i] 作为LPF的输入。 - tracker->acc_world_lpf[i] = (1.0f - LPF_ALPHA) * tracker->acc_world_filtered[i] + LPF_ALPHA * tracker->acc_world_lpf[i]; - - // 将最终处理完的加速度值存入临时变量 - acc_world_temp[i] = tracker->acc_world_lpf[i]; - } - - // 计算处理后加速度的水平模长 - float acc_horizontal_mag = sqrtf(acc_world_temp[0] * acc_world_temp[0] + - acc_world_temp[1] * acc_world_temp[1]); - #if XTELL_TEST - debug2.acc_magnitude = acc_horizontal_mag; - #endif - // 应用死区，并积分 - if (acc_horizontal_mag > ACC_DEAD_ZONE_THRESHOLD) { - tracker->velocity[0] += acc_world_temp[0] * dt; - tracker->velocity[1] += acc_world_temp[1] * dt; - } - - // 更新速度和距离 - tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] + - tracker->velocity[1] * tracker->velocity[1]); - tracker->distance += tracker->speed * dt; - break; - case CONSTANT_SPEED: - //保持上次的速度不变。只更新距离 - tracker->distance += tracker->speed * dt; - break; - case STATIC: - case WOBBLE: - // 速度清零，抑制漂移 - memset(tracker->velocity, 0, sizeof(tracker->velocity)); - tracker->speed = 0.0f; - memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev)); - memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered)); - memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态 - #if XTELL_TEST - debug2.acc_magnitude = 0; - #endif - break; - case FALLEN: - // TODO - break; - default: - break; - } - - - - - #if 1 - float linear_acc_device[3]; - float linear_acc_world[3]; + #if 1 //测试禁止状态下陀螺仪的三轴加速度，去掉重力分量后是否正常 + float tmp_device_acc[3]; float tmp_world_acc[3]; - // 在设备坐标系下，移除重力，得到线性加速度 - q_remove_gravity_with_quaternion(acc_device_ms2, quaternion_data, linear_acc_device); + // remove_gravity_in_device_frame(acc_device_ms2,angle,tmp_device_acc); + // transform_acc_to_world_frame(acc_device_ms2,angle,tmp_world_acc); - // 将设备坐标系下的线性加速度，旋转到世界坐标系 - q_transform_to_world_with_quaternion(linear_acc_device, quaternion_data, tmp_world_acc); + q_remove_gravity_with_quaternion(acc_device_ms2,quaternion_data,tmp_device_acc); + q_transform_to_world_with_quaternion(tmp_device_acc,quaternion_data,tmp_world_acc); + + // 计算处理后加速度的水平模长 + float all_device_mag = sqrtf(tmp_device_acc[0] * tmp_device_acc[0] + + tmp_device_acc[1] * tmp_device_acc[1] + + tmp_device_acc[2] * tmp_device_acc[2]); - float all_world_mag = sqrtf(tmp_world_acc[0] * tmp_world_acc[0] + tmp_world_acc[1] * tmp_world_acc[1] + tmp_world_acc[2] * tmp_world_acc[2]); - + + float gx_proj = 2.0f * (quaternion_data[1] * quaternion_data[3] - quaternion_data[0] * quaternion_data[2]); + float gy_proj = 2.0f * (quaternion_data[0] * quaternion_data[1] + quaternion_data[2] * quaternion_data[3]); + float gz_proj = quaternion_data[0] * quaternion_data[0] - quaternion_data[1] * quaternion_data[1] - quaternion_data[2] * quaternion_data[2] + quaternion_data[3] * quaternion_data[3]; + + static int count = 0; if(count > 100){ xlog("===original(g): x %.2f, y %.2f, z %.2f===\n",acc_g[0],acc_g[1],acc_g[2]); - xlog("===world(m/s^2) no g: x %.2f, y %.2f, z %.2f, all %.2f===\n",tmp_world_acc[0],tmp_world_acc[1],tmp_world_acc[2],all_world_mag); //去掉重力加速度 + xlog("===device(m/s^2) no g: x %.2f, y %.2f, z %.2f, all %.2f===\n",tmp_device_acc[0],tmp_device_acc[1],tmp_device_acc[2],all_device_mag); + xlog("===world(m/s^2) no g: x %.2f, y %.2f, z %.2f, all %.2f===\n",tmp_world_acc[0],tmp_world_acc[1],tmp_world_acc[2],all_world_mag); xlog("===gyr(dps) : x %.2f, y %.2f, z %.2f, all %.2f===\n",gyr_dps[0],gyr_dps[1],gyr_dps[2]); //angle xlog("===angle : x %.2f, y %.2f, z %.2f,===\n",angle[0],angle[1],angle[2]); + xlog("GRAVITY VECTOR in device frame: gx=%.2f, gy=%.2f, gz=%.2f\n", gx_proj, gy_proj, gz_proj); + extern mmc5603nj_cal_data_t cal_data; + xlog("cal_data:X: %.4f, Y: %.4f, Z: %.4f\n", cal_data.offset_x,cal_data.offset_y,cal_data.offset_z); count = 0; } count++; + #endif - } @@ -577,7 +197,6 @@ BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* static int initialized = 0; static float acc_data_g[3]; static float gyr_data_dps[3]; - if(quaternion != NULL){ memcpy(quaternion_data, quaternion, 4 * sizeof(float)); } @@ -630,21 +249,21 @@ BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* skiing_tracker_update(&my_skiing_tracker, acc_data_g, gyr_data_dps, angle_data, delta_time); - BLE_send_data.skiing_state = my_skiing_tracker.state; - for (int i = 0; i < 3; i++) { - #ifdef XTELL_TEST - BLE_send_data.acc_data[i] = (short)(acc_data_g[i] * 9.8f) * 100; //cm/^s2 - BLE_send_data.gyr_data[i] = (short)gyr_data_dps[i]; //dps - BLE_send_data.angle_data[i] = angle_data[i]; - #else - BLE_send_data.acc_data[i] = (short)acc_data_buf[i]; //原始adc数据 - BLE_send_data.gyr_data[i] = (short)gyr_data_buf[i]; //原始adc数据 - BLE_send_data.angle_data[i] = angle_data[i]; - #endif - } - BLE_send_data.speed_cms = (int)(my_skiing_tracker.speed * 100); - BLE_send_data.distance_cm = (int)(my_skiing_tracker.distance * 100); - // printf("Calculate the time interval =============== end\n"); + // BLE_send_data.skiing_state = my_skiing_tracker.state; + // for (int i = 0; i < 3; i++) { + // #ifdef XTELL_TEST + // BLE_send_data.acc_data[i] = (short)(acc_data_g[i] * 9.8f) * 100; //cm/^s2 + // BLE_send_data.gyr_data[i] = (short)gyr_data_dps[i]; //dps + // BLE_send_data.angle_data[i] = angle_data[i]; + // #else + // BLE_send_data.acc_data[i] = (short)acc_data_buf[i]; //原始adc数据 + // BLE_send_data.gyr_data[i] = (short)gyr_data_buf[i]; //原始adc数据 + // BLE_send_data.angle_data[i] = angle_data[i]; + // #endif + // } + // BLE_send_data.speed_cms = (int)(my_skiing_tracker.speed * 100); + // BLE_send_data.distance_cm = (int)(my_skiing_tracker.distance * 100); + // // printf("Calculate the time interval =============== end\n"); return BLE_send_data; } diff --git a/apps/earphone/xtell_Sensor/calculate/skiing_tracker.h b/apps/earphone/xtell_Sensor/calculate/skiing_tracker.h index 750180d..b1021f2 100644 --- a/apps/earphone/xtell_Sensor/calculate/skiing_tracker.h +++ b/apps/earphone/xtell_Sensor/calculate/skiing_tracker.h @@ -30,7 +30,7 @@ typedef struct { skiing_state_t state; // 当前滑雪状态 // 内部计算使用的私有成员 - float acc_no_g[3]; // 去掉重力分量后的加速度 + float acc_world[3]; // 在世界坐标系下的加速度 // 用于空中距离计算 float time_in_air; // 滞空时间计时器 diff --git a/apps/earphone/xtell_Sensor/send_data.c b/apps/earphone/xtell_Sensor/send_data.c index dfbab79..bc9bba8 100644 --- a/apps/earphone/xtell_Sensor/send_data.c +++ b/apps/earphone/xtell_Sensor/send_data.c @@ -23,6 +23,7 @@ #include "./ano/ano_protocol.h" #include "./sensor/MMC56.h" #include "./sensor/BMP280.h" +#include "./sensor/AK8963.h" /////////////////////////////////////////////////////////////////////////////////////////////////// //宏定义 #define ENABLE_XLOG 1 @@ -60,6 +61,7 @@ static u16 calculate_data_id; static u8 sensor_data_buffer[SENSOR_DATA_BUFFER_SIZE]; static circle_buffer_t sensor_cb; +static int count = 0; //--- test --- // 全局变量 @@ -219,9 +221,8 @@ void sensor_read_data(){ // status = SIX_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0); // status = Original_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0); - status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0, tmp.quaternion_output); + status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle,NULL, 0, tmp.quaternion_output); - int count = 0; if(count > 100){ count = 0; char log_buffer[100]; // 100个字符应该足够了 @@ -240,7 +241,7 @@ void sensor_read_data(){ // status = SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0); // status = SIX_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0); // status = Original_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0); - status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0, tmp.quaternion_output); + status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle,NULL, 0, tmp.quaternion_output); memcpy(tmp.acc_data, &combined_raw_data[0], 3 * sizeof(signed short)); memcpy(tmp.gyr_data, &combined_raw_data[3], 3 * sizeof(signed short)); @@ -295,7 +296,6 @@ void calculate_data(){ // xlog("=======end\n"); } -static int count = 0; extern char xt_Check_Flag; void BLE_send_data(){ // xlog("=======start\n"); @@ -462,66 +462,63 @@ void xt_hw_iic_test(){ void sensor_measure(void){ // xlog("=======sensor_read_data START\n"); - // static signed short combined_raw_data[6]; - // static int initialized = 0; - // static int calibration_done = 0; - // char status = 0; + static signed short combined_raw_data[6]; + static int initialized = 0; + static int calibration_done = 0; + char status = 0; - // if(count_test1 >= 100){ - // count_test1 = 0; - // xlog("count_test1\n"); - // } - // count_test1++; - // static sensor_data_t tmp; - // SL_SC7U22_RawData_Read(tmp.acc_data,tmp.gyr_data); - // // xlog("=======sensor_read_data middle 1\n"); - // memcpy(&combined_raw_data[0], tmp.acc_data, 3 * sizeof(signed short)); - // memcpy(&combined_raw_data[3], tmp.gyr_data, 3 * sizeof(signed short)); - - // if (!calibration_done) { //第1次启动，开启零漂检测 - // // status = SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0); - // // status = SIX_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0); - // // status = Original_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0); - // status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0, tmp.quaternion_output); - - // int count = 0; - // if(count > 100){ - // count = 0; - // char log_buffer[100]; - // // snprintf( log_buffer, sizeof(log_buffer),"status:%d\n",status); - // // send_data_to_ble_client(&log_buffer,strlen(log_buffer)); - // xlog("status:%d\n", status); - // } - // count++; - - // if (status == 1) { - // calibration_done = 1; - // printf("Sensor calibration successful! Skiing mode is active.\n"); - // } - // } else { - // // printf("Calculate the time interval =============== start\n"); - // // status = SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0); - // // status = SIX_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0); - // // status = Original_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0); - // status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0, tmp.quaternion_output); - // memcpy(tmp.acc_data, &combined_raw_data[0], 3 * sizeof(signed short)); - // memcpy(tmp.gyr_data, &combined_raw_data[3], 3 * sizeof(signed short)); - // BLE_send_data_t data_by_calculate = sensor_processing_task(tmp.acc_data, tmp.gyr_data,tmp.angle, tmp.quaternion_output); - // extern void ano_send_attitude_data(float rol, float pit, float yaw, uint8_t fusion_sta) ; - // ano_send_attitude_data(tmp.angle[0],tmp.angle[1],tmp.angle[2], 1); - - - // } - + static sensor_data_t tmp; mmc5603nj_mag_data_t mag_data; - // mmc5603nj_read_mag_data(&mag_data); + SL_SC7U22_RawData_Read(tmp.acc_data,tmp.gyr_data); + // os_time_dly(1); + mmc5603nj_read_mag_data(&mag_data); + // xlog("=======sensor_read_data middle 1\n"); + memcpy(&combined_raw_data[0], tmp.acc_data, 3 * sizeof(signed short)); + memcpy(&combined_raw_data[3], tmp.gyr_data, 3 * sizeof(signed short)); - // if(count_test2 > 100){ - // count_test2++; - printf("Mag X: %.4f, Y: %.4f, Z: %.4f Gauss\n", mag_data.x, mag_data.y, mag_data.z); - // } - // count_test2++; + if (!calibration_done) { //第1次启动，开启零漂检测 + // status = SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0); + // status = SIX_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0); + // status = Original_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0); + status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle,&mag_data, 0, tmp.quaternion_output); + + if(count > 100){ + count = 0; + char log_buffer[100]; + // snprintf( log_buffer, sizeof(log_buffer),"status:%d\n",status); + // send_data_to_ble_client(&log_buffer,strlen(log_buffer)); + xlog("status:%d\n", status); + xlog("RawData:AX=%d,AY=%d,AZ=%d,GX=%d,GY=%d,GZ=%d\r\n",combined_raw_data[0],combined_raw_data[1],combined_raw_data[2],combined_raw_data[3],combined_raw_data[4],combined_raw_data[5]); + } + count++; + + if (status == 1) { + calibration_done = 1; + printf("Sensor calibration successful! Skiing mode is active.\n"); + } + } else { + // printf("Calculate the time interval =============== start\n"); + // status = SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0); + // status = SIX_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0); + // status = Original_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0); + status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle,&mag_data, 0, tmp.quaternion_output); + memcpy(tmp.acc_data, &combined_raw_data[0], 3 * sizeof(signed short)); + memcpy(tmp.gyr_data, &combined_raw_data[3], 3 * sizeof(signed short)); + BLE_send_data_t data_by_calculate = sensor_processing_task(tmp.acc_data, tmp.gyr_data,tmp.angle, tmp.quaternion_output); + extern void ano_send_attitude_data(float rol, float pit, float yaw, uint8_t fusion_sta) ; + ano_send_attitude_data(tmp.angle[0],tmp.angle[1],tmp.angle[2], 1); + } + + // mmc5603nj_mag_data_t mag_data; + // mmc5603nj_read_mag_data(&mag_data); + // float temperature = mmc5603nj_get_temperature(); + // count_test1++; + // if(count_test1 >500){ + // count_test1 =0; + // xlog("Mag X: %.4f, Y: %.4f, Z: %.4f Gauss\n", mag_data.x, mag_data.y, mag_data.z); + // } + // xlog("=======sensor_read_data END\n"); } @@ -550,27 +547,21 @@ void xtell_task_create(void){ // os_time_dly(10); // delay_2ms(10); - SL_SC7U22_Config(); - // if (mmc5603nj_init() != 0) { - // xlog("MMC5603NJ initialization failed!\n"); + + + // if(bmp280_init() != 0){ + // xlog("bmp280 init error\n"); // } - // xlog("MMC5603NJ PID: 0x%02X\n", mmc5603nj_get_pid()); - // // 启用连续测量模式 - // mmc5603nj_enable_continuous_mode(); - // xlog("Continuous measurement mode enabled.\n"); + // float temp, press; + // bmp280_read_data(&temp, &press); + // xlog("get temp: %d, get press: %d\n",temp, press); + + + // MPU9250_Mag_Init(); //iic总线设备扫描 - extern void i2c_scanner_probe(void); - i2c_scanner_probe(); - - if(bmp280_init() != 0){ - xlog("bmp280 init error\n"); - } - float temp, press; - bmp280_read_data(&temp, &press); - xlog("get temp: %d, get press: %d\n",temp, press); - - + // extern void i2c_scanner_probe(void); + // i2c_scanner_probe(); xlog("xtell_task_create\n"); @@ -579,7 +570,7 @@ void xtell_task_create(void){ ano_protocol_init(115200); - create_process(&calculate_data_id, "calculate",NULL, sensor_measure, 2000); + circle_buffer_init(&sensor_read, sensor_read_buffer, SENSOR_DATA_BUFFER_SIZE, sizeof(sensor_data_t)); diff --git a/apps/earphone/xtell_Sensor/sensor/AK8963.c b/apps/earphone/xtell_Sensor/sensor/AK8963.c new file mode 100644 index 0000000..e4e38ae --- /dev/null +++ b/apps/earphone/xtell_Sensor/sensor/AK8963.c @@ -0,0 +1,133 @@ + +#include "AK8963.h" +#include "math.h" +#include "os/os_api.h" +#include "../xtell.h" +#include "printf.h" + +// 用于存放从Fuse ROM读取的磁力计灵敏度校准值 +static float mag_asa_x = 1.0f; +static float mag_asa_y = 1.0f; +static float mag_asa_z = 1.0f; + +// 磁力计在16-bit分辨率下的转换因子 (单位: uT/LSB) +#define MAG_RAW_TO_UT_FACTOR (4912.0f / 32760.0f) + +/** + * @brief 初始化MPU9250的磁力计AK8963 + * @return 0: 成功, 1: MPU9250连接失败, 2: AK8963连接失败 + */ +u8 MPU9250_Mag_Init(void) { + + u8 temp_data[3]; + + // --- 检查 MPU9250 连接并复位 --- + _gravity_sensor_get_ndata(MPU9250_ADDR_R, MPU9250_WHO_AM_I, temp_data, 1); + if (temp_data[0] != 0x71 && temp_data[0] != 0x73) { + printf("MPU9250 comm failed, read ID: 0x%X\n", temp_data[0]); + return 1; + } + printf("MPU9250 get id:0x%X\n", temp_data[0]); + + gravity_sensor_command(MPU9250_ADDR_W, MPU9250_PWR_MGMT_1, 0x80); // 软复位 + os_time_dly(10); // 等待复位完成 + + gravity_sensor_command(MPU9250_ADDR_W, MPU9250_PWR_MGMT_1, 0x01); // 退出睡眠，选择时钟源 + os_time_dly(2); + + // --- 强制复位 I2C Master 模块并开启旁路 --- + + gravity_sensor_command(MPU9250_ADDR_W, MPU9250_USER_CTRL, 0x20); + os_time_dly(1); + gravity_sensor_command(MPU9250_ADDR_W, MPU9250_USER_CTRL, 0x00); + os_time_dly(1); + + gravity_sensor_command(MPU9250_ADDR_W, MPU9250_INT_PIN_CFG, 0x02); + os_time_dly(2); + + // --- 再次验证 AK8963 连接 --- + _gravity_sensor_get_ndata(AK8963_ADDR_R, AK8963_WIA, temp_data, 1); + if (temp_data[0] != 0x48) { + printf("AK8963 comm failed after final attempt, read ID: 0x%X\n", temp_data[0]); + return 2; + } + printf("AK8963 get id: 0x%X\n", temp_data[0]); + + // ------------------ 配置 AK8963 ------------------ + // Power-down模式 + gravity_sensor_command(AK8963_ADDR_W, AK8963_CNTL1, 0x00); + os_time_dly(1); + + // Fuse ROM access模式 + gravity_sensor_command(AK8963_ADDR_W, AK8963_CNTL1, 0x0F); + os_time_dly(1); + _gravity_sensor_get_ndata(AK8963_ADDR_R, AK8963_ASAX, temp_data, 3); + + // 计算校准系数 + mag_asa_x = (float)(temp_data[0] - 128) / 256.0f + 1.0f; + mag_asa_y = (float)(temp_data[1] - 128) / 256.0f + 1.0f; + mag_asa_z = (float)(temp_data[2] - 128) / 256.0f + 1.0f; + + // 再次进入Power-down模式 + gravity_sensor_command(AK8963_ADDR_W, AK8963_CNTL1, 0x00); + os_time_dly(1); + + // 设置工作模式：16-bit分辨率，100Hz连续测量模式 (0x16) + gravity_sensor_command(AK8963_ADDR_W, AK8963_CNTL1, 0x16); + os_time_dly(1); + + printf("AK8963 configured successfully.\n"); + return 0; // 初始化成功 +} + + +/** + * @brief 读取磁力计的三轴原始数据 + * @param mx, my, mz - 用于存放X, Y, Z轴数据的指针 (int16_t类型) + * @return 0: 成功, 1: 数据未就绪, 2: 数据溢出 + */ +u8 MPU9250_Read_Mag_Raw(int16_t *mx, int16_t *my, int16_t *mz) { + u8 read_buf[7]; + + // 检查数据是否准备好 (使用8位读地址) + _gravity_sensor_get_ndata(AK8963_ADDR_R, AK8963_ST1, read_buf, 1); + if (!(read_buf[0] & 0x01)) { + return 1; // 数据未就绪 + } + + // 连续读取7个字节 (使用8位读地址) + _gravity_sensor_get_ndata(AK8963_ADDR_R, AK8963_HXL, read_buf, 7); + + // 检查数据是否溢出 + if (read_buf[6] & 0x08) { + return 2; // 数据溢出 + } + + // 组合数据 + *mx = (int16_t)((read_buf[1] << 8) | read_buf[0]); + *my = (int16_t)((read_buf[3] << 8) | read_buf[2]); + *mz = (int16_t)((read_buf[5] << 8) | read_buf[4]); + + return 0; // 读取成功 +} + +/** + * @brief 读取磁力计的三轴数据，并转换为uT(微特斯拉) (此函数内部逻辑不变) + * @param mx, my, mz - 用于存放X, Y, Z轴数据的指针 (float类型) + * @return 0: 成功, 1: 数据未就绪, 2: 数据溢出 + */ +u8 MPU9250_Read_Mag_uT(float *mx, float *my, float *mz) { + int16_t raw_mx, raw_my, raw_mz; + + u8 status = MPU9250_Read_Mag_Raw(&raw_mx, &raw_my, &raw_mz); + if (status != 0) { + return status; + } + + // 应用灵敏度校准，并转换为uT单位 + *mx = (float)raw_mx * mag_asa_x * MAG_RAW_TO_UT_FACTOR; + *my = (float)raw_my * mag_asa_y * MAG_RAW_TO_UT_FACTOR; + *mz = (float)raw_mz * mag_asa_z * MAG_RAW_TO_UT_FACTOR; + + return 0; +} \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/sensor/AK8963.h b/apps/earphone/xtell_Sensor/sensor/AK8963.h new file mode 100644 index 0000000..c2d9238 --- /dev/null +++ b/apps/earphone/xtell_Sensor/sensor/AK8963.h @@ -0,0 +1,46 @@ +// mpu9250_mag.h + +#ifndef __MPU9250_MAG_H +#define __MPU9250_MAG_H + +#include "stdint.h" // 假设你有标准整数类型，u8 对应 uint8_t +#include "gSensor/gSensor_manage.h" + +//================================================================================== +// MPU9250 和 AK8963 的 I2C 地址 (已转换为8位格式) +//================================================================================== +// MPU9250的7位地址是 0x68(接地) +#define MPU9250_ADDR_7BIT 0x69 +#define MPU9250_ADDR_W (MPU9250_ADDR_7BIT << 1 | 0) // 8位写地址: 0xD0 +#define MPU9250_ADDR_R (MPU9250_ADDR_7BIT << 1 | 1) // 8位读地址: 0xD1 + +// AK8963磁力计的7位地址是 0x0C +#define AK8963_ADDR_7BIT 0x0C +#define AK8963_ADDR_W (AK8963_ADDR_7BIT << 1 | 0) // 8位写地址: 0x18 +#define AK8963_ADDR_R (AK8963_ADDR_7BIT << 1 | 1) // 8位读地址: 0x19 + + +//================================================================================== +// MPU9250 相关寄存器 (用于开启旁路模式) +//================================================================================== +#define MPU9250_WHO_AM_I 0x75 +#define MPU9250_INT_PIN_CFG 0x37 +#define MPU9250_USER_CTRL 0x6A +#define MPU9250_PWR_MGMT_1 0x6B +//================================================================================== +// AK8963 磁力计相关寄存器 +//================================================================================== +#define AK8963_WIA 0x00 +#define AK8963_ST1 0x02 +#define AK8963_HXL 0x03 +#define AK8963_ST2 0x09 +#define AK8963_CNTL1 0x0A +#define AK8963_ASAX 0x10 + + +u8 MPU9250_Mag_Init(void); +u8 MPU9250_Read_Mag_Raw(int16_t *mx, int16_t *my, int16_t *mz); +u8 MPU9250_Read_Mag_uT(float *mx, float *my, float *mz); + + +#endif // __MPU9250_MAG_H \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/sensor/BMP280.h b/apps/earphone/xtell_Sensor/sensor/BMP280.h index c1683e5..103a950 100644 --- a/apps/earphone/xtell_Sensor/sensor/BMP280.h +++ b/apps/earphone/xtell_Sensor/sensor/BMP280.h @@ -17,9 +17,7 @@ #define BMP_IIC_READ_ADDRESS (BMP_IIC_WRITE_ADDRESS | 0x01) #endif -// BMP280 I2C 地址 (SDO/ADO 引脚接地) -#define BMP280_I2C_ADDR_LOW (0x76*2) -//7位地址:76, 8位地址:EC (接地) + // BMP280 寄存器地址 #define BMP280_REG_CALIB_START 0x88 diff --git a/apps/earphone/xtell_Sensor/sensor/MMC56.c b/apps/earphone/xtell_Sensor/sensor/MMC56.c index ac9e344..bacd250 100644 --- a/apps/earphone/xtell_Sensor/sensor/MMC56.c +++ b/apps/earphone/xtell_Sensor/sensor/MMC56.c @@ -1,6 +1,3 @@ -/* - 三轴磁力计 - MMC5603NJ -*/ #include "MMC56.h" #include "math.h" @@ -9,45 +6,18 @@ #include "gSensor/gSensor_manage.h" #include "printf.h" - -#define ENABLE_XLOG 1 -#ifdef xlog -#undef xlog -#endif -#if ENABLE_XLOG - #define xlog(format, ...) printf("[XT:%s] " format, __func__, ##__VA_ARGS__) -#else - #define xlog(format, ...) ((void)0) -#endif - -/*==================================================================================*/ -/* MMC5603NJ 内部定义 */ -/*==================================================================================*/ - // 用于跟踪当前是否处于连续测量模式 -static u8 g_continuous_mode_enabled = 0; +static uint8_t g_continuous_mode_enabled = 0; +mmc5603nj_cal_data_t cal_data; //校准数据 -/*==================================================================================*/ -/* 封装的底层I2C读写函数 */ -/*==================================================================================*/ - -/** - * @brief 写入单个字节到MMC5603NJ寄存器 - */ static void mmc5603nj_write_reg(uint8_t reg, uint8_t data) { gravity_sensor_command(MMC_IIC_WRITE_ADDRESS, reg, data); } - -/** - * @brief 从MMC5603NJ读取多个字节 - */ static uint32_t mmc5603nj_read_regs(uint8_t reg, uint8_t *buf, uint8_t len) { return _gravity_sensor_get_ndata(MMC_IIC_READ_ADDRESS, reg, buf, len); } -/*==================================================================================*/ -/* 外部接口函数实现 */ -/*==================================================================================*/ +// 外部接口函数实现 uint8_t mmc5603nj_get_pid(void) { uint8_t pid = 0; @@ -56,69 +26,145 @@ uint8_t mmc5603nj_get_pid(void) { } int mmc5603nj_init(void) { - // 检查产品ID是否正确 - if (mmc5603nj_get_pid() != 0x10) { - xlog("init error: check id error %d\n", mmc5603nj_get_pid()); - return -1; // 设备ID不匹配 + // ID + if (mmc5603nj_get_pid() != 0x80) { + printf("MMC5603NJ init failed: wrong Product ID (read: 0x%X)\n", mmc5603nj_get_pid()); + return -1; } - // 对传感器执行软件复位 (将 INCTRL0 寄存器的 Do_reset 位置1) - mmc5603nj_write_reg(MMC_INCTRL0, 0x08); + // 软件复位 + mmc5603nj_write_reg(MMC_INCTRL1, 0x80); // SW_RESET bit os_time_dly(20); // 等待复位完成 + // 设置20位分辨率 (BW[1:0] = 11) + // 同时确保所有轴都使能 (X/Y/Z_inhibit = 0) + mmc5603nj_write_reg(MMC_INCTRL1, 0x03); + os_time_dly(1); + + // 设置内部控制寄存器2 + // CMM_EN = 1 (使能连续模式功能) + // HPOWER = 1 (高功耗模式，更稳定) + mmc5603nj_write_reg(MMC_INCTRL2, 0x90); // 0b10010000 + + // 设置自动SET/RESET功能 + // AUTO_SR_EN = 1 + mmc5603nj_write_reg(MMC_INCTRL0, 0x20); // 0b00100000 + g_continuous_mode_enabled = 0; - return 0; // 初始化成功 + printf("MMC5603NJ initialized successfully.\n"); + + mmc5603nj_enable_continuous_mode(0x04); + + + printf("\n--- Magnetometer Calibration Start ---\n"); + printf("Slowly rotate the device in all directions (like drawing a 3D '8')...\n"); + printf("Calibration will last for 20 seconds.\n\n"); + printf("will start after 5 seconds\n\n"); + os_time_dly(500); + + // 定义校准时长和采样间隔 + const uint32_t calibration_duration_ms = 20000; // 20秒 + const uint32_t sample_interval_ms = 100; // 每100ms采样一次 + + // 初始化最大最小值 + // 使用一个临时变量来读取数据，避免干扰read函数的正常逻辑 + mmc5603nj_mag_data_t temp_mag_data; + // 首次读取以获取初始值 + mmc5603nj_read_mag_data(&temp_mag_data); // 首次读取不应用校准 + + float max_x = temp_mag_data.x; + float min_x = temp_mag_data.x; + float max_y = temp_mag_data.y; + float min_y = temp_mag_data.y; + float max_z = temp_mag_data.z; + float min_z = temp_mag_data.z; + + uint32_t start_time = os_time_get(); // 假设os_time_get()返回毫秒级时间戳 + int samples = 0; + int over = calibration_duration_ms/sample_interval_ms; + + while (samples <= over) { + // 读取原始磁力计数据 + mmc5603nj_read_mag_data(&temp_mag_data); + + // 更新最大最小值 + if (temp_mag_data.x > max_x) max_x = temp_mag_data.x; + if (temp_mag_data.x < min_x) min_x = temp_mag_data.x; + + if (temp_mag_data.y > max_y) max_y = temp_mag_data.y; + if (temp_mag_data.y < min_y) min_y = temp_mag_data.y; + + if (temp_mag_data.z > max_z) max_z = temp_mag_data.z; + if (temp_mag_data.z < min_z) min_z = temp_mag_data.z; + + samples++; + os_time_dly(sample_interval_ms / 10); // os_time_dly的参数通常是ticks (1 tick = 10ms) + } + + // 检查数据范围是否合理，防止传感器未动或故障 + if ((max_x - min_x < 0.1f) || (max_y - min_y < 0.1f) || (max_z - min_z < 0.1f)) { + printf("\n--- Calibration Failed ---\n"); + printf("Device might not have been rotated enough.\n"); + printf("X range: %.2f, Y range: %.2f, Z range: %.2f\n", max_x - min_x, max_y - min_y, max_z - min_z); + return -1; + } + + // 计算硬磁偏移 (椭球中心) + cal_data.offset_x = (max_x + min_x) / 2.0f; + cal_data.offset_y = (max_y + min_y) / 2.0f; + cal_data.offset_z = (max_z + min_z) / 2.0f; + + printf("\n--- Calibration Complete ---\n"); + printf("Collected %d samples.\n", samples); + printf("Offsets (Gauss):\n"); + printf(" X: %.4f\n", cal_data.offset_x); + printf(" Y: %.4f\n", cal_data.offset_y); + printf(" Z: %.4f\n", cal_data.offset_z); + printf("Please save these values and apply them in your code.\n\n"); + + return 0; } -void mmc5603nj_set_data_rate(uint8_t rate) { - mmc5603nj_write_reg(MMC_ODR, rate); -} -void mmc5603nj_enable_continuous_mode(void) { - uint8_t reg_val; - // 启用连续模式需要设置 INCTRL0 和 INCTRL2 寄存器 - // 1. 设置 INCTRL0 的 Cmm_en 位 (bit 7) - mmc5603nj_read_regs(MMC_INCTRL0, ®_val, 1); - reg_val |= 0x80; - mmc5603nj_write_reg(MMC_INCTRL0, reg_val); - - // 2. 设置 INCTRL2 的 Cmm_freq_en 位 (bit 4) - mmc5603nj_read_regs(MMC_INCTRL2, ®_val, 1); - reg_val |= 0x10; - mmc5603nj_write_reg(MMC_INCTRL2, reg_val); +void mmc5603nj_enable_continuous_mode(uint8_t rate) { + // 在连续模式下，ODR寄存器必须被设置 + mmc5603nj_write_reg(MMC_ODR, rate); //要设置频率 + // mmc5603nj_set_data_rate(0x04); + // 启用连续模式 (INCTRL2的CMM_EN位已在init中设置) + // 只需要设置 INCTRL0 的 CMM_FREQ_EN 位 + mmc5603nj_write_reg(MMC_INCTRL0, 0xA0); // 0b10100000 (CMM_FREQ_EN=1, AUTO_SR_EN=1) g_continuous_mode_enabled = 1; } void mmc5603nj_disable_continuous_mode(void) { - uint8_t reg_val; - - // 禁用连续模式只需要清除 INCTRL2 的 Cmm_freq_en 位 - mmc5603nj_read_regs(MMC_INCTRL2, ®_val, 1); - reg_val &= ~0x10; // 清除 bit 4 - mmc5603nj_write_reg(MMC_INCTRL2, reg_val); - + // 禁用连续模式 + mmc5603nj_write_reg(MMC_INCTRL0, 0x20); // 恢复到仅使能 AUTO_SR_EN 的状态 g_continuous_mode_enabled = 0; } float mmc5603nj_get_temperature(void) { uint8_t status = 0; uint8_t temp_raw = 0; + uint8_t timeout = 20; - // 1. 触发一次温度测量 (写入 0x02 到 INCTRL0 寄存器) - mmc5603nj_write_reg(MMC_INCTRL0, 0x02); + // 触发一次温度测量 + mmc5603nj_write_reg(MMC_INCTRL0, 0x02); // TAKE_MEAS_T - // 2. 等待测量完成 (轮询 STATUS1 寄存器的 Meas_T_done 位) + // 等待测量完成 do { - os_time_dly(10); // 等待一下，避免过于频繁的I2C读取 + os_time_dly(10); mmc5603nj_read_regs(MMC_STATUS1, &status, 1); - } while ((status & 0x80) == 0); + timeout--; + } while ((status & 0x80) == 0 && timeout > 0); + + if (timeout == 0) { + printf("Error: Temperature measurement timeout!\n"); + return -273.15f; // 返回一个绝对零度的错误值 + } - // 3. 读取温度原始值 mmc5603nj_read_regs(MMC_TOUT, &temp_raw, 1); - - // 4. 根据公式计算实际温度: Temp(°C) = -75 + 0.8 * TOUT return ((float)temp_raw * 0.8f) - 75.0f; } @@ -126,32 +172,50 @@ void mmc5603nj_read_mag_data(mmc5603nj_mag_data_t *mag_data) { uint8_t buffer[9]; if (g_continuous_mode_enabled) { - // 连续模式下，直接读取数据即可 - mmc5603nj_read_regs(MMC_XOUT0, buffer, 9); + // 连续模式下，只需检查数据是否就绪 + uint8_t status = 0; + mmc5603nj_read_regs(MMC_STATUS1, &status, 1); + if ((status & 0x40) == 0) { // Meas_M_done bit + // 数据未就绪，可以选择返回或等待，这里我们直接返回旧数据 + return; + } } else { // 单次测量模式 uint8_t status = 0; - // 1. 触发一次磁场测量 (写入 0x01 到 INCTRL0 寄存器) - mmc5603nj_write_reg(MMC_INCTRL0, 0x01); + uint8_t timeout = 20; - // 2. 等待测量完成 (轮询 STATUS1 寄存器的 Meas_M_done 位) + // 触发一次带自动SET/RESET的磁场测量 + mmc5603nj_write_reg(MMC_INCTRL0, 0x21); // 0b00100001 (TAKE_MEAS_M=1, AUTO_SR_EN=1) + + // 等待测量完成 do { - os_time_dly(10); // 等待一下 + os_time_dly(10); mmc5603nj_read_regs(MMC_STATUS1, &status, 1); - } while ((status & 0x40) == 0); - - // 3. 读取9个字节的原始数据 - mmc5603nj_read_regs(MMC_XOUT0, buffer, 9); + timeout--; + } while ((status & 0x40) == 0 && timeout > 0); + + if (timeout == 0) { + printf("Error: Magnetic measurement timeout!\n"); + mag_data->x = mag_data->y = mag_data->z = 0.0f; + return; + } } + // 读取9个字节的原始数据 + mmc5603nj_read_regs(MMC_XOUT0, buffer, 9); + // 解析数据 (20位分辨率) - // 零点偏置: 2^19 = 524288, 灵敏度: 2^14 = 16384 LSB/Gauss - int32_t raw_x = (buffer[0] << 12) | (buffer[1] << 4) | (buffer[6] >> 4); - int32_t raw_y = (buffer[2] << 12) | (buffer[3] << 4) | (buffer[7] >> 4); - int32_t raw_z = (buffer[4] << 12) | (buffer[5] << 4) | (buffer[8] >> 4); + int32_t raw_x = ((uint32_t)buffer[0] << 12) | ((uint32_t)buffer[1] << 4) | ((uint32_t)buffer[6] & 0x0F); + int32_t raw_y = ((uint32_t)buffer[2] << 12) | ((uint32_t)buffer[3] << 4) | ((uint32_t)buffer[6] >> 4); + int32_t raw_z = ((uint32_t)buffer[4] << 12) | ((uint32_t)buffer[5] << 4) | ((uint32_t)buffer[8] & 0x0F); // 应用偏置和灵敏度进行转换 mag_data->x = ((float)raw_x - 524288.0f) / 16384.0f; mag_data->y = ((float)raw_y - 524288.0f) / 16384.0f; mag_data->z = ((float)raw_z - 524288.0f) / 16384.0f; + + //减去偏移 + mag_data->x -= cal_data.offset_x; + mag_data->y -= cal_data.offset_y; + mag_data->z -= cal_data.offset_z; } \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/sensor/MMC56.h b/apps/earphone/xtell_Sensor/sensor/MMC56.h index 8733121..3ac1f3e 100644 --- a/apps/earphone/xtell_Sensor/sensor/MMC56.h +++ b/apps/earphone/xtell_Sensor/sensor/MMC56.h @@ -43,6 +43,12 @@ typedef struct { float z; } mmc5603nj_mag_data_t; +// 定义一个结构体来存放磁力计的硬磁偏移校准数据 +typedef struct { + float offset_x; + float offset_y; + float offset_z; +} mmc5603nj_cal_data_t; /** * @brief 初始化MMC5603NJ传感器 @@ -60,7 +66,7 @@ void mmc5603nj_set_data_rate(uint8_t rate); /** * @brief 启用连续测量模式 */ -void mmc5603nj_enable_continuous_mode(void); +void mmc5603nj_enable_continuous_mode(uint8_t rate); /** * @brief 禁用连续测量模式 diff --git a/apps/earphone/xtell_Sensor/sensor/SC7U22.c b/apps/earphone/xtell_Sensor/sensor/SC7U22.c index 93d182c..3f08a9e 100644 --- a/apps/earphone/xtell_Sensor/sensor/SC7U22.c +++ b/apps/earphone/xtell_Sensor/sensor/SC7U22.c @@ -1182,15 +1182,31 @@ unsigned char get_calibration_state(void){ // Kp: 比例增益，决定了加速度计数据校正陀螺仪的权重。值越大，对加速度计的响应越快，但对运动加速度更敏感。 // Ki: 积分增益，决定了用于校正陀螺仪静态漂移的权重。 // Q_dt: 采样时间间隔（单位：秒），这里是10ms (0.01s)，对应100Hz的采样率。 +#define HAVE_MAG 1 +#if HAVE_MAG == 0 +// -- 无地磁 -- const float Kp = 2.0f; const float Ki = 0.005f; const float Q_dt = 0.01f; +#else +// -- 有地磁 -- +const float Kp = 0.3f; +const float Ki = 0.001f; +const float Q_dt = 0.01f; +#endif // --- 状态变量 --- // 四元数 (Quaternion)，表示当前的姿态。初始化为 (1, 0, 0, 0)，代表初始姿态为水平。 static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // 陀螺仪积分误差项，用于补偿静态漂移 static float exInt = 0.0f, eyInt = 0.0f, ezInt = 0.0f; + +// 磁力计校准相关的变量 +float Error_Mag_f[3] = {0.0f, 0.0f, 0.0f}; +double Sum_Avg_Mag_f[3] = {0.0, 0.0, 0.0}; // 使用double避免累加过程中的精度损失 +// 临时存储校准后数据的数组 +signed short Temp_AccGyro[6] = {0}; +float Temp_Mag[3] = {0.0f, 0.0f, 0.0f}; // ================================================================================================= @@ -1207,15 +1223,237 @@ static float exInt = 0.0f, eyInt = 0.0f, ezInt = 0.0f; * @param calibration_en 传入：外部校准使能标志。如果为0，则强制认为已经校准完成。 * @param acc_gyro_input 传入和传出：包含6轴原始数据的数组指针，顺序为 [ACC_X, ACC_Y, ACC_Z, GYR_X, GYR_Y, GYR_Z]。该函数会对其进行原地修改，填充为校准后的数据。 * @param Angle_output 传出：滤波后的结果，顺序为 [Pitch, Roll, Yaw]，单位为度。 +* @param mag_data_input 传入：指向包含三轴磁力计数据的结构体指针。数据单位应为高斯(Gauss)。已经8面校准过的数据 * @param yaw_rst 传入：Yaw轴重置标志。如果为1，则将整个姿态滤波器状态重置。 -* +* @param quaternion_output 传出, 四元数,用于后续重力分量的去除计算 * @return * - 0: 正在进行静态校准。 * - 1: 姿态角计算成功。 * - 2: 校准未完成，无法进行计算。 */ -unsigned char Q_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst, float *quaternion_output) +unsigned char Q_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, const mmc5603nj_mag_data_t* _mag_data_input, unsigned char yaw_rst, float *quaternion_output) { +#if 1 //有地磁置1 + unsigned char sl_i = 0; + // 如果外部强制禁用校准，则将标志位置1 + if (calibration_en == 0) { + SL_SC7U22_Error_Flag = 1; + } + + // ====================== 坐标对齐 ====================== + mmc5603nj_mag_data_t mag_data_input; + + mag_data_input.x = - _mag_data_input->x; + mag_data_input.y = - _mag_data_input->y; + mag_data_input.z = _mag_data_input->z; + // ================================================================ + + + // ================================================================================= + // 静态校准 + // --------------------------------------------------------------------------------- + if (SL_SC7U22_Error_Flag == 0) { + unsigned short acc_gyro_delta[2]; + acc_gyro_delta[0] = 0; + acc_gyro_delta[1] = 0; + for (sl_i = 0; sl_i < 3; sl_i++) { + acc_gyro_delta[0] += SL_GetAbsShort(acc_gyro_input[sl_i] - Temp_Accgyro[sl_i]); + acc_gyro_delta[1] += SL_GetAbsShort(acc_gyro_input[3 + sl_i] - Temp_Accgyro[3 + sl_i]); + } + for (sl_i = 0; sl_i < 6; sl_i++) { + Temp_Accgyro[sl_i] = acc_gyro_input[sl_i]; + } +#if (ACC_RANGE == 2) + if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 16384) < 3000)) { +#elif (ACC_RANGE == 4) + if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 8192) < 3000)) { +#elif (ACC_RANGE == 8) + if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 4096) < 3000)) { +#elif (ACC_RANGE == 16) + if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 2048) < 3000)) { +#endif + if (SL_SC7U22_Error_cnt < 200) SL_SC7U22_Error_cnt++; + } else { + SL_SC7U22_Error_cnt = 0; + } + if (SL_SC7U22_Error_cnt > 190) { + //累加6轴数据 + for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] += acc_gyro_input[sl_i]; + + Sum_Avg_Mag_f[0] += mag_data_input.x; + Sum_Avg_Mag_f[1] += mag_data_input.y; + Sum_Avg_Mag_f[2] += mag_data_input.z; + + SL_SC7U22_Error_cnt2++; + if (SL_SC7U22_Error_cnt2 > 49) { + SL_SC7U22_Error_Flag = 1; + SL_SC7U22_Error_cnt2 = 0; + SL_SC7U22_Error_cnt = 0; + //6轴偏置 + for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] = Sum_Avg_Accgyro[sl_i] / 50; + Error_Accgyro[0] = 0 - Sum_Avg_Accgyro[0]; + Error_Accgyro[1] = 0 - Sum_Avg_Accgyro[1]; + #if ACC_RANGE==2 + Error_Accgyro[2] = 16384 - Sum_Avg_Accgyro[2]; + #elif ACC_RANGE==4 + Error_Accgyro[2] = 8192 - Sum_Avg_Accgyro[2]; + #elif ACC_RANGE==8 + Error_Accgyro[2] = 4096 - Sum_Avg_Accgyro[2]; + #elif ACC_RANGE==16 + Error_Accgyro[2] = 2048 - Sum_Avg_Accgyro[2]; + #endif + Error_Accgyro[3] = 0 - Sum_Avg_Accgyro[3]; + Error_Accgyro[4] = 0 - Sum_Avg_Accgyro[4]; + Error_Accgyro[5] = 0 - Sum_Avg_Accgyro[5]; + // //磁力计偏置 -- 不在这弄,在磁力计初始化的时候开始 + // Sum_Avg_Mag_f[0] /= 50.0; + // Sum_Avg_Mag_f[1] /= 50.0; + // Sum_Avg_Mag_f[2] /= 50.0; + // Error_Mag_f[0] = 0.0f - (float)Sum_Avg_Mag_f[0]; + // Error_Mag_f[1] = 0.0f - (float)Sum_Avg_Mag_f[1]; + // Error_Mag_f[2] = 0.0f - (float)Sum_Avg_Mag_f[2]; + // xlog("AVG_Recode AX:%d,AY:%d,AZ:%d,GX:%d,GY:%d,GZ:%d\r\n", Sum_Avg_Accgyro[0], Sum_Avg_Accgyro[1], Sum_Avg_Accgyro[2], Sum_Avg_Accgyro[3], Sum_Avg_Accgyro[4], Sum_Avg_Accgyro[5]); + // xlog("Error_Recode AX:%d,AY:%d,AZ:%d,GX:%d,GY:%d,GZ:%d\r\n", Error_Accgyro[0], Error_Accgyro[1], Error_Accgyro[2], Error_Accgyro[3], Error_Accgyro[4], Error_Accgyro[5]); + } + } else { + SL_SC7U22_Error_cnt2 = 0; + for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] = 0; + // Sum_Avg_Mag_f[0] = 0.0; + // Sum_Avg_Mag_f[1] = 0.0; + // Sum_Avg_Mag_f[2] = 0.0; + } + return 0; // 返回0，表示正在校准 + } + + // ================================================================================= + // 姿态解算 (Mahony AHRS) + // --------------------------------------------------------------------------------- + if (SL_SC7U22_Error_Flag == 1) { // 确认已经校准完成 + + // --- Yaw轴/姿态重置 --- + // 注意：重置yaw会重置整个姿态滤波器，使设备回到初始水平姿态 + if (yaw_rst == 1) { + q0 = 1.0f; q1 = 0.0f; q2 = 0.0f; q3 = 0.0f; + exInt = 0.0f; eyInt = 0.0f; ezInt = 0.0f; + } + + // --- 数据预处理 --- + // 应用零点偏移补偿 + for (sl_i = 0; sl_i < 6; sl_i++) { + Temp_Accgyro[sl_i] = acc_gyro_input[sl_i] + Error_Accgyro[sl_i]; + } + + // Temp_Mag[0] = mag_data_input.x + Error_Mag_f[0]; + // Temp_Mag[1] = mag_data_input.y + Error_Mag_f[1]; + // Temp_Mag[2] = mag_data_input.z + Error_Mag_f[2]; + Temp_Mag[0] = mag_data_input.x; + Temp_Mag[1] = mag_data_input.y; + Temp_Mag[2] = mag_data_input.z; + + // 将校准后的数据写回输入数组 +#if 1 + for (sl_i = 0; sl_i < 6; sl_i++) { + acc_gyro_input[sl_i] = Temp_Accgyro[sl_i]; + } +#endif + + // 获取校准后的数据 + float ax = (float)Temp_Accgyro[0]; + float ay = (float)Temp_Accgyro[1]; + float az = (float)Temp_Accgyro[2]; + // 将陀螺仪数据从 LSB 转换为弧度/秒 (rad/s) + // 转换系数 0.061 ≈ 2000dps / 32768 LSB; PI/180 ≈ 0.01745 + float gx = (float)Temp_Accgyro[3] * 0.061f * 0.0174533f; // Roll rate + float gy = (float)Temp_Accgyro[4] * 0.061f * 0.0174533f; // Pitch rate + float gz = (float)Temp_Accgyro[5] * 0.061f * 0.0174533f; // Yaw rate + float mx = Temp_Mag[0]; + float my = Temp_Mag[1]; + float mz = Temp_Mag[2]; + + // --- Mahony算法核心 --- + float norm; + float q0q0 = q0 * q0; + float q0q1 = q0 * q1; + float q0q2 = q0 * q2; + float q0q3 = q0 * q3; + float q1q1 = q1 * q1; + float q1q2 = q1 * q2; + float q1q3 = q1 * q3; + float q2q2 = q2 * q2; + float q2q3 = q2 * q3; + float q3q3 = q3 * q3; + float hx, hy, bx, bz; + float vx, vy, vz, wx, wy, wz; + float ex, ey, ez; + + // 归一化加速度计测量值，得到单位重力向量 + norm = sqrtf(ax * ax + ay * ay + az * az); + if (norm > 0.0f) { ax /= norm; ay /= norm; az /= norm; } + else { return 1; } + + norm = sqrtf(mx * mx + my * my + mz * mz); + if (norm > 0.0f) { mx /= norm; my /= norm; mz /= norm; } + + // 根据当前姿态(四元数)估计重力向量的方向 + vx = 2.0f * (q1q3 - q0q2); + vy = 2.0f * (q0q1 + q2q3); + vz = q0q0 - q1q1 - q2q2 + q3q3; + + // 计算磁场误差 (倾斜补偿) + hx = 2.0f * mx * (0.5f - q2q2 - q3q3) + 2.0f * my * (q1q2 - q0q3) + 2.0f * mz * (q1q3 + q0q2); + hy = 2.0f * mx * (q1q2 + q0q3) + 2.0f * my * (0.5f - q1q1 - q3q3) + 2.0f * mz * (q2q3 - q0q1); + bx = sqrtf(hx * hx + hy * hy); + bz = 2.0f * mx * (q1q3 - q0q2) + 2.0f * my * (q2q3 + q0q1) + 2.0f * mz * (0.5f - q1q1 - q2q2); + wx = 2.0f * bx * (0.5f - q2q2 - q3q3) + 2.0f * bz * (q1q3 - q0q2); + wy = 2.0f * bx * (q1q2 - q0q3) + 2.0f * bz * (q0q1 + q2q3); + wz = 2.0f * bx * (q1q3 + q0q2) + 2.0f * bz * (0.5f - q1q1 - q2q2); + + // 合并重力和磁场误差 + ex = (ay * vz - az * vy) + (my * wz - mz * wy); + ey = (az * vx - ax * vz) + (mz * wx - mx * wz); + ez = (ax * vy - ay * vx) + (mx * wy - my * wx); + + // PI控制器 + if (Ki > 0.0f) { + exInt += ex * Ki * Q_dt; + eyInt += ey * Ki * Q_dt; + ezInt += ez * Ki * Q_dt; + } + gx += Kp * ex + exInt; + gy += Kp * ey + eyInt; + gz += Kp * ez + ezInt; + + // 使用校正后的角速度更新四元数 (一阶毕卡法) + q0 += (-q1 * gx - q2 * gy - q3 * gz) * 0.5f * Q_dt; + q1 += ( q0 * gx + q2 * gz - q3 * gy) * 0.5f * Q_dt; + q2 += ( q0 * gy - q1 * gz + q3 * gx) * 0.5f * Q_dt; + q3 += ( q0 * gz + q1 * gy - q2 * gx) * 0.5f * Q_dt; + + // 归一化四元数，保持其单位长度 + norm = sqrtf(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 /= norm; q1 /= norm; q2 /= norm; q3 /= norm; + + // --- 将四元数转换为欧拉角 (Pitch, Roll, Yaw) --- + // Pitch (绕Y轴旋转) + Angle_output[0] = asinf(-2.0f * (q1 * q3 - q0 * q2)) * 57.29578f; + // Roll (绕X轴旋转) + Angle_output[1] = atan2f(2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3) * 57.29578f; + // Yaw (绕Z轴旋转) + Angle_output[2] = atan2f(2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3) * 57.29578f; + + //将四元数传出去 + if (quaternion_output != NULL) { + quaternion_output[0] = q0; // w + quaternion_output[1] = q1; // x + quaternion_output[2] = q2; // y + quaternion_output[3] = q3; // z + } + return 1; // 返回1，表示计算成功 + } + + return 2; // 校准未完成，返回错误状态 + +#else unsigned char sl_i = 0; // 如果外部强制禁用校准，则将标志位置1 @@ -1248,6 +1486,7 @@ unsigned char Q_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed shor #endif if (SL_SC7U22_Error_cnt < 200) SL_SC7U22_Error_cnt++; } else { + // printf("error: The calibration process has undergone a shift.\n"); SL_SC7U22_Error_cnt = 0; } if (SL_SC7U22_Error_cnt > 190) { @@ -1388,6 +1627,7 @@ unsigned char Q_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed shor } return 2; // 校准未完成，返回错误状态 +#endif } #endif diff --git a/apps/earphone/xtell_Sensor/sensor/SC7U22.h b/apps/earphone/xtell_Sensor/sensor/SC7U22.h index 60f05e3..d8cdfce 100644 --- a/apps/earphone/xtell_Sensor/sensor/SC7U22.h +++ b/apps/earphone/xtell_Sensor/sensor/SC7U22.h @@ -9,6 +9,7 @@ Copyright (c) 2022 Silan MEMS. All Rights Reserved. #include "gSensor/gSensor_manage.h" #include "printf.h" +#include "MMC56.h" //是否使能串口打印调试 #define SL_Sensor_Algo_Release_Enable 0x00 @@ -132,7 +133,7 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en,signed short * unsigned char Original_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst); unsigned char SIX_SL_SC7U22_Angle_Output(unsigned char auto_calib_start, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst); -unsigned char Q_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst, float *quaternion_output); +unsigned char Q_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, const mmc5603nj_mag_data_t *mag_data_input, unsigned char yaw_rst, float *quaternion_output); unsigned char get_calibration_state(void); /**寄存器宏定义*******************************/ #define SC7U22_WHO_AM_I 0x01 diff --git a/apps/earphone/xtell_Sensor/xtell_handler.c b/apps/earphone/xtell_Sensor/xtell_handler.c index efada02..a3bdb0f 100644 --- a/apps/earphone/xtell_Sensor/xtell_handler.c +++ b/apps/earphone/xtell_Sensor/xtell_handler.c @@ -45,6 +45,10 @@ #include "default_event_handler.h" #include "debug.h" #include "system/event.h" +#include "./ano/ano_protocol.h" +#include "./sensor/MMC56.h" +#include "./sensor/BMP280.h" +#include "./sensor/AK8963.h" /////////////////////////////////////////////////////////////////////////////////////////////////// //宏定义 #define LOG_TAG_CONST EARPHONE @@ -199,18 +203,40 @@ void le_user_app_event_handler(struct sys_event* event){ if(event->u.app.buffer[2] == 0x01){ //后面的数据长度 1 switch (event->u.app.buffer[3]){ case 0x01: - extern void start_detection(void); - start_detection(); - char* send_tmp = "start_detection\n"; + char* send_start = "will start after 5 seconds\n"; + send_data_to_ble_client(send_start,strlen(send_start)); + + if (mmc5603nj_init() != 0) { + xlog("MMC5603NJ initialization failed!\n"); + char* send_error = "calibration error\n"; + send_data_to_ble_client(send_error,strlen(send_error)); + } + xlog("MMC5603NJ PID: 0x%02X\n", mmc5603nj_get_pid()); + char* send_tmp = "8th calibration completed\n"; send_data_to_ble_client(send_tmp,strlen(send_tmp)); break; case 0x02: + extern void create_process(u16* pid,char* name, void *priv, void (*func)(void *priv), u32 msec); + extern void sensor_measure(void); + static int test_id; + SL_SC7U22_Config(); + create_process(&test_id, "test",NULL, sensor_measure, 10); + send_tmp = "start_detection\n"; + send_data_to_ble_client(send_tmp,strlen(send_tmp)); + break; + case 0x03: + extern void start_detection(void); + start_detection(); + send_tmp = "start_detection\n"; + send_data_to_ble_client(send_tmp,strlen(send_tmp)); + break; + case 0x04: extern void stop_detection(void); stop_detection(); send_tmp = "stop_detection\n"; send_data_to_ble_client(send_tmp,strlen(send_tmp)); break; - case 0x03: + case 0x05: extern void clear_speed(void); clear_speed(); send_tmp = "Reset speed and distances to zero\n"; diff --git a/cpu/br28/iic_hw.c b/cpu/br28/iic_hw.c index 3c3ce46..29a9601 100644 --- a/cpu/br28/iic_hw.c +++ b/cpu/br28/iic_hw.c @@ -188,12 +188,18 @@ void hw_iic_stop(hw_iic_dev iic) u8 hw_iic_tx_byte(hw_iic_dev iic, u8 byte) { + // printf("====debug1=======\n"); u8 id = iic_get_id(iic); + // printf("====debug2=======\n"); iic_dir_out(iic_regs[id]); + // printf("====debug3=======\n"); iic_buf_reg(iic_regs[id]) = byte; + // printf("====debug4=======\n"); iic_cfg_done(iic_regs[id]); + // printf("====debug5=======\n"); /* putchar('a'); */ while (!iic_pnd(iic_regs[id])); + // printf("====debug6=======\n"); iic_pnd_clr(iic_regs[id]); /* putchar('b'); */ return iic_send_is_ack(iic_regs[id]);