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

feat: Add rfid feature and .gitignore file

Browse Source
This commit is contained in:
lmx
2025-11-28 16:25:35 +08:00
parent 818e8c3778
commit ade4b0a1f8
1244 changed files with 342105 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

219
apps/earphone/xtell_Sensor/A_hide/1/skiing_tracker.c Normal file
View File

@ -0,0 +1,219 @@
/*
静态ZUPT+卡尔曼，效果貌似还行
*/
#include "skiing_tracker.h"
#include "../sensor/SC7U22.h" // 包含传感器驱动头文件以调用姿态解算函数
#include <math.h>
#include <string.h>
#define G_ACCELERATION 9.81f // 重力加速度 (m/s^2)
#define DEG_TO_RAD (3.14159265f / 180.0f)
// --- 状态检测阈值 ---
// 判断是否静止的加速度阈值 (m/s^2)。当加速度的模长减去重力后，小于此值，则认为可能静止。
#define STATIC_ACC_THRESHOLD 1.0f
// 连续多少帧满足静止条件才确认为静止状态
#define STATIC_FRAMES_REQUIRED 50 // 累加，超过这个数加速度仍变化不大，说明近似静止
/**
* @brief 初始化滑雪追踪器
*/
void skiing_tracker_init(skiing_tracker_t *tracker)
{
if (!tracker) {
return;
}
memset(tracker, 0, sizeof(skiing_tracker_t));
tracker->state = SKIING_STATE_STATIC; // 初始状态为静止
}
/**
* @brief 将设备坐标系下的加速度转换为世界坐标系
* @param acc_device 设备坐标系下的加速度 [x, y, z]
* @param angle 姿态角 [pitch, roll, yaw]，单位: 度
* @param acc_world 输出：世界坐标系下的加速度 [x, y, z]
*/
static void transform_acc_to_world_frame(const float *acc_device, const float *angle, float *acc_world)
{
// 修正#1：驱动输出的角度与标准航空定义相反，需要取反才能用于标准旋转矩阵。
float pitch = -angle[0] * DEG_TO_RAD;
float roll = -angle[1] * DEG_TO_RAD;
float cp = cosf(pitch);
float sp = sinf(pitch);
float cr = cosf(roll);
float sr = sinf(roll);
float ax = acc_device[0];
float ay = acc_device[1];
float az = acc_device[2];
// 修正#2：使用经过验证的、正确的身体坐标系到世界坐标系的旋转矩阵 (基于 Y-X 旋转顺序)
// 这个矩阵将设备测量的加速度(ax, ay, az)正确地转换到世界坐标系(acc_world)。
acc_world[0] = cp * ax + sp * sr * ay + sp * cr * az;
acc_world[1] = 0 * ax + cr * ay - sr * az;
acc_world[2] = -sp * ax + cp * sr * ay + cp * cr * az;
}
/**
* @brief 更新滑雪状态机
*/
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device)
{
// 计算当前加速度的模长
float acc_magnitude = sqrtf(acc_device[0] * acc_device[0] + acc_device[1] * acc_device[1] + acc_device[2] * acc_device[2]);
// 状态判断逻辑
switch (tracker->state) {
case SKIING_STATE_STATIC:
// 如果加速度变化较大，则切换到滑雪状态
if (fabsf(acc_magnitude - G_ACCELERATION) > STATIC_ACC_THRESHOLD * 2.0f) { // 使用一个更大的阈值来启动
tracker->state = SKIING_STATE_SKIING;
tracker->static_frames_count = 0;
}
break;
case SKIING_STATE_SKIING:
// 检测是否进入静止状态 (零速更新 ZUPT)
if (fabsf(acc_magnitude - G_ACCELERATION) < STATIC_ACC_THRESHOLD) {
tracker->static_frames_count++;
if (tracker->static_frames_count >= STATIC_FRAMES_REQUIRED) {
tracker->state = SKIING_STATE_STATIC;
// 进入静止状态，强制将速度清零以消除漂移
memset(tracker->velocity, 0, sizeof(tracker->velocity));
tracker->speed = 0.0f;
}
} else {
// 如果在运动，则重置静止计数器
tracker->static_frames_count = 0;
}
break;
// 可以在此添加摔倒等其他状态的判断
case SKIING_STATE_FALLEN:
// TODO: 添加从摔倒状态恢复的逻辑
break;
default:
tracker->state = SKIING_STATE_UNKNOWN;
break;
}
}
/**
* @brief 主更新函数
*/
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc, float *angle, float dt)
{
if (!tracker || !acc || !angle || dt <= 0) {
return;
}
// 将原始g单位的加速度转换为 m/s^2
float acc_device_ms2[3];
acc_device_ms2[0] = acc[0] * G_ACCELERATION;
acc_device_ms2[1] = acc[1] * G_ACCELERATION;
acc_device_ms2[2] = acc[2] * G_ACCELERATION;
// 更新状态机
update_state_machine(tracker, acc_device_ms2);
// 只有在滑雪状态下才进行计算
if (tracker->state == SKIING_STATE_SKIING) {
// 坐标系转换: 首先，利用姿态角(Pitch, Roll)将传感器测得的总加速度（运动加速度+重力）
// 从不断变化的“设备坐标系”转换到一个固定的“世界坐标系”。在这个世界坐标系里，Z轴永远垂直于地面指向上方。
// 执行坐标系转换
transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world);
// 转换完成后，重力就变成了一个恒定的、方向沿Z轴向下的矢量。
// 修正#3：经过正确的坐标转换后，静止时重力在世界坐标系Z轴上的分量精确为+g。
// 必须减去这个g，才能得到纯粹的运动加速度。
tracker->acc_world[2] -= G_ACCELERATION;
// 积分计算速度 (v = v0 + a*t)
tracker->velocity[0] += tracker->acc_world[0] * dt;
tracker->velocity[1] += tracker->acc_world[1] * dt;
tracker->velocity[2] += tracker->acc_world[2] * dt; // 垂直方向速度也计算在内
// 计算当前速率
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
tracker->velocity[1] * tracker->velocity[1] +
tracker->velocity[2] * tracker->velocity[2]);
// 积分计算距离 (d = d0 + v*t)
tracker->distance += tracker->speed * dt;
}
}
// 传感器数据采集与处理任务
void sensor_processing_task(signed short * acc_data_buf,signed short * gyr_data_buf) {
// --- 1. 定义静态变量 ---
static skiing_tracker_t my_skiing_tracker;
static int initialized = 0;
static int calibration_done = 0; // 新增：用于标记一次性静态校准是否完成
static signed short combined_raw_data[6];
static float final_angle_data[3];
static float calibrated_acc_g[3];
// sensor_processing_task的调用频率, dt = 0.001s
const float delta_time = 0.01f;
// --- 2. 初始化 ---
if (!initialized) {
skiing_tracker_init(&my_skiing_tracker);
initialized = 1;
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
}
// --- 3. 数据处理 ---
//合并加速度和陀螺仪数据
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
unsigned char status;
if (get_calibration_state() == 0) { //正在校准
//领票校准
status = SL_SC7U22_Angle_Output(1, combined_raw_data, final_angle_data, 0);
if (status == 1) {
calibration_done = 1;
printf("Sensor calibration successful! Skiing mode is active.\n");
}
} else {
status = SL_SC7U22_Angle_Output(0, combined_raw_data, final_angle_data, 0);
}
// c. 检查姿态解算是否成功
if (status == 1) {
// 将校准后的加速度数据从 LSB (原始值) 转换为 g (重力单位)
// ±8g量程下，8192 LSB 对应 1g
calibrated_acc_g[0] = (float)combined_raw_data[0] / 8192.0f;
calibrated_acc_g[1] = (float)combined_raw_data[1] / 8192.0f;
calibrated_acc_g[2] = (float)combined_raw_data[2] / 8192.0f;
skiing_tracker_update(&my_skiing_tracker, calibrated_acc_g, final_angle_data, delta_time);
static count = 0;
if(count < 10){
count++;
return;
}else{
count = 0;
}
printf("State: %d, Speed: %.2f m/s, Distance: %.2f m\n",
my_skiing_tracker.state,
my_skiing_tracker.speed,
my_skiing_tracker.distance);
} else if (status == 0) {
// 传感器正在进行静态校准
// printf("Sensor is calibrating...\n");
} else {
// status == 2, 表示校准失败或发生错误
// printf("Angle calculation error or calibration not finished.\n");
}
}

41
apps/earphone/xtell_Sensor/A_hide/1/skiing_tracker.h Normal file
View File

@ -0,0 +1,41 @@
#ifndef SKIING_TRACKER_H
#define SKIING_TRACKER_H
// 定义滑雪者可能的状态
typedef enum {
SKIING_STATE_STATIC, // 静止
SKIING_STATE_SKIING, // 正在滑雪
SKIING_STATE_FALLEN, // 已摔倒
SKIING_STATE_UNKNOWN // 未知状态
} skiing_state_t;
// 追踪器数据结构体
typedef struct {
float velocity[3]; // 当前速度 (x, y, z)，单位: m/s
float distance; // 总滑行距离，单位: m
float speed; // 当前速率 (标量)，单位: m/s
skiing_state_t state; // 当前滑雪状态
// 私有成员，用于内部计算
int static_frames_count; // 用于判断静止状态的帧计数器
float acc_world[3]; // 在世界坐标系下的加速度
} skiing_tracker_t;
/**
* @brief 初始化滑雪追踪器
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
*/
void skiing_tracker_init(skiing_tracker_t *tracker);
/**
* @brief 处理传感器数据并更新滑雪状态
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
* @param acc 校准后的加速度数据 [x, y, z]，单位: g (1g = 9.8m/s^2)
* @param angle 姿态角数据 [pitch, roll, yaw]，单位: 度
* @param dt 采样时间间隔，单位: 秒 (s)
*/
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc, float *angle, float dt);
#endif // SKIING_TRACKER_H

596
apps/earphone/xtell_Sensor/A_hide/10/skiing_tracker.c Normal file
View File

