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

cun

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

153
apps/earphone/xtell_Sensor/A_hide/1/1/skiing_tracker.c
View File

@ -1,153 +0,0 @@
#include "skiing_tracker.h"
#include <math.h> // 使用 sqrtf, fabsf, atan2f
#include <string.h> // 使用 memset
// ======================= 用户可配置参数 =======================
// IMU的采样率 (Hz)
#define SAMPLE_RATE 100.0f
#define DT (1.0f / SAMPLE_RATE)
// 传感器灵敏度配置 (必须与硬件配置匹配)
// 加速度计量程: ±8G -> 1G = 32768 / 8 = 4096 LSB
#define ACCEL_SENSITIVITY 4096.0f // LSB/g
#define GRAVITY_MSS 9.80665f // 标准重力加速度 (m/s^2)
// 陀螺仪灵敏度 (2000dps)
#define GYRO_SENSITIVITY 16.4f // LSB/(deg/s)
// 状态检测阈值
#define MOTION_ACCEL_THRESHOLD (ACCEL_SENSITIVITY * 0.3f) // 加速度变化超过0.3g认为在运动
#define MOTION_GYRO_THRESHOLD (GYRO_SENSITIVITY * 90.0f) // 角速度超过xx dps认为在运动
#define STILL_SAMPLES_FOR_CALIBRATION 100 // 连续静止1秒 (100个点) 开始校准
#define CALIBRATION_SAMPLE_COUNT 50 // 用于平均的校准样本数 (0.5秒)
#define MOTION_SAMPLES_TO_START_SKIING 10 // 连续运动0.1秒开始滑行
#define STILL_SAMPLES_TO_STOP_SKIING 20 // 连续静止0.2秒停止滑行
// 角度转弧度
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#define RAD_TO_DEG(rad) ((rad) * 180.0f / M_PI)
// ======================= 内部实现 =======================
void SkiingTracker_Init(SkiingTracker* tracker) {
memset(tracker, 0, sizeof(SkiingTracker));
tracker->state = STATE_UNCALIBRATED;
}
// 简单的低通滤波器
static float low_pass_filter(float new_input, float prev_output, float alpha) {
return alpha * new_input + (1.0f - alpha) * prev_output;
}
static void reset_calibration(SkiingTracker* tracker) {
tracker->calib_samples_count = 0;
tracker->calib_acc_sum[0] = 0;
tracker->calib_acc_sum[1] = 0;
tracker->calib_acc_sum[2] = 0;
}
// 核心更新函数
void SkiingTracker_Update(SkiingTracker* tracker, signed short* raw_accel, signed short* raw_gyro) {
// 运动状态检测
// 使用原始数据进行判断，避免校准误差影响
float acc_mag = sqrtf((float)raw_accel[0] * raw_accel[0] + (float)raw_accel[1] * raw_accel[1] + (float)raw_accel[2] * raw_accel[2]);
float gyro_mag = sqrtf((float)raw_gyro[0] * raw_gyro[0] + (float)raw_gyro[1] * raw_gyro[1] + (float)raw_gyro[2] * raw_gyro[2]);
int is_moving = (fabsf(acc_mag - ACCEL_SENSITIVITY) > MOTION_ACCEL_THRESHOLD) || (gyro_mag > MOTION_GYRO_THRESHOLD);
if (is_moving) {
tracker->still_counter = 0;
tracker->motion_counter++;
printf("===motion count===\n");
} else {
tracker->motion_counter = 0;
tracker->still_counter++;
printf("===still count===\n");
}
// 2. 状态机处理
switch (tracker->state) {
case STATE_UNCALIBRATED:
if (tracker->still_counter > STILL_SAMPLES_FOR_CALIBRATION) {
tracker->state = STATE_CALIBRATING;
reset_calibration(tracker);
}
break;
case STATE_CALIBRATING:
if (is_moving) { // 如果在校准时移动了，则校准失败，返回未校准状态
tracker->state = STATE_UNCALIBRATED;
reset_calibration(tracker);
break;
}
// 累加采样数据
tracker->calib_acc_sum[0] += raw_accel[0];
tracker->calib_acc_sum[1] += raw_accel[1];
tracker->calib_acc_sum[2] += raw_accel[2];
tracker->calib_samples_count++;
if (tracker->calib_samples_count >= CALIBRATION_SAMPLE_COUNT) {
// 校准完成，计算平均重力矢量
tracker->static_gravity[0] = (short)(tracker->calib_acc_sum[0] / CALIBRATION_SAMPLE_COUNT);
tracker->static_gravity[1] = (short)(tracker->calib_acc_sum[1] / CALIBRATION_SAMPLE_COUNT);
tracker->static_gravity[2] = (short)(tracker->calib_acc_sum[2] / CALIBRATION_SAMPLE_COUNT);
// 计算坡度（可选，用于显示）
float horiz_g = sqrtf((float)tracker->static_gravity[0] * tracker->static_gravity[0] + (float)tracker->static_gravity[1] * tracker->static_gravity[1]);
float vert_g = fabsf((float)tracker->static_gravity[2]);
tracker->slope_angle_deg = RAD_TO_DEG(atan2f(horiz_g, vert_g));
tracker->state = STATE_READY;
}
break;
case STATE_READY:
if (tracker->motion_counter > MOTION_SAMPLES_TO_START_SKIING) {
tracker->state = STATE_SKIING;
}
break;
case STATE_SKIING:
if (tracker->still_counter > STILL_SAMPLES_TO_STOP_SKIING) {
tracker->state = STATE_STOPPED;
tracker->velocity = 0.0f; // 零速更新
tracker->forward_accel = 0.0f;
break;
}
// 计算线性加速度 (重力抵消)
// 假设传感器的X轴指向滑雪板前进方向
long linear_accel_x_lsb = (long)raw_accel[0] - tracker->static_gravity[0];
// 转换为 m/s^2
float current_accel_mss = (float)linear_accel_x_lsb / ACCEL_SENSITIVITY * GRAVITY_MSS;
// 低通滤波以平滑加速度
tracker->forward_accel = low_pass_filter(current_accel_mss, tracker->forward_accel, 0.3f);
// 积分计算速度和距离 (梯形积分)
float prev_velocity = tracker->velocity;
tracker->velocity += tracker->forward_accel * DT;
// 物理约束：速度不能为负（不能往坡上滑）
if (tracker->velocity < 0) {
tracker->velocity = 0;
}
tracker->distance += (prev_velocity + tracker->velocity) / 2.0f * DT;
break;
case STATE_STOPPED:
// 在停止状态下，如果再次检测到运动，则重新进入滑行状态
if (tracker->motion_counter > MOTION_SAMPLES_TO_START_SKIING) {
tracker->state = STATE_SKIING;
}
// 如果长时间静止，返回未校准状态，以应对更换雪道的情况
// if (tracker->still_counter > 3000) { // e.g., 30 seconds
// tracker->state = STATE_UNCALIBRATED;
// }
break;
}
}

