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

存档

Browse Source
This commit is contained in:
lmx
2025-11-07 17:14:58 +08:00
parent ae980789b6
commit 289a6b780b
23 changed files with 167650 additions and 167301 deletions
Download Patch File Download Diff File Expand all files Collapse all files

208
apps/earphone/xtell_Sensor/calculate/tmp.c → apps/earphone/xtell_Sensor/A_hide/7/skiing_tracker.c
View File

@ -1,8 +1,15 @@
/*
动态ZUPT+卡尔曼
多了加速度死区、摩擦力速度衰减、高通滤波
原地摆动产生的速度、距离变化还是没法消除
水平移动、斜坡移动效果貌似还行
动态ZUPT+卡尔曼+巴特沃斯一阶滤波器
针对启动滑雪和停止滑雪，设置不同阈值
启动滑雪和ZUPT更新的陀螺仪方差阈值分开设置
- 启动滑雪的陀螺仪阈值会更宽松一些
原地旋转和ZUPT更新的加速度方差阈值分开设置
- 原地旋转的加速度阈值更宽松
能够从静止状态到变化状态，去根据阈值来判断这个“变化”：进入滑行状态 / 只是原地摆动
- 但是还是不够灵敏
添加了滑雪过程中转弯判断，参数待调
*/
#include "skiing_tracker.h"
#include "../sensor/SC7U22.h"
@ -12,33 +19,51 @@
#define G_ACCELERATION 9.81f
#define DEG_TO_RAD (3.14159265f / 180.0f)
// --- 算法阈值定义 ---
//两个判断是否静止的必要条件
// 动态零速更新(ZUPT)阈值
// 提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
// --- ZUPT ---
//两个判断是否静止的必要条件：动态零速更新(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 SKIING_GYR_VARIANCE_THRESHOLD 15.0f
// --- 原地旋转抖动 ---
// 用于原地旋转判断的加速度方差阈值。此值比ZUPT阈值更宽松，
// 以允许原地旋转时身体的正常晃动，但仍能与真实滑行时的剧烈加速度变化区分开。
#define ROTATING_ACC_VARIANCE_THRESHOLD 0.8f
// 旋转/摆动检测阈值：角速度总模长大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
#define ROTATION_GYR_MAG_THRESHOLD 45.0f
// --- 滑雪转弯动 ---
// 加速度方差阈值，大于此值，滑雪过程可能发生了急转弯
#define WHEEL_ACC_VARIANCE_THRESHOLD 3.0f
// 旋转/摆动检测阈值：角速度总模长大于此值(度/秒)，认为滑雪过程中进行急转弯
#define WHEEL_GYR_MAG_THRESHOLD 180.0f
// --- 用于消除积分漂移的滤波器和阈值 ---
// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
// alpha = RC / (RC + dt)，
#define HPF_ALPHA 0.95f
#define HPF_ALPHA 0.969f
// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
#define ACC_DEAD_ZONE_THRESHOLD 0.1f
#define ACC_DEAD_ZONE_THRESHOLD 0.05f
// --- 模拟摩擦力，进行速度衰减 ---
#define SPEED_ATTENUATION 0.98f
#define SPEED_ATTENUATION 1.0f //暂不模拟
//////////////////////////////////////////////////////////////////////////////////////////////////
//实现
/**
* @brief 初始化滑雪追踪器
* @brief 初始化滑雪追踪器
*
* @param tracker
*/
void skiing_tracker_init(skiing_tracker_t *tracker)
{
@ -87,13 +112,16 @@ static void transform_acc_to_world_frame(const float *acc_device, const float *a
/**
* @brief 计算缓冲区内三轴数据的方差之和
*
* @param buffer 传进来的三轴数据：陀螺仪/加速度
* @return float 返回方差和
*/
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];
@ -103,7 +131,7 @@ static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
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]);
@ -118,8 +146,13 @@ static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
}
/**
* @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)
{
@ -135,48 +168,54 @@ static void update_state_machine(skiing_tracker_t *tracker, const float *acc_dev
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) {
// --- 状态切换逻辑 (按优先级) ---
// 优先级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;
}
if(tracker->state == SKIING_STATE_SKIING && gyr_magnitude > WHEEL_GYR_MAG_THRESHOLD && acc_variance > WHEEL_ACC_VARIANCE_THRESHOLD){
//TODO：可以考虑清掉速度消除积分带来的漂移
tracker->state = SKIING_STATE_ROTATING;
return;
}
// 动态零速更新 (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) {
// 最终版启动逻辑：必须同时满足“有足够大的线性加速度”和“旋转不剧烈”两个条件
// 新增 gyr_magnitude 判断，防止原地旋转产生的离心加速度被误判为启动
if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD &&
gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD &&
gyr_magnitude < ROTATION_GYR_MAG_THRESHOLD) {
tracker->state = SKIING_STATE_SKIING;
return;
}
// 优先级2：原地旋转 - 特殊的、非滑雪的运动状态
// 条件：角速度很大，同时线性加速度的晃动在一个“中等”范围内。
if (tracker->state == SKIING_STATE_STATIC && gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && acc_variance < ROTATING_ACC_VARIANCE_THRESHOLD) {
tracker->state = SKIING_STATE_ROTATING;
return;
}
// 最后的 fall-through 逻辑已移除，以修复原地旋转被错误判断为滑雪的bug。
// 如果不满足任何状态切换条件，状态将保持不变，直到ZUPT或启动条件被满足。
// 优先级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)
{
@ -244,8 +283,15 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d
}
// 传感器数据采集与处理任务
void sensor_processing_task(signed short * acc_data_buf, signed short * gyr_data_buf) {
BLE_KS_send_data_t KS_data;
/**
* @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;
@ -255,7 +301,11 @@ void sensor_processing_task(signed short * acc_data_buf, signed short * gyr_data
static float calibrated_acc_g[3]; // 转换后的加速度计数据
static float calibrated_gyr_dps[3]; // 转换后的陀螺仪数据
const float delta_time = 0.01f;
const float delta_time = DELTA_TIME;
BLE_send_data_t BLE_send_data;
if (!initialized) {
skiing_tracker_init(&my_skiing_tracker);
@ -274,6 +324,7 @@ void sensor_processing_task(signed short * acc_data_buf, signed short * gyr_data
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);
}
@ -291,22 +342,51 @@ void sensor_processing_task(signed short * acc_data_buf, signed short * gyr_data
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;
// 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
}
printf("State: %d, Speed: %.2f m/s, Distance: %.2f m\n",
my_skiing_tracker.state,
my_skiing_tracker.speed,
my_skiing_tracker.distance);
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;
}

74
apps/earphone/xtell_Sensor/A_hide/7/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[3]; //卡尔曼后，LSB转换后的 三轴加速度数据（cm/s^2）
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

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

@ -7,6 +7,8 @@
- 原地旋转的加速度阈值更宽松
能够从静止状态到变化状态，去根据阈值来判断这个“变化”：进入滑行状态 / 只是原地摆动
- 但是还是不够灵敏
添加了滑雪过程中转弯判断，参数待调
*/
#include "skiing_tracker.h"
@ -17,41 +19,49 @@
#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
// --- 原地旋转抖动 ---
// 用于原地旋转判断的加速度方差阈值。此值比ZUPT阈值更宽松，
// 以允许原地旋转时身体的正常晃动，但仍能与真实滑行时的剧烈加速度变化区分开。
#define ROTATING_ACC_VARIANCE_THRESHOLD 0.8f
// 用于启动滑雪判断的陀螺仪方差阈值。此值比ZUPT阈值更宽松，
// 以允许启动瞬间的正常抖动，但仍能过滤掉混乱的、非滑雪的晃动。
#define SKIING_GYR_VARIANCE_THRESHOLD 15.0f
// 旋转/摆动检测阈值：角速度总模长大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
#define ROTATION_GYR_MAG_THRESHOLD 90.0f //测试记录：45.0f、90.0f
// 启动滑雪阈值：加速度模长与重力的差值大于此值，认为开始运动
// 降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
#define START_SKIING_ACC_THRESHOLD 0.5f
#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)，
#define HPF_ALPHA 0.95f // 换算大概就是衰减频率低于约 0.84 Hz 的信号
// 任何比“大约1秒钟变化一次”还要慢的运动，其加速度信号也会被部分衰减。
// 而滑雪时的快速转弯、加减速等动作，其频率远高于 0.84 Hz，它们的信号会被保留下来。
// alpha = RC / (RC + dt)，参考RC电路而来
#define HPF_ALPHA 0.995
// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
#define ACC_DEAD_ZONE_THRESHOLD 0.15f
#define ACC_DEAD_ZONE_THRESHOLD 0.05f
// --- 模拟摩擦力，进行速度衰减 ---
#define SPEED_ATTENUATION 0.98f
#define SPEED_ATTENUATION 1.0f //暂不模拟
#ifdef XTELL_TEST
BLE_KS_send_data_t KS_data;
debug_t debug1;
debug_t debug2;
#endif
//////////////////////////////////////////////////////////////////////////////////////////////////
//实现
/**
@ -161,8 +171,13 @@ static void update_state_machine(skiing_tracker_t *tracker, const float *acc_dev
float gyr_variance = calculate_variance(tracker->gyr_buffer); // 计算陀螺仪方差
float gyr_magnitude = sqrtf(gyr_dps[0]*gyr_dps[0] + gyr_dps[1]*gyr_dps[1] + gyr_dps[2]*gyr_dps[2]);
float acc_magnitude = sqrtf(acc_device_ms2[0]*acc_device_ms2[0] + acc_device_ms2[1]*acc_device_ms2[1] + acc_device_ms2[2]*acc_device_ms2[2]);
#ifdef XTELL_TEST
debug1.acc_variance =acc_variance;
debug1.gyr_variance =gyr_variance;
debug1.gyr_magnitude=gyr_magnitude;
debug1.acc_magnitude=acc_magnitude - G_ACCELERATION;
#endif
// --- 状态切换逻辑 (按优先级) ---
// 优先级1：动态零速更新 (ZUPT) - 最严格和最优先的“刹车”
@ -177,9 +192,15 @@ static void update_state_machine(skiing_tracker_t *tracker, const float *acc_dev
return;
}
if(tracker->state == SKIING_STATE_SKIING && gyr_magnitude > WHEEL_GYR_MAG_THRESHOLD && acc_variance > WHEEL_ACC_VARIANCE_THRESHOLD){
//TODO：可以考虑清掉速度消除积分带来的漂移
tracker->state = SKIING_STATE_ROTATING;
return;
}
// 优先级2：原地旋转 - 特殊的、非滑雪的运动状态
// 条件：角速度很大，同时线性加速度的晃动在一个“中等”范围内。
if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && acc_variance < ROTATING_ACC_VARIANCE_THRESHOLD) {
if (tracker->state == SKIING_STATE_STATIC && gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && acc_variance < ROTATING_ACC_VARIANCE_THRESHOLD) {
tracker->state = SKIING_STATE_ROTATING;
return;
}
@ -253,6 +274,9 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d
// 垂直方向的速度暂时不积分，极易受姿态误差影响而漂移
// tracker->velocity[2] += tracker->acc_world_filtered[2] * dt;
}
#ifdef XTELL_TEST
debug2.acc_magnitude = acc_horizontal_mag;
#endif
// 如果加速度小于阈值，则不更新速度，相当于速度保持不变（或受下一步的阻尼影响而衰减）
} else {
@ -291,7 +315,7 @@ BLE_send_data_t sensor_processing_task(signed short * acc_data_buf, signed short
const float delta_time = DELTA_TIME;
BLE_send_data_t BLE_send_data;
BLE_KS_send_data_t KS_data;
@ -330,13 +354,16 @@ BLE_send_data_t sensor_processing_task(signed short * acc_data_buf, signed short
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++;
@ -350,7 +377,7 @@ BLE_send_data_t sensor_processing_task(signed short * acc_data_buf, signed short
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.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

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

@ -51,11 +51,19 @@ typedef struct __attribute__((packed)){ //该结构体取消内存对齐
}BLE_send_data_t;
typedef struct{
int acc_KS_g[3]; //卡尔曼后，LSB to g 三轴加速度数据
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 初始化滑雪追踪器
*

102
apps/earphone/xtell_Sensor/send_data.c
View File

@ -137,11 +137,13 @@ void ble_send_data(signed short *acc_gyro_input, float *Angle_output){
send_data_to_ble_client(&buffer,len);
}
// 从环形缓冲区读取数据并发送
void send_sensor_data_task(void) {
// printf("xtell_ble_send\n");
}
#ifdef XTELL_TEST
void test(){
signed short acc_data_buf[3] = {0};
signed short gyr_data_buf[3] = {0};
@ -162,15 +164,9 @@ void test(){
static int count = 0;
if(count >=10){
count = 0;
#ifdef XTELL_TEST
xlog("BLE_send_data_t:%d\n",sizeof(BLE_send_data_t));
xlog("ACC_X:%d, ACC_Y:%d, ACC_Z:%d, GYR_X:%.d, GYR_Y:%d, GYR_Z:%d",
acc_data_buf[0],acc_data_buf[1],acc_data_buf[2],gyr_data_buf[0],gyr_data_buf[1],gyr_data_buf[2]
);
printf("State: %d, Speed: %d cm/s, Distance: %d cm\n",
BLE_send_data.skiing_state,
BLE_send_data.speed_cms,
BLE_send_data.distance_cm);
char* division = "==========\n";
send_data_to_ble_client(division,strlen(division));
char log_buffer[100]; // 100个字符应该足够了
@ -178,18 +174,61 @@ void test(){
int num_chars_written = snprintf(
log_buffer, // 目标缓冲区
sizeof(log_buffer), // 目标缓冲区的最大容量
"State: %d, Speed: %d cm/s, Distance: %d cm\n", // 格式化字符串
"s %d, %dcm/s, %dcm\n", // 格式化字符串
BLE_send_data.skiing_state, // 第一个 %d 的参数
BLE_send_data.speed_cms, // 第二个 %d 的参数
BLE_send_data.distance_cm // 第三个 %d 的参数
);
send_data_to_ble_client(&log_buffer,strlen(log_buffer));
// xlog("Pitch:%.2f, Roll:%.2f, Yaw:%.2f\n",
// Angle_output[0],Angle_output[1],Angle_output[2]
// );
#else
send_data_to_ble_client(&data,sizeof(BLE_send_data_t)+4);
#endif
extern BLE_KS_send_data_t KS_data;
memset(&log_buffer, 0, 100);
num_chars_written = snprintf(
log_buffer,
sizeof(log_buffer),
"Acc:%d, %d, %d\n",
KS_data.acc_KS[0],KS_data.acc_KS[1],KS_data.acc_KS[2]
); // cm/s^2
send_data_to_ble_client(&log_buffer,strlen(log_buffer));
memset(&log_buffer, 0, 100);
num_chars_written = snprintf(
log_buffer,
sizeof(log_buffer),
"Gyr_dps:%d, %d, %d\n",
KS_data.gyr_KS_dps[0],
KS_data.gyr_KS_dps[1],
KS_data.gyr_KS_dps[2]
);
send_data_to_ble_client(&log_buffer,strlen(log_buffer));
memset(&log_buffer, 0, 100);
num_chars_written = snprintf(
log_buffer,
sizeof(log_buffer),
"angle: %d, %d, %d\n",
KS_data.angle_KS[0],
KS_data.angle_KS[1],
KS_data.angle_KS[2]
);
send_data_to_ble_client(&log_buffer,strlen(log_buffer));
extern debug_t debug1;
extern debug_t debug2;
memset(&log_buffer, 0, 100);
num_chars_written = snprintf(
log_buffer,
sizeof(log_buffer),
"debug:%.2f,%.2f,%.2f(%.2f),%.2f\n",
debug1.acc_variance,
debug1.gyr_variance,
debug1.acc_magnitude,
debug2.acc_magnitude, //滤波后的加速度
debug1.gyr_magnitude
);
send_data_to_ble_client(&log_buffer,strlen(log_buffer));
}
count++;
@ -197,6 +236,37 @@ void test(){
memset(&data, 0, 50);
}
#else
void BLE_send_data(){
signed short acc_data_buf[3] = {0};
signed short gyr_data_buf[3] = {0};
signed short acc_gyro_input[6] = {0};
float Angle_output[3] = {0};
SL_SC7U22_RawData_Read(acc_data_buf,gyr_data_buf);
BLE_send_data = sensor_processing_task(acc_data_buf, gyr_data_buf);
u8 data[50];
data[0] = 0xBB;
data[1] = 0xBE;
data[2] = 0x01;
data[3] = sizeof(BLE_send_data_t); //后续包的数据长度
// send_data_to_ble_client(&data,sizeof(BLE_send_data_t)+4);
memcpy(&data[4], &BLE_send_data, sizeof(BLE_send_data_t));
static int count = 0;
if(count >=10){
count = 0;
send_data_to_ble_client(&data,sizeof(BLE_send_data_t)+4);
}
count++;
memset(&BLE_send_data, 0, sizeof(BLE_send_data_t));
memset(&data, 0, 50);
}
#endif
void gsensor_test(){
sys_timer_del(gsensor_id);

2
apps/earphone/xtell_Sensor/xtell.h
View File

@ -1,7 +1,7 @@
#ifndef XTELL_H
#define XTELL_H
#define KS_BLE 0
#define KS_BLE 1
#define XTELL_TEST 1