@ -0,0 +1,596 @@
/*
*/
#include "skiing_tracker.h"
#include "../sensor/SC7U22.h"
#include <math.h>
#include <string.h>
#define G_ACCELERATION 9.81f
#define DEG_TO_RAD (3.14159265f / 180.0f)
// --- 静止检测 ---
//两个判断是否静止的必要条件：动态零速更新(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;
#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 在设备坐标系下，从原始加速度数据中移除重力分量
* @param acc_device 输入：设备坐标系下的原始加速度 [x, y, z], 单位 m/s^2
* @param angle 输入：姿态角 [pitch, roll, yaw]，单位: 度
* @param acc_linear_device 输出：设备坐标系下移除重力后的线性加速度 [x, y, z]
*/
void remove_gravity_in_device_frame(const float *acc_device, const float *angle, 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 轴
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];
}
/**
* @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 && 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:
remove_gravity_in_device_frame(acc_device_ms2, angle, 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;
}
}
/**
* @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) {
static int initialized = 0;
static float acc_data_g[3];
static float gyr_data_dps[3];
// 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;
}

88
apps/earphone/xtell_Sensor/A_hide/10/skiing_tracker.h Normal file
View File

@ -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) ;
#endif // SKIING_TRACKER_H

377
apps/earphone/xtell_Sensor/A_hide/11_test/skiing_tracker.c Normal file
View File

@ -0,0 +1,377 @@
/*
使用四元数求角度和去掉重力分量
*/
#include "skiing_tracker.h"
#include "../sensor/SC7U22.h"
#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;
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 将设备坐标系下的加速度转换为世界坐标系，去掉重力分量
*
* @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 在设备坐标系下，从原始加速度数据中移除重力分量
* @param acc_device 输入：设备坐标系下的原始加速度 [x, y, z], 单位 m/s^2
* @param angle 输入：姿态角 [pitch, roll, yaw]，单位: 度
* @param acc_linear_device 输出：设备坐标系下移除重力后的线性加速度 [x, y, z]
*/
void remove_gravity_in_device_frame(const float *acc_device, const float *angle, 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 轴
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];
}
/**
* @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 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;
#if 1 //测试禁止状态下陀螺仪的三轴加速度，去掉重力分量后是否正常
float tmp_device_acc[3];
float tmp_world_acc[3];
// 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_remove_gravity_with_quaternion(acc_device_ms2,quaternion_data,tmp_device_acc);
q_transform_to_world_with_quaternion(acc_device_ms2,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]);
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],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]);
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;
}

88
apps/earphone/xtell_Sensor/A_hide/11_test/skiing_tracker.h Normal file
View File

@ -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_world[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

259
apps/earphone/xtell_Sensor/A_hide/2/skiing_tracker.c Normal file
View File

@ -0,0 +1,259 @@
/*
动态ZUPT+卡尔曼
*/
#include "skiing_tracker.h"
#include "../sensor/SC7U22.h"
#include <math.h>
#include <string.h>
#define G_ACCELERATION 9.81f
#define DEG_TO_RAD (3.14159265f / 180.0f)
// --- 算法阈值定义 ---
// 动态零速更新(ZUPT)阈值：加速度方差小于此值，认为处于动态稳定状态
#define ZUPT_ACC_VARIANCE_THRESHOLD 0.05f
// 旋转检测阈值：Z轴角速度大于此值(度/秒)，认为正在原地旋转
#define ROTATION_GYR_Z_THRESHOLD 60.0f
// 启动滑雪阈值：加速度模长与重力的差值大于此值，认为开始运动
#define START_SKIING_ACC_THRESHOLD 1.5f
/**
* @brief 初始化滑雪追踪器
*/
void skiing_tracker_init(skiing_tracker_t *tracker)
{
if (!tracker) {
return;
}
// 使用memset一次性清零整个结构体，包括新增的缓冲区
memset(tracker, 0, sizeof(skiing_tracker_t));
tracker->state = SKIING_STATE_STATIC;
}
/**
* @brief 将设备坐标系下的加速度转换为世界坐标系
* @param acc_device 设备坐标系下的加速度 [x, y, z]
* @param angle 姿态角 [pitch, roll, yaw]，单位: 度
* @param acc_world 输出：世界坐标系下的加速度 [x, y, z]
*/
static void transform_acc_to_world_frame(const float *acc_device, const float *angle, float *acc_world)
{
// 驱动输出的角度与标准航空定义相反，需要取反才能用于标准旋转矩阵。
float pitch = -angle[0] * DEG_TO_RAD;
float roll = -angle[1] * DEG_TO_RAD;
float cp = cosf(pitch);
float sp = sinf(pitch);
float cr = cosf(roll);
float sr = sinf(roll);
float ax = acc_device[0];
float ay = acc_device[1];
float az = acc_device[2];
// 使用经过验证的、正确的身体坐标系到世界坐标系的旋转矩阵 (基于 Y-X 旋转顺序)
// 这个矩阵将设备测量的加速度(ax, ay, az)正确地转换到世界坐标系(acc_world)。
acc_world[0] = cp * ax + sp * sr * ay + sp * cr * az;
acc_world[1] = 0 * ax + cr * ay - sr * az;
acc_world[2] = -sp * ax + cp * sr * ay + cp * cr * az;
}
/**
* @brief 计算缓冲区内三轴数据的方差之和
*/
static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
{
float mean[3] = {0};
float variance[3] = {0};
// 1. 计算均值
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;
// 2. 计算方差
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 升级后的状态机，包含旋转检测和动态零速更新
*/
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps)
{
// 缓冲区未填满时，不进行状态判断，默认为静止
if (!tracker->buffer_filled) {
tracker->state = SKIING_STATE_STATIC;
return;
}
// --- 计算关键指标 ---
float acc_variance = calculate_variance(tracker->acc_buffer);
float gyr_z_abs = fabsf(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]);
// --- 状态切换逻辑 (按优先级) ---
// 旋转检测
// 如果Z轴角速度很大，则判断为原地旋转，暂停积分
if (gyr_z_abs > ROTATION_GYR_Z_THRESHOLD) {
tracker->state = SKIING_STATE_ROTATING;
return; // 直接返回，不执行后续判断
}
// 动态零速更新 (ZUPT)
// 如果加速度在窗口期内非常稳定（方差很小），则认为是动态稳定状态，进行零速校正
if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD) {
tracker->state = SKIING_STATE_STATIC;
// 速度清零，抑制漂移
memset(tracker->velocity, 0, sizeof(tracker->velocity));
tracker->speed = 0.0f;
return;
}
// 从静止/旋转状态启动
if (tracker->state == SKIING_STATE_STATIC || tracker->state == SKIING_STATE_ROTATING) {
if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD) {
tracker->state = SKIING_STATE_SKIING;
return;
}
}
// 滑雪
if (tracker->state != SKIING_STATE_STATIC) {
tracker->state = SKIING_STATE_SKIING;
}
}
/**
* @brief 主更新函数
*/
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;
}
// --- 1. 数据预处理和缓冲 ---
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; // 标记缓冲区已满
}
// --- 2. 更新状态机 ---
update_state_machine(tracker, acc_device_ms2, gyr_dps);
// --- 3. 根据状态进行计算 ---
// 只有在明确的“滑雪”状态下才进行积分
if (tracker->state == SKIING_STATE_SKIING) {
transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world);
tracker->acc_world[2] -= G_ACCELERATION;
tracker->velocity[0] += tracker->acc_world[0] * dt;
tracker->velocity[1] += tracker->acc_world[1] * dt;
tracker->velocity[2] += tracker->acc_world[2] * dt;
}
// 在其他状态下（静止、旋转），速度已经在状态机内部被清零或保持不变
// --- 4. 更新速率和距离 ---
// 速率和距离总是在更新，但在非滑雪状态下，速度为0，所以它们不会增加
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
tracker->velocity[1] * tracker->velocity[1] +
tracker->velocity[2] * tracker->velocity[2]);
tracker->distance += tracker->speed * dt;
}
// 传感器数据采集与处理任务
void 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;
static signed short combined_raw_data[6];
static float final_angle_data[3];
static float calibrated_acc_g[3];
static float calibrated_gyr_dps[3]; // 新增：用于存放转换后的陀螺仪数据
const float delta_time = 0.01f;
if (!initialized) {
skiing_tracker_init(&my_skiing_tracker);
initialized = 1;
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
}
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
unsigned char status;
if (!calibration_done) {
status = SL_SC7U22_Angle_Output(1, combined_raw_data, final_angle_data, 0);
if (status == 1) {
calibration_done = 1;
printf("Sensor calibration successful! Skiing mode is active.\n");
}
} else {
status = SL_SC7U22_Angle_Output(0, combined_raw_data, final_angle_data, 0);
}
if (status == 1) {
// 加速度 LSB to g
calibrated_acc_g[0] = (float)combined_raw_data[0] / 8192.0f;
calibrated_acc_g[1] = (float)combined_raw_data[1] / 8192.0f;
calibrated_acc_g[2] = (float)combined_raw_data[2] / 8192.0f;
// 陀螺仪 LSB to dps (度/秒)
// ±2000dps量程下，转换系数约为 0.061
calibrated_gyr_dps[0] = (float)combined_raw_data[3] * 0.061f;
calibrated_gyr_dps[1] = (float)combined_raw_data[4] * 0.061f;
calibrated_gyr_dps[2] = (float)combined_raw_data[5] * 0.061f;
skiing_tracker_update(&my_skiing_tracker, calibrated_acc_g, calibrated_gyr_dps, final_angle_data, delta_time);
// 打印逻辑保持不变
static int count = 0;
if(count < 10){
count++;
return;
} else {
count = 0;
}
printf("State: %d, Speed: %.2f m/s, Distance: %.2f m\n",
my_skiing_tracker.state,
my_skiing_tracker.speed,
my_skiing_tracker.distance);
} else if (status == 0) {
// printf("Sensor is calibrating...\n");
} else {
// printf("Angle calculation error or calibration not finished.\n");
}
}

50
apps/earphone/xtell_Sensor/A_hide/2/skiing_tracker.h Normal file
View File

@ -0,0 +1,50 @@
#ifndef SKIING_TRACKER_H
#define SKIING_TRACKER_H
// 定义滑雪者可能的状态
typedef enum {
SKIING_STATE_STATIC, // 静止或动态稳定
SKIING_STATE_SKIING, // 正在滑雪
SKIING_STATE_ROTATING, // 正在原地旋转 (新增)
SKIING_STATE_FALLEN, // 已摔倒
SKIING_STATE_UNKNOWN // 未知状态
} skiing_state_t;
#define VARIANCE_BUFFER_SIZE 15 // 用于计算方差的数据窗口大小 (15个样本 @ 100Hz = 150ms)
// 追踪器数据结构体
typedef struct {
// 公开数据
float velocity[3]; // 当前速度 (x, y, z)，单位: m/s
float distance; // 总滑行距离，单位: m
float speed; // 当前速率 (标量)，单位: m/s
skiing_state_t state; // 当前滑雪状态
// 内部计算使用的私有成员
float acc_world[3]; // 在世界坐标系下的加速度
// 用于动态零速更新和旋转检测的缓冲区
float acc_buffer[VARIANCE_BUFFER_SIZE][3]; // 加速度数据窗口
float gyr_buffer[VARIANCE_BUFFER_SIZE][3]; // 角速度数据窗口
int buffer_index; // 缓冲区当前索引
int buffer_filled; // 缓冲区是否已填满的标志
} skiing_tracker_t;
/**
* @brief 初始化滑雪追踪器
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
*/
void skiing_tracker_init(skiing_tracker_t *tracker);
/**
* @brief 处理传感器数据并更新滑雪状态
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
* @param acc_g 校准后的加速度数据 [x, y, z]，单位: g (1g = 9.8m/s^2)
* @param gyr_dps 角速度
* @param angle 姿态角数据 [pitch, roll, yaw]，单位: 度
* @param dt 采样时间间隔，单位: 秒 (s)
*/
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt);
#endif // SKIING_TRACKER_H

277
apps/earphone/xtell_Sensor/A_hide/3/skiing_tracker.c Normal file
View File

