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

cun

Browse Source
This commit is contained in:
lmx
2025-11-04 19:20:58 +08:00
parent 6be3cd1070
commit ac7299e7ad
28 changed files with 156026 additions and 155617 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1,16 +1,25 @@
/*
动态ZUPT+卡尔曼
*/
#include "skiing_tracker.h"
#include "../sensor/SC7U22.h" // 包含传感器驱动头文件以调用姿态解算函数
#include "../sensor/SC7U22.h"
#include <math.h>
#include <string.h>
#define G_ACCELERATION 9.81f // 重力加速度 (m/s^2)
#define G_ACCELERATION 9.81f
#define DEG_TO_RAD (3.14159265f / 180.0f)
// --- 状态检测阈值 ---
// 判断是否静止的加速度阈值 (m/s^2)。当加速度的模长减去重力后，小于此值，则认为可能静止。
#define STATIC_ACC_THRESHOLD 1.0f
// 连续多少帧满足静止条件才确认为静止状态
#define STATIC_FRAMES_REQUIRED 50 // 累加，超过这个数加速度仍变化不大，说明近似静止
// --- 算法阈值定义 ---
//两个判断是否静止的必要条件
// 动态零速更新(ZUPT)阈值
#define ZUPT_ACC_VARIANCE_THRESHOLD 0.05f
// 陀螺仪方差阈值
#define ZUPT_GYR_VARIANCE_THRESHOLD 5.0f
// 旋转/摆动检测阈值：角速度总模长大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
#define ROTATION_GYR_MAG_THRESHOLD 100.0f
// 启动滑雪阈值：加速度模长与重力的差值大于此值，认为开始运动
#define START_SKIING_ACC_THRESHOLD 1.5f
/**
* @brief 初始化滑雪追踪器
@ -20,8 +29,9 @@ void skiing_tracker_init(skiing_tracker_t *tracker)
if (!tracker) {
return;
}
// 使用memset一次性清零整个结构体，包括新增的缓冲区
memset(tracker, 0, sizeof(skiing_tracker_t));
tracker->state = SKIING_STATE_STATIC; // 初始状态为静止
tracker->state = SKIING_STATE_STATIC;
}
/**
@ -32,8 +42,9 @@ void skiing_tracker_init(skiing_tracker_t *tracker)
*/
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 pitch = -angle[0] * DEG_TO_RAD;
float roll = -angle[1] * DEG_TO_RAD;
float cp = cosf(pitch);
float sp = sinf(pitch);
@ -44,56 +55,95 @@ static void transform_acc_to_world_frame(const float *acc_device, const float *a
float ay = acc_device[1];
float az = acc_device[2];
// 旋转矩阵（简化版，仅用Pitch和Roll）
// Z-Y-X 顺序旋转
acc_world[0] = cr * ax + sr * sp * ay + sr * cp * az;
acc_world[1] = cp * ay - sp * az;
acc_world[2] = -sr * ax + cr * sp * ay + cr * cp * az;
// 使用经过验证的、正确的身体坐标系到世界坐标系的旋转矩阵 (基于 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 更新滑雪状态机
* @brief 计算缓冲区内三轴数据的方差之和
*/
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device)
static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
{
// 计算当前加速度的模长
float acc_magnitude = sqrtf(acc_device[0] * acc_device[0] + acc_device[1] * acc_device[1] + acc_device[2] * acc_device[2]);
float mean[3] = {0};
float variance[3] = {0};
// 状态判断逻辑
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;
// 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;
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;
// 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;
default:
tracker->state = SKIING_STATE_UNKNOWN;
break;
// 返回三轴方差之和，作为一个综合的稳定度指标
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) {
tracker->state = SKIING_STATE_SKIING;
return;
}
}
// 滑雪
if (tracker->state != SKIING_STATE_STATIC) {
tracker->state = SKIING_STATE_SKIING;
}
}
@ -101,77 +151,76 @@ static void update_state_machine(skiing_tracker_t *tracker, const float *acc_dev
/**
* @brief 主更新函数
*/
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc, float *angle, float dt)
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt)
{
if (!tracker || !acc || !angle || dt <= 0) {
if (!tracker || !acc_g || !gyr_dps || !angle || dt <= 0) {
return;
}
// 将原始g单位的加速度转换为 m/s^2
// --- 1. 数据预处理和缓冲 ---
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;
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;
// 更新状态机
update_state_machine(tracker, acc_device_ms2);
// 将最新数据存入缓冲区
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) {
// 坐标系转换: 首先，利用姿态角(Pitch, Roll)将传感器测得的总加速度（运动加速度+重力）
// 从不断变化的“设备坐标系”转换到一个固定的“世界坐标系”。在这个世界坐标系里，Z轴永远垂直于地面指向上方。
// 执行坐标系转换
transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world);
// 转换完成后，重力就变成了一个恒定的、方向沿Z轴向下的矢量。
// 在世界坐标系下，移除Z轴上的重力分量
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;
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) {
// --- 1. 定义静态变量 ---
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 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 final_angle_data[3]; // 计算得到的欧若拉角
static float calibrated_acc_g[3]; // 转换后的加速度计数据
static float calibrated_gyr_dps[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) { //正在校准
//领票校准
if (!calibration_done) { //第1次启动，开启零漂检测
status = SL_SC7U22_Angle_Output(1, combined_raw_data, final_angle_data, 0);
if (status == 1) {
calibration_done = 1;
@ -181,21 +230,26 @@ void sensor_processing_task(signed short * acc_data_buf,signed short * gyr_data_
status = SL_SC7U22_Angle_Output(0, combined_raw_data, final_angle_data, 0);
}
// c. 检查姿态解算是否成功
if (status == 1) {
// 将校准后的加速度数据从 LSB (原始值) 转换为 g (重力单位)
// ±8g量程下，8192 LSB 对应 1g
// 加速度 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;
skiing_tracker_update(&my_skiing_tracker, calibrated_acc_g, final_angle_data, delta_time);
// 陀螺仪 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;
static count = 0;
if(count < 10){
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{
} else {
count = 0;
}
printf("State: %d, Speed: %.2f m/s, Distance: %.2f m\n",
@ -204,10 +258,8 @@ void sensor_processing_task(signed short * acc_data_buf,signed short * gyr_data_
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");
}
}

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

@ -3,22 +3,31 @@
// 定义滑雪者可能的状态
typedef enum {
SKIING_STATE_STATIC, // 静止
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; // 当前滑雪状态
// 私有成员，用于内部计算
int static_frames_count; // 用于判断静止状态的帧计数器
// 内部计算使用的私有成员
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;
/**
@ -32,10 +41,10 @@ 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 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, float *angle, float dt);
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt);
#endif // SKIING_TRACKER_H