53
apps/earphone/xtell_Sensor/A_hide/1/1/skiing_tracker.h
View File

@ -1,53 +0,0 @@
#ifndef SKIING_TRACKER_H
#define SKIING_TRACKER_H
// 定义滑雪者的运动状态
typedef enum {
STATE_UNCALIBRATED, // 未校准，等待在斜坡上静止
STATE_CALIBRATING, // 正在校准重力矢量
STATE_READY, // 校准完成，准备滑行
STATE_SKIING, // 正在滑行
STATE_STOPPED // 在斜坡上中途停止
} MotionState;
// 存储所有运动学数据
typedef struct {
// 最终输出
float velocity; // 沿斜坡方向的速度 (m/s)
float distance; // 沿斜坡方向的滑行距离 (m)
float slope_angle_deg; // 动态计算出的坡度 (度)
// 内部状态变量
MotionState state; // 当前运动状态
// 校准相关
short static_gravity[3]; // 校准后得到的静态重力矢量 (LSB)
long calib_acc_sum[3]; // 用于计算平均值的累加器
int calib_samples_count; // 校准采样计数
// 运动检测
int motion_counter;
int still_counter; // 静止状态计数器
// 物理量
float forward_accel; // 沿滑行方向的加速度 (m/s^2)
} SkiingTracker;
/**
* @brief 初始化滑雪追踪器
* @param tracker 指向 SkiingTracker 实例的指针
*/
void SkiingTracker_Init(SkiingTracker* tracker);
/**
* @brief 处理IMU数据，自动校准并计算速度和距离
* @details 这是核心处理函数，应在每次获取新的IMU数据后调用。
*
* @param tracker 指向 SkiingTracker 实例的指针
* @param raw_accel 未经校准的原始加速度数据 [X, Y, Z]，单位是 LSB
* @param raw_gyro 未经校准的原始陀螺仪数据 [X, Y, Z]，单位是 LSB
*/
void SkiingTracker_Update(SkiingTracker* tracker, signed short* raw_accel, signed short* raw_gyro);
#endif // SKIING_TRACKER_H

207
apps/earphone/xtell_Sensor/A_hide/1/2/skiing_tracker.c
View File