@ -0,0 +1,277 @@
/*
动态ZUPT+卡尔曼
*/
#include "skiing_tracker.h"
#include "../sensor/SC7U22.h"
#include <math.h>
#include <string.h>
#define G_ACCELERATION 9.81f
#define DEG_TO_RAD (3.14159265f / 180.0f)
// --- 算法阈值定义 ---
//两个判断是否静止的必要条件
// 动态零速更新(ZUPT)阈值
// 提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
#define ZUPT_ACC_VARIANCE_THRESHOLD 0.2f
// 陀螺仪方差阈值
#define ZUPT_GYR_VARIANCE_THRESHOLD 5.0f
// 旋转/摆动检测阈值：角速度总模长大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
#define ROTATION_GYR_MAG_THRESHOLD 45.0f
// 启动滑雪阈值：加速度模长与重力的差值大于此值，认为开始运动
// 降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
#define START_SKIING_ACC_THRESHOLD 0.5f
// 新增：速度阻尼系数，用于模拟摩擦力，抑制漂移
#define VELOCITY_DAMPING_FACTOR 0.98f
/**
* @brief 初始化滑雪追踪器
*/
void skiing_tracker_init(skiing_tracker_t *tracker)
{
if (!tracker) {
return;
}
// 使用memset一次性清零整个结构体，包括新增的缓冲区
memset(tracker, 0, sizeof(skiing_tracker_t));
tracker->state = SKIING_STATE_STATIC;
}
/**
* @brief 将设备坐标系下的加速度转换为世界坐标系
* @param acc_device 设备坐标系下的加速度 [x, y, z]
* @param angle 姿态角 [pitch, roll, yaw]，单位: 度
* @param acc_world 输出：世界坐标系下的加速度 [x, y, z]
*/
static void transform_acc_to_world_frame(const float *acc_device, const float *angle, float *acc_world)
{
// 驱动输出的角度与标准航空定义相反，需要取反才能用于标准旋转矩阵。
float pitch = -angle[0] * DEG_TO_RAD;
float roll = -angle[1] * DEG_TO_RAD;
float cp = cosf(pitch);
float sp = sinf(pitch);
float cr = cosf(roll);
float sr = sinf(roll);
float ax = acc_device[0];
float ay = acc_device[1];
float az = acc_device[2];
// 使用经过验证的、正确的身体坐标系到世界坐标系的旋转矩阵 (基于 Y-X 旋转顺序)
// 这个矩阵将设备测量的加速度(ax, ay, az)正确地转换到世界坐标系(acc_world)。
acc_world[0] = cp * ax + sp * sr * ay + sp * cr * az;
acc_world[1] = 0 * ax + cr * ay - sr * az;
acc_world[2] = -sp * ax + cp * sr * ay + cp * cr * az;
}
/**
* @brief 计算缓冲区内三轴数据的方差之和
*/
static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
{
float mean[3] = {0};
float variance[3] = {0};
// 1. 计算均值
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;
// 2. 计算方差
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 升级后的状态机，包含旋转检测和动态零速更新
*/
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps)
{
// 缓冲区未填满时，不进行状态判断，默认为静止
if (!tracker->buffer_filled) {
tracker->state = SKIING_STATE_STATIC;
return;
}
// --- 1. 计算关键指标 ---
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]);
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]);
// --- 2. 状态切换逻辑 (按优先级) ---
// 优先级1：原地旋转/摆动检测 (最终版)
// 增加一个关键前提：只在当前不处于滑雪状态时，才检测原地旋转。
// 这可以防止滑雪过程中的高速转弯被误判为原地旋转。
if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && tracker->state != SKIING_STATE_SKIING) {
tracker->state = SKIING_STATE_ROTATING;
return;
}
// 动态零速更新 (ZUPT)
// 必须同时满足加速度和角速度都稳定，才能判断为“真静止”，以区分匀速运动
if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
tracker->state = SKIING_STATE_STATIC;
// 速度清零，抑制漂移
memset(tracker->velocity, 0, sizeof(tracker->velocity));
tracker->speed = 0.0f;
return;
}
// 从静止/旋转状态启动
if (tracker->state == SKIING_STATE_STATIC || tracker->state == SKIING_STATE_ROTATING) {
// 最终版启动逻辑：必须同时满足“有足够大的线性加速度”和“旋转稳定”两个条件
if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
tracker->state = SKIING_STATE_SKIING;
return;
}
}
// 滑雪
if (tracker->state != SKIING_STATE_STATIC) {
tracker->state = SKIING_STATE_SKIING;
}
}
/**
* @brief 主更新函数
*/
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;
}
// --- 1. 数据预处理和缓冲 ---
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; // 标记缓冲区已满
}
// --- 2. 更新状态机 ---
update_state_machine(tracker, acc_device_ms2, gyr_dps);
// --- 3. 根据状态进行计算 ---
// 只有在明确的“滑雪”状态下才进行积分
if (tracker->state == SKIING_STATE_SKIING) {
transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world);
tracker->acc_world[2] -= G_ACCELERATION;
tracker->velocity[0] += tracker->acc_world[0] * dt;
tracker->velocity[1] += tracker->acc_world[1] * dt;
tracker->velocity[2] += tracker->acc_world[2] * dt;
}
// 在其他状态下（静止、旋转），速度已经在状态机内部被清零或保持不变
// --- 4. 引入速度阻尼（软件摩擦力） ---
// 只要不处于滑雪状态，就对速度进行衰减，以对抗漂移和抑制抖动产生的微小速度
if (tracker->state != SKIING_STATE_SKIING) {
tracker->velocity[0] *= VELOCITY_DAMPING_FACTOR;
tracker->velocity[1] *= VELOCITY_DAMPING_FACTOR;
tracker->velocity[2] *= VELOCITY_DAMPING_FACTOR;
}
// --- 5. 更新速率和距离 ---
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
tracker->velocity[1] * tracker->velocity[1] +
tracker->velocity[2] * tracker->velocity[2]);
tracker->distance += tracker->speed * dt;
}
// 传感器数据采集与处理任务
void 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;
static signed short combined_raw_data[6];
static float final_angle_data[3]; // 计算得到的欧若拉角
static float calibrated_acc_g[3]; // 转换后的加速度计数据
static float calibrated_gyr_dps[3]; // 转换后的陀螺仪数据
const float delta_time = 0.01f;
if (!initialized) {
skiing_tracker_init(&my_skiing_tracker);
initialized = 1;
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
}
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
unsigned char status;
if (!calibration_done) { //第1次启动，开启零漂检测
status = SL_SC7U22_Angle_Output(1, combined_raw_data, final_angle_data, 0);
if (status == 1) {
calibration_done = 1;
printf("Sensor calibration successful! Skiing mode is active.\n");
}
} else {
status = SL_SC7U22_Angle_Output(0, combined_raw_data, final_angle_data, 0);
}
if (status == 1) {
// 加速度 LSB to g
calibrated_acc_g[0] = (float)combined_raw_data[0] / 8192.0f;
calibrated_acc_g[1] = (float)combined_raw_data[1] / 8192.0f;
calibrated_acc_g[2] = (float)combined_raw_data[2] / 8192.0f;
// 陀螺仪 LSB to dps (度/秒)
// ±2000dps量程下，转换系数约为 0.061
calibrated_gyr_dps[0] = (float)combined_raw_data[3] * 0.061f;
calibrated_gyr_dps[1] = (float)combined_raw_data[4] * 0.061f;
calibrated_gyr_dps[2] = (float)combined_raw_data[5] * 0.061f;
skiing_tracker_update(&my_skiing_tracker, calibrated_acc_g, calibrated_gyr_dps, final_angle_data, delta_time);
// 打印逻辑保持不变
static int count = 0;
if(count < 10){
count++;
return;
} else {
count = 0;
}
printf("State: %d, Speed: %.2f m/s, Distance: %.2f m\n",
my_skiing_tracker.state,
my_skiing_tracker.speed,
my_skiing_tracker.distance);
} else if (status == 0) {
// printf("Sensor is calibrating...\n");
} else {
// printf("Angle calculation error or calibration not finished.\n");
}
}

50
apps/earphone/xtell_Sensor/A_hide/3/skiing_tracker.h Normal file
View File

@ -0,0 +1,50 @@
#ifndef SKIING_TRACKER_H
#define SKIING_TRACKER_H
// 定义滑雪者可能的状态
typedef enum {
SKIING_STATE_STATIC, // 静止或动态稳定
SKIING_STATE_SKIING, // 正在滑雪
SKIING_STATE_ROTATING, // 正在原地旋转 (新增)
SKIING_STATE_FALLEN, // 已摔倒
SKIING_STATE_UNKNOWN // 未知状态
} skiing_state_t;
#define VARIANCE_BUFFER_SIZE 15 // 用于计算方差的数据窗口大小 (15个样本 @ 100Hz = 150ms)
// 追踪器数据结构体
typedef struct {
// 公开数据
float velocity[3]; // 当前速度 (x, y, z)，单位: m/s
float distance; // 总滑行距离，单位: m
float speed; // 当前速率 (标量)，单位: m/s
skiing_state_t state; // 当前滑雪状态
// 内部计算使用的私有成员
float acc_world[3]; // 在世界坐标系下的加速度
// 用于动态零速更新和旋转检测的缓冲区
float acc_buffer[VARIANCE_BUFFER_SIZE][3]; // 加速度数据窗口
float gyr_buffer[VARIANCE_BUFFER_SIZE][3]; // 角速度数据窗口
int buffer_index; // 缓冲区当前索引
int buffer_filled; // 缓冲区是否已填满的标志
} skiing_tracker_t;
/**
* @brief 初始化滑雪追踪器
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
*/
void skiing_tracker_init(skiing_tracker_t *tracker);
/**
* @brief 处理传感器数据并更新滑雪状态
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
* @param acc_g 校准后的加速度数据 [x, y, z]，单位: g (1g = 9.8m/s^2)
* @param gyr_dps 角速度
* @param angle 姿态角数据 [pitch, roll, yaw]，单位: 度
* @param dt 采样时间间隔，单位: 秒 (s)
*/
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt);
#endif // SKIING_TRACKER_H

465
apps/earphone/xtell_Sensor/A_hide/4/skiing_tracker.c Normal file
View File

