第6版
This commit is contained in:
3
.vscode/settings.json
vendored
3
.vscode/settings.json
vendored
@ -22,6 +22,7 @@
|
|||||||
"math.h": "c",
|
"math.h": "c",
|
||||||
"avctp_user.h": "c",
|
"avctp_user.h": "c",
|
||||||
"string.h": "c",
|
"string.h": "c",
|
||||||
"dev_manager.h": "c"
|
"dev_manager.h": "c",
|
||||||
|
"bt_tws.h": "c"
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
277
apps/earphone/xtell_Sensor/A_hide/3/skiing_tracker.c
Normal file
277
apps/earphone/xtell_Sensor/A_hide/3/skiing_tracker.c
Normal 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
50
apps/earphone/xtell_Sensor/A_hide/3/skiing_tracker.h
Normal 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
465
apps/earphone/xtell_Sensor/A_hide/4/skiing_tracker.c
Normal 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
40
apps/earphone/xtell_Sensor/A_hide/4/skiing_tracker.h
Normal 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
311
apps/earphone/xtell_Sensor/A_hide/5/skiing_tracker.c
Normal 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
55
apps/earphone/xtell_Sensor/A_hide/5/skiing_tracker.h
Normal 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
374
apps/earphone/xtell_Sensor/A_hide/6/skiing_tracker.c
Normal 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
74
apps/earphone/xtell_Sensor/A_hide/6/skiing_tracker.h
Normal 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
|
||||||
@ -1,5 +1,13 @@
|
|||||||
/*
|
/*
|
||||||
动态ZUPT+卡尔曼
|
动态ZUPT+卡尔曼+巴特沃斯一阶滤波器
|
||||||
|
针对启动滑雪和停止滑雪，设置不同阈值
|
||||||
|
启动滑雪和ZUPT更新的陀螺仪方差阈值分开设置
|
||||||
|
- 启动滑雪的陀螺仪阈值会更宽松一些
|
||||||
|
原地旋转和ZUPT更新的加速度方差阈值分开设置
|
||||||
|
- 原地旋转的加速度阈值更宽松
|
||||||
|
能够从静止状态到变化状态，去根据阈值来判断这个“变化”：进入滑行状态 / 只是原地摆动
|
||||||
|
- 但是还是不够灵敏
|
||||||
|
|
||||||
*/
|
*/
|
||||||
#include "skiing_tracker.h"
|
#include "skiing_tracker.h"
|
||||||
#include "../sensor/SC7U22.h"
|
#include "../sensor/SC7U22.h"
|
||||||
@ -10,19 +18,46 @@
|
|||||||
#define DEG_TO_RAD (3.14159265f / 180.0f)
|
#define DEG_TO_RAD (3.14159265f / 180.0f)
|
||||||
|
|
||||||
// --- 算法阈值定义 ---
|
// --- 算法阈值定义 ---
|
||||||
//两个判断是否静止的必要条件
|
//两个判断是否静止的必要条件：动态零速更新(ZUPT)阈值
|
||||||
// 动态零速更新(ZUPT)阈值
|
// 加速方差阈值，提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
|
||||||
#define ZUPT_ACC_VARIANCE_THRESHOLD 0.05f
|
#define ZUPT_ACC_VARIANCE_THRESHOLD 0.2f
|
||||||
// 陀螺仪方差阈值
|
// 陀螺仪方差阈值
|
||||||
#define ZUPT_GYR_VARIANCE_THRESHOLD 5.0f
|
#define ZUPT_GYR_VARIANCE_THRESHOLD 5.0f
|
||||||
|
|
||||||
// 旋转/摆动检测阈值：角速度总模长大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
|
// 用于原地旋转判断的加速度方差阈值。此值比ZUPT阈值更宽松，
|
||||||
#define ROTATION_GYR_MAG_THRESHOLD 100.0f
|
// 以允许原地旋转时身体的正常晃动，但仍能与真实滑行时的剧烈加速度变化区分开。
|
||||||
// 启动滑雪阈值：加速度模长与重力的差值大于此值，认为开始运动
|
#define ROTATING_ACC_VARIANCE_THRESHOLD 0.8f
|
||||||
#define START_SKIING_ACC_THRESHOLD 1.5f
|
// 用于启动滑雪判断的陀螺仪方差阈值。此值比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
|
||||||
|
|
||||||
|
// --- 用于消除积分漂移的滤波器和阈值 ---
|
||||||
|
// 高通滤波器系数 (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 初始化滑雪追踪器
|
* @brief 初始化滑雪追踪器
|
||||||
|
*
|
||||||
|
* @param tracker
|
||||||
*/
|
*/
|
||||||
void skiing_tracker_init(skiing_tracker_t *tracker)
|
void skiing_tracker_init(skiing_tracker_t *tracker)
|
||||||
{
|
{
|
||||||
@ -46,6 +81,10 @@ static void transform_acc_to_world_frame(const float *acc_device, const float *a
|
|||||||
float pitch = -angle[0] * DEG_TO_RAD;
|
float pitch = -angle[0] * DEG_TO_RAD;
|
||||||
float roll = -angle[1] * 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 cp = cosf(pitch);
|
||||||
float sp = sinf(pitch);
|
float sp = sinf(pitch);
|
||||||
float cr = cosf(roll);
|
float cr = cosf(roll);
|
||||||
@ -57,6 +96,8 @@ static void transform_acc_to_world_frame(const float *acc_device, const float *a
|
|||||||
|
|
||||||
// 使用经过验证的、正确的身体坐标系到世界坐标系的旋转矩阵 (基于 Y-X 旋转顺序)
|
// 使用经过验证的、正确的身体坐标系到世界坐标系的旋转矩阵 (基于 Y-X 旋转顺序)
|
||||||
// 这个矩阵将设备测量的加速度(ax, ay, az)正确地转换到世界坐标系(acc_world)。
|
// 这个矩阵将设备测量的加速度(ax, ay, az)正确地转换到世界坐标系(acc_world)。
|
||||||
|
// 注意：这里没有使用yaw，主要关心的是坡面上的运动，绝对航向暂时不影响速度和距离的计算。
|
||||||
|
// TODO
|
||||||
acc_world[0] = cp * ax + sp * sr * ay + sp * cr * az;
|
acc_world[0] = cp * ax + sp * sr * ay + sp * cr * az;
|
||||||
acc_world[1] = 0 * ax + cr * ay - sr * az;
|
acc_world[1] = 0 * ax + cr * ay - sr * az;
|
||||||
acc_world[2] = -sp * ax + cp * sr * ay + cp * cr * az;
|
acc_world[2] = -sp * ax + cp * sr * ay + cp * cr * az;
|
||||||
@ -65,13 +106,16 @@ static void transform_acc_to_world_frame(const float *acc_device, const float *a
|
|||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief 计算缓冲区内三轴数据的方差之和
|
* @brief 计算缓冲区内三轴数据的方差之和
|
||||||
|
*
|
||||||
|
* @param buffer 传进来的三轴数据：陀螺仪/加速度
|
||||||
|
* @return float 返回方差和
|
||||||
*/
|
*/
|
||||||
static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
|
static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
|
||||||
{
|
{
|
||||||
float mean[3] = {0};
|
float mean[3] = {0};
|
||||||
float variance[3] = {0};
|
float variance[3] = {0};
|
||||||
|
|
||||||
// 1. 计算均值
|
// 计算均值
|
||||||
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
|
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
|
||||||
mean[0] += buffer[i][0];
|
mean[0] += buffer[i][0];
|
||||||
mean[1] += buffer[i][1];
|
mean[1] += buffer[i][1];
|
||||||
@ -81,7 +125,7 @@ static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
|
|||||||
mean[1] /= VARIANCE_BUFFER_SIZE;
|
mean[1] /= VARIANCE_BUFFER_SIZE;
|
||||||
mean[2] /= VARIANCE_BUFFER_SIZE;
|
mean[2] /= VARIANCE_BUFFER_SIZE;
|
||||||
|
|
||||||
// 2. 计算方差
|
// 计算方差
|
||||||
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
|
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
|
||||||
variance[0] += (buffer[i][0] - mean[0]) * (buffer[i][0] - mean[0]);
|
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[1] += (buffer[i][1] - mean[1]) * (buffer[i][1] - mean[1]);
|
||||||
@ -96,8 +140,13 @@ static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief 升级后的状态机，包含旋转检测和动态零速更新
|
* @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)
|
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps)
|
||||||
{
|
{
|
||||||
@ -107,49 +156,54 @@ static void update_state_machine(skiing_tracker_t *tracker, const float *acc_dev
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
// --- 1. 计算关键指标 ---
|
// --- 计算关键指标 ---
|
||||||
float acc_variance = calculate_variance(tracker->acc_buffer); // 计算加速度方差
|
float acc_variance = calculate_variance(tracker->acc_buffer); // 计算加速度方差
|
||||||
float gyr_variance = calculate_variance(tracker->gyr_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 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]);
|
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)
|
// 优先级1：动态零速更新 (ZUPT) - 最严格和最优先的“刹车”
|
||||||
// 必须同时满足加速度和角速度都稳定，才能判断为“真静止”，以区分匀速运动
|
|
||||||
if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
|
if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
|
||||||
tracker->state = SKIING_STATE_STATIC;
|
tracker->state = SKIING_STATE_STATIC;
|
||||||
// 速度清零，抑制漂移
|
// 速度清零，抑制漂移
|
||||||
memset(tracker->velocity, 0, sizeof(tracker->velocity));
|
memset(tracker->velocity, 0, sizeof(tracker->velocity));
|
||||||
tracker->speed = 0.0f;
|
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;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
// 从静止/旋转状态启动
|
// 优先级2：原地旋转 - 特殊的、非滑雪的运动状态
|
||||||
if (tracker->state == SKIING_STATE_STATIC || tracker->state == SKIING_STATE_ROTATING) {
|
// 条件：角速度很大，同时线性加速度的晃动在一个“中等”范围内。
|
||||||
if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD) {
|
if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && acc_variance < ROTATING_ACC_VARIANCE_THRESHOLD) {
|
||||||
tracker->state = SKIING_STATE_SKIING;
|
tracker->state = SKIING_STATE_ROTATING;
|
||||||
return;
|
return;
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// 滑雪
|
// 优先级3：启动滑雪 - “油门”
|
||||||
if (tracker->state != SKIING_STATE_STATIC) {
|
// 条件：有足够大的线性加速度，同时陀螺仪的抖动在一个“合理”（而非“完全静止”）的范围内。
|
||||||
|
if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD && gyr_variance < SKIING_GYR_VARIANCE_THRESHOLD) {
|
||||||
tracker->state = SKIING_STATE_SKIING;
|
tracker->state = SKIING_STATE_SKIING;
|
||||||
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// 如果不满足任何启动或停止条件，则保持当前状态（滑雪中）
|
||||||
|
// 如果当前是静止或旋转，但没有满足启动条件，则状态会保持，直到满足ZUPT或旋转条件。
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief 主更新函数
|
* @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)
|
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt)
|
||||||
{
|
{
|
||||||
@ -157,7 +211,7 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
// --- 1. 数据预处理和缓冲 ---
|
// --- 数据预处理和缓冲 ---
|
||||||
float acc_device_ms2[3];
|
float acc_device_ms2[3];
|
||||||
acc_device_ms2[0] = acc_g[0] * G_ACCELERATION;
|
acc_device_ms2[0] = acc_g[0] * G_ACCELERATION;
|
||||||
acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
|
acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
|
||||||
@ -173,32 +227,59 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d
|
|||||||
tracker->buffer_filled = 1; // 标记缓冲区已满
|
tracker->buffer_filled = 1; // 标记缓冲区已满
|
||||||
}
|
}
|
||||||
|
|
||||||
// --- 2. 更新状态机 ---
|
// --- 更新状态机 ---
|
||||||
update_state_machine(tracker, acc_device_ms2, gyr_dps);
|
update_state_machine(tracker, acc_device_ms2, gyr_dps);
|
||||||
|
|
||||||
// --- 3. 根据状态进行计算 ---
|
// --- 根据状态进行计算 ---
|
||||||
// 只有在明确的“滑雪”状态下才进行积分
|
|
||||||
if (tracker->state == SKIING_STATE_SKIING) {
|
if (tracker->state == SKIING_STATE_SKIING) {
|
||||||
|
// 坐标转换 & 移除重力
|
||||||
transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world);
|
transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world);
|
||||||
tracker->acc_world[2] -= G_ACCELERATION;
|
tracker->acc_world[2] -= G_ACCELERATION;
|
||||||
|
|
||||||
tracker->velocity[0] += tracker->acc_world[0] * dt;
|
// 对世界坐标系下的加速度进行高通滤波，消除直流偏置和重力残差
|
||||||
tracker->velocity[1] += tracker->acc_world[1] * dt;
|
for (int i = 0; i < 3; i++) {
|
||||||
tracker->velocity[2] += tracker->acc_world[2] * dt;
|
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];
|
||||||
// 在其他状态下（静止、旋转），速度已经在状态机内部被清零或保持不变
|
}
|
||||||
|
|
||||||
// --- 4. 更新速率和距离 ---
|
// 应用加速度死区，忽略微小抖动和噪声
|
||||||
// 速率和距离总是在更新，但在非滑雪状态下，速度为0，所以它们不会增加
|
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->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
|
||||||
tracker->velocity[1] * tracker->velocity[1] +
|
tracker->velocity[1] * tracker->velocity[1]);
|
||||||
tracker->velocity[2] * tracker->velocity[2]);
|
|
||||||
tracker->distance += tracker->speed * dt;
|
tracker->distance += tracker->speed * dt;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// 传感器数据采集与处理任务
|
|
||||||
void sensor_processing_task(signed short * acc_data_buf, signed short * gyr_data_buf) {
|
/**
|
||||||
|
* @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 skiing_tracker_t my_skiing_tracker;
|
||||||
static int initialized = 0;
|
static int initialized = 0;
|
||||||
static int calibration_done = 0;
|
static int calibration_done = 0;
|
||||||
@ -208,7 +289,11 @@ void sensor_processing_task(signed short * acc_data_buf, signed short * gyr_data
|
|||||||
static float calibrated_acc_g[3]; // 转换后的加速度计数据
|
static float calibrated_acc_g[3]; // 转换后的加速度计数据
|
||||||
static float calibrated_gyr_dps[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;
|
||||||
|
BLE_KS_send_data_t KS_data;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
if (!initialized) {
|
if (!initialized) {
|
||||||
skiing_tracker_init(&my_skiing_tracker);
|
skiing_tracker_init(&my_skiing_tracker);
|
||||||
@ -227,6 +312,7 @@ void sensor_processing_task(signed short * acc_data_buf, signed short * gyr_data
|
|||||||
printf("Sensor calibration successful! Skiing mode is active.\n");
|
printf("Sensor calibration successful! Skiing mode is active.\n");
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
|
// printf("Calculate the time interval =============== start\n");
|
||||||
status = SL_SC7U22_Angle_Output(0, combined_raw_data, final_angle_data, 0);
|
status = SL_SC7U22_Angle_Output(0, combined_raw_data, final_angle_data, 0);
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -245,21 +331,47 @@ 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);
|
skiing_tracker_update(&my_skiing_tracker, calibrated_acc_g, calibrated_gyr_dps, final_angle_data, delta_time);
|
||||||
|
|
||||||
// 打印逻辑保持不变
|
// 打印逻辑保持不变
|
||||||
static int count = 0;
|
// static int count = 0;
|
||||||
if(count < 10){
|
// if(count >= 10){
|
||||||
count++;
|
// printf("State: %d, Speed: %.2f m/s, Distance: %.2f m\n",
|
||||||
return;
|
// my_skiing_tracker.state,
|
||||||
} else {
|
// my_skiing_tracker.speed,
|
||||||
count = 0;
|
// 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
|
||||||
}
|
}
|
||||||
printf("State: %d, Speed: %.2f m/s, Distance: %.2f m\n",
|
BLE_send_data.speed_cms = (int)(my_skiing_tracker.speed * 100);
|
||||||
my_skiing_tracker.state,
|
BLE_send_data.distance_cm = (int)(my_skiing_tracker.distance * 100);
|
||||||
my_skiing_tracker.speed,
|
// printf("Calculate the time interval =============== end\n");
|
||||||
my_skiing_tracker.distance);
|
|
||||||
|
|
||||||
} else if (status == 0) {
|
} 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");
|
// printf("Sensor is calibrating...\n");
|
||||||
} else {
|
} 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");
|
// printf("Angle calculation error or calibration not finished.\n");
|
||||||
}
|
}
|
||||||
|
return BLE_send_data;
|
||||||
}
|
}
|
||||||
@ -1,6 +1,7 @@
|
|||||||
#ifndef SKIING_TRACKER_H
|
#ifndef SKIING_TRACKER_H
|
||||||
#define SKIING_TRACKER_H
|
#define SKIING_TRACKER_H
|
||||||
|
|
||||||
|
#include "../xtell.h"
|
||||||
// 定义滑雪者可能的状态
|
// 定义滑雪者可能的状态
|
||||||
typedef enum {
|
typedef enum {
|
||||||
SKIING_STATE_STATIC, // 静止或动态稳定
|
SKIING_STATE_STATIC, // 静止或动态稳定
|
||||||
@ -10,7 +11,9 @@ typedef enum {
|
|||||||
SKIING_STATE_UNKNOWN // 未知状态
|
SKIING_STATE_UNKNOWN // 未知状态
|
||||||
} skiing_state_t;
|
} skiing_state_t;
|
||||||
|
|
||||||
#define VARIANCE_BUFFER_SIZE 15 // 用于计算方差的数据窗口大小 (15个样本 @ 100Hz = 150ms)
|
#define VARIANCE_BUFFER_SIZE 5 // 用于计算方差的数据窗口大小 (5个样本 @ 100Hz = 50ms)，减小延迟，提高实时性
|
||||||
|
#define DELTA_TIME 0.01f
|
||||||
|
|
||||||
|
|
||||||
// 追踪器数据结构体
|
// 追踪器数据结构体
|
||||||
typedef struct {
|
typedef struct {
|
||||||
@ -23,13 +26,36 @@ typedef struct {
|
|||||||
// 内部计算使用的私有成员
|
// 内部计算使用的私有成员
|
||||||
float acc_world[3]; // 在世界坐标系下的加速度
|
float acc_world[3]; // 在世界坐标系下的加速度
|
||||||
|
|
||||||
|
// --- 内部计算使用的私有成员 ---
|
||||||
// 用于动态零速更新和旋转检测的缓冲区
|
// 用于动态零速更新和旋转检测的缓冲区
|
||||||
float acc_buffer[VARIANCE_BUFFER_SIZE][3]; // 加速度数据窗口
|
float acc_buffer[VARIANCE_BUFFER_SIZE][3]; // 加速度数据窗口
|
||||||
float gyr_buffer[VARIANCE_BUFFER_SIZE][3]; // 角速度数据窗口
|
float gyr_buffer[VARIANCE_BUFFER_SIZE][3]; // 角速度数据窗口
|
||||||
int buffer_index; // 缓冲区当前索引
|
int buffer_index; // 缓冲区当前索引
|
||||||
int buffer_filled; // 缓冲区是否已填满的标志
|
int buffer_filled; // 缓冲区是否已填满的标志
|
||||||
|
|
||||||
|
// 用于高通滤波器（巴特沃斯一阶滤波器）的私有成员，以消除加速度的直流偏置
|
||||||
|
float acc_world_filtered[3]; //过滤过的
|
||||||
|
float acc_world_unfiltered_prev[3]; //上一次没过滤的
|
||||||
} skiing_tracker_t;
|
} 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 初始化滑雪追踪器
|
* @brief 初始化滑雪追踪器
|
||||||
*
|
*
|
||||||
@ -37,14 +63,12 @@ typedef struct {
|
|||||||
*/
|
*/
|
||||||
void skiing_tracker_init(skiing_tracker_t *tracker);
|
void skiing_tracker_init(skiing_tracker_t *tracker);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief 处理传感器数据并更新滑雪状态
|
* @brief 传感器数据采集与处理任务，外部每10ms调用一次，如果需要更新时间间隔，也需要同步更新宏“ DELTA_TIME ”
|
||||||
*
|
*
|
||||||
* @param tracker 指向 skiing_tracker_t 结构体的指针
|
* @param acc_data_buf 三轴加速度原始数据
|
||||||
* @param acc_g 校准后的加速度数据 [x, y, z]，单位: g (1g = 9.8m/s^2)
|
* @param gyr_data_buf 三轴陀螺仪原始数据
|
||||||
* @param gyr_dps 角速度
|
* @return BLE_send_data_t
|
||||||
* @param angle 姿态角数据 [pitch, roll, yaw]，单位: 度
|
*/
|
||||||
* @param dt 采样时间间隔，单位: 秒 (s)
|
BLE_send_data_t sensor_processing_task(signed short * acc_data_buf, signed short * gyr_data_buf) ;
|
||||||
*/
|
|
||||||
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt);
|
|
||||||
#endif // SKIING_TRACKER_H
|
#endif // SKIING_TRACKER_H
|
||||||
312
apps/earphone/xtell_Sensor/calculate/tmp.c
Normal file
312
apps/earphone/xtell_Sensor/calculate/tmp.c
Normal file
@ -0,0 +1,312 @@
|
|||||||
|
/*
|
||||||
|
动态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) {
|
||||||
|
// 最终版启动逻辑：必须同时满足“有足够大的线性加速度”和“旋转不剧烈”两个条件
|
||||||
|
// 新增 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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
// 最后的 fall-through 逻辑已移除，以修复原地旋转被错误判断为滑雪的bug。
|
||||||
|
// 如果不满足任何状态切换条件，状态将保持不变，直到ZUPT或启动条件被满足。
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @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");
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -20,6 +20,7 @@
|
|||||||
#include "circle_buffer.h"
|
#include "circle_buffer.h"
|
||||||
#include "btstack/avctp_user.h"
|
#include "btstack/avctp_user.h"
|
||||||
#include "calculate/skiing_tracker.h"
|
#include "calculate/skiing_tracker.h"
|
||||||
|
#include "xtell.h"
|
||||||
///////////////////////////////////////////////////////////////////////////////////////////////////
|
///////////////////////////////////////////////////////////////////////////////////////////////////
|
||||||
//宏定义
|
//宏定义
|
||||||
#define ENABLE_XLOG 1
|
#define ENABLE_XLOG 1
|
||||||
@ -55,7 +56,7 @@ static u16 send_data_id;
|
|||||||
static u8 sensor_data_buffer[SENSOR_DATA_BUFFER_SIZE];
|
static u8 sensor_data_buffer[SENSOR_DATA_BUFFER_SIZE];
|
||||||
static circle_buffer_t sensor_cb;
|
static circle_buffer_t sensor_cb;
|
||||||
|
|
||||||
|
BLE_send_data_t BLE_send_data;
|
||||||
//--- test ---
|
//--- test ---
|
||||||
// 全局变量
|
// 全局变量
|
||||||
u16 gsensor_id=0;
|
u16 gsensor_id=0;
|
||||||
@ -142,36 +143,59 @@ void send_sensor_data_task(void) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
void test(){
|
void test(){
|
||||||
|
|
||||||
signed short acc_data_buf[3] = {0};
|
signed short acc_data_buf[3] = {0};
|
||||||
signed short gyr_data_buf[3] = {0};
|
signed short gyr_data_buf[3] = {0};
|
||||||
|
|
||||||
signed short acc_gyro_input[6] = {0};
|
signed short acc_gyro_input[6] = {0};
|
||||||
float Angle_output[3] = {0};
|
float Angle_output[3] = {0};
|
||||||
extern void SL_SC7U22_RawData_Read(signed short * acc_data_buf,signed short * gyr_data_buf);
|
|
||||||
SL_SC7U22_RawData_Read(acc_data_buf,gyr_data_buf);
|
SL_SC7U22_RawData_Read(acc_data_buf,gyr_data_buf);
|
||||||
acc_gyro_input[0] = acc_data_buf[0];
|
|
||||||
acc_gyro_input[1] = acc_data_buf[1];
|
BLE_send_data = sensor_processing_task(acc_data_buf, gyr_data_buf);
|
||||||
acc_gyro_input[2] = acc_data_buf[2];
|
u8 data[50];
|
||||||
acc_gyro_input[3] = gyr_data_buf[0];
|
data[0] = 0xBB;
|
||||||
acc_gyro_input[4] = gyr_data_buf[1];
|
data[1] = 0xBE;
|
||||||
acc_gyro_input[5] = gyr_data_buf[2];
|
data[2] = 0x01;
|
||||||
// extern unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst);
|
data[3] = sizeof(BLE_send_data_t); //后续包的数据长度
|
||||||
// SL_SC7U22_Angle_Output(0,acc_gyro_input,Angle_output,1);
|
// 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;
|
static int count = 0;
|
||||||
if(count >=100){ //1s打印1次
|
if(count >=10){
|
||||||
count = 0;
|
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",
|
xlog("ACC_X:%d, ACC_Y:%d, ACC_Z:%d, GYR_X:%.d, GYR_Y:%d, GYR_Z:%d",
|
||||||
acc_gyro_input[0],acc_gyro_input[1],acc_gyro_input[2],acc_gyro_input[3],acc_gyro_input[4],acc_gyro_input[5]
|
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 log_buffer[100]; // 100个字符应该足够了
|
||||||
|
|
||||||
|
// 使用 snprintf 进行格式化
|
||||||
|
int num_chars_written = snprintf(
|
||||||
|
log_buffer, // 目标缓冲区
|
||||||
|
sizeof(log_buffer), // 目标缓冲区的最大容量
|
||||||
|
"State: %d, Speed: %d cm/s, Distance: %d cm\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",
|
// xlog("Pitch:%.2f, Roll:%.2f, Yaw:%.2f\n",
|
||||||
// Angle_output[0],Angle_output[1],Angle_output[2]
|
// Angle_output[0],Angle_output[1],Angle_output[2]
|
||||||
// );
|
// );
|
||||||
|
#else
|
||||||
|
send_data_to_ble_client(&data,sizeof(BLE_send_data_t)+4);
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
count++;
|
count++;
|
||||||
|
|
||||||
void sensor_processing_task(signed short * acc_data_buf,signed short * gyr_data_buf);
|
memset(&BLE_send_data, 0, sizeof(BLE_send_data_t));
|
||||||
sensor_processing_task(acc_data_buf, gyr_data_buf);
|
memset(&data, 0, 50);
|
||||||
|
|
||||||
}
|
}
|
||||||
void gsensor_test(){
|
void gsensor_test(){
|
||||||
|
|
||||||
@ -206,6 +230,6 @@ void xtell_task_create(void){
|
|||||||
xlog("SkiingTracker_Init\n");
|
xlog("SkiingTracker_Init\n");
|
||||||
|
|
||||||
// create_process(&gsensor_id, "gsensor",NULL, gsensor_test, 1000);
|
// create_process(&gsensor_id, "gsensor",NULL, gsensor_test, 1000);
|
||||||
create_process(&test_id, "test",NULL, test, 10);
|
create_process(&test_id, "test",NULL, test, (int)(DELTA_TIME*1000));
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
8
apps/earphone/xtell_Sensor/xtell.h
Normal file
8
apps/earphone/xtell_Sensor/xtell.h
Normal file
@ -0,0 +1,8 @@
|
|||||||
|
#ifndef XTELL_H
|
||||||
|
#define XTELL_H
|
||||||
|
|
||||||
|
#define KS_BLE 0
|
||||||
|
#define XTELL_TEST 1
|
||||||
|
|
||||||
|
|
||||||
|
#endif
|
||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -6341,8 +6341,10 @@ objs/apps/earphone/xtell_Sensor/send_data.c.o
|
|||||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,send_sensor_data_task,pl
|
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,send_sensor_data_task,pl
|
||||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,test,pl
|
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,test,pl
|
||||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,SL_SC7U22_RawData_Read,l
|
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,SL_SC7U22_RawData_Read,l
|
||||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,printf,l
|
|
||||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,sensor_processing_task,l
|
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,sensor_processing_task,l
|
||||||
|
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,printf,l
|
||||||
|
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,snprintf,l
|
||||||
|
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,strlen,l
|
||||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,gsensor_test,pl
|
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,gsensor_test,pl
|
||||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,sys_timer_del,l
|
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,sys_timer_del,l
|
||||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,xtell_task_create,pl
|
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,xtell_task_create,pl
|
||||||
@ -6355,6 +6357,7 @@ objs/apps/earphone/xtell_Sensor/send_data.c.o
|
|||||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,create_process,l
|
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,create_process,l
|
||||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,gsensor_id,pl
|
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,gsensor_id,pl
|
||||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,test_id,pl
|
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,test_id,pl
|
||||||
|
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,BLE_send_data,pl
|
||||||
objs/apps/earphone/xtell_Sensor/buffer/circle_buffer.c.o
|
objs/apps/earphone/xtell_Sensor/buffer/circle_buffer.c.o
|
||||||
-r=objs/apps/earphone/xtell_Sensor/buffer/circle_buffer.c.o,circle_buffer_init,pl
|
-r=objs/apps/earphone/xtell_Sensor/buffer/circle_buffer.c.o,circle_buffer_init,pl
|
||||||
-r=objs/apps/earphone/xtell_Sensor/buffer/circle_buffer.c.o,circle_buffer_write,pl
|
-r=objs/apps/earphone/xtell_Sensor/buffer/circle_buffer.c.o,circle_buffer_write,pl
|
||||||
@ -6440,7 +6443,6 @@ objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o
|
|||||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,skiing_tracker_update,pl
|
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,skiing_tracker_update,pl
|
||||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,sqrtf,l
|
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,sqrtf,l
|
||||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,sensor_processing_task,pl
|
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,sensor_processing_task,pl
|
||||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,printf,l
|
|
||||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,SL_SC7U22_Angle_Output,l
|
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,SL_SC7U22_Angle_Output,l
|
||||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,fabsf,l
|
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,fabsf,l
|
||||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,cosf,l
|
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,cosf,l
|
||||||
|
|||||||
323072
cpu/br28/tools/sdk.lst
323072
cpu/br28/tools/sdk.lst
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@ -1,6 +1,7 @@
|
|||||||
objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o: \
|
objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o: \
|
||||||
apps/earphone/xtell_Sensor/calculate/skiing_tracker.c \
|
apps/earphone/xtell_Sensor/calculate/skiing_tracker.c \
|
||||||
apps/earphone/xtell_Sensor/calculate/skiing_tracker.h \
|
apps/earphone/xtell_Sensor/calculate/skiing_tracker.h \
|
||||||
|
apps/earphone/xtell_Sensor/calculate/../xtell.h \
|
||||||
apps/earphone/xtell_Sensor/calculate/../sensor/SC7U22.h \
|
apps/earphone/xtell_Sensor/calculate/../sensor/SC7U22.h \
|
||||||
apps/common/device\gSensor/gSensor_manage.h \
|
apps/common/device\gSensor/gSensor_manage.h \
|
||||||
include_lib/system/generic\printf.h \
|
include_lib/system/generic\printf.h \
|
||||||
|
|||||||
Binary file not shown.
@ -157,4 +157,5 @@ objs/apps/earphone/xtell_Sensor/send_data.c.o: \
|
|||||||
cpu/br28/audio_anc_fade_ctr.h \
|
cpu/br28/audio_anc_fade_ctr.h \
|
||||||
apps/earphone/xtell_Sensor/buffer\circle_buffer.h \
|
apps/earphone/xtell_Sensor/buffer\circle_buffer.h \
|
||||||
include_lib\btstack/avctp_user.h include_lib/btstack/btstack_typedef.h \
|
include_lib\btstack/avctp_user.h include_lib/btstack/btstack_typedef.h \
|
||||||
apps/earphone/xtell_Sensor/calculate/skiing_tracker.h
|
apps/earphone/xtell_Sensor/calculate/skiing_tracker.h \
|
||||||
|
apps/earphone/xtell_Sensor/calculate/../xtell.h
|
||||||
|
|||||||
Binary file not shown.
Reference in New Issue
Block a user