@ -1,207 +0,0 @@
#include "skiing_tracker.h"
#include <math.h> // 使用 sqrtf, fabsf, atan2f
#include <string.h> // 使用 memset
#include "system/includes.h"
#include "btstack/btstack_task.h"
#include "app_config.h"
#include "app_action.h"
#include "asm/pwm_led.h"
#include "tone_player.h"
#include "ui_manage.h"
#include "gpio.h"
#include "app_main.h"
#include "asm/charge.h"
#include "update.h"
#include "app_power_manage.h"
#include "audio_config.h"
#include "app_charge.h"
#include "bt_profile_cfg.h"
#include "dev_manager/dev_manager.h"
#include "update_loader_download.h"
#include "../sensor/LIS2DH12.h"
#include "../buffer/circle_buffer.h"
#include "btstack/avctp_user.h"
#define ENABLE_XLOG 1
#ifdef xlog
#undef xlog
#endif
#if ENABLE_XLOG
#define xlog(format, ...) printf("[XT:%s] " format, __func__, ##__VA_ARGS__)
#else
#define xlog(format, ...) ((void)0)
#endif
// --- 算法核心参数 ---
#define SAMPLE_RATE_HZ 100 // 传感器采样率，100Hz
#define DT (1.0f / SAMPLE_RATE_HZ) // 每次处理的时间间隔
#define TASK_PERIOD_MS (1000 / SAMPLE_RATE_HZ) // 任务执行周期, 10ms for 100Hz
Skiing_State_t skiing_state = {0};
// --- 用于BLE发送的数据包结构 ---
typedef struct {
float speed_ms;
float distance_m;
} sensor_ble_packet_t;
/**
* @brief 处理滑雪数据，计算速度和距离
* @param acc_gyro_input 传入：6轴原始数据 [AX, AY, AZ, GX, GY, GZ]
* @param dt 传入：两次调用之间的时间间隔（秒），例如0.01s for 100Hz
* @retval None
*/
void SL_SC7U22_Process_Skiing_Data(signed short* acc_gyro_input, float dt)
{
// =================================================================================
// 步骤 1: 姿态解算和静态校准
// =================================================================================
float angles_deg[3] = {0}; // [Pitch, Roll, Yaw] in degrees
// 调用您现有的姿态解算函数
// 假设 calibration_en = 1, yaw_rst = 0
unsigned char angle_status = SL_SC7U22_Angle_Output(1, acc_gyro_input, angles_deg, 0);
if (angle_status != 1) {
// 如果校准未完成或计算失败，则不进行后续处理
skiing_state.is_calibrated = 0;
return;
}
skiing_state.is_calibrated = 1;
// 将角度转换为弧度，用于后续三角函数计算
skiing_state.angles_rad[0] = angles_deg[0] * DEG_TO_RAD; // Pitch
skiing_state.angles_rad[1] = angles_deg[1] * DEG_TO_RAD; // Roll
skiing_state.angles_rad[2] = angles_deg[2] * DEG_TO_RAD; // Yaw
// =================================================================================
// 步骤 2: 检测滑雪者是否静止 (这是漂移抑制的关键)
// =================================================================================
// 静止检测逻辑：
// 1. 加速度的模长约等于1g
// 2. 角速度的模长非常小
long long accel_mag_sq = (long long)acc_gyro_input[0] * acc_gyro_input[0] +
(long long)acc_gyro_input[1] * acc_gyro_input[1] +
(long long)acc_gyro_input[2] * acc_gyro_input[2];
long long gyro_mag_sq = (long long)acc_gyro_input[3] * acc_gyro_input[3] +
(long long)acc_gyro_input[4] * acc_gyro_input[4] +
(long long)acc_gyro_input[5] * acc_gyro_input[5];
// --- 这些阈值需要根据实际测试进行精细调整 ---
const float G_SQ_UPPER = (G_ACCEL * 1.05f) * (G_ACCEL * 1.05f); // (1.05g)^2
const float G_SQ_LOWER = (G_ACCEL * 0.95f) * (G_ACCEL * 0.95f); // (0.95g)^2
const long long GYRO_MAG_SQ_THRESH = 300; // 对应约 1 deg/s 的抖动
const int STILLNESS_DURATION = 100; // 需要持续100个采样点（1秒 @ 100Hz）才确认为静止
if (accel_mag_sq > G_SQ_LOWER && accel_mag_sq < G_SQ_UPPER && gyro_mag_sq < GYRO_MAG_SQ_THRESH) {
if (skiing_state.stillness_counter < STILLNESS_DURATION) {
skiing_state.stillness_counter++;
}
} else {
skiing_state.stillness_counter = 0;
skiing_state.is_still = 0;
}
if (skiing_state.stillness_counter >= STILLNESS_DURATION) {
skiing_state.is_still = 1;
}
// 如果检测到静止，则重置速度（零速更新）
if (skiing_state.is_still) {
skiing_state.velocity[0] = 0.0f;
skiing_state.velocity[1] = 0.0f;
skiing_state.velocity[2] = 0.0f;
skiing_state.speed = 0.0f;
return; // 静止时，不需要进行积分
}
// =================================================================================
// 步骤 3: 坐标转换与重力补偿
// =================================================================================
float accel_body_g[3]; // 机体坐标系下的加速度，单位 g
accel_body_g[0] = (float)acc_gyro_input[0] / G_ACCEL;
accel_body_g[1] = (float)acc_gyro_input[1] / G_ACCEL;
accel_body_g[2] = (float)acc_gyro_input[2] / G_ACCEL;
// 使用旋转矩阵将机体坐标系加速度(Ab)转换到世界坐标系(An)
// An = C_b^n * Ab
// C_b^n 是从机体(b)到导航(n)的旋转矩阵，由姿态角决定
float sp = sinf(skiing_state.angles_rad[0]); // sin(pitch)
float cp = cosf(skiing_state.angles_rad[0]); // cos(pitch)
float sr = sinf(skiing_state.angles_rad[1]); // sin(roll)
float cr = cosf(skiing_state.angles_rad[1]); // cos(roll)
float sy = sinf(skiing_state.angles_rad[2]); // sin(yaw)
float cy = cosf(skiing_state.angles_rad[2]); // cos(yaw)
// 世界坐标系下的加速度（单位g），此时仍包含重力
float accel_nav_g[3];
accel_nav_g[0] = cy*cr*accel_body_g[0] + (cy*sp*sr - sy*cr)*accel_body_g[1] + (cy*sp*cr + sy*sr)*accel_body_g[2];
accel_nav_g[1] = sy*cr*accel_body_g[0] + (sy*sp*sr + cy*cr)*accel_body_g[1] + (sy*sp*cr - cy*sr)*accel_body_g[2];
accel_nav_g[2] = -sr*accel_body_g[0] + cp*sr*accel_body_g[1] + cp*cr*accel_body_g[2];
// 减去重力分量。在世界坐标系下，重力总是指向Z轴负方向（假设Z轴向上），大小为1g
// 如果您的世界坐标系Z轴向下，则应为 accel_nav_g[2] -= 1.0f;
accel_nav_g[2] += 1.0f;
// 将线性加速度从 g 转换为 m/s^2
float linear_accel_ms2[3];
const float G_IN_MS2 = 9.80665f;
linear_accel_ms2[0] = accel_nav_g[0] * G_IN_MS2;
linear_accel_ms2[1] = accel_nav_g[1] * G_IN_MS2;
linear_accel_ms2[2] = accel_nav_g[2] * G_IN_MS2;
// =================================================================================
// 步骤 4: 积分计算速度和距离
// =================================================================================
// 积分得到速度
skiing_state.velocity[0] += linear_accel_ms2[0] * dt;
skiing_state.velocity[1] += linear_accel_ms2[1] * dt;
skiing_state.velocity[2] += linear_accel_ms2[2] * dt;
// 计算标量速度
skiing_state.speed = sqrtf(skiing_state.velocity[0] * skiing_state.velocity[0] +
skiing_state.velocity[1] * skiing_state.velocity[1] +
skiing_state.velocity[2] * skiing_state.velocity[2]);
// 积分得到距离
skiing_state.distance += skiing_state.speed * dt;
}
// 传感器数据采集与处理任务
void sensor_processing_task(signed short * acc_data_buf,signed short * gyr_data_buf) {
signed short acc_gyro_data[6];
acc_gyro_data[0] = acc_data_buf[0];
acc_gyro_data[1] = acc_data_buf[1];
acc_gyro_data[2] = acc_data_buf[2];
acc_gyro_data[3] = gyr_data_buf[0];
acc_gyro_data[4] = gyr_data_buf[1];
acc_gyro_data[5] = gyr_data_buf[2];
// 调用滑雪数据处理函数，传入原始数据和时间间隔DT
SL_SC7U22_Process_Skiing_Data(acc_gyro_data, DT);
// 处理和输出结果
if (skiing_state.is_calibrated) {
sensor_ble_packet_t packet;
packet.speed_ms = skiing_state.speed;
packet.distance_m = skiing_state.distance;
static int print_counter = 0;
if (++print_counter >= 10) {
print_counter = 0;
xlog(": %.2f m/s, Dist: %.2f m, Still: %d",
packet.speed_ms, packet.distance_m, skiing_state.is_still);
}
} else {
// 如果还未校准完成，可以打印提示信息
static int calib_print_counter = 0;
if (++calib_print_counter >= 100) { // 每1秒打印一次
calib_print_counter = 0;
xlog("Calibrating... Please keep the device still.");
}
}
}