@ -0,0 +1,465 @@
/**
* 效果不行，对原地转动太灵敏了
*/
#include "skiing_tracker.h"
#include <math.h>
#include <string.h>
// --- 核心算法参数 ---
#define COMPLEMENTARY_FILTER_ALPHA 0.96f // 互补滤波器alpha值 (降低alpha, 增加对加速度计的信任, 更好地抑制陀螺仪漂移)
#define ACC_SENSITIVITY 8192.0f // 加速度计灵敏度 (LSB/g), 对应 ±4g 量程
#define GYRO_SENSITIVITY 16.4f // 陀螺仪灵敏度 (LSB/dps), 对应 ±2000dps 量程
#define SKI_DT_FIXED 0.01f // 固定时间间隔 10ms (100Hz)
// --- 状态机和阈值 ---
#define SKI_STATIC_THRESHOLD_ACC 0.3f // 静止检测加速度阈值 (m/s^2) - 提高以忽略更多噪声
#define SKI_STATIC_THRESHOLD_GYRO 3.0f // 静止检测角速度阈值 (dps) - 更严格
#define SKI_MOVEMENT_THRESHOLD 1.2f // 运动启动阈值 (m/s^2) - 显著提高，需要更明确的运动意图
#define SKI_WINDOW_SIZE 10 // 滑动窗口大小 (当前未使用)
#define SKI_CALIBRATION_SAMPLES 100 // 校准采样点数 (恢复到100，确保校准精度)
// 滑雪数据结构
typedef struct {
// 状态相关
SkiState_t current_state;
uint32_t state_duration; // 状态持续时间(ms)
uint8_t static_counter; // 静态计数器
// --- 核心运动数据 ---
// 姿态数据 (Roll, Pitch)
float attitude[2];
// 世界坐标系下的加速度
float world_acc[3];
// 世界坐标系下的速度
float velocity[3];
// 水平面速度和总距离
float horizontal_speed;
float total_horizontal_distance;
// 历史数据 (用于滤波)
float acc_history[SKI_WINDOW_SIZE][3];
float gyro_history[SKI_WINDOW_SIZE][3];
uint8_t history_index;
// 时间相关
float dt; // 时间间隔
// 运动检测
float movement_score; // 运动评分
float turning_score; // 转弯评分
// 校准数据 (用于陀螺仪零偏)
float gyro_bias[3];
float acc_bias[3];
float calibration_sum[6];
uint32_t calibration_count;
uint8_t calibrated;
// 状态标志和计数器
uint32_t update_count;
float velocity_decay_factor;
uint8_t is_moving;
} SkiData_t;
static SkiData_t ski_data = {0};
// --- 内部核心函数声明 ---
// 校准
static void Ski_CalibrateSensor(float *acc, float *gyro); // 校准传感器
// 姿态解算
static void Ski_UpdateAttitude(float *acc, float *gyro); // 更新姿态
// 坐标系转换
static void Ski_TransformToWorldFrame(float *acc, float *world_acc); // 转换到世界坐标系
// 速度与距离积分
static void Ski_UpdateVelocityAndDistance(void); // 更新速度和距离
// --- 辅助函数声明 ---
static float Ski_CalculateMovementScore(void); // 计算运动评分
static float Ski_CalculateTurningScore(float *gyro);
static void Ski_UpdateStateMachine(void);
static void Ski_MovingAverageFilter(float *new_acc, float *new_gyro, float *acc_filtered, float *gyro_filtered);
// 修改初始化函数
void Ski_Init(void)
{
memset(&ski_data, 0, sizeof(SkiData_t));
ski_data.current_state = SKI_STATE_STATIC;
// 初始化参数
ski_data.velocity_decay_factor = 0.8f; // 速度衰减因子
ski_data.is_moving = 0;
ski_data.update_count = 0;
ski_data.dt = SKI_DT_FIXED;
}
// 滑动窗口滤波
static void Ski_MovingAverageFilter(float *new_acc, float *new_gyro,
float *acc_filtered, float *gyro_filtered)
{
// 更新历史数据
ski_data.acc_history[ski_data.history_index][0] = new_acc[0];
ski_data.acc_history[ski_data.history_index][1] = new_acc[1];
ski_data.acc_history[ski_data.history_index][2] = new_acc[2];
ski_data.gyro_history[ski_data.history_index][0] = new_gyro[0];
ski_data.gyro_history[ski_data.history_index][1] = new_gyro[1];
ski_data.gyro_history[ski_data.history_index][2] = new_gyro[2];
ski_data.history_index = (ski_data.history_index + 1) % SKI_WINDOW_SIZE;
// 计算平均值
memset(acc_filtered, 0, 3 * sizeof(float));
memset(gyro_filtered, 0, 3 * sizeof(float));
for (int i = 0; i < SKI_WINDOW_SIZE; i++) {
acc_filtered[0] += ski_data.acc_history[i][0];
acc_filtered[1] += ski_data.acc_history[i][1];
acc_filtered[2] += ski_data.acc_history[i][2];
gyro_filtered[0] += ski_data.gyro_history[i][0];
gyro_filtered[1] += ski_data.gyro_history[i][1];
gyro_filtered[2] += ski_data.gyro_history[i][2];
}
acc_filtered[0] /= SKI_WINDOW_SIZE;
acc_filtered[1] /= SKI_WINDOW_SIZE;
acc_filtered[2] /= SKI_WINDOW_SIZE;
gyro_filtered[0] /= SKI_WINDOW_SIZE;
gyro_filtered[1] /= SKI_WINDOW_SIZE;
gyro_filtered[2] /= SKI_WINDOW_SIZE;
}
// =================================================================================
// 核心更新函数 - 已重构
// =================================================================================
void Ski_UpdateData(int16_t acc_x, int16_t acc_y, int16_t acc_z,
int16_t gyro_x, int16_t gyro_y, int16_t gyro_z)
{
ski_data.update_count++;
// 1. LSB原始数据转换为物理单位 (g 和 度/秒)
float acc[3] = { (float)acc_x / ACC_SENSITIVITY, (float)acc_y / ACC_SENSITIVITY, (float)acc_z / ACC_SENSITIVITY };
float gyro[3] = { (float)gyro_x / GYRO_SENSITIVITY, (float)gyro_y / GYRO_SENSITIVITY, (float)gyro_z / GYRO_SENSITIVITY };
// 2. 传感器校准 (移除零偏)
if (!ski_data.calibrated) {
Ski_CalibrateSensor(acc, gyro);
return; // 等待校准完成
}
acc[0] -= ski_data.acc_bias[0];
acc[1] -= ski_data.acc_bias[1];
acc[2] -= ski_data.acc_bias[2];
gyro[0] -= ski_data.gyro_bias[0];
gyro[1] -= ski_data.gyro_bias[1];
gyro[2] -= ski_data.gyro_bias[2];
// 3. 姿态解算 (互补滤波器)
Ski_UpdateAttitude(acc, gyro);
// 4. 将身体坐标系的加速度转换为世界坐标系
Ski_TransformToWorldFrame(acc, ski_data.world_acc);
// 5. 在世界坐标系中移除重力 (重力永远是 [0, 0, 1]g)
ski_data.world_acc[2] -= 1.0f;
// 6. 对世界坐标系下的加速度进行滤波 (可选，但推荐)
// 注意: 如果需要，可在此处重用滑动平均滤波器，但暂时我们直接使用原始世界坐标系加速度
// 7. 基于世界坐标系的数据更新状态机
Ski_UpdateStateMachine();
// 8. 基于世界坐标系的数据更新速度和距离
Ski_UpdateVelocityAndDistance();
}
// 计算转弯评分
static float Ski_CalculateTurningScore(float *gyro)
{
// 主要考虑Z轴旋转(偏航)和X轴旋转(俯仰)
float turning_score = fabsf(gyro[2]) * 0.6f + fabsf(gyro[0]) * 0.4f;
return turning_score;
}
// =================================================================================
// 新增核心算法函数
// =================================================================================
/**
* @brief 在静止状态下计算传感器零偏
*/
static void Ski_CalibrateSensor(float *acc, float *gyro)
{
float acc_mag = sqrtf(acc[0]*acc[0] + acc[1]*acc[1] + acc[2]*acc[2]);
float gyro_mag = sqrtf(gyro[0]*gyro[0] + gyro[1]*gyro[1] + gyro[2]*gyro[2]);
// 放宽静止判断条件，使其更容易满足
if (fabsf(acc_mag - 1.0f) < 0.1f && gyro_mag < SKI_STATIC_THRESHOLD_GYRO) {
ski_data.calibration_sum[0] += acc[0];
ski_data.calibration_sum[1] += acc[1];
ski_data.calibration_sum[2] += acc[2];
ski_data.calibration_sum[3] += gyro[0];
ski_data.calibration_sum[4] += gyro[1];
ski_data.calibration_sum[5] += gyro[2];
ski_data.calibration_count++;
if (ski_data.calibration_count >= SKI_CALIBRATION_SAMPLES) {
// 计算加速度计零偏
ski_data.acc_bias[0] = ski_data.calibration_sum[0] / ski_data.calibration_count;
ski_data.acc_bias[1] = ski_data.calibration_sum[1] / ski_data.calibration_count;
// Z轴的零偏是相对于1g的, 需要找到重力轴
float gravity_mag = sqrtf(powf(ski_data.calibration_sum[0] / ski_data.calibration_count, 2) +
powf(ski_data.calibration_sum[1] / ski_data.calibration_count, 2) +
powf(ski_data.calibration_sum[2] / ski_data.calibration_count, 2));
ski_data.acc_bias[2] = ski_data.calibration_sum[2] / ski_data.calibration_count - gravity_mag;
// 计算陀螺仪零偏
ski_data.gyro_bias[0] = ski_data.calibration_sum[3] / ski_data.calibration_count;
ski_data.gyro_bias[1] = ski_data.calibration_sum[4] / ski_data.calibration_count;
ski_data.gyro_bias[2] = ski_data.calibration_sum[5] / ski_data.calibration_count;
ski_data.calibrated = 1;
printf("传感器校准完成!\n");
}
} else {
// 如果检测到移动，则重置校准计数
ski_data.calibration_count = 0;
memset(ski_data.calibration_sum, 0, sizeof(ski_data.calibration_sum));
// 增加调试打印，方便定位问题
// printf("Calibration reset. Acc mag: %.3f, Gyro mag: %.3f\n", acc_mag, gyro_mag);
}
}
/**
* @brief 使用互补滤波器更新姿态角(Roll, Pitch)
*/
static void Ski_UpdateAttitude(float *acc, float *gyro)
{
float dt = ski_data.dt;
float alpha = COMPLEMENTARY_FILTER_ALPHA;
// 从加速度计计算角度 (单位: 度)
float roll_acc = atan2f(acc[1], acc[2]) * 180.0f / M_PI;
float pitch_acc = atan2f(-acc[0], sqrtf(acc[1] * acc[1] + acc[2] * acc[2])) * 180.0f / M_PI;
// 陀螺仪积分预测角度
// 注意: 这里的gyro单位是 度/秒
float roll_gyro = ski_data.attitude[0] + gyro[0] * dt;
float pitch_gyro = ski_data.attitude[1] + gyro[1] * dt;
// 互补滤波融合
ski_data.attitude[0] = alpha * roll_gyro + (1.0f - alpha) * roll_acc;
ski_data.attitude[1] = alpha * pitch_gyro + (1.0f - alpha) * pitch_acc;
}
/**
* @brief 将身体坐标系的加速度转换为世界坐标系
*/
static void Ski_TransformToWorldFrame(float *acc, float *world_acc)
{
// 将姿态角转换为弧度
float roll = ski_data.attitude[0] * M_PI / 180.0f;
float pitch = ski_data.attitude[1] * M_PI / 180.0f;
float cos_roll = cosf(roll);
float sin_roll = sinf(roll);
float cos_pitch = cosf(pitch);
float sin_pitch = sinf(pitch);
// 通过旋转矩阵将身体坐标系的加速度(acc)转换到世界坐标系(world_acc)
// (简化版，假设偏航角yaw=0)
world_acc[0] = acc[0] * cos_pitch + acc[2] * sin_pitch;
world_acc[1] = acc[0] * sin_pitch * sin_roll + acc[1] * cos_roll - acc[2] * cos_pitch * sin_roll;
world_acc[2] = -acc[0] * sin_pitch * cos_roll + acc[1] * sin_roll + acc[2] * cos_pitch * cos_roll;
}
// 改进的运动评分计算
static float Ski_CalculateMovementScore(void)
{
// 在世界坐标系下计算运动评分，只考虑水平加速度
float horizontal_acc_mag = sqrtf(ski_data.world_acc[0]*ski_data.world_acc[0] + ski_data.world_acc[1]*ski_data.world_acc[1]);
return horizontal_acc_mag * 9.81f; // 从 g 转换为 m/s^2
}
// 改进的状态机 - 添加更严格的转换条件
/**
* @brief 基于世界坐标系下的加速度更新状态机
*/
static void Ski_UpdateStateMachine(void)
{
ski_data.state_duration++;
ski_data.movement_score = Ski_CalculateMovementScore();
switch (ski_data.current_state) {
case SKI_STATE_STATIC:
if (ski_data.movement_score > SKI_MOVEMENT_THRESHOLD) {
ski_data.static_counter++;
if (ski_data.static_counter > 5) { // 需要连续几帧的运动才切换
ski_data.current_state = SKI_STATE_MOVING;
ski_data.is_moving = 1;
ski_data.state_duration = 0;
ski_data.static_counter = 0;
}
} else {
ski_data.static_counter = 0;
}
break;
case SKI_STATE_MOVING:
// 使用更低的阈值来检测停止，形成迟滞效应，防止状态抖动
if (ski_data.movement_score < SKI_STATIC_THRESHOLD_ACC) {
ski_data.static_counter++;
if (ski_data.static_counter > 8) { // 减少进入静止状态的等待时间
ski_data.current_state = SKI_STATE_STATIC;
ski_data.is_moving = 0;
ski_data.state_duration = 0;
ski_data.static_counter = 0;
// 停止时强制将速度清零，以防止漂移
memset(ski_data.velocity, 0, sizeof(ski_data.velocity));
}
} else {
ski_data.static_counter = 0;
}
// TODO: 如果需要，可以增加转弯状态的检测
break;
// 如果需要，可以增加其他状态 (例如 TURNING, STOPPING)
default:
ski_data.current_state = SKI_STATE_STATIC;
break;
}
}
// 改进的速度和距离更新 - 添加零速度更新(ZUPT)
/**
* @brief 在世界坐标系下更新速度和距离
*/
static void Ski_UpdateVelocityAndDistance(void)
{
// 将世界坐标系下的加速度从 g 转换为 m/s^2
float acc_ms2[3];
acc_ms2[0] = ski_data.world_acc[0] * 9.81f;
acc_ms2[1] = ski_data.world_acc[1] * 9.81f;
acc_ms2[2] = ski_data.world_acc[2] * 9.81f;
// 是否进行积分的决定由状态机控制
if (ski_data.is_moving) {
// 速度积分
ski_data.velocity[0] += acc_ms2[0] * ski_data.dt;
ski_data.velocity[1] += acc_ms2[1] * ski_data.dt;
// 如果需要，也可以对垂直速度进行积分
// ski_data.velocity[2] += acc_ms2[2] * ski_data.dt;
// 通过累加每一步的位移来计算总距离
float distance_step = sqrtf(
(ski_data.velocity[0] * ski_data.dt) * (ski_data.velocity[0] * ski_data.dt) +
(ski_data.velocity[1] * ski_data.dt) * (ski_data.velocity[1] * ski_data.dt)
);
ski_data.total_horizontal_distance += distance_step;
} else {
// 当静止时，对速度进行衰减
ski_data.velocity[0] *= ski_data.velocity_decay_factor;
ski_data.velocity[1] *= ski_data.velocity_decay_factor;
ski_data.velocity[2] *= ski_data.velocity_decay_factor;
}
// 更新当前水平速度
ski_data.horizontal_speed = sqrtf(ski_data.velocity[0]*ski_data.velocity[0] + ski_data.velocity[1]*ski_data.velocity[1]);
}
// 获取当前速度 (m/s)
float Ski_GetSpeed(void)
{
return ski_data.horizontal_speed;
}
// 获取总移动距离 (m)
float Ski_GetDistance(void)
{
return ski_data.total_horizontal_distance;
}
// 获取当前状态
SkiState_t Ski_GetState(void)
{
return ski_data.current_state;
}
// 重置距离计数
void Ski_ResetDistance(void)
{
ski_data.total_horizontal_distance = 0;
// 速度也应该一起重置
memset(ski_data.velocity, 0, sizeof(ski_data.velocity));
ski_data.horizontal_speed = 0;
}
// 获取校准状态
uint8_t Ski_IsCalibrated(void)
{
return ski_data.calibrated;
}
// 手动设置重力向量
// 这个函数不再需要，将被姿态估计算法取代
/*
void Ski_SetGravityVector(float gx, float gy, float gz)
{
...
}
*/
// 添加调试信息函数
void Ski_PrintDebugInfo(void)
{
printf("State: %d, MoveScore: %.2f, Speed: %.3f, Dist: %.3f, Roll: %.2f, Pitch: %.2f\n",
ski_data.current_state, ski_data.movement_score, ski_data.horizontal_speed, ski_data.total_horizontal_distance,
ski_data.attitude[0], ski_data.attitude[1]);
}
void sensor_processing_task(void){
static int first_init = 0;
if(!first_init){
Ski_Init();
first_init = 1;
}
static signed short acc_data[3], gyr_data[3];
static signed short combined_raw_data[6];
static float final_angle_data[3];
// 读取传感器数据
SL_SC7U22_RawData_Read(acc_data, gyr_data);
memcpy(&combined_raw_data[0], acc_data, 3 * sizeof(signed short));
memcpy(&combined_raw_data[3], gyr_data, 3 * sizeof(signed short));
// 校准逻辑已内置于Ski_UpdateData中，不再需要外部检查
// if(get_calibration_state() == 0){ ... }
// 更新滑雪数据
Ski_UpdateData(combined_raw_data[0], combined_raw_data[1], combined_raw_data[2],
combined_raw_data[3], combined_raw_data[4], combined_raw_data[5]);
static int count = 0;
if(count < 10){
count++;
return;
}else{
count = 0;
}
// 打印结果和调试信息
float speed = Ski_GetSpeed();
float distance = Ski_GetDistance();
SkiState_t state = Ski_GetState();
// printf("Speed: %.3f m/s, Distance: %.3f m, State: %d\n", speed, distance, state);
Ski_PrintDebugInfo();
}

