lmx/99_7018_lmx
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

cun

Browse Source
This commit is contained in:
lmx
2025-11-18 17:27:06 +08:00
parent d0d9c0a630
commit ebca849be3
8 changed files with 1123 additions and 165 deletions
Download Patch File Download Diff File Expand all files Collapse all files

541
apps/earphone/xtell_Sensor/calculate/skiing_tracker.c
View File

@ -6,22 +6,86 @@
#include <math.h>
#include <string.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
#define G_ACCELERATION 9.81f
#define DEG_TO_RAD (3.14159265f / 180.0f)
BLE_KS_send_data_t KS_data;
// --- 静止检测 ---
//两个判断是否静止的必要条件：动态零速更新(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;
@ -79,108 +143,56 @@ static void calculate_air_distance(skiing_tracker_t *tracker) {
}
/**
* @brief 将设备坐标系下的加速度转换为世界坐标系，去掉重力分量
*
* @param acc_device
* @param angle
* @param acc_linear_world
*/
static void transform_acc_to_world_frame(const float *acc_device, const float *angle, float *acc_linear_world)
{
// 1. 将输入的角度从度转换为弧度
// angle[0] -> pitch, angle[1] -> roll, angle[2] -> yaw
float pitch_rad = -angle[0] * (M_PI / 180.0f);
float roll_rad = -angle[1] * (M_PI / 180.0f);
float yaw_rad = -angle[2] * (M_PI / 180.0f);
// 2. 预先计算三角函数值，以提高效率
float c_r = cosf(roll_rad);
float s_r = sinf(roll_rad);
float c_p = cosf(pitch_rad);
float s_p = sinf(pitch_rad);
float c_y = cosf(yaw_rad);
float s_y = sinf(yaw_rad);
// 3. 构建从设备坐标系到世界坐标系的旋转矩阵 R_device_to_world
// 该矩阵基于 Z-Y-X (Yaw-Pitch-Roll) 欧拉角顺序
// R = R_z(yaw) * R_y(pitch) * R_x(roll)
float R[3][3];
R[0][0] = c_y * c_p;
R[0][1] = c_y * s_p * s_r - s_y * c_r;
R[0][2] = c_y * s_p * c_r + s_y * s_r;
R[1][0] = s_y * c_p;
R[1][1] = s_y * s_p * s_r + c_y * c_r;
R[1][2] = s_y * s_p * c_r - c_y * s_r;
R[2][0] = -s_p;
R[2][1] = c_p * s_r;
R[2][2] = c_p * c_r;
// 4. 将设备坐标系的加速度计总读数旋转到世界坐标系
// a_raw_world = R * acc_device
float ax_raw_world = R[0][0] * acc_device[0] + R[0][1] * acc_device[1] + R[0][2] * acc_device[2];
float ay_raw_world = R[1][0] * acc_device[0] + R[1][1] * acc_device[1] + R[1][2] * acc_device[2];
float az_raw_world = R[2][0] * acc_device[0] + R[2][1] * acc_device[1] + R[2][2] * acc_device[2];
// 5. 在世界坐标系中减去重力分量，得到线性加速度
// 假设世界坐标系Z轴垂直向上，重力矢量为 [0, 0, -g]
// 线性加速度 = 总加速度 - 重力加速度
// az_linear = az_raw_world - (-g) = az_raw_world + g (如果Z轴向上)
// az_linear = az_raw_world - (+g) = az_raw_world - g (如果Z轴向下)
// 这里我们采用 Z 轴向上的标准惯性系 (ENU)
acc_linear_world[0] = ax_raw_world;
acc_linear_world[1] = ay_raw_world;
acc_linear_world[2] = az_raw_world - G_ACCELERATION; // Z轴向上，重力为正值，所以减去
}
/**
* @brief 在设备坐标系下，从原始加速度数据中移除重力分量
* @brief 使用四元数直接从设备坐标系的加速度中移除重力分量
* @details 这种方法比使用欧拉角更精确、更稳定，且避免了万向节死锁。
* @param acc_device 输入：设备坐标系下的原始加速度 [x, y, z], 单位 m/s^2
* @param angle 输入：姿态角 [pitch, roll, yaw]，单位: 度
* @param q 输入：表示姿态的四元数 [w, x, y, z]
* @param acc_linear_device 输出：设备坐标系下移除重力后的线性加速度 [x, y, z]
*/
void remove_gravity_in_device_frame(const float *acc_device, const float *angle, float *acc_linear_device)
void q_remove_gravity_with_quaternion(const float *acc_device, const float *q, float *acc_linear_device)
{
float pitch = -angle[0] * DEG_TO_RAD; // 绕 Y 轴
float roll = -angle[1] * DEG_TO_RAD; // 绕 X 轴
float yaw = -angle[2] * DEG_TO_RAD; // 绕 Z 轴
// 从四元数计算重力在设备坐标系下的投影
// 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];
float cp = cosf(pitch);
float sp = sinf(pitch);
float cr = cosf(roll);
float sr = sinf(roll);
float cy = cosf(yaw);
float sy = sinf(yaw);
// 世界坐标系下的重力矢量
const float g_world[3] = {0.0f, 0.0f, G_ACCELERATION};
// 计算旋转矩阵 R 的转置矩阵 R_transpose
// R (Z-Y-X) =
// [ cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr]
// [ sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr]
// [ -sp, cp*sr, cp*cr ]
//
// R_transpose =
// [ cy*cp, sy*cp, -sp ]
// [ cy*sp*sr - sy*cr, sy*sp*sr + cy*cr, cp*sr ]
// [ cy*sp*cr + sy*sr, sy*sp*cr - cy*sr, cp*cr ]
// 计算重力在设备坐标系下的投影 G_device = R_transpose * G_world
// 由于 G_world 只有 z 分量，计算可以简化
float g_device[3];
g_device[0] = (-sp) * g_world[2];
g_device[1] = (cp * sr) * g_world[2];
g_device[2] = (cp * cr) * g_world[2];
// 从原始设备加速度中减去重力投影
acc_linear_device[0] = acc_device[0] - g_device[0];
acc_linear_device[1] = acc_device[1] - g_device[1];
acc_linear_device[2] = acc_device[2] - g_device[2];
// 从原始加速度中减去重力分量
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 计算缓冲区内三轴数据的方差之和
*
@ -216,7 +228,170 @@ static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
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;
}
}
/**
@ -242,36 +417,100 @@ 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;
#if 1 //测试禁止状态下陀螺仪的三轴加速度，去掉重力分量后是否正常
float tmp_device_acc[3];
float tmp_world_acc[3];
extern void remove_gravity_in_device_frame(const float *acc_device, const float *angle, float *acc_linear_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);
// 计算处理后加速度的水平模长
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]);
// 将最新数据存入缓冲区
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));
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("===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]),tmp_world_acc; //去掉重力加速度
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]);
count = 0;
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:
q_transform_to_world_with_quaternion(acc_device_ms2, quaternion_data, tracker->acc_no_g);
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;
}
count++;
#endif
}
@ -285,12 +524,16 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d
* @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) {
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;
@ -339,21 +582,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;
}

4
apps/earphone/xtell_Sensor/calculate/skiing_tracker.h
View File

@ -30,7 +30,7 @@ typedef struct {
skiing_state_t state; // 当前滑雪状态
// 内部计算使用的私有成员
float acc_world[3]; // 在世界坐标系下的加速度
float acc_no_g[3]; // 去掉重力分量后的加速度
// 用于空中距离计算
float time_in_air; // 滞空时间计时器
@ -84,5 +84,5 @@ typedef struct{
*/
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) ;
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