28
apps/earphone/xtell_Sensor/A_hide/1/2/skiing_tracker.h
View File

@ -1,28 +0,0 @@
#ifndef SKIING_TRACKER_H
#define SKIING_TRACKER_H
#include "system/includes.h"
// 在您的文件顶部添加
#define G_ACCEL 8192.0f // 传感器在静止时Z轴的理论读数，对应1g。根据您的代码，±8G量程下，1g ≈ 32768 / 4 = 8192 LSB
#define DEG_TO_RAD 0.0174532925f // 角度转弧度 M_PI / 180.0
typedef struct {
// 输出结果
float velocity[3]; // 速度 (m/s) 在世界坐标系 [Vx, Vy, Vz]
float speed; // 标量速度 (m/s)
float distance; // 总滑行距离 (m)
// 内部状态
unsigned char is_calibrated;
unsigned char is_still;
int stillness_counter;
// 姿态
float angles_rad[3]; // 姿态角 [Pitch, Roll, Yaw] in radians
} Skiing_State_t;
void SL_SC7U22_Process_Skiing_Data(signed short* acc_gyro_input, float dt);
#endif // SKIING_TRACKER_H

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

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

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

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

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

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

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

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

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

@ -1,16 +1,25 @@
/*
动态ZUPT+卡尔曼
*/
#include "skiing_tracker.h"
#include "../sensor/SC7U22.h" // 包含传感器驱动头文件以调用姿态解算函数
#include "../sensor/SC7U22.h"
#include <math.h>
#include <string.h>
#define G_ACCELERATION 9.81f // 重力加速度 (m/s^2)
#define G_ACCELERATION 9.81f
#define DEG_TO_RAD (3.14159265f / 180.0f)
// --- 状态检测阈值 ---
// 判断是否静止的加速度阈值 (m/s^2)。当加速度的模长减去重力后，小于此值，则认为可能静止。
#define STATIC_ACC_THRESHOLD 1.0f
// 连续多少帧满足静止条件才确认为静止状态
#define STATIC_FRAMES_REQUIRED 50 // 累加，超过这个数加速度仍变化不大，说明近似静止
// --- 算法阈值定义 ---
//两个判断是否静止的必要条件
// 动态零速更新(ZUPT)阈值
#define ZUPT_ACC_VARIANCE_THRESHOLD 0.05f
// 陀螺仪方差阈值
#define ZUPT_GYR_VARIANCE_THRESHOLD 5.0f
// 旋转/摆动检测阈值：角速度总模长大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
#define ROTATION_GYR_MAG_THRESHOLD 100.0f
// 启动滑雪阈值：加速度模长与重力的差值大于此值，认为开始运动
#define START_SKIING_ACC_THRESHOLD 1.5f
/**
* @brief 初始化滑雪追踪器
@ -20,8 +29,9 @@ void skiing_tracker_init(skiing_tracker_t *tracker)
if (!tracker) {
return;
}
// 使用memset一次性清零整个结构体，包括新增的缓冲区
memset(tracker, 0, sizeof(skiing_tracker_t));
tracker->state = SKIING_STATE_STATIC; // 初始状态为静止
tracker->state = SKIING_STATE_STATIC;
}
/**
@ -32,8 +42,9 @@ void skiing_tracker_init(skiing_tracker_t *tracker)
*/
static void transform_acc_to_world_frame(const float *acc_device, const float *angle, float *acc_world)
{
float pitch = angle[0] * DEG_TO_RAD;
float roll = angle[1] * DEG_TO_RAD;
// 驱动输出的角度与标准航空定义相反，需要取反才能用于标准旋转矩阵。
float pitch = -angle[0] * DEG_TO_RAD;
float roll = -angle[1] * DEG_TO_RAD;
float cp = cosf(pitch);
float sp = sinf(pitch);
@ -44,56 +55,95 @@ static void transform_acc_to_world_frame(const float *acc_device, const float *a
float ay = acc_device[1];
float az = acc_device[2];
// 旋转矩阵（简化版，仅用Pitch和Roll）
// Z-Y-X 顺序旋转
acc_world[0] = cr * ax + sr * sp * ay + sr * cp * az;
acc_world[1] = cp * ay - sp * az;
acc_world[2] = -sr * ax + cr * sp * ay + cr * cp * az;
// 使用经过验证的、正确的身体坐标系到世界坐标系的旋转矩阵 (基于 Y-X 旋转顺序)
// 这个矩阵将设备测量的加速度(ax, ay, az)正确地转换到世界坐标系(acc_world)。
acc_world[0] = cp * ax + sp * sr * ay + sp * cr * az;
acc_world[1] = 0 * ax + cr * ay - sr * az;
acc_world[2] = -sp * ax + cp * sr * ay + cp * cr * az;
}
/**
* @brief 更新滑雪状态机
* @brief 计算缓冲区内三轴数据的方差之和
*/
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device)
static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
{
// 计算当前加速度的模长
float acc_magnitude = sqrtf(acc_device[0] * acc_device[0] + acc_device[1] * acc_device[1] + acc_device[2] * acc_device[2]);
float mean[3] = {0};
float variance[3] = {0};
// 状态判断逻辑
switch (tracker->state) {
case SKIING_STATE_STATIC:
// 如果加速度变化较大，则切换到滑雪状态
if (fabsf(acc_magnitude - G_ACCELERATION) > STATIC_ACC_THRESHOLD * 2.0f) { // 使用一个更大的阈值来启动
tracker->state = SKIING_STATE_SKIING;
tracker->static_frames_count = 0;
}
break;
// 1. 计算均值
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
mean[0] += buffer[i][0];
mean[1] += buffer[i][1];
mean[2] += buffer[i][2];
}
mean[0] /= VARIANCE_BUFFER_SIZE;
mean[1] /= VARIANCE_BUFFER_SIZE;
mean[2] /= VARIANCE_BUFFER_SIZE;
case SKIING_STATE_SKIING:
// 检测是否进入静止状态 (零速更新 ZUPT)
if (fabsf(acc_magnitude - G_ACCELERATION) < STATIC_ACC_THRESHOLD) {
tracker->static_frames_count++;
if (tracker->static_frames_count >= STATIC_FRAMES_REQUIRED) {
tracker->state = SKIING_STATE_STATIC;
// 进入静止状态，强制将速度清零以消除漂移
memset(tracker->velocity, 0, sizeof(tracker->velocity));
tracker->speed = 0.0f;
}
} else {
// 如果在运动，则重置静止计数器
tracker->static_frames_count = 0;
}
break;
// 可以在此添加摔倒等其他状态的判断
case SKIING_STATE_FALLEN:
// TODO: 添加从摔倒状态恢复的逻辑
break;
// 2. 计算方差
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
variance[0] += (buffer[i][0] - mean[0]) * (buffer[i][0] - mean[0]);
variance[1] += (buffer[i][1] - mean[1]) * (buffer[i][1] - mean[1]);
variance[2] += (buffer[i][2] - mean[2]) * (buffer[i][2] - mean[2]);
}
variance[0] /= VARIANCE_BUFFER_SIZE;
variance[1] /= VARIANCE_BUFFER_SIZE;
variance[2] /= VARIANCE_BUFFER_SIZE;
default:
tracker->state = SKIING_STATE_UNKNOWN;
break;
// 返回三轴方差之和，作为一个综合的稳定度指标
return variance[0] + variance[1] + variance[2];
}
/**
* @brief 升级后的状态机，包含旋转检测和动态零速更新
*/
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps)
{
// 缓冲区未填满时，不进行状态判断，默认为静止
if (!tracker->buffer_filled) {
tracker->state = SKIING_STATE_STATIC;
return;
}
// --- 1. 计算关键指标 ---
float acc_variance = calculate_variance(tracker->acc_buffer); // 计算加速度方差
float gyr_variance = calculate_variance(tracker->gyr_buffer); // 计算陀螺仪方差
float gyr_magnitude = sqrtf(gyr_dps[0]*gyr_dps[0] + gyr_dps[1]*gyr_dps[1] + gyr_dps[2]*gyr_dps[2]);
float acc_magnitude = sqrtf(acc_device_ms2[0]*acc_device_ms2[0] + acc_device_ms2[1]*acc_device_ms2[1] + acc_device_ms2[2]*acc_device_ms2[2]);
// --- 2. 状态切换逻辑 (按优先级) ---
// 优先级1：原地旋转/摆动检测 (最终版)
// 增加一个关键前提：只在当前不处于滑雪状态时，才检测原地旋转。
// 这可以防止滑雪过程中的高速转弯被误判为原地旋转。
if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && tracker->state != SKIING_STATE_SKIING) {
tracker->state = SKIING_STATE_ROTATING;
return;
}
// 动态零速更新 (ZUPT)
// 必须同时满足加速度和角速度都稳定，才能判断为“真静止”，以区分匀速运动
if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
tracker->state = SKIING_STATE_STATIC;
// 速度清零，抑制漂移
memset(tracker->velocity, 0, sizeof(tracker->velocity));
tracker->speed = 0.0f;
return;
}
// 从静止/旋转状态启动
if (tracker->state == SKIING_STATE_STATIC || tracker->state == SKIING_STATE_ROTATING) {
if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD) {
tracker->state = SKIING_STATE_SKIING;
return;
}
}
// 滑雪
if (tracker->state != SKIING_STATE_STATIC) {
tracker->state = SKIING_STATE_SKIING;
}
}
@ -101,77 +151,76 @@ static void update_state_machine(skiing_tracker_t *tracker, const float *acc_dev
/**
* @brief 主更新函数
*/
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc, float *angle, float dt)
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt)
{
if (!tracker || !acc || !angle || dt <= 0) {
if (!tracker || !acc_g || !gyr_dps || !angle || dt <= 0) {
return;
}
// 将原始g单位的加速度转换为 m/s^2
// --- 1. 数据预处理和缓冲 ---
float acc_device_ms2[3];
acc_device_ms2[0] = acc[0] * G_ACCELERATION;
acc_device_ms2[1] = acc[1] * G_ACCELERATION;
acc_device_ms2[2] = acc[2] * G_ACCELERATION;
acc_device_ms2[0] = acc_g[0] * G_ACCELERATION;
acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
acc_device_ms2[2] = acc_g[2] * G_ACCELERATION;
// 更新状态机
update_state_machine(tracker, acc_device_ms2);
// 将最新数据存入缓冲区
memcpy(tracker->acc_buffer[tracker->buffer_index], acc_device_ms2, sizeof(acc_device_ms2));
memcpy(tracker->gyr_buffer[tracker->buffer_index], gyr_dps, 3 * sizeof(float));
tracker->buffer_index++;
if (tracker->buffer_index >= VARIANCE_BUFFER_SIZE) {
tracker->buffer_index = 0;
tracker->buffer_filled = 1; // 标记缓冲区已满
}
// 只有在滑雪状态下才进行计算
// --- 2. 更新状态机 ---
update_state_machine(tracker, acc_device_ms2, gyr_dps);
// --- 3. 根据状态进行计算 ---
// 只有在明确的“滑雪”状态下才进行积分
if (tracker->state == SKIING_STATE_SKIING) {
// 坐标系转换: 首先，利用姿态角(Pitch, Roll)将传感器测得的总加速度（运动加速度+重力）
// 从不断变化的“设备坐标系”转换到一个固定的“世界坐标系”。在这个世界坐标系里，Z轴永远垂直于地面指向上方。
// 执行坐标系转换
transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world);
// 转换完成后，重力就变成了一个恒定的、方向沿Z轴向下的矢量。
// 在世界坐标系下，移除Z轴上的重力分量
tracker->acc_world[2] -= G_ACCELERATION;
// 积分计算速度 (v = v0 + a*t)
tracker->velocity[0] += tracker->acc_world[0] * dt;
tracker->velocity[1] += tracker->acc_world[1] * dt;
tracker->velocity[2] += tracker->acc_world[2] * dt; // 垂直方向速度也计算在内
// 计算当前速率
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
tracker->velocity[1] * tracker->velocity[1] +
tracker->velocity[2] * tracker->velocity[2]);
// 积分计算距离 (d = d0 + v*t)
tracker->distance += tracker->speed * dt;
tracker->velocity[2] += tracker->acc_world[2] * dt;
}
// 在其他状态下（静止、旋转），速度已经在状态机内部被清零或保持不变
// --- 4. 更新速率和距离 ---
// 速率和距离总是在更新，但在非滑雪状态下，速度为0，所以它们不会增加
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
tracker->velocity[1] * tracker->velocity[1] +
tracker->velocity[2] * tracker->velocity[2]);
tracker->distance += tracker->speed * dt;
}
// 传感器数据采集与处理任务
void sensor_processing_task(signed short * acc_data_buf,signed short * gyr_data_buf) {
// --- 1. 定义静态变量 ---
void sensor_processing_task(signed short * acc_data_buf, signed short * gyr_data_buf) {
static skiing_tracker_t my_skiing_tracker;
static int initialized = 0;
static int calibration_done = 0; // 新增：用于标记一次性静态校准是否完成
static int calibration_done = 0;
static signed short combined_raw_data[6];
static float final_angle_data[3];
static float calibrated_acc_g[3];
static float final_angle_data[3]; // 计算得到的欧若拉角
static float calibrated_acc_g[3]; // 转换后的加速度计数据
static float calibrated_gyr_dps[3]; // 转换后的陀螺仪数据
// sensor_processing_task的调用频率, dt = 0.001s
const float delta_time = 0.01f;
// --- 2. 初始化 ---
if (!initialized) {
skiing_tracker_init(&my_skiing_tracker);
initialized = 1;
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
}
// --- 3. 数据处理 ---
//合并加速度和陀螺仪数据
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
unsigned char status;
if (get_calibration_state() == 0) { //正在校准
//领票校准
if (!calibration_done) { //第1次启动，开启零漂检测
status = SL_SC7U22_Angle_Output(1, combined_raw_data, final_angle_data, 0);
if (status == 1) {
calibration_done = 1;
@ -181,21 +230,26 @@ void sensor_processing_task(signed short * acc_data_buf,signed short * gyr_data_
status = SL_SC7U22_Angle_Output(0, combined_raw_data, final_angle_data, 0);
}
// c. 检查姿态解算是否成功
if (status == 1) {
// 将校准后的加速度数据从 LSB (原始值) 转换为 g (重力单位)
// ±8g量程下，8192 LSB 对应 1g
// 加速度 LSB to g
calibrated_acc_g[0] = (float)combined_raw_data[0] / 8192.0f;
calibrated_acc_g[1] = (float)combined_raw_data[1] / 8192.0f;
calibrated_acc_g[2] = (float)combined_raw_data[2] / 8192.0f;
skiing_tracker_update(&my_skiing_tracker, calibrated_acc_g, final_angle_data, delta_time);
// 陀螺仪 LSB to dps (度/秒)
// ±2000dps量程下，转换系数约为 0.061
calibrated_gyr_dps[0] = (float)combined_raw_data[3] * 0.061f;
calibrated_gyr_dps[1] = (float)combined_raw_data[4] * 0.061f;
calibrated_gyr_dps[2] = (float)combined_raw_data[5] * 0.061f;
static count = 0;
if(count < 10){
skiing_tracker_update(&my_skiing_tracker, calibrated_acc_g, calibrated_gyr_dps, final_angle_data, delta_time);
// 打印逻辑保持不变
static int count = 0;
if(count < 10){
count++;
return;
}else{
} else {
count = 0;
}
printf("State: %d, Speed: %.2f m/s, Distance: %.2f m\n",
@ -204,10 +258,8 @@ void sensor_processing_task(signed short * acc_data_buf,signed short * gyr_data_
my_skiing_tracker.distance);
} else if (status == 0) {
// 传感器正在进行静态校准
// printf("Sensor is calibrating...\n");
} else {
// status == 2, 表示校准失败或发生错误
// printf("Angle calculation error or calibration not finished.\n");
}
}

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

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

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

@ -64,6 +64,78 @@ u16 test_id=0;
//////////////////////////////////////////////////////////////////////////////////////////////////
// /**
// * @brief 向匿名上位机发送数据帧
// * @param function_id 功能码 (例如 0x01, 0x03)
// * @param data 指向 int16_t 数据数组的指针 (例如加速度、欧拉角)
// * @param data_len int16_t 数据的个数 (例如发送6轴数据时为6，发送3个欧拉角时为3)
// * @param status_byte 附加的状态字节 (例如 SHOCK_STA 或 FUSION_STA)
// */
// void send_data_anotc(uint8_t function_id, int16_t* data, uint8_t data_len, uint8_t status_byte) {
// // 定义一个足够大的缓冲区来构建数据帧
// // 最大长度(ID 0x01): 1(HEAD)+1(D_ADDR)+1(ID)+1(LEN)+13(DATA)+1(SC)+1(AC) = 19字节
// uint8_t buffer[32];
// uint8_t data_payload_len = data_len * sizeof(int16_t) + sizeof(uint8_t);
// // 1. 填充帧头和地址
// buffer[0] = 0xAA; // 帧头 HEAD
// buffer[1] = 0xFF; // 目标地址 D_ADDR
// // 2. 填充功能码和数据长度
// buffer[2] = function_id;
// buffer[3] = data_payload_len;
// // 3. 填充数据内容 (DATA)
// // 首先使用 memcpy 拷贝主要的 int16_t 数组数据
// // &buffer[4] 是数据区的起始地址
// memcpy(&buffer[4], (uint8_t*)data, data_len * sizeof(int16_t));
// // 然后在数据区末尾填充状态字节
// buffer[4 + data_len * sizeof(int16_t)] = status_byte;
// // 4. 计算校验和 (SC 和 AC)
// uint8_t sum_check = 0;
// uint8_t add_check = 0;
// // SC: 和校验 (从帧头到数据区最后一个字节)
// for (int i = 0; i < 4 + data_payload_len; ++i) {
// sum_check += buffer[i];
// }
// // 将SC填充到缓冲区
// buffer[4 + data_payload_len] = sum_check;
// // AC: 附加校验 (从帧头到SC)
// for (int i = 0; i < 4 + data_payload_len + 1; ++i) {
// add_check += buffer[i];
// }
// // 将AC填充到缓冲区
// buffer[4 + data_payload_len + 1] = add_check;
// // 5. 发送整个数据帧
// uint16_t frame_length = 4 + data_payload_len + 2;
// // Serial_Send_Buffer(buffer, frame_length);
// for (int i = 0; i < frame_length; ++i) {
// // 使用 %c 格式化字符来发送单个字节的原始值
// printf("%c", buffer[i]);
// }
// printf("\n");
// }
void ble_send_data(signed short *acc_gyro_input, float *Angle_output){
char buffer[50]; //一次最多发送50字节
u8 len = 0;
//AA FF 01 六轴数据 EE
//TO DO
send_data_to_ble_client(&buffer,len);
//AA FF 02 欧若拉角数据 EE
// TO DO
send_data_to_ble_client(&buffer,len);
}
// 从环形缓冲区读取数据并发送
void send_sensor_data_task(void) {
// printf("xtell_ble_send\n");

2
apps/earphone/xtell_Sensor/sensor/SC7U22.c
View File

@ -120,7 +120,7 @@ unsigned char SL_SC7U22_Config(void)
os_time_dly(1);//10ms
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0x06);//ACC_CONF 0x07=50Hz 0x06=25Hz
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x41, 0x01);//ACC_RANGE ±8G
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x41, 0x01);//ACC_RANGE 10：±8G 01：±4G
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0x86);//GYR_CONF 0x87=50Hz 0x86=25Hz
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, 0x00);//GYR_RANGE 2000dps
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, 0x00);//GYR_RANGE 2000dps

BIN
cpu/br28/tools/app.bin
View File

Binary file not shown.

BIN
cpu/br28/tools/data_code.bin
View File

Binary file not shown.

BIN
cpu/br28/tools/download/earphone/jl_isd.bin
View File

Binary file not shown.

BIN
cpu/br28/tools/download/earphone/jl_isd.fw
View File

Binary file not shown.

BIN
cpu/br28/tools/download/earphone/update.ufw
View File

Binary file not shown.

BIN
cpu/br28/tools/sdk.elf
View File

Binary file not shown.

BIN
cpu/br28/tools/sdk.elf.0.0.preopt.bc
View File

Binary file not shown.

BIN
cpu/br28/tools/sdk.elf.0.2.internalize.bc
View File

Binary file not shown.

BIN
cpu/br28/tools/sdk.elf.0.4.opt.bc
View File

Binary file not shown.

BIN
cpu/br28/tools/sdk.elf.0.5.precodegen.bc
View File

Binary file not shown.

BIN
cpu/br28/tools/sdk.elf.o
View File

Binary file not shown.

3
cpu/br28/tools/sdk.elf.resolution.txt
View File

@ -6336,6 +6336,8 @@ objs/apps/earphone/xtell_Sensor/xtell_handler.c.o
-r=objs/apps/earphone/xtell_Sensor/xtell_handler.c.o,config_btctler_mode,l
-r=objs/apps/earphone/xtell_Sensor/xtell_handler.c.o,sniff_out,l
objs/apps/earphone/xtell_Sensor/send_data.c.o
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,ble_send_data,pl
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,send_data_to_ble_client,l
-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,SL_SC7U22_RawData_Read,l
@ -6439,7 +6441,6 @@ objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o
-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,printf,l
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,get_calibration_state,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,cosf,l

308912
cpu/br28/tools/sdk.lst
View File

File diff suppressed because it is too large Load Diff

1379
cpu/br28/tools/sdk.map
View File

File diff suppressed because it is too large Load Diff

BIN
objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o
View File

Binary file not shown.

BIN
objs/apps/earphone/xtell_Sensor/send_data.c.o
View File

Binary file not shown.