40
apps/earphone/xtell_Sensor/A_hide/4/skiing_tracker.h Normal file
View File

@ -0,0 +1,40 @@
#ifndef SKI_SPEED_DISTANCE_H
#define SKI_SPEED_DISTANCE_H
#include <stdint.h>
// 滑雪状态定义
typedef enum {
SKI_STATE_STATIC = 0, // 静止状态
SKI_STATE_STARTING, // 启动状态
SKI_STATE_MOVING, // 运动状态
SKI_STATE_TURNING, // 转弯状态
SKI_STATE_STOPPING // 停止状态
} SkiState_t;
// 初始化滑雪模块
void Ski_Init(void);
// 更新滑雪数据
void Ski_UpdateData(int16_t acc_x, int16_t acc_y, int16_t acc_z,
int16_t gyro_x, int16_t gyro_y, int16_t gyro_z);
// 获取当前速度 (m/s)
float Ski_GetSpeed(void);
// 获取总移动距离 (m)
float Ski_GetDistance(void);
// 获取当前状态
SkiState_t Ski_GetState(void);
// 重置距离计数
void Ski_ResetDistance(void);
// 获取校准状态
uint8_t Ski_IsCalibrated(void);
// 手动设置重力向量（可选）
void Ski_SetGravityVector(float gx, float gy, float gz);
#endif

311
apps/earphone/xtell_Sensor/A_hide/5/skiing_tracker.c Normal file
View File

@ -0,0 +1,311 @@
/*
动态ZUPT+卡尔曼
多了加速度死区、摩擦力速度衰减、高通滤波
原地摆动产生的速度、距离变化还是没法消除
水平移动、斜坡移动效果貌似还行
*/
#include "skiing_tracker.h"
#include "../sensor/SC7U22.h"
#include <math.h>
#include <string.h>
#define G_ACCELERATION 9.81f
#define DEG_TO_RAD (3.14159265f / 180.0f)
// --- 算法阈值定义 ---
//两个判断是否静止的必要条件
// 动态零速更新(ZUPT)阈值
// 提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
#define ZUPT_ACC_VARIANCE_THRESHOLD 0.2f
// 陀螺仪方差阈值
#define ZUPT_GYR_VARIANCE_THRESHOLD 5.0f
// 旋转/摆动检测阈值：角速度总模长大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动 -- 没法完全消除
#define ROTATION_GYR_MAG_THRESHOLD 45.0f
// 启动滑雪阈值：加速度模长与重力的差值大于此值，认为开始运动
// 降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
#define START_SKIING_ACC_THRESHOLD 0.5f
// --- 用于消除积分漂移的滤波器和阈值 ---
// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
// alpha = RC / (RC + dt)，
#define HPF_ALPHA 0.95f
// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
#define ACC_DEAD_ZONE_THRESHOLD 0.1f
#define SPEED_ATTENUATION 0.98f //模拟摩擦力，进行速度衰减
/**
* @brief 初始化滑雪追踪器
*/
void skiing_tracker_init(skiing_tracker_t *tracker)
{
if (!tracker) {
return;
}
// 使用memset一次性清零整个结构体，包括新增的缓冲区
memset(tracker, 0, sizeof(skiing_tracker_t));
tracker->state = SKIING_STATE_STATIC;
}
/**
* @brief 将设备坐标系下的加速度转换为世界坐标系
* @param acc_device 设备坐标系下的加速度 [x, y, z]
* @param angle 姿态角 [pitch, roll, yaw]，单位: 度
* @param acc_world 输出：世界坐标系下的加速度 [x, y, z]
*/
static void transform_acc_to_world_frame(const float *acc_device, const float *angle, float *acc_world)
{
// 驱动输出的角度与标准航空定义相反，需要取反才能用于标准旋转矩阵。
float pitch = -angle[0] * DEG_TO_RAD;
float roll = -angle[1] * DEG_TO_RAD;
// TODO: 当引入三轴磁力计后，这里的 yaw 应由磁力计和陀螺仪融合解算得出，以解决航向漂移问题。
// 目前 yaw 暂时不参与计算，因为仅靠加速度计和陀螺仪无法获得准确的绝对航向角。
// float yaw = -angle[2] * DEG_TO_RAD;
float cp = cosf(pitch);
float sp = sinf(pitch);
float cr = cosf(roll);
float sr = sinf(roll);
float ax = acc_device[0];
float ay = acc_device[1];
float az = acc_device[2];
// 使用经过验证的、正确的身体坐标系到世界坐标系的旋转矩阵 (基于 Y-X 旋转顺序)
// 这个矩阵将设备测量的加速度(ax, ay, az)正确地转换到世界坐标系(acc_world)。
// 注意：这里没有使用yaw，主要关心的是坡面上的运动，绝对航向暂时不影响速度和距离的计算。
// TODO
acc_world[0] = cp * ax + sp * sr * ay + sp * cr * az;
acc_world[1] = 0 * ax + cr * ay - sr * az;
acc_world[2] = -sp * ax + cp * sr * ay + cp * cr * az;
}
/**
* @brief 计算缓冲区内三轴数据的方差之和
*/
static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
{
float mean[3] = {0};
float variance[3] = {0};
// 1. 计算均值
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;
// 2. 计算方差
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 状态机更新
*/
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps)
{
// 缓冲区未填满时，不进行状态判断，默认为静止
if (!tracker->buffer_filled) {
tracker->state = SKIING_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]);
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]);
// --- 状态切换逻辑---
// 原地旋转/摆动检测
// 增加一个关键前提：只在当前不处于滑雪状态时，才检测原地旋转。
// 这可以防止滑雪过程中的高速转弯被误判为原地旋转。
// 暂时没办法完全消除
if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && tracker->state != SKIING_STATE_SKIING) {
tracker->state = SKIING_STATE_ROTATING;
return;
}
// 动态零速更新 (ZUPT)
// 必须同时满足加速度和角速度都稳定，才能判断为“真静止”，以区分匀速运动
if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
tracker->state = SKIING_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_STATIC || tracker->state == SKIING_STATE_ROTATING) {
// 最终版启动逻辑：必须同时满足“有足够大的线性加速度”和“旋转稳定”两个条件
if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
tracker->state = SKIING_STATE_SKIING;
return;
}
}
// 滑雪
if (tracker->state != SKIING_STATE_STATIC) {
tracker->state = SKIING_STATE_SKIING;
}
}
/**
* @brief 主更新函数
*/
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;
}
// --- 数据预处理和缓冲 ---
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);
// --- 根据状态进行计算 ---
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 (acc_horizontal_mag > ACC_DEAD_ZONE_THRESHOLD) {
// 只有当水平加速度足够大时，才进行速度积分
tracker->velocity[0] += tracker->acc_world_filtered[0] * dt;
tracker->velocity[1] += tracker->acc_world_filtered[1] * dt;
// 垂直方向的速度暂时不积分，极易受姿态误差影响而漂移
// tracker->velocity[2] += tracker->acc_world_filtered[2] * dt;
}
// 如果加速度小于阈值，则不更新速度，相当于速度保持不变（或受下一步的阻尼影响而衰减）
} else {
// 在静止或旋转状态下，速度已经在状态机内部被清零
// 额外增加速度衰减，模拟摩擦力，进一步抑制漂移
tracker->velocity[0] *= SPEED_ATTENUATION;
tracker->velocity[1] *= SPEED_ATTENUATION;
tracker->velocity[2] = 0; // 垂直速度强制归零
}
// --- 更新速率和距离 ---
// 只基于水平速度计算速率和距离
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
tracker->velocity[1] * tracker->velocity[1]);
tracker->distance += tracker->speed * dt;
}
// 传感器数据采集与处理任务
void 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;
static signed short combined_raw_data[6];
static float final_angle_data[3]; // 计算得到的欧若拉角
static float calibrated_acc_g[3]; // 转换后的加速度计数据
static float calibrated_gyr_dps[3]; // 转换后的陀螺仪数据
const float delta_time = 0.01f;
if (!initialized) {
skiing_tracker_init(&my_skiing_tracker);
initialized = 1;
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
}
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
unsigned char status;
if (!calibration_done) { //第1次启动，开启零漂检测
status = SL_SC7U22_Angle_Output(1, combined_raw_data, final_angle_data, 0);
if (status == 1) {
calibration_done = 1;
printf("Sensor calibration successful! Skiing mode is active.\n");
}
} else {
status = SL_SC7U22_Angle_Output(0, combined_raw_data, final_angle_data, 0);
}
if (status == 1) {
// 加速度 LSB to g
calibrated_acc_g[0] = (float)combined_raw_data[0] / 8192.0f;
calibrated_acc_g[1] = (float)combined_raw_data[1] / 8192.0f;
calibrated_acc_g[2] = (float)combined_raw_data[2] / 8192.0f;
// 陀螺仪 LSB to dps (度/秒)
// ±2000dps量程下，转换系数约为 0.061
calibrated_gyr_dps[0] = (float)combined_raw_data[3] * 0.061f;
calibrated_gyr_dps[1] = (float)combined_raw_data[4] * 0.061f;
calibrated_gyr_dps[2] = (float)combined_raw_data[5] * 0.061f;
skiing_tracker_update(&my_skiing_tracker, calibrated_acc_g, calibrated_gyr_dps, final_angle_data, delta_time);
// 打印逻辑保持不变
static int count = 0;
if(count < 10){
count++;
return;
} else {
count = 0;
}
printf("State: %d, Speed: %.2f m/s, Distance: %.2f m\n",
my_skiing_tracker.state,
my_skiing_tracker.speed,
my_skiing_tracker.distance);
} else if (status == 0) {
// printf("Sensor is calibrating...\n");
} else {
// printf("Angle calculation error or calibration not finished.\n");
}
}

55
apps/earphone/xtell_Sensor/A_hide/5/skiing_tracker.h Normal file
View File

