diff --git a/apps/earphone/xtell_Sensor/A_hide/8/skiing_tracker.c b/apps/earphone/xtell_Sensor/A_hide/8/skiing_tracker.c index 22f93d2..c80e178 100644 --- a/apps/earphone/xtell_Sensor/A_hide/8/skiing_tracker.c +++ b/apps/earphone/xtell_Sensor/A_hide/8/skiing_tracker.c @@ -1,14 +1,6 @@ /* - 动态ZUPT+卡尔曼+巴特沃斯一阶滤波器 - 针对启动滑雪和停止滑雪，设置不同阈值 - 启动滑雪和ZUPT更新的陀螺仪方差阈值分开设置 - - 启动滑雪的陀螺仪阈值会更宽松一些 - 原地旋转和ZUPT更新的加速度方差阈值分开设置 - - 原地旋转的加速度阈值更宽松 - 能够从静止状态到变化状态，去根据阈值来判断这个“变化”：进入滑行状态 / 只是原地摆动 - - 但是还是不够灵敏 - - 添加了滑雪过程中转弯判断，参数待调 + 虽然sensor_processing_task是10ms调用一次 + 但是实际上上一次调用该函数的时间点和下一次调用该函数的时间点，会相差40ms */ #include "skiing_tracker.h" @@ -32,6 +24,12 @@ // 陀螺仪方差阈值，以允许启动瞬间的正常抖动，但仍能过滤掉混乱的、非滑雪的晃动。 #define SKIING_GYR_VARIANCE_THRESHOLD 15.0f +// --- 滑雪过程 --- +//加速度 模长，低于此值视为 在做匀速运动 +#define SKIING_ACC_MAG_THRESHOLD 0.5f +//陀螺仪 模长，高于此值视为 摔倒了 +#define FALLEN_GRY_MAG_THRESHOLD 1000.0f //未确定 + // --- 原地旋转抖动 --- // 用于原地旋转判断的加速度方差阈值。此值比ZUPT阈值更宽松， // 以允许原地旋转时身体的正常晃动，但仍能与真实滑行时的剧烈加速度变化区分开。 @@ -62,8 +60,30 @@ BLE_KS_send_data_t KS_data; 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 初始化滑雪追踪器 * @@ -76,7 +96,7 @@ void skiing_tracker_init(skiing_tracker_t *tracker) } // 使用memset一次性清零整个结构体，包括新增的缓冲区 memset(tracker, 0, sizeof(skiing_tracker_t)); - tracker->state = SKIING_STATE_STATIC; + tracker->state = STATIC; } /** @@ -154,7 +174,7 @@ static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3]) /** * @brief 状态机更新 * - * @param tracker + * @param tracker 传入同步修改后传出 * @param acc_device_ms2 三轴加速度，m/s^2 * @param gyr_dps 三轴陀螺仪，dps */ @@ -162,7 +182,7 @@ static void update_state_machine(skiing_tracker_t *tracker, const float *acc_dev { // 缓冲区未填满时，不进行状态判断，默认为静止 if (!tracker->buffer_filled) { - tracker->state = SKIING_STATE_STATIC; + tracker->state = STATIC; return; } @@ -171,49 +191,119 @@ static void update_state_machine(skiing_tracker_t *tracker, const float *acc_dev 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]); 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]); + #ifdef XTELL_TEST debug1.acc_variance =acc_variance; debug1.gyr_variance =gyr_variance; debug1.gyr_magnitude=gyr_magnitude; - debug1.acc_magnitude=acc_magnitude - G_ACCELERATION; + debug1.acc_magnitude=fabsf(acc_magnitude - G_ACCELERATION); #endif - // --- 状态切换逻辑 (按优先级) --- +#if 0 + //正在滑雪 + if(tracker->state == NO_CONSTANT_SPEED ) { + //摔倒了 + if(gyr_magnitude > FALLEN_GRY_MAG_THRESHOLD){ + tracker->state = FALLEN; + return; + } + //可能进入了匀速状态 + if(gyr_magnitude > SKIING_GYR_MAG_THRESHOLD && acc_magnitude < SKIING_ACC_MAG_THRESHOLD){ + tracker->state = CONSTANT_SPEED; + return; + } + //急转弯 + if(gyr_magnitude > WHEEL_GYR_MAG_THRESHOLD && acc_variance > WHEEL_ACC_VARIANCE_THRESHOLD){ + //TODO：可以考虑清掉速度消除积分带来的漂移 + tracker->state = WHEEL; + return; + } + } - // 优先级1：动态零速更新 (ZUPT) - 最严格和最优先的“刹车” + // --- 状态切换逻辑 (按优先级) --- + // 优先级1：静止 if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) { - tracker->state = SKIING_STATE_STATIC; + tracker->state = STATIC; // 速度清零，抑制漂移 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)); return; } - if(tracker->state == SKIING_STATE_SKIING && gyr_magnitude > WHEEL_GYR_MAG_THRESHOLD && acc_variance > WHEEL_ACC_VARIANCE_THRESHOLD){ - //TODO：可以考虑清掉速度消除积分带来的漂移 - tracker->state = SKIING_STATE_ROTATING; - return; - } + // 优先级2：原地旋转 - 特殊的、非滑雪的运动状态 // 条件：角速度很大，同时线性加速度的晃动在一个“中等”范围内。 - if (tracker->state == SKIING_STATE_STATIC && gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && acc_variance < ROTATING_ACC_VARIANCE_THRESHOLD) { - tracker->state = SKIING_STATE_ROTATING; + if (tracker->state == STATIC && gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && acc_variance < ROTATING_ACC_VARIANCE_THRESHOLD) { + tracker->state = ROTATING; return; } // 优先级3：启动滑雪 - “油门” // 条件：有足够大的线性加速度，同时陀螺仪的抖动在一个“合理”（而非“完全静止”）的范围内。 if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD && gyr_variance < SKIING_GYR_VARIANCE_THRESHOLD) { - tracker->state = SKIING_STATE_SKIING; + tracker->state = NO_CONSTANT_SPEED; return; } // 如果不满足任何启动或停止条件，则保持当前状态（滑雪中） // 如果当前是静止或旋转，但没有满足启动条件，则状态会保持，直到满足ZUPT或旋转条件。 +#else + + // 无论当前是什么状态，静止总是最高优先级 + if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) { + tracker->state = STATIC; + // 速度清零，抑制漂移 + 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)); + return; + } + + switch (tracker->state) { + case STATIC: + //不break，会往下执行，判断是否进入非匀速状态 + case ROTATING: // 从静止或原地旋转可以启动 + if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD && gyr_variance < SKIING_GYR_VARIANCE_THRESHOLD) { + tracker->state = NO_CONSTANT_SPEED; + } else if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && acc_variance < ROTATING_ACC_VARIANCE_THRESHOLD) { + tracker->state = ROTATING; + } + 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_SKIING_ACC_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; + } +#endif + } @@ -231,6 +321,8 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d if (!tracker || !acc_g || !gyr_dps || !angle || dt <= 0) { return; } + if(my_skiing_tracker.state == STOP_DETECTION) + return; // --- 数据预处理和缓冲 --- float acc_device_ms2[3]; @@ -251,22 +343,20 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d // --- 更新状态机 --- update_state_machine(tracker, acc_device_ms2, gyr_dps); + // 坐标转换 & 移除重力 + transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world); + tracker->acc_world[2] -= G_ACCELERATION; + + // 对世界坐标系下的加速度进行高通滤波，消除直流偏置和重力残差 + for (int i = 0; i < 3; i++) { + tracker->acc_world_filtered[i] = HPF_ALPHA * (tracker->acc_world_filtered[i] + tracker->acc_world[i] - tracker->acc_world_unfiltered_prev[i]); + tracker->acc_world_unfiltered_prev[i] = tracker->acc_world[i]; + } + // 应用加速度死区，忽略微小抖动和噪声 + float acc_horizontal_mag = sqrtf(tracker->acc_world_filtered[0] * tracker->acc_world_filtered[0] + + tracker->acc_world_filtered[1] * tracker->acc_world_filtered[1]); // --- 根据状态进行计算 --- - if (tracker->state == SKIING_STATE_SKIING) { - // 坐标转换 & 移除重力 - transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world); - tracker->acc_world[2] -= G_ACCELERATION; - - // 对世界坐标系下的加速度进行高通滤波，消除直流偏置和重力残差 - for (int i = 0; i < 3; i++) { - tracker->acc_world_filtered[i] = HPF_ALPHA * (tracker->acc_world_filtered[i] + tracker->acc_world[i] - tracker->acc_world_unfiltered_prev[i]); - tracker->acc_world_unfiltered_prev[i] = tracker->acc_world[i]; - } - - // 应用加速度死区，忽略微小抖动和噪声 - float acc_horizontal_mag = sqrtf(tracker->acc_world_filtered[0] * tracker->acc_world_filtered[0] + - tracker->acc_world_filtered[1] * tracker->acc_world_filtered[1]); - + if (tracker->state == NO_CONSTANT_SPEED) { if (acc_horizontal_mag > ACC_DEAD_ZONE_THRESHOLD) { // 只有当水平加速度足够大时，才进行速度积分 tracker->velocity[0] += tracker->acc_world_filtered[0] * dt; @@ -278,8 +368,22 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d debug2.acc_magnitude = acc_horizontal_mag; #endif // 如果加速度小于阈值，则不更新速度，相当于速度保持不变（或受下一步的阻尼影响而衰减） + + // --- 更新速率和距离 --- + // 只基于水平速度计算速率和距离 + tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] + + tracker->velocity[1] * tracker->velocity[1]); + tracker->distance += tracker->speed * dt; + - } else { + }else if(tracker->state == CONSTANT_SPEED){ //匀速 + #ifdef XTELL_TEST + debug2.acc_magnitude = acc_horizontal_mag; + #endif + //保持上次的速度不变。只更新距离 + tracker->distance += tracker->speed * dt; + + }else{ // 在静止或旋转状态下，速度已经在状态机内部被清零 // 额外增加速度衰减，模拟摩擦力，进一步抑制漂移 tracker->velocity[0] *= SPEED_ATTENUATION; @@ -287,11 +391,7 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d tracker->velocity[2] = 0; // 垂直速度强制归零 } - // --- 更新速率和距离 --- - // 只基于水平速度计算速率和距离 - tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] + - tracker->velocity[1] * tracker->velocity[1]); - tracker->distance += tracker->speed * dt; + } @@ -304,7 +404,7 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d * @return BLE_send_data_t */ BLE_send_data_t sensor_processing_task(signed short * acc_data_buf, signed short * gyr_data_buf) { - static skiing_tracker_t my_skiing_tracker; + static int initialized = 0; static int calibration_done = 0; @@ -316,9 +416,6 @@ BLE_send_data_t sensor_processing_task(signed short * acc_data_buf, signed short const float delta_time = DELTA_TIME + 0.03f; BLE_send_data_t BLE_send_data; - - - if (!initialized) { skiing_tracker_init(&my_skiing_tracker); initialized = 1; diff --git a/apps/earphone/xtell_Sensor/A_hide/8/skiing_tracker.h b/apps/earphone/xtell_Sensor/A_hide/8/skiing_tracker.h index 74f6215..3a6b36c 100644 --- a/apps/earphone/xtell_Sensor/A_hide/8/skiing_tracker.h +++ b/apps/earphone/xtell_Sensor/A_hide/8/skiing_tracker.h @@ -4,11 +4,14 @@ #include "../xtell.h" // 定义滑雪者可能的状态 typedef enum { - SKIING_STATE_STATIC, // 静止或动态稳定 - SKIING_STATE_SKIING, // 正在滑雪 - SKIING_STATE_ROTATING, // 正在原地旋转 (新增) - SKIING_STATE_FALLEN, // 已摔倒 - SKIING_STATE_UNKNOWN // 未知状态 + STATIC, // 静止或动态稳定 + NO_CONSTANT_SPEED, // 正在滑雪，非匀速 + CONSTANT_SPEED, // 正在滑雪，匀速 + ROTATING, // 正在原地旋转 + WHEEL, // 转弯 + FALLEN, // 已摔倒 + STOP_DETECTION, // 停止检测 + UNKNOWN // 未知状态 } skiing_state_t; #define VARIANCE_BUFFER_SIZE 5 // 用于计算方差的数据窗口大小 (5个样本 @ 100Hz = 50ms)，减小延迟，提高实时性 diff --git a/apps/earphone/xtell_Sensor/calculate/read_send_data.c b/apps/earphone/xtell_Sensor/calculate/read_send_data.c new file mode 100644 index 0000000..e69de29 diff --git a/apps/earphone/xtell_Sensor/send_data.c b/apps/earphone/xtell_Sensor/send_data.c index ad4cbe9..80c76e8 100644 --- a/apps/earphone/xtell_Sensor/send_data.c +++ b/apps/earphone/xtell_Sensor/send_data.c @@ -49,7 +49,9 @@ extern void create_process(u16* pid, const char* name, void *priv, void (*func)( // --- 任务ID --- static u16 xtell_i2c_test_id; static u16 collect_data_id; -static u16 send_data_id; +static u16 ble_send_data_id; +static u16 sensor_read_data_id; +static u16 calculate_data_id; // --- 环形缓冲区 --- #define SENSOR_DATA_BUFFER_SIZE 512 @@ -240,41 +242,25 @@ void test(){ } #else -void BLE_send_data(){ - signed short acc_data_buf[3] = {0}; - signed short gyr_data_buf[3] = {0}; - signed short acc_gyro_input[6] = {0}; - float Angle_output[3] = {0}; - SL_SC7U22_RawData_Read(acc_data_buf,gyr_data_buf); - - BLE_send_data = sensor_processing_task(acc_data_buf, gyr_data_buf); - u8 data[50]; - data[0] = 0xBB; - data[1] = 0xBE; - data[2] = 0x01; - data[3] = sizeof(BLE_send_data_t); //后续包的数据长度 - // send_data_to_ble_client(&data,sizeof(BLE_send_data_t)+4); - memcpy(&data[4], &BLE_send_data, sizeof(BLE_send_data_t)); - - static int count = 0; - if(count >=10){ - count = 0; - send_data_to_ble_client(&data,sizeof(BLE_send_data_t)+4); - } - count++; - - memset(&BLE_send_data, 0, sizeof(BLE_send_data_t)); - memset(&data, 0, 50); -} #endif -void gsensor_test(){ - - sys_timer_del(gsensor_id); - +void sensor_read_data(){ + //读取传感器的原始六轴数据放进缓冲区 } +void calculate_data(){ + //从缓冲区取出原始六轴数据，进行计算得到速度和距离 + //计算方面已经实现，直接调用函数就行了 +} + +void BLE_send_data(){ + //蓝牙发送数据：计算得到的速度、距离以及读到的原始六轴数据 +} + + + + void xtell_task_create(void){ @@ -301,11 +287,10 @@ void xtell_task_create(void){ // SkiingTracker_Init(&skiing_data); xlog("SkiingTracker_Init\n"); - // create_process(&gsensor_id, "gsensor",NULL, gsensor_test, 1000); - create_process(&test_id, "test",NULL, test, (int)(DELTA_TIME*1000)); - // while(1){ - // test(); - // os_time_dly((int)(DELTA_TIME*1000)); - // } - + // create_process(&test_id, "test",NULL, test, (int)(DELTA_TIME*1000)); + + create_process(&ble_send_data_id, "send",NULL, ble_send_data, (int)(DELTA_TIME*1000)); + create_process(&sensor_read_data_id, "read",NULL, sensor_read_data, (int)(DELTA_TIME*1000)); + create_process(&calculate_data_id, "calculate",NULL, calculate_data, (int)(DELTA_TIME*1000)); + }