@ -0,0 +1,55 @@
#ifndef SKIING_TRACKER_H
#define SKIING_TRACKER_H
// 定义滑雪者可能的状态
typedef enum {
SKIING_STATE_STATIC, // 静止或动态稳定
SKIING_STATE_SKIING, // 正在滑雪
SKIING_STATE_ROTATING, // 正在原地旋转 (新增)
SKIING_STATE_FALLEN, // 已摔倒
SKIING_STATE_UNKNOWN // 未知状态
} skiing_state_t;
#define VARIANCE_BUFFER_SIZE 15 // 用于计算方差的数据窗口大小 (15个样本 @ 100Hz = 150ms)
// 追踪器数据结构体
typedef struct {
// 公开数据
float velocity[3]; // 当前速度 (x, y, z)，单位: m/s
float distance; // 总滑行距离，单位: m
float speed; // 当前速率 (标量)，单位: m/s
skiing_state_t state; // 当前滑雪状态
// 内部计算使用的私有成员
float acc_world[3]; // 在世界坐标系下的加速度
// --- 内部计算使用的私有成员 ---
// 用于动态零速更新和旋转检测的缓冲区
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];
} skiing_tracker_t;
/**
* @brief 初始化滑雪追踪器
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
*/
void skiing_tracker_init(skiing_tracker_t *tracker);
/**
* @brief 处理传感器数据并更新滑雪状态
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
* @param acc_g 校准后的加速度数据 [x, y, z]，单位: g (1g = 9.8m/s^2)
* @param gyr_dps 角速度
* @param angle 姿态角数据 [pitch, roll, yaw]，单位: 度
* @param dt 采样时间间隔，单位: 秒 (s)
*/
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt);
#endif // SKIING_TRACKER_H

374
apps/earphone/xtell_Sensor/A_hide/6/skiing_tracker.c Normal file
View File

@ -0,0 +1,374 @@
/*
动态ZUPT+卡尔曼+巴特沃斯一阶滤波器
针对启动滑雪和停止滑雪，设置不同阈值
启动滑雪和ZUPT更新的陀螺仪方差阈值分开设置
- 启动滑雪的陀螺仪阈值会更宽松一些
原地旋转和ZUPT更新的加速度方差阈值分开设置
- 原地旋转的加速度阈值更宽松
能够从静止状态到变化状态，去根据阈值来判断这个“变化”：进入滑行状态 / 只是原地摆动
- 但是还是不够灵敏
*/
#include "skiing_tracker.h"
#include "../sensor/SC7U22.h"
#include <math.h>
#include <string.h>
#define G_ACCELERATION 9.81f
#define DEG_TO_RAD (3.14159265f / 180.0f)
// --- 算法阈值定义 ---
//两个判断是否静止的必要条件：动态零速更新(ZUPT)阈值
// 加速方差阈值，提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
#define ZUPT_ACC_VARIANCE_THRESHOLD 0.2f
// 陀螺仪方差阈值
#define ZUPT_GYR_VARIANCE_THRESHOLD 5.0f
// 用于原地旋转判断的加速度方差阈值。此值比ZUPT阈值更宽松，
// 以允许原地旋转时身体的正常晃动，但仍能与真实滑行时的剧烈加速度变化区分开。
#define ROTATING_ACC_VARIANCE_THRESHOLD 0.8f
// 用于启动滑雪判断的陀螺仪方差阈值。此值比ZUPT阈值更宽松，
// 以允许启动瞬间的正常抖动，但仍能过滤掉混乱的、非滑雪的晃动。
#define SKIING_GYR_VARIANCE_THRESHOLD 15.0f
// 旋转/摆动检测阈值：角速度总模长大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
#define ROTATION_GYR_MAG_THRESHOLD 45.0f
// 启动滑雪阈值：加速度模长与重力的差值大于此值，认为开始运动
// 降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
#define START_SKIING_ACC_THRESHOLD 0.5f
// --- 用于消除积分漂移的滤波器和阈值 ---
// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
// alpha = RC / (RC + dt)，
#define HPF_ALPHA 0.95f // 换算大概就是衰减频率低于约 0.84 Hz 的信号
// 任何比“大约1秒钟变化一次”还要慢的运动，其加速度信号也会被部分衰减。
// 而滑雪时的快速转弯、加减速等动作，其频率远高于 0.84 Hz，它们的信号会被保留下来。
// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
#define ACC_DEAD_ZONE_THRESHOLD 0.15f
// --- 模拟摩擦力，进行速度衰减 ---
#define SPEED_ATTENUATION 0.98f
//////////////////////////////////////////////////////////////////////////////////////////////////
//实现
/**
* @brief 初始化滑雪追踪器
*
* @param tracker
*/
void skiing_tracker_init(skiing_tracker_t *tracker)
{
if (!tracker) {
return;
}
// 使用memset一次性清零整个结构体，包括新增的缓冲区
memset(tracker, 0, sizeof(skiing_tracker_t));
tracker->state = SKIING_STATE_STATIC;
}
/**
* @brief 将设备坐标系下的加速度转换为世界坐标系
* @param acc_device 设备坐标系下的加速度 [x, y, z]
* @param angle 姿态角 [pitch, roll, yaw]，单位: 度
* @param acc_world 输出：世界坐标系下的加速度 [x, y, z]
*/
static void transform_acc_to_world_frame(const float *acc_device, const float *angle, float *acc_world)
{
// 驱动输出的角度与标准航空定义相反，需要取反才能用于标准旋转矩阵。
float pitch = -angle[0] * DEG_TO_RAD;
float roll = -angle[1] * DEG_TO_RAD;
// TODO: 当引入三轴磁力计后，这里的 yaw 应由磁力计和陀螺仪融合解算得出，以解决航向漂移问题。
// 目前 yaw 暂时不参与计算，因为仅靠加速度计和陀螺仪无法获得准确的绝对航向角。
// float yaw = -angle[2] * DEG_TO_RAD;
float cp = cosf(pitch);
float sp = sinf(pitch);
float cr = cosf(roll);
float sr = sinf(roll);
float ax = acc_device[0];
float ay = acc_device[1];
float az = acc_device[2];
// 使用经过验证的、正确的身体坐标系到世界坐标系的旋转矩阵 (基于 Y-X 旋转顺序)
// 这个矩阵将设备测量的加速度(ax, ay, az)正确地转换到世界坐标系(acc_world)。
// 注意：这里没有使用yaw，主要关心的是坡面上的运动，绝对航向暂时不影响速度和距离的计算。
// TODO
acc_world[0] = cp * ax + sp * sr * ay + sp * cr * az;
acc_world[1] = 0 * ax + cr * ay - sr * az;
acc_world[2] = -sp * ax + cp * sr * ay + cp * cr * az;
}
/**
* @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
* @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 = SKIING_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]);
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]);
// --- 状态切换逻辑 (按优先级) ---
// 优先级1：动态零速更新 (ZUPT) - 最严格和最优先的“刹车”
if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
tracker->state = SKIING_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;
}
// 优先级2：原地旋转 - 特殊的、非滑雪的运动状态
// 条件：角速度很大，同时线性加速度的晃动在一个“中等”范围内。
if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && acc_variance < ROTATING_ACC_VARIANCE_THRESHOLD) {
tracker->state = SKIING_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;
return;
}
// 如果不满足任何启动或停止条件，则保持当前状态（滑雪中）
// 如果当前是静止或旋转，但没有满足启动条件，则状态会保持，直到满足ZUPT或旋转条件。
}
/**
* @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;
}
// --- 数据预处理和缓冲 ---
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);
// --- 根据状态进行计算 ---
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 (acc_horizontal_mag > ACC_DEAD_ZONE_THRESHOLD) {
// 只有当水平加速度足够大时，才进行速度积分
tracker->velocity[0] += tracker->acc_world_filtered[0] * dt;
tracker->velocity[1] += tracker->acc_world_filtered[1] * dt;
// 垂直方向的速度暂时不积分，极易受姿态误差影响而漂移
// tracker->velocity[2] += tracker->acc_world_filtered[2] * dt;
}
// 如果加速度小于阈值，则不更新速度，相当于速度保持不变（或受下一步的阻尼影响而衰减）
} else {
// 在静止或旋转状态下，速度已经在状态机内部被清零
// 额外增加速度衰减，模拟摩擦力，进一步抑制漂移
tracker->velocity[0] *= SPEED_ATTENUATION;
tracker->velocity[1] *= SPEED_ATTENUATION;
tracker->velocity[2] = 0; // 垂直速度强制归零
}
// --- 更新速率和距离 ---
// 只基于水平速度计算速率和距离
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
tracker->velocity[1] * tracker->velocity[1]);
tracker->distance += tracker->speed * dt;
}
/**
* @brief 传感器数据采集与处理任务，外部每10ms调用一次，如果需要更新时间间隔，也需要同步更新宏“ DELTA_TIME ”
*
* @param acc_data_buf 三轴加速度原始数据
* @param gyr_data_buf 三轴陀螺仪原始数据
* @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;
static signed short combined_raw_data[6];
static float final_angle_data[3]; // 计算得到的欧若拉角
static float calibrated_acc_g[3]; // 转换后的加速度计数据
static float calibrated_gyr_dps[3]; // 转换后的陀螺仪数据
const float delta_time = DELTA_TIME;
BLE_send_data_t BLE_send_data;
BLE_KS_send_data_t KS_data;
if (!initialized) {
skiing_tracker_init(&my_skiing_tracker);
initialized = 1;
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
}
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
unsigned char status;
if (!calibration_done) { //第1次启动，开启零漂检测
status = SL_SC7U22_Angle_Output(1, combined_raw_data, final_angle_data, 0);
if (status == 1) {
calibration_done = 1;
printf("Sensor calibration successful! Skiing mode is active.\n");
}
} else {
status = SL_SC7U22_Angle_Output(0, combined_raw_data, final_angle_data, 0);
}
if (status == 1) {
// 加速度 LSB to g
calibrated_acc_g[0] = (float)combined_raw_data[0] / 8192.0f;
calibrated_acc_g[1] = (float)combined_raw_data[1] / 8192.0f;
calibrated_acc_g[2] = (float)combined_raw_data[2] / 8192.0f;
// 陀螺仪 LSB to dps (度/秒)
// ±2000dps量程下，转换系数约为 0.061
calibrated_gyr_dps[0] = (float)combined_raw_data[3] * 0.061f;
calibrated_gyr_dps[1] = (float)combined_raw_data[4] * 0.061f;
calibrated_gyr_dps[2] = (float)combined_raw_data[5] * 0.061f;
skiing_tracker_update(&my_skiing_tracker, calibrated_acc_g, calibrated_gyr_dps, final_angle_data, delta_time);
// 打印逻辑保持不变
// static int count = 0;
// if(count >= 10){
// printf("State: %d, Speed: %.2f m/s, Distance: %.2f m\n",
// my_skiing_tracker.state,
// my_skiing_tracker.speed,
// my_skiing_tracker.distance);
// count = 0;
// } else {
// count++;
// }
BLE_send_data.sensor_state = status;
BLE_send_data.skiing_state = my_skiing_tracker.state;
for (int i = 0; i < 3; i++) {
#ifndef XTELL_TEST
BLE_send_data.acc_original[i] = (int)acc_data_buf[i];
BLE_send_data.gyr_original[i] = (int)gyr_data_buf[i];
#endif
#if KS_BLE
KS_data.acc_KS_g[i] = (int)calibrated_acc_g[i];
KS_data.gyr_KS_dps[i] = (int)calibrated_gyr_dps[i];
KS_data.angle_KS[i] = (int)final_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);
} else if (status == 0) {
memset(&BLE_send_data, 0, sizeof(BLE_send_data_t));
BLE_send_data.sensor_state = status;
#if KS_BLE
memset(&KS_data, 0, sizeof(BLE_send_data_t));
#endif
// printf("Sensor is calibrating...\n");
} else {
memset(&BLE_send_data, 0, sizeof(BLE_send_data_t));
BLE_send_data.sensor_state = status;
#if KS_BLE
memset(&KS_data, 0, sizeof(BLE_send_data_t));
#endif
// printf("Angle calculation error or calibration not finished.\n");
}
return BLE_send_data;
}

74
apps/earphone/xtell_Sensor/A_hide/6/skiing_tracker.h Normal file
View File

@ -0,0 +1,74 @@
#ifndef SKIING_TRACKER_H
#define SKIING_TRACKER_H
#include "../xtell.h"
// 定义滑雪者可能的状态
typedef enum {
SKIING_STATE_STATIC, // 静止或动态稳定
SKIING_STATE_SKIING, // 正在滑雪
SKIING_STATE_ROTATING, // 正在原地旋转 (新增)
SKIING_STATE_FALLEN, // 已摔倒
SKIING_STATE_UNKNOWN // 未知状态
} 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_world[3]; // 在世界坐标系下的加速度
// --- 内部计算使用的私有成员 ---
// 用于动态零速更新和旋转检测的缓冲区
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]; //上一次没过滤的
} skiing_tracker_t;
//ble发送的数据
typedef struct __attribute__((packed)){ //该结构体取消内存对齐
char sensor_state;
char skiing_state;
int speed_cms; //求出的速度，cm/s
int distance_cm; //求出的距离，cm
#ifndef XTELL_TEST
int acc_original[3]; //直接读取传感器得到的原始三轴加速度
int gyr_original[3]; //直接读取传感器得到的原始三轴陀螺仪
#endif
}BLE_send_data_t;
typedef struct{
int acc_KS_g[3]; //卡尔曼后，LSB to g 三轴加速度数据
int gyr_KS_dps[3]; //卡尔曼后，LSB to dps 三轴陀螺仪数据
int angle_KS[3]; //卡尔曼后，计算得到的欧若拉角数据
}BLE_KS_send_data_t;
/**
* @brief 初始化滑雪追踪器
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
*/
void skiing_tracker_init(skiing_tracker_t *tracker);
/**
* @brief 传感器数据采集与处理任务，外部每10ms调用一次，如果需要更新时间间隔，也需要同步更新宏“ DELTA_TIME ”
*
* @param acc_data_buf 三轴加速度原始数据
* @param gyr_data_buf 三轴陀螺仪原始数据
* @return BLE_send_data_t
*/
BLE_send_data_t sensor_processing_task(signed short * acc_data_buf, signed short * gyr_data_buf) ;
#endif // SKIING_TRACKER_H

276
apps/earphone/xtell_Sensor/A_hide/7/skiing_tracker.c Normal file
View File

@ -0,0 +1,276 @@
/*
简化的滑雪追踪器：
- 直接读取六轴数据
- 分离重力分量
- 直接积分求速度和距离
*/
#include "skiing_tracker.h"
#include "../sensor/SC7U22.h"
#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)
// 运动检测阈值 (m/s^2)。当水平加速度的模长大于此值时，认为处于运动状态。
#define MOTION_THRESHOLD 0.2f
static skiing_tracker_t my_skiing_tracker;
#ifdef XTELL_TEST
BLE_KS_send_data_t KS_data;
debug_t debug1;
debug_t debug2;
#endif
//////////////////////////////////////////////////////////////////////////////////////////////////
//实现
/**
* @brief 初始化滑雪追踪器
*
* @param tracker
*/
void skiing_tracker_init(skiing_tracker_t *tracker)
{
if (!tracker) {
return;
}
memset(tracker, 0, sizeof(skiing_tracker_t));
tracker->state = SKIING_STATE_STATIC;
}
/**
* @brief 将设备坐标系下的加速度转换为世界坐标系
* @param acc_device 设备坐标系下的加速度 [x, y, z]
* @param angle 姿态角 [pitch, roll, yaw]，单位: 度
* @param acc_world 输出：世界坐标系下的加速度 [x, y, z]
*/
static void transform_acc_to_world_frame(const float *acc_device, const float *angle, float *acc_world)
{
// 驱动输出的角度与标准航空定义相反，需要取反才能用于标准旋转矩阵。
float pitch = -angle[0] * DEG_TO_RAD;
float roll = -angle[1] * DEG_TO_RAD;
float cp = cosf(pitch);
float sp = sinf(pitch);
float cr = cosf(roll);
float sr = sinf(roll);
float ax = acc_device[0];
float ay = acc_device[1];
float az = acc_device[2];
// 使用经过验证的、正确的身体坐标系到世界坐标系的旋转矩阵 (基于 Y-X 旋转顺序)
acc_world[0] = cp * ax + sp * sr * ay + sp * cr * az;
acc_world[1] = 0 * ax + cr * ay - sr * az;
acc_world[2] = -sp * ax + cp * sr * ay + cp * cr * az;
}
void clear_speed(void){
my_skiing_tracker.state = SKIING_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 = SKIING_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 = SKINNG_STOP_DETECTION;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.speed = 0;
}
/**
* @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 || !angle || dt <= 0) {
return;
}
//停止检测
if(my_skiing_tracker.state == SKINNG_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;
// --- 坐标转换 & 移除重力 ---
// 1. 将设备坐标系下的总加速度转换到世界坐标系
transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world);
// 2. 在世界坐标系下，减去Z轴的重力分量，得到线性加速度
tracker->acc_world[2] -= G_ACCELERATION;
// --- 运动状态判断与计算 ---
// 计算水平方向的线性加速度模长
float acc_horizontal_mag = sqrtf(tracker->acc_world[0] * tracker->acc_world[0] +
tracker->acc_world[1] * tracker->acc_world[1]);
if (acc_horizontal_mag > MOTION_THRESHOLD) {
// 加速
tracker->state = SKIING_STATE_SKIING;
// 积分计算速度（只考虑水平方向，避免垂直方向漂移）
tracker->velocity[0] += tracker->acc_world[0] * dt;
tracker->velocity[1] += tracker->acc_world[1] * dt;
tracker->velocity[2] = 0; // 垂直速度强制为0
// --- 更新速率和距离 ---
// 只基于水平速度计算速率
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
tracker->velocity[1] * tracker->velocity[1]);
// 累加距离
tracker->distance += tracker->speed * dt;
return;
} else {
//匀速
tracker->distance += tracker->speed * dt;
return;
}
}
// 获取当前 RTC 时间戳
time_t get_rtc_timestamp(void) {
static void *rtc_hdl = NULL;
struct sys_time rtc_time = {0};
dev_ioctl(rtc_hdl, IOCTL_GET_SYS_TIME, (u32)&rtc_time);
return rtc_sys_time_to_timestamp(&rtc_time);
}
/**
* @brief 传感器数据采集与处理任务，外部每10ms调用一次，如果需要更新时间间隔，也需要同步更新宏“ DELTA_TIME ”
*
* @param acc_data_buf 三轴加速度原始数据
* @param gyr_data_buf 三轴陀螺仪原始数据
* @return BLE_send_data_t
*/
BLE_send_data_t sensor_processing_task(signed short * acc_data_buf, signed short * gyr_data_buf) {
static int initialized = 0;
static int calibration_done = 0;
static signed short combined_raw_data[6];
static float final_angle_data[3]; // 计算得到的欧若拉角
static float calibrated_acc_g[3]; // 转换后的加速度计数据
static float calibrated_gyr_dps[3]; // 转换后的陀螺仪数据
float delta_time = DELTA_TIME + 0.03f; //观察得到实际上调用间隔会多30ms
BLE_send_data_t BLE_send_data;
// xlog("===========start\n");
if (!initialized) {
skiing_tracker_init(&my_skiing_tracker);
initialized = 1;
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
}
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
unsigned char status;
if (!calibration_done) { //第1次启动，开启零漂检测
status = SL_SC7U22_Angle_Output(1, combined_raw_data, final_angle_data, 0);
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, final_angle_data, 0);
// printf("Calculate the time interval =============== end\n");
}
if (status == 1) {
// 加速度 LSB to g
calibrated_acc_g[0] = (float)combined_raw_data[0] / 8192.0f;
calibrated_acc_g[1] = (float)combined_raw_data[1] / 8192.0f;
calibrated_acc_g[2] = (float)combined_raw_data[2] / 8192.0f;
// 陀螺仪 LSB to dps (度/秒)
// ±2000dps量程下，转换系数约为 0.061
calibrated_gyr_dps[0] = (float)combined_raw_data[3] * 0.061f;
calibrated_gyr_dps[1] = (float)combined_raw_data[4] * 0.061f;
calibrated_gyr_dps[2] = (float)combined_raw_data[5] * 0.061f;
skiing_tracker_update(&my_skiing_tracker, calibrated_acc_g, calibrated_gyr_dps, final_angle_data, delta_time);
// static int count = 0;
// if(count >= 10){
// printf("State: %d, Speed: %.2f m/s, Distance: %.2f m\n",
// my_skiing_tracker.state,
// my_skiing_tracker.speed,
// my_skiing_tracker.distance);
// printf("calibrated_acc_g: %.2f, %.2f, %.2f\n",
// calibrated_acc_g[0],
// calibrated_acc_g[1],
// calibrated_acc_g[2]);
// count = 0;
// } else {
// count++;
// }
BLE_send_data.sensor_state = status;
BLE_send_data.skiing_state = my_skiing_tracker.state;
for (int i = 0; i < 3; i++) {
#ifndef XTELL_TEST
BLE_send_data.acc_original[i] = (int)acc_data_buf[i];
BLE_send_data.gyr_original[i] = (int)gyr_data_buf[i];
#endif
#if KS_BLE
KS_data.acc_KS[i] = (int)(calibrated_acc_g[i] * G_ACCELERATION * 100); //cm/s^s
KS_data.gyr_KS_dps[i] = (int)calibrated_gyr_dps[i];
KS_data.angle_KS[i] = (int)final_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);
//
} else if (status == 0) {
memset(&BLE_send_data, 0, sizeof(BLE_send_data_t));
BLE_send_data.sensor_state = status;
#if KS_BLE
memset(&KS_data, 0, sizeof(BLE_send_data_t));
#endif
// printf("Sensor is calibrating...\n");
} else {
memset(&BLE_send_data, 0, sizeof(BLE_send_data_t));
BLE_send_data.sensor_state = status;
#if KS_BLE
memset(&KS_data, 0, sizeof(BLE_send_data_t));
#endif
// printf("Angle calculation error or calibration not finished.\n");
}
// xlog("end++++++++++++++\n");
return BLE_send_data;
}

67
apps/earphone/xtell_Sensor/A_hide/7/skiing_tracker.h Normal file
View File

@ -0,0 +1,67 @@
#ifndef SKIING_TRACKER_H
#define SKIING_TRACKER_H
#include "../xtell.h"
// 定义滑雪者可能的状态
typedef enum {
SKIING_STATE_STATIC, // 静止
SKIING_STATE_SKIING, // 正在滑雪
SKINNG_STOP_DETECTION, // 停止检测
} skiing_state_t;
#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_world[3]; // 在世界坐标系下的线性加速度
} skiing_tracker_t;
//ble发送的数据
typedef struct __attribute__((packed)){ //该结构体取消内存对齐
char sensor_state;
char skiing_state;
int speed_cms; //求出的速度，cm/s
int distance_cm; //求出的距离，cm
#ifndef XTELL_TEST
int acc_original[3]; //直接读取传感器得到的原始三轴加速度
int gyr_original[3]; //直接读取传感器得到的原始三轴陀螺仪
#endif
}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);
/**
* @brief 传感器数据采集与处理任务，外部每10ms调用一次，如果需要更新时间间隔，也需要同步更新宏“ DELTA_TIME ”
*
* @param acc_data_buf 三轴加速度原始数据
* @param gyr_data_buf 三轴陀螺仪原始数据
* @return BLE_send_data_t
*/
BLE_send_data_t sensor_processing_task(signed short * acc_data_buf, signed short * gyr_data_buf) ;
#endif // SKIING_TRACKER_H

501
apps/earphone/xtell_Sensor/A_hide/8/skiing_tracker.c Normal file
View File

@ -0,0 +1,501 @@
/*
虽然sensor_processing_task是10ms调用一次
但是实际上上一次调用该函数的时间点和下一次调用该函数的时间点，会相差40ms
*/
#include "skiing_tracker.h"
#include "../sensor/SC7U22.h"
#include <math.h>
#include <string.h>
#define G_ACCELERATION 9.81f
#define DEG_TO_RAD (3.14159265f / 180.0f)
// --- ZUPT ---
//两个判断是否静止的必要条件：动态零速更新(ZUPT)阈值
// 加速方差阈值，提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
#define ZUPT_ACC_VARIANCE_THRESHOLD 0.2f
// 陀螺仪方差阈值
#define ZUPT_GYR_VARIANCE_THRESHOLD 5.0f
// --- 启动滑雪阈值 ---
// 加速度模长与重力的差值大于此值，认为开始运动；降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
#define START_SKIING_ACC_THRESHOLD 0.5f
// 陀螺仪方差阈值，以允许启动瞬间的正常抖动，但仍能过滤掉混乱的、非滑雪的晃动。
#define SKIING_GYR_VARIANCE_THRESHOLD 15.0f
// --- 滑雪过程 ---
//加速度 模长，低于此值视为 在做匀速运动
#define SKIING_ACC_MAG_THRESHOLD 0.5f
//陀螺仪 模长，高于此值视为 摔倒了
#define FALLEN_GRY_MAG_THRESHOLD 1000.0f //未确定
// --- 原地旋转抖动 ---
// 用于原地旋转判断的加速度方差阈值。此值比ZUPT阈值更宽松，
// 以允许原地旋转时身体的正常晃动，但仍能与真实滑行时的剧烈加速度变化区分开。
#define ROTATING_ACC_VARIANCE_THRESHOLD 0.8f
// 旋转/摆动检测阈值：角速度总模长大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
#define ROTATION_GYR_MAG_THRESHOLD 120.0f
// --- 滑雪转弯动 ---
// 加速度方差阈值，大于此值，滑雪过程可能发生了急转弯
#define WHEEL_ACC_VARIANCE_THRESHOLD 7.0f
// 旋转/摆动检测阈值：角速度总模长大于此值(度/秒)，认为滑雪过程中进行急转弯
#define WHEEL_GYR_MAG_THRESHOLD 220.0f // 150.0f 到 250.0f之间进行调整
// --- 用于消除积分漂移的滤波器和阈值 ---
// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
// alpha = RC / (RC + dt)，参考RC电路而来
#define HPF_ALPHA 0.995
// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
#define ACC_DEAD_ZONE_THRESHOLD 0.05f
// --- 模拟摩擦力，进行速度衰减 ---
#define SPEED_ATTENUATION 1.0f //暂不模拟
#ifdef XTELL_TEST
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 初始化滑雪追踪器
*
* @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 将设备坐标系下的加速度转换为世界坐标系
* @param acc_device 设备坐标系下的加速度 [x, y, z]
* @param angle 姿态角 [pitch, roll, yaw]，单位: 度
* @param acc_world 输出：世界坐标系下的加速度 [x, y, z]
*/
static void transform_acc_to_world_frame(const float *acc_device, const float *angle, float *acc_world)
{
// 驱动输出的角度与标准航空定义相反，需要取反才能用于标准旋转矩阵。
float pitch = -angle[0] * DEG_TO_RAD;
float roll = -angle[1] * DEG_TO_RAD;
// TODO: 当引入三轴磁力计后，这里的 yaw 应由磁力计和陀螺仪融合解算得出，以解决航向漂移问题。
// 目前 yaw 暂时不参与计算，因为仅靠加速度计和陀螺仪无法获得准确的绝对航向角。
// float yaw = -angle[2] * DEG_TO_RAD;
float cp = cosf(pitch);
float sp = sinf(pitch);
float cr = cosf(roll);
float sr = sinf(roll);
float ax = acc_device[0];
float ay = acc_device[1];
float az = acc_device[2];
// 使用经过验证的、正确的身体坐标系到世界坐标系的旋转矩阵 (基于 Y-X 旋转顺序)
// 这个矩阵将设备测量的加速度(ax, ay, az)正确地转换到世界坐标系(acc_world)。
// 注意：这里没有使用yaw，主要关心的是坡面上的运动，绝对航向暂时不影响速度和距离的计算。
// TODO
acc_world[0] = cp * ax + sp * sr * ay + sp * cr * az;
acc_world[1] = 0 * ax + cr * ay - sr * az;
acc_world[2] = -sp * ax + cp * sr * ay + cp * cr * az;
}
/**
* @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 传入同步修改后传出
* @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]);
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=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：静止
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;
}
// 优先级2：原地旋转 - 特殊的、非滑雪的运动状态
// 条件：角速度很大，同时线性加速度的晃动在一个“中等”范围内。
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 = 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
}
/**
* @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);
// 坐标转换 & 移除重力
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;
tracker->velocity[1] += tracker->acc_world_filtered[1] * dt;
// 垂直方向的速度暂时不积分，极易受姿态误差影响而漂移
// tracker->velocity[2] += tracker->acc_world_filtered[2] * dt;
}
#ifdef XTELL_TEST
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 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;
tracker->velocity[1] *= SPEED_ATTENUATION;
tracker->velocity[2] = 0; // 垂直速度强制归零
}
}
/**
* @brief 传感器数据采集与处理任务，外部每10ms调用一次，如果需要更新时间间隔，也需要同步更新宏“ DELTA_TIME ”
*
* @param acc_data_buf 三轴加速度原始数据
* @param gyr_data_buf 三轴陀螺仪原始数据
* @return BLE_send_data_t
*/
BLE_send_data_t sensor_processing_task(signed short * acc_data_buf, signed short * gyr_data_buf) {
static int initialized = 0;
static int calibration_done = 0;
static signed short combined_raw_data[6];
static float final_angle_data[3]; // 计算得到的欧若拉角
static float calibrated_acc_g[3]; // 转换后的加速度计数据
static float calibrated_gyr_dps[3]; // 转换后的陀螺仪数据
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;
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
}
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
unsigned char status;
if (!calibration_done) { //第1次启动，开启零漂检测
status = SL_SC7U22_Angle_Output(1, combined_raw_data, final_angle_data, 0);
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, final_angle_data, 0);
}
if (status == 1) {
// 加速度 LSB to g
calibrated_acc_g[0] = (float)combined_raw_data[0] / 8192.0f;
calibrated_acc_g[1] = (float)combined_raw_data[1] / 8192.0f;
calibrated_acc_g[2] = (float)combined_raw_data[2] / 8192.0f;
// 陀螺仪 LSB to dps (度/秒)
// ±2000dps量程下，转换系数约为 0.061
calibrated_gyr_dps[0] = (float)combined_raw_data[3] * 0.061f;
calibrated_gyr_dps[1] = (float)combined_raw_data[4] * 0.061f;
calibrated_gyr_dps[2] = (float)combined_raw_data[5] * 0.061f;
skiing_tracker_update(&my_skiing_tracker, calibrated_acc_g, calibrated_gyr_dps, final_angle_data, delta_time);
// static int count = 0;
// if(count >= 10){
// printf("State: %d, Speed: %.2f m/s, Distance: %.2f m\n",
// my_skiing_tracker.state,
// my_skiing_tracker.speed,
// my_skiing_tracker.distance);
// printf("calibrated_acc_g: %.2f, %.2f, %.2f\n",
// calibrated_acc_g[0],
// calibrated_acc_g[1],
// calibrated_acc_g[2]);
// count = 0;
// } else {
// count++;
// }
BLE_send_data.sensor_state = status;
BLE_send_data.skiing_state = my_skiing_tracker.state;
for (int i = 0; i < 3; i++) {
#ifndef XTELL_TEST
BLE_send_data.acc_original[i] = (int)acc_data_buf[i];
BLE_send_data.gyr_original[i] = (int)gyr_data_buf[i];
#endif
#if KS_BLE
KS_data.acc_KS[i] = (int)(calibrated_acc_g[i] * G_ACCELERATION * 100); //cm/s^s
KS_data.gyr_KS_dps[i] = (int)calibrated_gyr_dps[i];
KS_data.angle_KS[i] = (int)final_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");
} else if (status == 0) {
memset(&BLE_send_data, 0, sizeof(BLE_send_data_t));
BLE_send_data.sensor_state = status;
#if KS_BLE
memset(&KS_data, 0, sizeof(BLE_send_data_t));
#endif
// printf("Sensor is calibrating...\n");
} else {
memset(&BLE_send_data, 0, sizeof(BLE_send_data_t));
BLE_send_data.sensor_state = status;
#if KS_BLE
memset(&KS_data, 0, sizeof(BLE_send_data_t));
#endif
// printf("Angle calculation error or calibration not finished.\n");
}
return BLE_send_data;
}

85
apps/earphone/xtell_Sensor/A_hide/8/skiing_tracker.h Normal file
View File

@ -0,0 +1,85 @@
#ifndef SKIING_TRACKER_H
#define SKIING_TRACKER_H
#include "../xtell.h"
// 定义滑雪者可能的状态
typedef enum {
STATIC, // 静止或动态稳定
NO_CONSTANT_SPEED, // 正在滑雪，非匀速
CONSTANT_SPEED, // 正在滑雪，匀速
ROTATING, // 正在原地旋转
WHEEL, // 转弯
FALLEN, // 已摔倒
STOP_DETECTION, // 停止检测
UNKNOWN // 未知状态
} 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_world[3]; // 在世界坐标系下的加速度
// --- 内部计算使用的私有成员 ---
// 用于动态零速更新和旋转检测的缓冲区
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]; //上一次没过滤的
} skiing_tracker_t;
//ble发送的数据
typedef struct __attribute__((packed)){ //该结构体取消内存对齐
char sensor_state;
char skiing_state;
int speed_cms; //求出的速度，cm/s
int distance_cm; //求出的距离，cm
#ifndef XTELL_TEST
int acc_original[3]; //直接读取传感器得到的原始三轴加速度
int gyr_original[3]; //直接读取传感器得到的原始三轴陀螺仪
#endif
}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);
/**
* @brief 传感器数据采集与处理任务，外部每10ms调用一次，如果需要更新时间间隔，也需要同步更新宏“ DELTA_TIME ”
*
* @param acc_data_buf 三轴加速度原始数据
* @param gyr_data_buf 三轴陀螺仪原始数据
* @return BLE_send_data_t
*/
BLE_send_data_t sensor_processing_task(signed short * acc_data_buf, signed short * gyr_data_buf) ;
#endif // SKIING_TRACKER_H

583
apps/earphone/xtell_Sensor/A_hide/9/skiing_tracker.c Normal file
View File

@ -0,0 +1,583 @@
/*
*/
#include "skiing_tracker.h"
#include "../sensor/SC7U22.h"
#include <math.h>
#include <string.h>
#define G_ACCELERATION 9.81f
#define DEG_TO_RAD (3.14159265f / 180.0f)
// --- 静止检测 ---
//两个判断是否静止的必要条件：动态零速更新(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;
#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 将设备坐标系下的加速度转换为世界坐标系
* @param acc_device 设备坐标系下的加速度 [x, y, z]
* @param angle 姿态角 [pitch, roll, yaw]，单位: 度
* @param acc_world 输出：世界坐标系下的加速度 [x, y, z]
*/
static void transform_acc_to_world_frame(const float *acc_device, const float *angle, float *acc_world)
{
// 驱动输出的角度与标准航空定义相反，需要取反才能用于标准旋转矩阵。
float pitch = -angle[0] * DEG_TO_RAD;
float roll = -angle[1] * DEG_TO_RAD;
// TODO: 当引入三轴磁力计后，这里的 yaw 应由磁力计和陀螺仪融合解算得出，以解决航向漂移问题。
// 目前 yaw 暂时不参与计算，因为仅靠加速度计和陀螺仪无法获得准确的绝对航向角。
// float yaw = -angle[2] * DEG_TO_RAD;
float cp = cosf(pitch);
float sp = sinf(pitch);
float cr = cosf(roll);
float sr = sinf(roll);
float ax = acc_device[0];
float ay = acc_device[1];
float az = acc_device[2];
// 使用经过验证的、正确的身体坐标系到世界坐标系的旋转矩阵 (基于 Y-X 旋转顺序)
// 这个矩阵将设备测量的加速度(ax, ay, az)正确地转换到世界坐标系(acc_world)。
// 注意：这里没有使用yaw，主要关心的是坡面上的运动，绝对航向暂时不影响速度和距离的计算。
// TODO
acc_world[0] = cp * ax + sp * sr * ay + sp * cr * az;
acc_world[1] = 0 * ax + cr * ay - sr * az;
acc_world[2] = -sp * ax + cp * sr * ay + cp * cr * az;
}
/**
* @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 && 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:
transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world);
tracker->acc_world[2] -= G_ACCELERATION;
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_world[i] - tracker->acc_world_unfiltered_prev[i]);
tracker->acc_world_unfiltered_prev[i] = tracker->acc_world[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;
}
}
/**
* @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) {
static int initialized = 0;
static float acc_data_g[3];
static float gyr_data_dps[3];
// 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;
}

88
apps/earphone/xtell_Sensor/A_hide/9/skiing_tracker.h Normal file
View File

@ -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_world[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) ;
#endif // SKIING_TRACKER_H

651
apps/earphone/xtell_Sensor/A_hide/calculate/skiing_tracker.c Normal file
View File

@ -0,0 +1,651 @@
/*
*/
#include "skiing_tracker.h"
#include "../sensor/SC7U22.h"
#include <math.h>
#include <string.h>
#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;
}

88
apps/earphone/xtell_Sensor/A_hide/calculate/skiing_tracker.h Normal file
View File

@ -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