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

chore: Remove tracked build artifacts

Browse Source
This commit is contained in:
lmx
2025-11-28 16:24:27 +08:00
parent 892ed9267b
commit 818e8c3778
2356 changed files with 0 additions and 587882 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1,219 +0,0 @@
/*
静态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
View File

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

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

@ -1,596 +0,0 @@
/*
*/
#include "skiing_tracker.h"
#include "../sensor/SC7U22.h"
#include <math.h>
#include <string.h>
#define G_ACCELERATION 9.81f
#define DEG_TO_RAD (3.14159265f / 180.0f)
// --- 静止检测 ---
//两个判断是否静止的必要条件：动态零速更新(ZUPT)阈值
// 加速方差阈值，提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
#define STOP_ACC_VARIANCE_THRESHOLD 0.2f
// 陀螺仪方差阈值
#define STOP_GYR_VARIANCE_THRESHOLD 5.0f
// 静止时候的陀螺仪模长
#define STOP_GYR_MAG_THRESHOLD 15
// --- --- ---
// --- 启动滑雪阈值 ---
// 加速度模长与重力的差值大于此值，认为开始运动；降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
#define START_ACC_MAG_THRESHOLD 1.0f //0.5、1
// 陀螺仪方差阈值，以允许启动瞬间的正常抖动，但仍能过滤掉混乱的、非滑雪的晃动。
#define START_GYR_VARIANCE_THRESHOLD 15.0f
// --- --- ---
// --- 滑雪过程 ---
//加速度 模长(不含重力），低于此值视为 在做匀速运动
#define SKIING_ACC_MAG_THRESHOLD 0.5f
//陀螺仪 模长，高于此值视为 摔倒了
#define FALLEN_GRY_MAG_THRESHOLD 2000.0f //未确定
// --- --- ---
// --- 原地旋转抖动 ---
// 加速度 方差 阈值。此值比 静止检测 阈值更宽松，
#define WOBBLE_ACC_VARIANCE_THRESHOLD 0.5f
// 加速度 模长 阈值
#define WOBBLE_ACC_MAG_THRESHOLD 1.0f
// 角速度 总模长 大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
#define ROTATION_GYR_MAG_THRESHOLD 30.0f
// --- --- ---
// --- 滑雪转弯动 ---
// 加速度 方差 阈值，大于此值，滑雪过程可能发生了急转弯
#define WHEEL_ACC_VARIANCE_THRESHOLD 7.0f
// 角速度 总模长 大于此值(度/秒)，认为滑雪过程中进行急转弯
#define WHEEL_GYR_MAG_THRESHOLD 500.0f //
// --- --- ---
// --- 跳跃 ---
// 加速度模长低于此值(g)，认为进入失重状态(IN_AIR)
#define AIRBORNE_ACC_MAG_LOW_THRESHOLD 0.4f
// 加速度模长高于此值(g)，认为发生落地冲击(LANDING)
#define LANDING_ACC_MAG_HIGH_THRESHOLD 3.5f
// 起跳加速度阈值(g)，用于进入TAKING_OFF状态
#define TAKEOFF_ACC_MAG_HIGH_THRESHOLD 1.8f
// 进入空中状态确认计数：需要连续3个采样点加速度低于阈值才判断为起跳
#define AIRBORNE_CONFIRM_COUNT 3
// 落地状态确认计数：加速度恢复到1g附近并持续2个采样点(20ms)则认为已落地
#define GROUNDED_CONFIRM_COUNT 2
// 最大滞空时间(秒)，超过此时间强制认为已落地，防止状态锁死
#define MAX_TIME_IN_AIR 12.5f
// --- --- ---
// --- 用于消除积分漂移的滤波器和阈值 ---
// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
// alpha = RC / (RC + dt)，参考RC电路而来，fc ≈ (1 - alpha) / (2 * π * dt)
#define HPF_ALPHA 0.999f
//0.995f： 0.08 Hz 的信号
//0.999f： 0.0159 Hz
// --- --- ---
// --- 低通滤波器 ---
// 低通滤波器系数 (alpha)。alpha 越小，滤波效果越强（更平滑），但延迟越大。
// alpha 推荐范围 0.7 ~ 0.95。可以从 0.85 开始尝试。
#define LPF_ALPHA 0.7f
// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
//参考：0.2f ~ 0.4f
#define ACC_DEAD_ZONE_THRESHOLD 0.05f
// --- 模拟摩擦力，进行速度衰减 ---
#define SPEED_ATTENUATION 1.0f //暂不模拟
BLE_KS_send_data_t KS_data;
#ifdef XTELL_TEST
debug_t debug1;
debug_t debug2;
#endif
static skiing_tracker_t my_skiing_tracker;
//////////////////////////////////////////////////////////////////////////////////////////////////
//实现
void clear_speed(void){
my_skiing_tracker.state = STATIC;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.speed = 0;
}
void start_detection(void){
my_skiing_tracker.state = STATIC;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.distance = 0;
my_skiing_tracker.speed = 0;
}
void stop_detection(void){
my_skiing_tracker.state = STOP_DETECTION;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.speed = 0;
}
/**
* @brief 初始化滑雪追踪器
*
* @param tracker
*/
void skiing_tracker_init(skiing_tracker_t *tracker)
{
if (!tracker) {
return;
}
// 使用memset一次性清零整个结构体，包括新增的缓冲区
memset(tracker, 0, sizeof(skiing_tracker_t));
tracker->state = STATIC;
}
/**
* @brief 当检测到落地时，计算空中的水平飞行距离并累加到总距离
*/
static void calculate_air_distance(skiing_tracker_t *tracker) {
float horizontal_speed_on_takeoff = sqrtf(
tracker->initial_velocity_on_takeoff[0] * tracker->initial_velocity_on_takeoff[0] +
tracker->initial_velocity_on_takeoff[1] * tracker->initial_velocity_on_takeoff[1]
);
float distance_in_air = horizontal_speed_on_takeoff * tracker->time_in_air;
tracker->distance += distance_in_air;
}
/**
* @brief 在设备坐标系下，从原始加速度数据中移除重力分量
* @param acc_device 输入：设备坐标系下的原始加速度 [x, y, z], 单位 m/s^2
* @param angle 输入：姿态角 [pitch, roll, yaw]，单位: 度
* @param acc_linear_device 输出：设备坐标系下移除重力后的线性加速度 [x, y, z]
*/
void remove_gravity_in_device_frame(const float *acc_device, const float *angle, float *acc_linear_device)
{
float pitch = angle[0] * DEG_TO_RAD; // 绕 Y 轴
float roll = angle[1] * DEG_TO_RAD; // 绕 X 轴
float yaw = angle[2] * DEG_TO_RAD; // 绕 Z 轴
float cp = cosf(pitch);
float sp = sinf(pitch);
float cr = cosf(roll);
float sr = sinf(roll);
float cy = cosf(yaw);
float sy = sinf(yaw);
// 世界坐标系下的重力矢量
const float g_world[3] = {0.0f, 0.0f, G_ACCELERATION};
// 计算旋转矩阵 R 的转置矩阵 R_transpose
// R (Z-Y-X) =
// [ cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr]
// [ sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr]
// [ -sp, cp*sr, cp*cr ]
//
// R_transpose =
// [ cy*cp, sy*cp, -sp ]
// [ cy*sp*sr - sy*cr, sy*sp*sr + cy*cr, cp*sr ]
// [ cy*sp*cr + sy*sr, sy*sp*cr - cy*sr, cp*cr ]
// 计算重力在设备坐标系下的投影 G_device = R_transpose * G_world
// 由于 G_world 只有 z 分量，计算可以简化
float g_device[3];
g_device[0] = (-sp) * g_world[2];
g_device[1] = (cp * sr) * g_world[2];
g_device[2] = (cp * cr) * g_world[2];
// 从原始设备加速度中减去重力投影
acc_linear_device[0] = acc_device[0] - g_device[0];
acc_linear_device[1] = acc_device[1] - g_device[1];
acc_linear_device[2] = acc_device[2] - g_device[2];
}
/**
* @brief 计算缓冲区内三轴数据的方差之和
*
* @param buffer 传进来的三轴数据：陀螺仪/加速度
* @return float 返回方差和
*/
static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
{
float mean[3] = {0};
float variance[3] = {0};
// 计算均值
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
mean[0] += buffer[i][0];
mean[1] += buffer[i][1];
mean[2] += buffer[i][2];
}
mean[0] /= VARIANCE_BUFFER_SIZE;
mean[1] /= VARIANCE_BUFFER_SIZE;
mean[2] /= VARIANCE_BUFFER_SIZE;
// 计算方差
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
variance[0] += (buffer[i][0] - mean[0]) * (buffer[i][0] - mean[0]);
variance[1] += (buffer[i][1] - mean[1]) * (buffer[i][1] - mean[1]);
variance[2] += (buffer[i][2] - mean[2]) * (buffer[i][2] - mean[2]);
}
variance[0] /= VARIANCE_BUFFER_SIZE;
variance[1] /= VARIANCE_BUFFER_SIZE;
variance[2] /= VARIANCE_BUFFER_SIZE;
// 返回三轴方差之和，作为一个综合的稳定度指标
return variance[0] + variance[1] + variance[2];
}
/**
* @brief 摩擦力模拟，进行速度衰减
*
* @param tracker
*/
void forece_of_friction(skiing_tracker_t *tracker){
// 增加速度衰减，模拟摩擦力
tracker->velocity[0] *= SPEED_ATTENUATION;
tracker->velocity[1] *= SPEED_ATTENUATION;
tracker->velocity[2] = 0; // 垂直速度强制归零
}
/**
* @brief 状态机更新
*
* @param tracker 传入同步修改后传出
* @param acc_device_ms2 三轴加速度，m/s^2
* @param gyr_dps 三轴陀螺仪，dps
*/
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps)
{
// 缓冲区未填满时，不进行状态判断，默认为静止
if (!tracker->buffer_filled) {
tracker->state = STATIC;
return;
}
// --- 计算关键指标 ---
float acc_variance = calculate_variance(tracker->acc_buffer); // 计算加速度方差
float gyr_variance = calculate_variance(tracker->gyr_buffer); // 计算陀螺仪方差
float gyr_magnitude = sqrtf(gyr_dps[0]*gyr_dps[0] + gyr_dps[1]*gyr_dps[1] + gyr_dps[2]*gyr_dps[2]); //dps
float acc_magnitude = sqrtf(acc_device_ms2[0]*acc_device_ms2[0] + acc_device_ms2[1]*acc_device_ms2[1] + acc_device_ms2[2]*acc_device_ms2[2]); //m/s^s
float acc_magnitude_g = acc_magnitude / G_ACCELERATION; // 转换为g单位，用于跳跃判断
#ifdef XTELL_TEST
debug1.acc_variance =acc_variance;
debug1.gyr_variance =gyr_variance;
debug1.gyr_magnitude=gyr_magnitude;
debug1.acc_magnitude=fabsf(acc_magnitude - G_ACCELERATION);
#endif
// --- 状态机逻辑 (核心修改区域) ---
#if 0 //暂时不考虑空中
// 1. 空中/落地状态的后续处理
if (tracker->state == IN_AIR) {
// A. 检测巨大冲击 -> 落地
if (acc_magnitude_g > LANDING_ACC_MAG_HIGH_THRESHOLD) {
tracker->state = LANDING;
// B. 检测超时 -> 强制落地 (安全机制)
} else if (tracker->time_in_air > MAX_TIME_IN_AIR) {
tracker->state = LANDING;
// C. 检测恢复正常重力 (平缓落地)
} else if (acc_magnitude_g > 0.8f && acc_magnitude_g < 1.5f) {
tracker->grounded_entry_counter++;
if (tracker->grounded_entry_counter >= GROUNDED_CONFIRM_COUNT) {
tracker->state = LANDING;
}
} else {
tracker->grounded_entry_counter = 0;
}
return; // 在空中或刚切换到落地，结束本次状态判断
}
// 2. 严格的 "起跳->空中" 状态转换逻辑
// 只有当处于滑行状态时，才去检测起跳意图
if (tracker->state == NO_CONSTANT_SPEED || tracker->state == CONSTANT_SPEED || tracker->state == WHEEL) {
if (acc_magnitude_g > TAKEOFF_ACC_MAG_HIGH_THRESHOLD) {
tracker->state = TAKING_OFF;
tracker->airborne_entry_counter = 0; // 准备检测失重
return;
}
}
// 只有在TAKING_OFF状态下，才去检测是否进入失重
if (tracker->state == TAKING_OFF) {
if (acc_magnitude_g < AIRBORNE_ACC_MAG_LOW_THRESHOLD) {
tracker->airborne_entry_counter++;
if (tracker->airborne_entry_counter >= AIRBORNE_CONFIRM_COUNT) {
memcpy(tracker->initial_velocity_on_takeoff, tracker->velocity, sizeof(tracker->velocity));
tracker->time_in_air = 0;
tracker->state = IN_AIR;
tracker->airborne_entry_counter = 0;
tracker->grounded_entry_counter = 0;
return;
}
} else {
// 如果在起跳冲击后一段时间内没有失重，说明只是一个颠簸，恢复滑行
// 可以加一个小的超时计数器，这里为了简单先直接恢复
tracker->state = NO_CONSTANT_SPEED;
}
return; // 无论是否切换，都结束本次判断
}
#endif
// --- 静止判断 ---
if (acc_variance < STOP_ACC_VARIANCE_THRESHOLD && gyr_variance < STOP_GYR_VARIANCE_THRESHOLD && gyr_magnitude < STOP_GYR_MAG_THRESHOLD) {
tracker->state = STATIC;
return;
}
// --- 地面状态切换逻辑 ---
switch (tracker->state) {
case LANDING:
tracker->state = STATIC;
break;
case STATIC:
// 优先判断是否进入 WOBBLE 状态
// 条件：陀螺仪活动剧烈，但整体加速度变化不大（说明是原地转或晃）
if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) < WOBBLE_ACC_MAG_THRESHOLD) {
tracker->state = WOBBLE;
}
// 只有在陀螺仪和加速度都满足“前进”特征时，才启动
else if (gyr_variance > START_GYR_VARIANCE_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
}
break;
case WOBBLE:
// 从 WOBBLE 状态启动的条件应该和从 STATIC 启动一样严格
if (gyr_variance > START_GYR_VARIANCE_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
}
// 如果陀螺仪活动减弱，则可能恢复静止
else if (gyr_magnitude < ROTATION_GYR_MAG_THRESHOLD * 0.8f) { // 增加迟滞，避免抖动
// 不直接跳回STATIC，而是依赖下一轮的全局静止判断
}
break;
case NO_CONSTANT_SPEED: //非匀速状态
//暂时不考虑摔倒
// if (gyr_magnitude > FALLEN_GRY_MAG_THRESHOLD) {
// tracker->state = FALLEN; //摔倒
// } else
if (gyr_magnitude > WHEEL_GYR_MAG_THRESHOLD && acc_variance > WHEEL_ACC_VARIANCE_THRESHOLD) {
tracker->state = WHEEL; //转弯
} else if (fabsf(acc_magnitude - G_ACCELERATION) < SKIING_ACC_MAG_THRESHOLD) {
tracker->state = CONSTANT_SPEED; //匀速
}
break;
case CONSTANT_SPEED: //匀速状态
if (fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
}
//TODO：可以添加进入转弯或摔倒的判断
break;
case WHEEL:
// 从转弯状态，检查转弯是否结束
// 如果角速度和加速度方差都降下来了，就回到普通滑行状态
if (gyr_magnitude < WHEEL_GYR_MAG_THRESHOLD * 0.8f && acc_variance < WHEEL_ACC_VARIANCE_THRESHOLD * 0.8f) { // 乘以一个滞后系数避免抖动
tracker->state = NO_CONSTANT_SPEED;
}
break;
case FALLEN:
// TODO：回到 STATIC
break;
}
}
/**
* @brief 主更新函数
*
* @param tracker
* @param acc_g 三轴加速度，g
* @param gyr_dps 三轴陀螺仪，dps
* @param angle 欧若拉角
* @param dt 采样时间间隔，会用来积分求速度
*/
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt)
{
if (!tracker || !acc_g || !gyr_dps || !angle || dt <= 0) {
return;
}
if(my_skiing_tracker.state == STOP_DETECTION)
return;
// --- 数据预处理和缓冲 ---
float acc_device_ms2[3];
acc_device_ms2[0] = acc_g[0] * G_ACCELERATION;
acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
acc_device_ms2[2] = acc_g[2] * G_ACCELERATION;
// 将最新数据存入缓冲区
memcpy(tracker->acc_buffer[tracker->buffer_index], acc_device_ms2, sizeof(acc_device_ms2));
memcpy(tracker->gyr_buffer[tracker->buffer_index], gyr_dps, 3 * sizeof(float));
tracker->buffer_index++;
if (tracker->buffer_index >= VARIANCE_BUFFER_SIZE) {
tracker->buffer_index = 0;
tracker->buffer_filled = 1; // 标记缓冲区已满
}
// --- 更新状态机 ---
update_state_machine(tracker, acc_device_ms2, gyr_dps);
// --- 根据状态执行不同的计算逻辑 ---
switch (tracker->state) {
case TAKING_OFF:
tracker->speed = 0.0f;
break;
case IN_AIR:
// 在空中时，只累加滞空时间
tracker->time_in_air += dt;
break;
case LANDING:
// 刚落地，计算空中距离
calculate_air_distance(tracker);
// 清理速度和滤波器状态，为恢复地面追踪做准备
memset(tracker->velocity, 0, sizeof(tracker->velocity));
tracker->speed = 0;
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态
break;
case WHEEL:
case NO_CONSTANT_SPEED:
remove_gravity_in_device_frame(acc_device_ms2, angle, tracker->acc_no_g);
float acc_world_temp[3]; // 临时变量存储当前周期的加速度
for (int i = 0; i < 2; i++) { // 只处理水平方向的 x 和 y 轴
// --- 核心修改：颠倒滤波器顺序为 HPF -> LPF ---
// 1. 高通滤波 (HPF) 先行: 消除因姿态误差导致的重力泄漏（直流偏置）
// HPF的瞬态响应会产生尖峰，这是正常的。
tracker->acc_world_filtered[i] = HPF_ALPHA * (tracker->acc_world_filtered[i] + tracker->acc_no_g[i] - tracker->acc_world_unfiltered_prev[i]);
tracker->acc_world_unfiltered_prev[i] = tracker->acc_no_g[i];
// 2. 低通滤波 (LPF) 殿后: 平滑掉HPF产生的尖峰和传感器自身的高频振动噪声。
// 这里使用 tracker->acc_world_filtered[i] 作为LPF的输入。
tracker->acc_world_lpf[i] = (1.0f - LPF_ALPHA) * tracker->acc_world_filtered[i] + LPF_ALPHA * tracker->acc_world_lpf[i];
// 将最终处理完的加速度值存入临时变量
acc_world_temp[i] = tracker->acc_world_lpf[i];
}
// 计算处理后加速度的水平模长
float acc_horizontal_mag = sqrtf(acc_world_temp[0] * acc_world_temp[0] +
acc_world_temp[1] * acc_world_temp[1]);
#if XTELL_TEST
debug2.acc_magnitude = acc_horizontal_mag;
#endif
// 应用死区，并积分
if (acc_horizontal_mag > ACC_DEAD_ZONE_THRESHOLD) {
tracker->velocity[0] += acc_world_temp[0] * dt;
tracker->velocity[1] += acc_world_temp[1] * dt;
}
// 更新速度和距离
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
tracker->velocity[1] * tracker->velocity[1]);
tracker->distance += tracker->speed * dt;
break;
case CONSTANT_SPEED:
//保持上次的速度不变。只更新距离
tracker->distance += tracker->speed * dt;
break;
case STATIC:
case WOBBLE:
// 速度清零，抑制漂移
memset(tracker->velocity, 0, sizeof(tracker->velocity));
tracker->speed = 0.0f;
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态
#if XTELL_TEST
debug2.acc_magnitude = 0;
#endif
break;
case FALLEN:
// TODO
break;
default:
break;
}
}
/**
* @brief 滑雪数据计算
*
* @param acc_data_buf 传入的三轴加速度数据
* @param gyr_data_buf 传入的三轴陀螺仪数据
* @param angle_data 传入的欧若拉角数据
* @return BLE_send_data_t 要发送给蓝牙的数据
*/
BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data) {
static int initialized = 0;
static float acc_data_g[3];
static float gyr_data_dps[3];
// const float delta_time = DELTA_TIME+0.01f;
// const float delta_time = DELTA_TIME + 0.005f;
const float delta_time = DELTA_TIME;
BLE_send_data_t BLE_send_data;
if (!initialized) {
skiing_tracker_init(&my_skiing_tracker);
initialized = 1;
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
}
#if ACC_RANGE==2
// 加速度 LSB to g
acc_data_g[0] = (float)acc_data_buf[0] / 16384.0f;
acc_data_g[1] = (float)acc_data_buf[1] / 16384.0f;
acc_data_g[2] = (float)acc_data_buf[2] / 16384.0f;
#endif
#if ACC_RANGE==4
// 加速度 LSB to g
acc_data_g[0] = (float)acc_data_buf[0] / 8192.0f;
acc_data_g[1] = (float)acc_data_buf[1] / 8192.0f;
acc_data_g[2] = (float)acc_data_buf[2] / 8192.0f;
#endif
#if ACC_RANGE==8
//±8g 4096
acc_data_g[0] = (float)acc_data_buf[0] / 4096.0f; //ax
acc_data_g[1] = (float)acc_data_buf[1] / 4096.0f; //ay
acc_data_g[2] = (float)acc_data_buf[2] / 4096.0f; //az
#endif
#if ACC_RANGE==16
//±16g 2048
acc_data_g[0] = (float)acc_data_buf[0] / 2048.0f; //ax
acc_data_g[1] = (float)acc_data_buf[1] / 2048.0f; //ay
acc_data_g[2] = (float)acc_data_buf[2] / 2048.0f; //az
#endif
// 陀螺仪 LSB to dps (度/秒)
// ±2000dps量程下，转换系数约为 0.061
gyr_data_dps[0] = (float)gyr_data_buf[0] * 0.061f;
gyr_data_dps[1] = (float)gyr_data_buf[1] * 0.061f;
gyr_data_dps[2] = (float)gyr_data_buf[2] * 0.061f;
skiing_tracker_update(&my_skiing_tracker, acc_data_g, gyr_data_dps, angle_data, delta_time);
BLE_send_data.skiing_state = my_skiing_tracker.state;
for (int i = 0; i < 3; i++) {
#ifdef XTELL_TEST
BLE_send_data.acc_data[i] = (short)(acc_data_g[i] * 9.8f) * 100; //cm/^s2
BLE_send_data.gyr_data[i] = (short)gyr_data_dps[i]; //dps
BLE_send_data.angle_data[i] = angle_data[i];
#else
BLE_send_data.acc_data[i] = (short)acc_data_buf[i]; //原始adc数据
BLE_send_data.gyr_data[i] = (short)gyr_data_buf[i]; //原始adc数据
BLE_send_data.angle_data[i] = angle_data[i];
#endif
}
BLE_send_data.speed_cms = (int)(my_skiing_tracker.speed * 100);
BLE_send_data.distance_cm = (int)(my_skiing_tracker.distance * 100);
// printf("Calculate the time interval =============== end\n");
return BLE_send_data;
}

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

@ -1,88 +0,0 @@
#ifndef SKIING_TRACKER_H
#define SKIING_TRACKER_H
#include "../xtell.h"
// 定义滑雪者可能的状态
typedef enum {
STATIC, // 静止或动态稳定：0
NO_CONSTANT_SPEED, // 正在滑雪，非匀速：1
CONSTANT_SPEED, // 正在滑雪，匀速：2
WOBBLE, // 正在原地旋转：3
WHEEL, // 转弯：4
FALLEN, // 已摔倒：5
TAKING_OFF, // 起跳冲击阶段：6
IN_AIR, // 空中失重阶段：7
LANDING, // 落地冲击阶段：8
STOP_DETECTION, // 停止检测：9
UNKNOWN // 未知状态：10
} skiing_state_t;
#define VARIANCE_BUFFER_SIZE 5 // 用于计算方差的数据窗口大小 (5个样本 @ 100Hz = 50ms)，减小延迟，提高实时性
#define DELTA_TIME 0.01f
// 追踪器数据结构体
typedef struct {
// 公开数据
float velocity[3]; // 当前速度 (x, y, z)，单位: m/s
float distance; // 总滑行距离，单位: m
float speed; // 当前速率 (标量)，单位: m/s
skiing_state_t state; // 当前滑雪状态
// 内部计算使用的私有成员
float acc_no_g[3]; // 去掉重力分量后的加速度
// 用于空中距离计算
float time_in_air; // 滞空时间计时器
float initial_velocity_on_takeoff[3]; // 起跳瞬间的速度向量
int airborne_entry_counter; // 进入空中状态的确认计数器
int grounded_entry_counter; // 落地确认计数器
// --- 内部计算使用的私有成员 ---
// 用于动态零速更新和旋转检测的缓冲区
float acc_buffer[VARIANCE_BUFFER_SIZE][3]; // 加速度数据窗口
float gyr_buffer[VARIANCE_BUFFER_SIZE][3]; // 角速度数据窗口
int buffer_index; // 缓冲区当前索引
int buffer_filled; // 缓冲区是否已填满的标志
// 用于高通滤波器（巴特沃斯一阶滤波器）的私有成员，以消除加速度的直流偏置
float acc_world_filtered[3]; //过滤过的
float acc_world_unfiltered_prev[3]; //上一次没过滤的
float acc_world_lpf[3]; // 经过低通滤波后的世界坐标系加速度
} skiing_tracker_t;
//ble发送的数据
typedef struct{ //__attribute__((packed)){ //该结构体取消内存对齐
char sensor_state;
char skiing_state;
int speed_cms; //求出的速度，cm/s
int distance_cm; //求出的距离，cm
short acc_data[3]; //三轴加速度, g
short gyr_data[3]; //三轴陀螺仪, dps
float angle_data[3]; //欧若拉角
}BLE_send_data_t;
typedef struct{
int acc_KS[3]; //卡尔曼后，LSB转换后的 三轴加速度数据（cm/s^2）
int gyr_KS_dps[3]; //卡尔曼后，LSB to dps 三轴陀螺仪数据
int angle_KS[3]; //卡尔曼后，计算得到的欧若拉角数据
}BLE_KS_send_data_t;
#ifdef XTELL_TEST
typedef struct{
float acc_variance; //三轴加速度方差之和
float gyr_variance; //三轴陀螺仪方差之和
float acc_magnitude; //三轴加速度模长
float gyr_magnitude; //三轴陀螺仪模长
}debug_t;
#endif
/**
* @brief 初始化滑雪追踪器
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
*/
void skiing_tracker_init(skiing_tracker_t *tracker);
BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data) ;
#endif // SKIING_TRACKER_H

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

@ -1,377 +0,0 @@
/*
使用四元数求角度和去掉重力分量
*/
#include "skiing_tracker.h"
#include "../sensor/SC7U22.h"
#include <math.h>
#include <string.h>
#define ENABLE_XLOG 1
#ifdef xlog
#undef xlog
#endif
#if ENABLE_XLOG
#define xlog(format, ...) printf("[XT:%s] " format, __func__, ##__VA_ARGS__)
#else
#define xlog(format, ...) ((void)0)
#endif
#define G_ACCELERATION 9.81f
#define DEG_TO_RAD (3.14159265f / 180.0f)
BLE_KS_send_data_t KS_data;
static float quaternion_data[4];
#ifdef XTELL_TEST
debug_t debug1;
debug_t debug2;
#endif
static skiing_tracker_t my_skiing_tracker;
//////////////////////////////////////////////////////////////////////////////////////////////////
//实现
void clear_speed(void){
my_skiing_tracker.state = STATIC;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.speed = 0;
}
void start_detection(void){
my_skiing_tracker.state = STATIC;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.distance = 0;
my_skiing_tracker.speed = 0;
}
void stop_detection(void){
my_skiing_tracker.state = STOP_DETECTION;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.speed = 0;
}
/**
* @brief 初始化滑雪追踪器
*
* @param tracker
*/
void skiing_tracker_init(skiing_tracker_t *tracker)
{
if (!tracker) {
return;
}
// 使用memset一次性清零整个结构体，包括新增的缓冲区
memset(tracker, 0, sizeof(skiing_tracker_t));
tracker->state = STATIC;
}
/**
* @brief 当检测到落地时，计算空中的水平飞行距离并累加到总距离
*/
static void calculate_air_distance(skiing_tracker_t *tracker) {
float horizontal_speed_on_takeoff = sqrtf(
tracker->initial_velocity_on_takeoff[0] * tracker->initial_velocity_on_takeoff[0] +
tracker->initial_velocity_on_takeoff[1] * tracker->initial_velocity_on_takeoff[1]
);
float distance_in_air = horizontal_speed_on_takeoff * tracker->time_in_air;
tracker->distance += distance_in_air;
}
/**
* @brief 将设备坐标系下的加速度转换为世界坐标系，去掉重力分量
*
* @param acc_device
* @param angle
* @param acc_linear_world
*/
static void transform_acc_to_world_frame(const float *acc_device, const float *angle, float *acc_linear_world)
{
// 1. 将输入的角度从度转换为弧度
// angle[0] -> pitch, angle[1] -> roll, angle[2] -> yaw
float pitch_rad = -angle[0] * (M_PI / 180.0f);
float roll_rad = -angle[1] * (M_PI / 180.0f);
float yaw_rad = -angle[2] * (M_PI / 180.0f);
// 2. 预先计算三角函数值，以提高效率
float c_r = cosf(roll_rad);
float s_r = sinf(roll_rad);
float c_p = cosf(pitch_rad);
float s_p = sinf(pitch_rad);
float c_y = cosf(yaw_rad);
float s_y = sinf(yaw_rad);
// 3. 构建从设备坐标系到世界坐标系的旋转矩阵 R_device_to_world
// 该矩阵基于 Z-Y-X (Yaw-Pitch-Roll) 欧拉角顺序
// R = R_z(yaw) * R_y(pitch) * R_x(roll)
float R[3][3];
R[0][0] = c_y * c_p;
R[0][1] = c_y * s_p * s_r - s_y * c_r;
R[0][2] = c_y * s_p * c_r + s_y * s_r;
R[1][0] = s_y * c_p;
R[1][1] = s_y * s_p * s_r + c_y * c_r;
R[1][2] = s_y * s_p * c_r - c_y * s_r;
R[2][0] = -s_p;
R[2][1] = c_p * s_r;
R[2][2] = c_p * c_r;
// 4. 将设备坐标系的加速度计总读数旋转到世界坐标系
// a_raw_world = R * acc_device
float ax_raw_world = R[0][0] * acc_device[0] + R[0][1] * acc_device[1] + R[0][2] * acc_device[2];
float ay_raw_world = R[1][0] * acc_device[0] + R[1][1] * acc_device[1] + R[1][2] * acc_device[2];
float az_raw_world = R[2][0] * acc_device[0] + R[2][1] * acc_device[1] + R[2][2] * acc_device[2];
// 5. 在世界坐标系中减去重力分量，得到线性加速度
// 假设世界坐标系Z轴垂直向上，重力矢量为 [0, 0, -g]
// 线性加速度 = 总加速度 - 重力加速度
// az_linear = az_raw_world - (-g) = az_raw_world + g (如果Z轴向上)
// az_linear = az_raw_world - (+g) = az_raw_world - g (如果Z轴向下)
// 这里我们采用 Z 轴向上的标准惯性系 (ENU)
acc_linear_world[0] = ax_raw_world;
acc_linear_world[1] = ay_raw_world;
acc_linear_world[2] = az_raw_world - G_ACCELERATION; // Z轴向上，重力为正值，所以减去
}
/**
* @brief 在设备坐标系下，从原始加速度数据中移除重力分量
* @param acc_device 输入：设备坐标系下的原始加速度 [x, y, z], 单位 m/s^2
* @param angle 输入：姿态角 [pitch, roll, yaw]，单位: 度
* @param acc_linear_device 输出：设备坐标系下移除重力后的线性加速度 [x, y, z]
*/
void remove_gravity_in_device_frame(const float *acc_device, const float *angle, float *acc_linear_device)
{
float pitch = -angle[0] * DEG_TO_RAD; // 绕 Y 轴
float roll = -angle[1] * DEG_TO_RAD; // 绕 X 轴
float yaw = -angle[2] * DEG_TO_RAD; // 绕 Z 轴
float cp = cosf(pitch);
float sp = sinf(pitch);
float cr = cosf(roll);
float sr = sinf(roll);
float cy = cosf(yaw);
float sy = sinf(yaw);
// 世界坐标系下的重力矢量
const float g_world[3] = {0.0f, 0.0f, G_ACCELERATION};
// 计算旋转矩阵 R 的转置矩阵 R_transpose
// R (Z-Y-X) =
// [ cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr]
// [ sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr]
// [ -sp, cp*sr, cp*cr ]
//
// R_transpose =
// [ cy*cp, sy*cp, -sp ]
// [ cy*sp*sr - sy*cr, sy*sp*sr + cy*cr, cp*sr ]
// [ cy*sp*cr + sy*sr, sy*sp*cr - cy*sr, cp*cr ]
// 计算重力在设备坐标系下的投影 G_device = R_transpose * G_world
// 由于 G_world 只有 z 分量，计算可以简化
float g_device[3];
g_device[0] = (-sp) * g_world[2];
g_device[1] = (cp * sr) * g_world[2];
g_device[2] = (cp * cr) * g_world[2];
// 从原始设备加速度中减去重力投影
acc_linear_device[0] = acc_device[0] - g_device[0];
acc_linear_device[1] = acc_device[1] - g_device[1];
acc_linear_device[2] = acc_device[2] - g_device[2];
}
/**
* @brief 使用四元数直接从设备坐标系的加速度中移除重力分量
* @details 这种方法比使用欧拉角更精确、更稳定，且避免了万向节死锁。
* @param acc_device 输入：设备坐标系下的原始加速度 [x, y, z], 单位 m/s^2
* @param q 输入：表示姿态的四元数 [w, x, y, z]
* @param acc_linear_device 输出：设备坐标系下移除重力后的线性加速度 [x, y, z]
*/
void q_remove_gravity_with_quaternion(const float *acc_device, const float *q, float *acc_linear_device)
{
// 从四元数计算重力在设备坐标系下的投影
// G_device = R_transpose * G_world
// G_world = [0, 0, g]
// R_transpose 的第三列即为重力投影方向
float gx = 2.0f * (q[1] * q[3] - q[0] * q[2]);
float gy = 2.0f * (q[0] * q[1] + q[2] * q[3]);
float gz = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
// 从原始加速度中减去重力分量
acc_linear_device[0] = acc_device[0] - gx * G_ACCELERATION;
acc_linear_device[1] = acc_device[1] - gy * G_ACCELERATION;
acc_linear_device[2] = acc_device[2] - gz * G_ACCELERATION;
}
/**
* @brief 使用四元数将设备坐标系的线性加速度转换到世界坐标系,并且移除重力分量
* @details 同样，此方法比使用欧拉角更优。
* @param acc_linear_device 输入：设备坐标系下的线性加速度 [x, y, z]
* @param q 输入：表示姿态的四元数 [w, x, y, z]
* @param acc_linear_world 输出：世界坐标系下的线性加速度 [x, y, z]
*/
void q_transform_to_world_with_quaternion(const float *acc_linear_device, const float *q, float *acc_linear_world)
{
// 这是 R_device_to_world * acc_linear_device 的展开形式
acc_linear_world[0] = (1.0f - 2.0f*q[2]*q[2] - 2.0f*q[3]*q[3]) * acc_linear_device[0] +
(2.0f*q[1]*q[2] - 2.0f*q[0]*q[3]) * acc_linear_device[1] +
(2.0f*q[1]*q[3] + 2.0f*q[0]*q[2]) * acc_linear_device[2];
acc_linear_world[1] = (2.0f*q[1]*q[2] + 2.0f*q[0]*q[3]) * acc_linear_device[0] +
(1.0f - 2.0f*q[1]*q[1] - 2.0f*q[3]*q[3]) * acc_linear_device[1] +
(2.0f*q[2]*q[3] - 2.0f*q[0]*q[1]) * acc_linear_device[2];
acc_linear_world[2] = (2.0f*q[1]*q[3] - 2.0f*q[0]*q[2]) * acc_linear_device[0] +
(2.0f*q[2]*q[3] + 2.0f*q[0]*q[1]) * acc_linear_device[1] +
(1.0f - 2.0f*q[1]*q[1] - 2.0f*q[2]*q[2]) * acc_linear_device[2];
acc_linear_world[2] -= G_ACCELERATION;
}
/**
* @brief 主更新函数
*
* @param tracker
* @param acc_g 三轴加速度，g
* @param gyr_dps 三轴陀螺仪，dps
* @param angle 欧若拉角
* @param dt 采样时间间隔，会用来积分求速度
*/
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt)
{
if (!tracker || !acc_g || !gyr_dps || !angle || dt <= 0) {
return;
}
if(my_skiing_tracker.state == STOP_DETECTION)
return;
// --- 数据预处理和缓冲 ---
float acc_device_ms2[3];
acc_device_ms2[0] = acc_g[0] * G_ACCELERATION;
acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
acc_device_ms2[2] = acc_g[2] * G_ACCELERATION;
#if 1 //测试禁止状态下陀螺仪的三轴加速度，去掉重力分量后是否正常
float tmp_device_acc[3];
float tmp_world_acc[3];
// remove_gravity_in_device_frame(acc_device_ms2,angle,tmp_device_acc);
// transform_acc_to_world_frame(acc_device_ms2,angle,tmp_world_acc);
q_remove_gravity_with_quaternion(acc_device_ms2,quaternion_data,tmp_device_acc);
q_transform_to_world_with_quaternion(acc_device_ms2,quaternion_data,tmp_world_acc);
// 计算处理后加速度的水平模长
float all_device_mag = sqrtf(tmp_device_acc[0] * tmp_device_acc[0] +
tmp_device_acc[1] * tmp_device_acc[1] +
tmp_device_acc[2] * tmp_device_acc[2]);
float all_world_mag = sqrtf(tmp_world_acc[0] * tmp_world_acc[0] +
tmp_world_acc[1] * tmp_world_acc[1] +
tmp_world_acc[2] * tmp_world_acc[2]);
static int count = 0;
if(count > 100){
xlog("===original(g): x %.2f, y %.2f, z %.2f===\n",acc_g[0],acc_g[1],acc_g[2]);
xlog("===device(m/s^2) no g: x %.2f, y %.2f, z %.2f, all %.2f===\n",tmp_device_acc[0],tmp_device_acc[1],tmp_device_acc[2],all_device_mag); //去掉重力加速度
xlog("===world(m/s^2) no g: x %.2f, y %.2f, z %.2f, all %.2f===\n",tmp_world_acc[0],tmp_world_acc[1],tmp_world_acc[2],all_world_mag); //去掉重力加速度
xlog("===gyr(dps) : x %.2f, y %.2f, z %.2f, all %.2f===\n",gyr_dps[0],gyr_dps[1],gyr_dps[2]); //angle
xlog("===angle : x %.2f, y %.2f, z %.2f,===\n",angle[0],angle[1],angle[2]);
count = 0;
}
count++;
#endif
}
/**
* @brief 滑雪数据计算
*
* @param acc_data_buf 传入的三轴加速度数据
* @param gyr_data_buf 传入的三轴陀螺仪数据
* @param angle_data 传入的欧若拉角数据
* @return BLE_send_data_t 要发送给蓝牙的数据
*/
BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data, float* quaternion) {
static int initialized = 0;
static float acc_data_g[3];
static float gyr_data_dps[3];
if(quaternion != NULL){
memcpy(quaternion_data, quaternion, 4 * sizeof(float));
}
// const float delta_time = DELTA_TIME+0.01f;
// const float delta_time = DELTA_TIME + 0.005f;
const float delta_time = DELTA_TIME;
BLE_send_data_t BLE_send_data;
if (!initialized) {
skiing_tracker_init(&my_skiing_tracker);
initialized = 1;
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
}
#if ACC_RANGE==2
// 加速度 LSB to g
acc_data_g[0] = (float)acc_data_buf[0] / 16384.0f;
acc_data_g[1] = (float)acc_data_buf[1] / 16384.0f;
acc_data_g[2] = (float)acc_data_buf[2] / 16384.0f;
#endif
#if ACC_RANGE==4
// 加速度 LSB to g
acc_data_g[0] = (float)acc_data_buf[0] / 8192.0f;
acc_data_g[1] = (float)acc_data_buf[1] / 8192.0f;
acc_data_g[2] = (float)acc_data_buf[2] / 8192.0f;
#endif
#if ACC_RANGE==8
//±8g 4096
acc_data_g[0] = (float)acc_data_buf[0] / 4096.0f; //ax
acc_data_g[1] = (float)acc_data_buf[1] / 4096.0f; //ay
acc_data_g[2] = (float)acc_data_buf[2] / 4096.0f; //az
#endif
#if ACC_RANGE==16
//±16g 2048
acc_data_g[0] = (float)acc_data_buf[0] / 2048.0f; //ax
acc_data_g[1] = (float)acc_data_buf[1] / 2048.0f; //ay
acc_data_g[2] = (float)acc_data_buf[2] / 2048.0f; //az
#endif
// 陀螺仪 LSB to dps (度/秒)
// ±2000dps量程下，转换系数约为 0.061
gyr_data_dps[0] = (float)gyr_data_buf[0] * 0.061f;
gyr_data_dps[1] = (float)gyr_data_buf[1] * 0.061f;
gyr_data_dps[2] = (float)gyr_data_buf[2] * 0.061f;
skiing_tracker_update(&my_skiing_tracker, acc_data_g, gyr_data_dps, angle_data, delta_time);
// BLE_send_data.skiing_state = my_skiing_tracker.state;
// for (int i = 0; i < 3; i++) {
// #ifdef XTELL_TEST
// BLE_send_data.acc_data[i] = (short)(acc_data_g[i] * 9.8f) * 100; //cm/^s2
// BLE_send_data.gyr_data[i] = (short)gyr_data_dps[i]; //dps
// BLE_send_data.angle_data[i] = angle_data[i];
// #else
// BLE_send_data.acc_data[i] = (short)acc_data_buf[i]; //原始adc数据
// BLE_send_data.gyr_data[i] = (short)gyr_data_buf[i]; //原始adc数据
// BLE_send_data.angle_data[i] = angle_data[i];
// #endif
// }
// BLE_send_data.speed_cms = (int)(my_skiing_tracker.speed * 100);
// BLE_send_data.distance_cm = (int)(my_skiing_tracker.distance * 100);
// // printf("Calculate the time interval =============== end\n");
return BLE_send_data;
}

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

@ -1,88 +0,0 @@
#ifndef SKIING_TRACKER_H
#define SKIING_TRACKER_H
#include "../xtell.h"
// 定义滑雪者可能的状态
typedef enum {
STATIC, // 静止或动态稳定：0
NO_CONSTANT_SPEED, // 正在滑雪，非匀速：1
CONSTANT_SPEED, // 正在滑雪，匀速：2
WOBBLE, // 正在原地旋转：3
WHEEL, // 转弯：4
FALLEN, // 已摔倒：5
TAKING_OFF, // 起跳冲击阶段：6
IN_AIR, // 空中失重阶段：7
LANDING, // 落地冲击阶段：8
STOP_DETECTION, // 停止检测：9
UNKNOWN // 未知状态：10
} skiing_state_t;
#define VARIANCE_BUFFER_SIZE 5 // 用于计算方差的数据窗口大小 (5个样本 @ 100Hz = 50ms)，减小延迟，提高实时性
#define DELTA_TIME 0.01f
// 追踪器数据结构体
typedef struct {
// 公开数据
float velocity[3]; // 当前速度 (x, y, z)，单位: m/s
float distance; // 总滑行距离，单位: m
float speed; // 当前速率 (标量)，单位: m/s
skiing_state_t state; // 当前滑雪状态
// 内部计算使用的私有成员
float acc_world[3]; // 在世界坐标系下的加速度
// 用于空中距离计算
float time_in_air; // 滞空时间计时器
float initial_velocity_on_takeoff[3]; // 起跳瞬间的速度向量
int airborne_entry_counter; // 进入空中状态的确认计数器
int grounded_entry_counter; // 落地确认计数器
// --- 内部计算使用的私有成员 ---
// 用于动态零速更新和旋转检测的缓冲区
float acc_buffer[VARIANCE_BUFFER_SIZE][3]; // 加速度数据窗口
float gyr_buffer[VARIANCE_BUFFER_SIZE][3]; // 角速度数据窗口
int buffer_index; // 缓冲区当前索引
int buffer_filled; // 缓冲区是否已填满的标志
// 用于高通滤波器（巴特沃斯一阶滤波器）的私有成员，以消除加速度的直流偏置
float acc_world_filtered[3]; //过滤过的
float acc_world_unfiltered_prev[3]; //上一次没过滤的
float acc_world_lpf[3]; // 经过低通滤波后的世界坐标系加速度
} skiing_tracker_t;
//ble发送的数据
typedef struct{ //__attribute__((packed)){ //该结构体取消内存对齐
char sensor_state;
char skiing_state;
int speed_cms; //求出的速度，cm/s
int distance_cm; //求出的距离，cm
short acc_data[3]; //三轴加速度, g
short gyr_data[3]; //三轴陀螺仪, dps
float angle_data[3]; //欧若拉角
}BLE_send_data_t;
typedef struct{
int acc_KS[3]; //卡尔曼后，LSB转换后的 三轴加速度数据（cm/s^2）
int gyr_KS_dps[3]; //卡尔曼后，LSB to dps 三轴陀螺仪数据
int angle_KS[3]; //卡尔曼后，计算得到的欧若拉角数据
}BLE_KS_send_data_t;
#ifdef XTELL_TEST
typedef struct{
float acc_variance; //三轴加速度方差之和
float gyr_variance; //三轴陀螺仪方差之和
float acc_magnitude; //三轴加速度模长
float gyr_magnitude; //三轴陀螺仪模长
}debug_t;
#endif
/**
* @brief 初始化滑雪追踪器
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
*/
void skiing_tracker_init(skiing_tracker_t *tracker);
BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data, float* quaternion);
#endif // SKIING_TRACKER_H

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

@ -1,259 +0,0 @@
/*
动态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
View File

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

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

@ -1,277 +0,0 @@
/*
动态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
View File

@ -1,50 +0,0 @@
#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
View File

@ -1,465 +0,0 @@
/**
* 效果不行，对原地转动太灵敏了
*/
#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
View File

@ -1,40 +0,0 @@
#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
View File

@ -1,311 +0,0 @@
/*
动态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
View File

@ -1,55 +0,0 @@
#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
View File

@ -1,374 +0,0 @@
/*
动态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
View File

@ -1,74 +0,0 @@
#ifndef SKIING_TRACKER_H
#define SKIING_TRACKER_H
#include "../xtell.h"
// 定义滑雪者可能的状态
typedef enum {
SKIING_STATE_STATIC, // 静止或动态稳定
SKIING_STATE_SKIING, // 正在滑雪
SKIING_STATE_ROTATING, // 正在原地旋转 (新增)
SKIING_STATE_FALLEN, // 已摔倒
SKIING_STATE_UNKNOWN // 未知状态
} skiing_state_t;
#define VARIANCE_BUFFER_SIZE 5 // 用于计算方差的数据窗口大小 (5个样本 @ 100Hz = 50ms)，减小延迟，提高实时性
#define DELTA_TIME 0.01f
// 追踪器数据结构体
typedef struct {
// 公开数据
float velocity[3]; // 当前速度 (x, y, z)，单位: m/s
float distance; // 总滑行距离，单位: m
float speed; // 当前速率 (标量)，单位: m/s
skiing_state_t state; // 当前滑雪状态
// 内部计算使用的私有成员
float acc_world[3]; // 在世界坐标系下的加速度
// --- 内部计算使用的私有成员 ---
// 用于动态零速更新和旋转检测的缓冲区
float acc_buffer[VARIANCE_BUFFER_SIZE][3]; // 加速度数据窗口
float gyr_buffer[VARIANCE_BUFFER_SIZE][3]; // 角速度数据窗口
int buffer_index; // 缓冲区当前索引
int buffer_filled; // 缓冲区是否已填满的标志
// 用于高通滤波器（巴特沃斯一阶滤波器）的私有成员，以消除加速度的直流偏置
float acc_world_filtered[3]; //过滤过的
float acc_world_unfiltered_prev[3]; //上一次没过滤的
} skiing_tracker_t;
//ble发送的数据
typedef struct __attribute__((packed)){ //该结构体取消内存对齐
char sensor_state;
char skiing_state;
int speed_cms; //求出的速度，cm/s
int distance_cm; //求出的距离，cm
#ifndef XTELL_TEST
int acc_original[3]; //直接读取传感器得到的原始三轴加速度
int gyr_original[3]; //直接读取传感器得到的原始三轴陀螺仪
#endif
}BLE_send_data_t;
typedef struct{
int acc_KS_g[3]; //卡尔曼后，LSB to g 三轴加速度数据
int gyr_KS_dps[3]; //卡尔曼后，LSB to dps 三轴陀螺仪数据
int angle_KS[3]; //卡尔曼后，计算得到的欧若拉角数据
}BLE_KS_send_data_t;
/**
* @brief 初始化滑雪追踪器
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
*/
void skiing_tracker_init(skiing_tracker_t *tracker);
/**
* @brief 传感器数据采集与处理任务，外部每10ms调用一次，如果需要更新时间间隔，也需要同步更新宏“ DELTA_TIME ”
*
* @param acc_data_buf 三轴加速度原始数据
* @param gyr_data_buf 三轴陀螺仪原始数据
* @return BLE_send_data_t
*/
BLE_send_data_t sensor_processing_task(signed short * acc_data_buf, signed short * gyr_data_buf) ;
#endif // SKIING_TRACKER_H

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

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

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

@ -1,67 +0,0 @@
#ifndef SKIING_TRACKER_H
#define SKIING_TRACKER_H
#include "../xtell.h"
// 定义滑雪者可能的状态
typedef enum {
SKIING_STATE_STATIC, // 静止
SKIING_STATE_SKIING, // 正在滑雪
SKINNG_STOP_DETECTION, // 停止检测
} skiing_state_t;
#define DELTA_TIME 0.01f
// 追踪器数据结构体
typedef struct {
// 公开数据
float velocity[3]; // 当前速度 (x, y, z)，单位: m/s
float distance; // 总滑行距离，单位: m
float speed; // 当前速率 (标量)，单位: m/s
skiing_state_t state; // 当前滑雪状态
// 内部计算使用的私有成员
float acc_world[3]; // 在世界坐标系下的线性加速度
} skiing_tracker_t;
//ble发送的数据
typedef struct __attribute__((packed)){ //该结构体取消内存对齐
char sensor_state;
char skiing_state;
int speed_cms; //求出的速度，cm/s
int distance_cm; //求出的距离，cm
#ifndef XTELL_TEST
int acc_original[3]; //直接读取传感器得到的原始三轴加速度
int gyr_original[3]; //直接读取传感器得到的原始三轴陀螺仪
#endif
}BLE_send_data_t;
typedef struct{
int acc_KS[3]; //卡尔曼后，LSB转换后的 三轴加速度数据（cm/s^2）
int gyr_KS_dps[3]; //卡尔曼后，LSB to dps 三轴陀螺仪数据
int angle_KS[3]; //卡尔曼后，计算得到的欧若拉角数据
}BLE_KS_send_data_t;
#ifdef XTELL_TEST
typedef struct{
float acc_variance; //三轴加速度方差之和
float gyr_variance; //三轴陀螺仪方差之和
float acc_magnitude; //三轴加速度模长
float gyr_magnitude; //三轴陀螺仪模长
}debug_t;
#endif
/**
* @brief 初始化滑雪追踪器
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
*/
void skiing_tracker_init(skiing_tracker_t *tracker);
/**
* @brief 传感器数据采集与处理任务，外部每10ms调用一次，如果需要更新时间间隔，也需要同步更新宏“ DELTA_TIME ”
*
* @param acc_data_buf 三轴加速度原始数据
* @param gyr_data_buf 三轴陀螺仪原始数据
* @return BLE_send_data_t
*/
BLE_send_data_t sensor_processing_task(signed short * acc_data_buf, signed short * gyr_data_buf) ;
#endif // SKIING_TRACKER_H

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

@ -1,501 +0,0 @@
/*
虽然sensor_processing_task是10ms调用一次
但是实际上上一次调用该函数的时间点和下一次调用该函数的时间点，会相差40ms
*/
#include "skiing_tracker.h"
#include "../sensor/SC7U22.h"
#include <math.h>
#include <string.h>
#define G_ACCELERATION 9.81f
#define DEG_TO_RAD (3.14159265f / 180.0f)
// --- ZUPT ---
//两个判断是否静止的必要条件：动态零速更新(ZUPT)阈值
// 加速方差阈值，提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
#define ZUPT_ACC_VARIANCE_THRESHOLD 0.2f
// 陀螺仪方差阈值
#define ZUPT_GYR_VARIANCE_THRESHOLD 5.0f
// --- 启动滑雪阈值 ---
// 加速度模长与重力的差值大于此值，认为开始运动；降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
#define START_SKIING_ACC_THRESHOLD 0.5f
// 陀螺仪方差阈值，以允许启动瞬间的正常抖动，但仍能过滤掉混乱的、非滑雪的晃动。
#define SKIING_GYR_VARIANCE_THRESHOLD 15.0f
// --- 滑雪过程 ---
//加速度 模长，低于此值视为 在做匀速运动
#define SKIING_ACC_MAG_THRESHOLD 0.5f
//陀螺仪 模长，高于此值视为 摔倒了
#define FALLEN_GRY_MAG_THRESHOLD 1000.0f //未确定
// --- 原地旋转抖动 ---
// 用于原地旋转判断的加速度方差阈值。此值比ZUPT阈值更宽松，
// 以允许原地旋转时身体的正常晃动，但仍能与真实滑行时的剧烈加速度变化区分开。
#define ROTATING_ACC_VARIANCE_THRESHOLD 0.8f
// 旋转/摆动检测阈值：角速度总模长大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
#define ROTATION_GYR_MAG_THRESHOLD 120.0f
// --- 滑雪转弯动 ---
// 加速度方差阈值，大于此值，滑雪过程可能发生了急转弯
#define WHEEL_ACC_VARIANCE_THRESHOLD 7.0f
// 旋转/摆动检测阈值：角速度总模长大于此值(度/秒)，认为滑雪过程中进行急转弯
#define WHEEL_GYR_MAG_THRESHOLD 220.0f // 150.0f 到 250.0f之间进行调整
// --- 用于消除积分漂移的滤波器和阈值 ---
// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
// alpha = RC / (RC + dt)，参考RC电路而来
#define HPF_ALPHA 0.995
// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
#define ACC_DEAD_ZONE_THRESHOLD 0.05f
// --- 模拟摩擦力，进行速度衰减 ---
#define SPEED_ATTENUATION 1.0f //暂不模拟
#ifdef XTELL_TEST
BLE_KS_send_data_t KS_data;
debug_t debug1;
debug_t debug2;
#endif
static skiing_tracker_t my_skiing_tracker;
//////////////////////////////////////////////////////////////////////////////////////////////////
//实现
void clear_speed(void){
my_skiing_tracker.state = STATIC;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.speed = 0;
}
void start_detection(void){
my_skiing_tracker.state = STATIC;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.distance = 0;
my_skiing_tracker.speed = 0;
}
void stop_detection(void){
my_skiing_tracker.state = STOP_DETECTION;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.speed = 0;
}
/**
* @brief 初始化滑雪追踪器
*
* @param tracker
*/
void skiing_tracker_init(skiing_tracker_t *tracker)
{
if (!tracker) {
return;
}
// 使用memset一次性清零整个结构体，包括新增的缓冲区
memset(tracker, 0, sizeof(skiing_tracker_t));
tracker->state = STATIC;
}
/**
* @brief 将设备坐标系下的加速度转换为世界坐标系
* @param acc_device 设备坐标系下的加速度 [x, y, z]
* @param angle 姿态角 [pitch, roll, yaw]，单位: 度
* @param acc_world 输出：世界坐标系下的加速度 [x, y, z]
*/
static void transform_acc_to_world_frame(const float *acc_device, const float *angle, float *acc_world)
{
// 驱动输出的角度与标准航空定义相反，需要取反才能用于标准旋转矩阵。
float pitch = -angle[0] * DEG_TO_RAD;
float roll = -angle[1] * DEG_TO_RAD;
// TODO: 当引入三轴磁力计后，这里的 yaw 应由磁力计和陀螺仪融合解算得出，以解决航向漂移问题。
// 目前 yaw 暂时不参与计算，因为仅靠加速度计和陀螺仪无法获得准确的绝对航向角。
// float yaw = -angle[2] * DEG_TO_RAD;
float cp = cosf(pitch);
float sp = sinf(pitch);
float cr = cosf(roll);
float sr = sinf(roll);
float ax = acc_device[0];
float ay = acc_device[1];
float az = acc_device[2];
// 使用经过验证的、正确的身体坐标系到世界坐标系的旋转矩阵 (基于 Y-X 旋转顺序)
// 这个矩阵将设备测量的加速度(ax, ay, az)正确地转换到世界坐标系(acc_world)。
// 注意：这里没有使用yaw，主要关心的是坡面上的运动，绝对航向暂时不影响速度和距离的计算。
// TODO
acc_world[0] = cp * ax + sp * sr * ay + sp * cr * az;
acc_world[1] = 0 * ax + cr * ay - sr * az;
acc_world[2] = -sp * ax + cp * sr * ay + cp * cr * az;
}
/**
* @brief 计算缓冲区内三轴数据的方差之和
*
* @param buffer 传进来的三轴数据：陀螺仪/加速度
* @return float 返回方差和
*/
static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
{
float mean[3] = {0};
float variance[3] = {0};
// 计算均值
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
mean[0] += buffer[i][0];
mean[1] += buffer[i][1];
mean[2] += buffer[i][2];
}
mean[0] /= VARIANCE_BUFFER_SIZE;
mean[1] /= VARIANCE_BUFFER_SIZE;
mean[2] /= VARIANCE_BUFFER_SIZE;
// 计算方差
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
variance[0] += (buffer[i][0] - mean[0]) * (buffer[i][0] - mean[0]);
variance[1] += (buffer[i][1] - mean[1]) * (buffer[i][1] - mean[1]);
variance[2] += (buffer[i][2] - mean[2]) * (buffer[i][2] - mean[2]);
}
variance[0] /= VARIANCE_BUFFER_SIZE;
variance[1] /= VARIANCE_BUFFER_SIZE;
variance[2] /= VARIANCE_BUFFER_SIZE;
// 返回三轴方差之和，作为一个综合的稳定度指标
return variance[0] + variance[1] + variance[2];
}
/**
* @brief 状态机更新
*
* @param tracker 传入同步修改后传出
* @param acc_device_ms2 三轴加速度，m/s^2
* @param gyr_dps 三轴陀螺仪，dps
*/
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps)
{
// 缓冲区未填满时，不进行状态判断，默认为静止
if (!tracker->buffer_filled) {
tracker->state = STATIC;
return;
}
// --- 计算关键指标 ---
float acc_variance = calculate_variance(tracker->acc_buffer); // 计算加速度方差
float gyr_variance = calculate_variance(tracker->gyr_buffer); // 计算陀螺仪方差
float gyr_magnitude = sqrtf(gyr_dps[0]*gyr_dps[0] + gyr_dps[1]*gyr_dps[1] + gyr_dps[2]*gyr_dps[2]);
float acc_magnitude = sqrtf(acc_device_ms2[0]*acc_device_ms2[0] + acc_device_ms2[1]*acc_device_ms2[1] + acc_device_ms2[2]*acc_device_ms2[2]);
#ifdef XTELL_TEST
debug1.acc_variance =acc_variance;
debug1.gyr_variance =gyr_variance;
debug1.gyr_magnitude=gyr_magnitude;
debug1.acc_magnitude=fabsf(acc_magnitude - G_ACCELERATION);
#endif
#if 0
//正在滑雪
if(tracker->state == NO_CONSTANT_SPEED ) {
//摔倒了
if(gyr_magnitude > FALLEN_GRY_MAG_THRESHOLD){
tracker->state = FALLEN;
return;
}
//可能进入了匀速状态
if(gyr_magnitude > SKIING_GYR_MAG_THRESHOLD && acc_magnitude < SKIING_ACC_MAG_THRESHOLD){
tracker->state = CONSTANT_SPEED;
return;
}
//急转弯
if(gyr_magnitude > WHEEL_GYR_MAG_THRESHOLD && acc_variance > WHEEL_ACC_VARIANCE_THRESHOLD){
//TODO：可以考虑清掉速度消除积分带来的漂移
tracker->state = WHEEL;
return;
}
}
// --- 状态切换逻辑 (按优先级) ---
// 优先级1：静止
if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
tracker->state = STATIC;
// 速度清零，抑制漂移
memset(tracker->velocity, 0, sizeof(tracker->velocity));
tracker->speed = 0.0f;
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
return;
}
// 优先级2：原地旋转 - 特殊的、非滑雪的运动状态
// 条件：角速度很大，同时线性加速度的晃动在一个“中等”范围内。
if (tracker->state == STATIC && gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && acc_variance < ROTATING_ACC_VARIANCE_THRESHOLD) {
tracker->state = ROTATING;
return;
}
// 优先级3：启动滑雪 - “油门”
// 条件：有足够大的线性加速度，同时陀螺仪的抖动在一个“合理”（而非“完全静止”）的范围内。
if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD && gyr_variance < SKIING_GYR_VARIANCE_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
return;
}
// 如果不满足任何启动或停止条件，则保持当前状态（滑雪中）
// 如果当前是静止或旋转，但没有满足启动条件，则状态会保持，直到满足ZUPT或旋转条件。
#else
// 无论当前是什么状态，静止总是最高优先级
if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
tracker->state = STATIC;
// 速度清零，抑制漂移
memset(tracker->velocity, 0, sizeof(tracker->velocity));
tracker->speed = 0.0f;
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
return;
}
switch (tracker->state) {
case STATIC:
//不break，会往下执行，判断是否进入非匀速状态
case ROTATING: // 从静止或原地旋转可以启动
if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD && gyr_variance < SKIING_GYR_VARIANCE_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
} else if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && acc_variance < ROTATING_ACC_VARIANCE_THRESHOLD) {
tracker->state = ROTATING;
}
break;
case NO_CONSTANT_SPEED: //非匀速状态
if (gyr_magnitude > FALLEN_GRY_MAG_THRESHOLD) {
tracker->state = FALLEN; //摔倒
} else if (gyr_magnitude > WHEEL_GYR_MAG_THRESHOLD && acc_variance > WHEEL_ACC_VARIANCE_THRESHOLD) {
tracker->state = WHEEL; //转弯
} else if (fabsf(acc_magnitude - G_ACCELERATION) < SKIING_ACC_MAG_THRESHOLD) {
tracker->state = CONSTANT_SPEED; //匀速
}
break;
case CONSTANT_SPEED: //匀速状态
if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
}
//TODO：可以添加进入转弯或摔倒的判断
break;
case WHEEL:
// 从转弯状态，检查转弯是否结束
// 如果角速度和加速度方差都降下来了，就回到普通滑行状态
if (gyr_magnitude < WHEEL_GYR_MAG_THRESHOLD * 0.8f && acc_variance < WHEEL_ACC_VARIANCE_THRESHOLD * 0.8f) { // 乘以一个滞后系数避免抖动
tracker->state = NO_CONSTANT_SPEED;
}
break;
case FALLEN:
// TODO：回到 STATIC
break;
}
#endif
}
/**
* @brief 主更新函数
*
* @param tracker
* @param acc_g 三轴加速度，g
* @param gyr_dps 三轴陀螺仪，dps
* @param angle 欧若拉角
* @param dt 采样时间间隔，会用来积分求速度
*/
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt)
{
if (!tracker || !acc_g || !gyr_dps || !angle || dt <= 0) {
return;
}
if(my_skiing_tracker.state == STOP_DETECTION)
return;
// --- 数据预处理和缓冲 ---
float acc_device_ms2[3];
acc_device_ms2[0] = acc_g[0] * G_ACCELERATION;
acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
acc_device_ms2[2] = acc_g[2] * G_ACCELERATION;
// 将最新数据存入缓冲区
memcpy(tracker->acc_buffer[tracker->buffer_index], acc_device_ms2, sizeof(acc_device_ms2));
memcpy(tracker->gyr_buffer[tracker->buffer_index], gyr_dps, 3 * sizeof(float));
tracker->buffer_index++;
if (tracker->buffer_index >= VARIANCE_BUFFER_SIZE) {
tracker->buffer_index = 0;
tracker->buffer_filled = 1; // 标记缓冲区已满
}
// --- 更新状态机 ---
update_state_machine(tracker, acc_device_ms2, gyr_dps);
// 坐标转换 & 移除重力
transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world);
tracker->acc_world[2] -= G_ACCELERATION;
// 对世界坐标系下的加速度进行高通滤波，消除直流偏置和重力残差
for (int i = 0; i < 3; i++) {
tracker->acc_world_filtered[i] = HPF_ALPHA * (tracker->acc_world_filtered[i] + tracker->acc_world[i] - tracker->acc_world_unfiltered_prev[i]);
tracker->acc_world_unfiltered_prev[i] = tracker->acc_world[i];
}
// 应用加速度死区，忽略微小抖动和噪声
float acc_horizontal_mag = sqrtf(tracker->acc_world_filtered[0] * tracker->acc_world_filtered[0] +
tracker->acc_world_filtered[1] * tracker->acc_world_filtered[1]);
// --- 根据状态进行计算 ---
if (tracker->state == NO_CONSTANT_SPEED) {
if (acc_horizontal_mag > ACC_DEAD_ZONE_THRESHOLD) {
// 只有当水平加速度足够大时，才进行速度积分
tracker->velocity[0] += tracker->acc_world_filtered[0] * dt;
tracker->velocity[1] += tracker->acc_world_filtered[1] * dt;
// 垂直方向的速度暂时不积分，极易受姿态误差影响而漂移
// tracker->velocity[2] += tracker->acc_world_filtered[2] * dt;
}
#ifdef XTELL_TEST
debug2.acc_magnitude = acc_horizontal_mag;
#endif
// 如果加速度小于阈值，则不更新速度，相当于速度保持不变（或受下一步的阻尼影响而衰减）
// --- 更新速率和距离 ---
// 只基于水平速度计算速率和距离
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
tracker->velocity[1] * tracker->velocity[1]);
tracker->distance += tracker->speed * dt;
}else if(tracker->state == CONSTANT_SPEED){ //匀速
#ifdef XTELL_TEST
debug2.acc_magnitude = acc_horizontal_mag;
#endif
//保持上次的速度不变。只更新距离
tracker->distance += tracker->speed * dt;
}else{
// 在静止或旋转状态下，速度已经在状态机内部被清零
// 额外增加速度衰减，模拟摩擦力，进一步抑制漂移
tracker->velocity[0] *= SPEED_ATTENUATION;
tracker->velocity[1] *= SPEED_ATTENUATION;
tracker->velocity[2] = 0; // 垂直速度强制归零
}
}
/**
* @brief 传感器数据采集与处理任务，外部每10ms调用一次，如果需要更新时间间隔，也需要同步更新宏“ DELTA_TIME ”
*
* @param acc_data_buf 三轴加速度原始数据
* @param gyr_data_buf 三轴陀螺仪原始数据
* @return BLE_send_data_t
*/
BLE_send_data_t sensor_processing_task(signed short * acc_data_buf, signed short * gyr_data_buf) {
static int initialized = 0;
static int calibration_done = 0;
static signed short combined_raw_data[6];
static float final_angle_data[3]; // 计算得到的欧若拉角
static float calibrated_acc_g[3]; // 转换后的加速度计数据
static float calibrated_gyr_dps[3]; // 转换后的陀螺仪数据
const float delta_time = DELTA_TIME + 0.03f;
BLE_send_data_t BLE_send_data;
if (!initialized) {
skiing_tracker_init(&my_skiing_tracker);
initialized = 1;
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
}
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
unsigned char status;
if (!calibration_done) { //第1次启动，开启零漂检测
status = SL_SC7U22_Angle_Output(1, combined_raw_data, final_angle_data, 0);
if (status == 1) {
calibration_done = 1;
printf("Sensor calibration successful! Skiing mode is active.\n");
}
} else {
// printf("Calculate the time interval =============== start\n");
status = SL_SC7U22_Angle_Output(0, combined_raw_data, final_angle_data, 0);
}
if (status == 1) {
// 加速度 LSB to g
calibrated_acc_g[0] = (float)combined_raw_data[0] / 8192.0f;
calibrated_acc_g[1] = (float)combined_raw_data[1] / 8192.0f;
calibrated_acc_g[2] = (float)combined_raw_data[2] / 8192.0f;
// 陀螺仪 LSB to dps (度/秒)
// ±2000dps量程下，转换系数约为 0.061
calibrated_gyr_dps[0] = (float)combined_raw_data[3] * 0.061f;
calibrated_gyr_dps[1] = (float)combined_raw_data[4] * 0.061f;
calibrated_gyr_dps[2] = (float)combined_raw_data[5] * 0.061f;
skiing_tracker_update(&my_skiing_tracker, calibrated_acc_g, calibrated_gyr_dps, final_angle_data, delta_time);
// static int count = 0;
// if(count >= 10){
// printf("State: %d, Speed: %.2f m/s, Distance: %.2f m\n",
// my_skiing_tracker.state,
// my_skiing_tracker.speed,
// my_skiing_tracker.distance);
// printf("calibrated_acc_g: %.2f, %.2f, %.2f\n",
// calibrated_acc_g[0],
// calibrated_acc_g[1],
// calibrated_acc_g[2]);
// count = 0;
// } else {
// count++;
// }
BLE_send_data.sensor_state = status;
BLE_send_data.skiing_state = my_skiing_tracker.state;
for (int i = 0; i < 3; i++) {
#ifndef XTELL_TEST
BLE_send_data.acc_original[i] = (int)acc_data_buf[i];
BLE_send_data.gyr_original[i] = (int)gyr_data_buf[i];
#endif
#if KS_BLE
KS_data.acc_KS[i] = (int)(calibrated_acc_g[i] * G_ACCELERATION * 100); //cm/s^s
KS_data.gyr_KS_dps[i] = (int)calibrated_gyr_dps[i];
KS_data.angle_KS[i] = (int)final_angle_data[i];
#endif
}
BLE_send_data.speed_cms = (int)(my_skiing_tracker.speed * 100);
BLE_send_data.distance_cm = (int)(my_skiing_tracker.distance * 100);
// printf("Calculate the time interval =============== end\n");
} else if (status == 0) {
memset(&BLE_send_data, 0, sizeof(BLE_send_data_t));
BLE_send_data.sensor_state = status;
#if KS_BLE
memset(&KS_data, 0, sizeof(BLE_send_data_t));
#endif
// printf("Sensor is calibrating...\n");
} else {
memset(&BLE_send_data, 0, sizeof(BLE_send_data_t));
BLE_send_data.sensor_state = status;
#if KS_BLE
memset(&KS_data, 0, sizeof(BLE_send_data_t));
#endif
// printf("Angle calculation error or calibration not finished.\n");
}
return BLE_send_data;
}

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

@ -1,85 +0,0 @@
#ifndef SKIING_TRACKER_H
#define SKIING_TRACKER_H
#include "../xtell.h"
// 定义滑雪者可能的状态
typedef enum {
STATIC, // 静止或动态稳定
NO_CONSTANT_SPEED, // 正在滑雪，非匀速
CONSTANT_SPEED, // 正在滑雪，匀速
ROTATING, // 正在原地旋转
WHEEL, // 转弯
FALLEN, // 已摔倒
STOP_DETECTION, // 停止检测
UNKNOWN // 未知状态
} skiing_state_t;
#define VARIANCE_BUFFER_SIZE 5 // 用于计算方差的数据窗口大小 (5个样本 @ 100Hz = 50ms)，减小延迟，提高实时性
#define DELTA_TIME 0.01f
// 追踪器数据结构体
typedef struct {
// 公开数据
float velocity[3]; // 当前速度 (x, y, z)，单位: m/s
float distance; // 总滑行距离，单位: m
float speed; // 当前速率 (标量)，单位: m/s
skiing_state_t state; // 当前滑雪状态
// 内部计算使用的私有成员
float acc_world[3]; // 在世界坐标系下的加速度
// --- 内部计算使用的私有成员 ---
// 用于动态零速更新和旋转检测的缓冲区
float acc_buffer[VARIANCE_BUFFER_SIZE][3]; // 加速度数据窗口
float gyr_buffer[VARIANCE_BUFFER_SIZE][3]; // 角速度数据窗口
int buffer_index; // 缓冲区当前索引
int buffer_filled; // 缓冲区是否已填满的标志
// 用于高通滤波器（巴特沃斯一阶滤波器）的私有成员，以消除加速度的直流偏置
float acc_world_filtered[3]; //过滤过的
float acc_world_unfiltered_prev[3]; //上一次没过滤的
} skiing_tracker_t;
//ble发送的数据
typedef struct __attribute__((packed)){ //该结构体取消内存对齐
char sensor_state;
char skiing_state;
int speed_cms; //求出的速度，cm/s
int distance_cm; //求出的距离，cm
#ifndef XTELL_TEST
int acc_original[3]; //直接读取传感器得到的原始三轴加速度
int gyr_original[3]; //直接读取传感器得到的原始三轴陀螺仪
#endif
}BLE_send_data_t;
typedef struct{
int acc_KS[3]; //卡尔曼后，LSB转换后的 三轴加速度数据（cm/s^2）
int gyr_KS_dps[3]; //卡尔曼后，LSB to dps 三轴陀螺仪数据
int angle_KS[3]; //卡尔曼后，计算得到的欧若拉角数据
}BLE_KS_send_data_t;
#ifdef XTELL_TEST
typedef struct{
float acc_variance; //三轴加速度方差之和
float gyr_variance; //三轴陀螺仪方差之和
float acc_magnitude; //三轴加速度模长
float gyr_magnitude; //三轴陀螺仪模长
}debug_t;
#endif
/**
* @brief 初始化滑雪追踪器
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
*/
void skiing_tracker_init(skiing_tracker_t *tracker);
/**
* @brief 传感器数据采集与处理任务，外部每10ms调用一次，如果需要更新时间间隔，也需要同步更新宏“ DELTA_TIME ”
*
* @param acc_data_buf 三轴加速度原始数据
* @param gyr_data_buf 三轴陀螺仪原始数据
* @return BLE_send_data_t
*/
BLE_send_data_t sensor_processing_task(signed short * acc_data_buf, signed short * gyr_data_buf) ;
#endif // SKIING_TRACKER_H

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

@ -1,583 +0,0 @@
/*
*/
#include "skiing_tracker.h"
#include "../sensor/SC7U22.h"
#include <math.h>
#include <string.h>
#define G_ACCELERATION 9.81f
#define DEG_TO_RAD (3.14159265f / 180.0f)
// --- 静止检测 ---
//两个判断是否静止的必要条件：动态零速更新(ZUPT)阈值
// 加速方差阈值，提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
#define STOP_ACC_VARIANCE_THRESHOLD 0.2f
// 陀螺仪方差阈值
#define STOP_GYR_VARIANCE_THRESHOLD 5.0f
// 静止时候的陀螺仪模长
#define STOP_GYR_MAG_THRESHOLD 15
// --- --- ---
// --- 启动滑雪阈值 ---
// 加速度模长与重力的差值大于此值，认为开始运动；降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
#define START_ACC_MAG_THRESHOLD 1.0f //0.5、1
// 陀螺仪方差阈值，以允许启动瞬间的正常抖动，但仍能过滤掉混乱的、非滑雪的晃动。
#define START_GYR_VARIANCE_THRESHOLD 15.0f
// --- --- ---
// --- 滑雪过程 ---
//加速度 模长(不含重力），低于此值视为 在做匀速运动
#define SKIING_ACC_MAG_THRESHOLD 0.5f
//陀螺仪 模长，高于此值视为 摔倒了
#define FALLEN_GRY_MAG_THRESHOLD 2000.0f //未确定
// --- --- ---
// --- 原地旋转抖动 ---
// 加速度 方差 阈值。此值比 静止检测 阈值更宽松，
#define WOBBLE_ACC_VARIANCE_THRESHOLD 0.5f
// 加速度 模长 阈值
#define WOBBLE_ACC_MAG_THRESHOLD 1.0f
// 角速度 总模长 大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
#define ROTATION_GYR_MAG_THRESHOLD 30.0f
// --- --- ---
// --- 滑雪转弯动 ---
// 加速度 方差 阈值，大于此值，滑雪过程可能发生了急转弯
#define WHEEL_ACC_VARIANCE_THRESHOLD 7.0f
// 角速度 总模长 大于此值(度/秒)，认为滑雪过程中进行急转弯
#define WHEEL_GYR_MAG_THRESHOLD 500.0f //
// --- --- ---
// --- 跳跃 ---
// 加速度模长低于此值(g)，认为进入失重状态(IN_AIR)
#define AIRBORNE_ACC_MAG_LOW_THRESHOLD 0.4f
// 加速度模长高于此值(g)，认为发生落地冲击(LANDING)
#define LANDING_ACC_MAG_HIGH_THRESHOLD 3.5f
// 起跳加速度阈值(g)，用于进入TAKING_OFF状态
#define TAKEOFF_ACC_MAG_HIGH_THRESHOLD 1.8f
// 进入空中状态确认计数：需要连续3个采样点加速度低于阈值才判断为起跳
#define AIRBORNE_CONFIRM_COUNT 3
// 落地状态确认计数：加速度恢复到1g附近并持续2个采样点(20ms)则认为已落地
#define GROUNDED_CONFIRM_COUNT 2
// 最大滞空时间(秒)，超过此时间强制认为已落地，防止状态锁死
#define MAX_TIME_IN_AIR 12.5f
// --- --- ---
// --- 用于消除积分漂移的滤波器和阈值 ---
// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
// alpha = RC / (RC + dt)，参考RC电路而来，fc ≈ (1 - alpha) / (2 * π * dt)
#define HPF_ALPHA 0.999f
//0.995f： 0.08 Hz 的信号
//0.999f： 0.0159 Hz
// --- --- ---
// --- 低通滤波器 ---
// 低通滤波器系数 (alpha)。alpha 越小，滤波效果越强（更平滑），但延迟越大。
// alpha 推荐范围 0.7 ~ 0.95。可以从 0.85 开始尝试。
#define LPF_ALPHA 0.7f
// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
//参考：0.2f ~ 0.4f
#define ACC_DEAD_ZONE_THRESHOLD 0.05f
// --- 模拟摩擦力，进行速度衰减 ---
#define SPEED_ATTENUATION 1.0f //暂不模拟
BLE_KS_send_data_t KS_data;
#ifdef XTELL_TEST
debug_t debug1;
debug_t debug2;
#endif
static skiing_tracker_t my_skiing_tracker;
//////////////////////////////////////////////////////////////////////////////////////////////////
//实现
void clear_speed(void){
my_skiing_tracker.state = STATIC;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.speed = 0;
}
void start_detection(void){
my_skiing_tracker.state = STATIC;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.distance = 0;
my_skiing_tracker.speed = 0;
}
void stop_detection(void){
my_skiing_tracker.state = STOP_DETECTION;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.speed = 0;
}
/**
* @brief 初始化滑雪追踪器
*
* @param tracker
*/
void skiing_tracker_init(skiing_tracker_t *tracker)
{
if (!tracker) {
return;
}
// 使用memset一次性清零整个结构体，包括新增的缓冲区
memset(tracker, 0, sizeof(skiing_tracker_t));
tracker->state = STATIC;
}
/**
* @brief 当检测到落地时，计算空中的水平飞行距离并累加到总距离
*/
static void calculate_air_distance(skiing_tracker_t *tracker) {
float horizontal_speed_on_takeoff = sqrtf(
tracker->initial_velocity_on_takeoff[0] * tracker->initial_velocity_on_takeoff[0] +
tracker->initial_velocity_on_takeoff[1] * tracker->initial_velocity_on_takeoff[1]
);
float distance_in_air = horizontal_speed_on_takeoff * tracker->time_in_air;
tracker->distance += distance_in_air;
}
/**
* @brief 将设备坐标系下的加速度转换为世界坐标系
* @param acc_device 设备坐标系下的加速度 [x, y, z]
* @param angle 姿态角 [pitch, roll, yaw]，单位: 度
* @param acc_world 输出：世界坐标系下的加速度 [x, y, z]
*/
static void transform_acc_to_world_frame(const float *acc_device, const float *angle, float *acc_world)
{
// 驱动输出的角度与标准航空定义相反，需要取反才能用于标准旋转矩阵。
float pitch = -angle[0] * DEG_TO_RAD;
float roll = -angle[1] * DEG_TO_RAD;
// TODO: 当引入三轴磁力计后，这里的 yaw 应由磁力计和陀螺仪融合解算得出，以解决航向漂移问题。
// 目前 yaw 暂时不参与计算，因为仅靠加速度计和陀螺仪无法获得准确的绝对航向角。
// float yaw = -angle[2] * DEG_TO_RAD;
float cp = cosf(pitch);
float sp = sinf(pitch);
float cr = cosf(roll);
float sr = sinf(roll);
float ax = acc_device[0];
float ay = acc_device[1];
float az = acc_device[2];
// 使用经过验证的、正确的身体坐标系到世界坐标系的旋转矩阵 (基于 Y-X 旋转顺序)
// 这个矩阵将设备测量的加速度(ax, ay, az)正确地转换到世界坐标系(acc_world)。
// 注意：这里没有使用yaw，主要关心的是坡面上的运动，绝对航向暂时不影响速度和距离的计算。
// TODO
acc_world[0] = cp * ax + sp * sr * ay + sp * cr * az;
acc_world[1] = 0 * ax + cr * ay - sr * az;
acc_world[2] = -sp * ax + cp * sr * ay + cp * cr * az;
}
/**
* @brief 计算缓冲区内三轴数据的方差之和
*
* @param buffer 传进来的三轴数据：陀螺仪/加速度
* @return float 返回方差和
*/
static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
{
float mean[3] = {0};
float variance[3] = {0};
// 计算均值
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
mean[0] += buffer[i][0];
mean[1] += buffer[i][1];
mean[2] += buffer[i][2];
}
mean[0] /= VARIANCE_BUFFER_SIZE;
mean[1] /= VARIANCE_BUFFER_SIZE;
mean[2] /= VARIANCE_BUFFER_SIZE;
// 计算方差
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
variance[0] += (buffer[i][0] - mean[0]) * (buffer[i][0] - mean[0]);
variance[1] += (buffer[i][1] - mean[1]) * (buffer[i][1] - mean[1]);
variance[2] += (buffer[i][2] - mean[2]) * (buffer[i][2] - mean[2]);
}
variance[0] /= VARIANCE_BUFFER_SIZE;
variance[1] /= VARIANCE_BUFFER_SIZE;
variance[2] /= VARIANCE_BUFFER_SIZE;
// 返回三轴方差之和，作为一个综合的稳定度指标
return variance[0] + variance[1] + variance[2];
}
/**
* @brief 摩擦力模拟，进行速度衰减
*
* @param tracker
*/
void forece_of_friction(skiing_tracker_t *tracker){
// 增加速度衰减，模拟摩擦力
tracker->velocity[0] *= SPEED_ATTENUATION;
tracker->velocity[1] *= SPEED_ATTENUATION;
tracker->velocity[2] = 0; // 垂直速度强制归零
}
/**
* @brief 状态机更新
*
* @param tracker 传入同步修改后传出
* @param acc_device_ms2 三轴加速度，m/s^2
* @param gyr_dps 三轴陀螺仪，dps
*/
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps)
{
// 缓冲区未填满时，不进行状态判断，默认为静止
if (!tracker->buffer_filled) {
tracker->state = STATIC;
return;
}
// --- 计算关键指标 ---
float acc_variance = calculate_variance(tracker->acc_buffer); // 计算加速度方差
float gyr_variance = calculate_variance(tracker->gyr_buffer); // 计算陀螺仪方差
float gyr_magnitude = sqrtf(gyr_dps[0]*gyr_dps[0] + gyr_dps[1]*gyr_dps[1] + gyr_dps[2]*gyr_dps[2]); //dps
float acc_magnitude = sqrtf(acc_device_ms2[0]*acc_device_ms2[0] + acc_device_ms2[1]*acc_device_ms2[1] + acc_device_ms2[2]*acc_device_ms2[2]); //m/s^s
float acc_magnitude_g = acc_magnitude / G_ACCELERATION; // 转换为g单位，用于跳跃判断
#ifdef XTELL_TEST
debug1.acc_variance =acc_variance;
debug1.gyr_variance =gyr_variance;
debug1.gyr_magnitude=gyr_magnitude;
debug1.acc_magnitude=fabsf(acc_magnitude - G_ACCELERATION);
#endif
// --- 状态机逻辑 (核心修改区域) ---
#if 0 //暂时不考虑空中
// 1. 空中/落地状态的后续处理
if (tracker->state == IN_AIR) {
// A. 检测巨大冲击 -> 落地
if (acc_magnitude_g > LANDING_ACC_MAG_HIGH_THRESHOLD) {
tracker->state = LANDING;
// B. 检测超时 -> 强制落地 (安全机制)
} else if (tracker->time_in_air > MAX_TIME_IN_AIR) {
tracker->state = LANDING;
// C. 检测恢复正常重力 (平缓落地)
} else if (acc_magnitude_g > 0.8f && acc_magnitude_g < 1.5f) {
tracker->grounded_entry_counter++;
if (tracker->grounded_entry_counter >= GROUNDED_CONFIRM_COUNT) {
tracker->state = LANDING;
}
} else {
tracker->grounded_entry_counter = 0;
}
return; // 在空中或刚切换到落地，结束本次状态判断
}
// 2. 严格的 "起跳->空中" 状态转换逻辑
// 只有当处于滑行状态时，才去检测起跳意图
if (tracker->state == NO_CONSTANT_SPEED || tracker->state == CONSTANT_SPEED || tracker->state == WHEEL) {
if (acc_magnitude_g > TAKEOFF_ACC_MAG_HIGH_THRESHOLD) {
tracker->state = TAKING_OFF;
tracker->airborne_entry_counter = 0; // 准备检测失重
return;
}
}
// 只有在TAKING_OFF状态下，才去检测是否进入失重
if (tracker->state == TAKING_OFF) {
if (acc_magnitude_g < AIRBORNE_ACC_MAG_LOW_THRESHOLD) {
tracker->airborne_entry_counter++;
if (tracker->airborne_entry_counter >= AIRBORNE_CONFIRM_COUNT) {
memcpy(tracker->initial_velocity_on_takeoff, tracker->velocity, sizeof(tracker->velocity));
tracker->time_in_air = 0;
tracker->state = IN_AIR;
tracker->airborne_entry_counter = 0;
tracker->grounded_entry_counter = 0;
return;
}
} else {
// 如果在起跳冲击后一段时间内没有失重，说明只是一个颠簸，恢复滑行
// 可以加一个小的超时计数器，这里为了简单先直接恢复
tracker->state = NO_CONSTANT_SPEED;
}
return; // 无论是否切换，都结束本次判断
}
#endif
// --- 静止判断 ---
if (acc_variance < STOP_ACC_VARIANCE_THRESHOLD && gyr_variance < STOP_GYR_VARIANCE_THRESHOLD && gyr_magnitude < STOP_GYR_MAG_THRESHOLD) {
tracker->state = STATIC;
return;
}
// --- 地面状态切换逻辑 ---
switch (tracker->state) {
case LANDING:
tracker->state = STATIC;
break;
case STATIC:
// 优先判断是否进入 WOBBLE 状态
// 条件：陀螺仪活动剧烈，但整体加速度变化不大（说明是原地转或晃）
if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) < WOBBLE_ACC_MAG_THRESHOLD) {
tracker->state = WOBBLE;
}
// 只有在陀螺仪和加速度都满足“前进”特征时，才启动
else if (gyr_variance > START_GYR_VARIANCE_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
}
break;
case WOBBLE:
// 从 WOBBLE 状态启动的条件应该和从 STATIC 启动一样严格
if (gyr_variance > START_GYR_VARIANCE_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
}
// 如果陀螺仪活动减弱，则可能恢复静止
else if (gyr_magnitude < ROTATION_GYR_MAG_THRESHOLD * 0.8f) { // 增加迟滞，避免抖动
// 不直接跳回STATIC，而是依赖下一轮的全局静止判断
}
break;
case NO_CONSTANT_SPEED: //非匀速状态
//暂时不考虑摔倒
// if (gyr_magnitude > FALLEN_GRY_MAG_THRESHOLD) {
// tracker->state = FALLEN; //摔倒
// } else
if (gyr_magnitude > WHEEL_GYR_MAG_THRESHOLD && acc_variance > WHEEL_ACC_VARIANCE_THRESHOLD) {
tracker->state = WHEEL; //转弯
} else if (fabsf(acc_magnitude - G_ACCELERATION) < SKIING_ACC_MAG_THRESHOLD) {
tracker->state = CONSTANT_SPEED; //匀速
}
break;
case CONSTANT_SPEED: //匀速状态
if (fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
}
//TODO：可以添加进入转弯或摔倒的判断
break;
case WHEEL:
// 从转弯状态，检查转弯是否结束
// 如果角速度和加速度方差都降下来了，就回到普通滑行状态
if (gyr_magnitude < WHEEL_GYR_MAG_THRESHOLD * 0.8f && acc_variance < WHEEL_ACC_VARIANCE_THRESHOLD * 0.8f) { // 乘以一个滞后系数避免抖动
tracker->state = NO_CONSTANT_SPEED;
}
break;
case FALLEN:
// TODO：回到 STATIC
break;
}
}
/**
* @brief 主更新函数
*
* @param tracker
* @param acc_g 三轴加速度，g
* @param gyr_dps 三轴陀螺仪，dps
* @param angle 欧若拉角
* @param dt 采样时间间隔，会用来积分求速度
*/
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt)
{
if (!tracker || !acc_g || !gyr_dps || !angle || dt <= 0) {
return;
}
if(my_skiing_tracker.state == STOP_DETECTION)
return;
// --- 数据预处理和缓冲 ---
float acc_device_ms2[3];
acc_device_ms2[0] = acc_g[0] * G_ACCELERATION;
acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
acc_device_ms2[2] = acc_g[2] * G_ACCELERATION;
// 将最新数据存入缓冲区
memcpy(tracker->acc_buffer[tracker->buffer_index], acc_device_ms2, sizeof(acc_device_ms2));
memcpy(tracker->gyr_buffer[tracker->buffer_index], gyr_dps, 3 * sizeof(float));
tracker->buffer_index++;
if (tracker->buffer_index >= VARIANCE_BUFFER_SIZE) {
tracker->buffer_index = 0;
tracker->buffer_filled = 1; // 标记缓冲区已满
}
// --- 更新状态机 ---
update_state_machine(tracker, acc_device_ms2, gyr_dps);
// --- 根据状态执行不同的计算逻辑 ---
switch (tracker->state) {
case TAKING_OFF:
tracker->speed = 0.0f;
break;
case IN_AIR:
// 在空中时，只累加滞空时间
tracker->time_in_air += dt;
break;
case LANDING:
// 刚落地，计算空中距离
calculate_air_distance(tracker);
// 清理速度和滤波器状态，为恢复地面追踪做准备
memset(tracker->velocity, 0, sizeof(tracker->velocity));
tracker->speed = 0;
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态
break;
case WHEEL:
case NO_CONSTANT_SPEED:
transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world);
tracker->acc_world[2] -= G_ACCELERATION;
float acc_world_temp[3]; // 临时变量存储当前周期的加速度
for (int i = 0; i < 2; i++) { // 只处理水平方向的 x 和 y 轴
// --- 核心修改：颠倒滤波器顺序为 HPF -> LPF ---
// 1. 高通滤波 (HPF) 先行: 消除因姿态误差导致的重力泄漏（直流偏置）
// HPF的瞬态响应会产生尖峰，这是正常的。
tracker->acc_world_filtered[i] = HPF_ALPHA * (tracker->acc_world_filtered[i] + tracker->acc_world[i] - tracker->acc_world_unfiltered_prev[i]);
tracker->acc_world_unfiltered_prev[i] = tracker->acc_world[i];
// 2. 低通滤波 (LPF) 殿后: 平滑掉HPF产生的尖峰和传感器自身的高频振动噪声。
// 这里使用 tracker->acc_world_filtered[i] 作为LPF的输入。
tracker->acc_world_lpf[i] = (1.0f - LPF_ALPHA) * tracker->acc_world_filtered[i] + LPF_ALPHA * tracker->acc_world_lpf[i];
// 将最终处理完的加速度值存入临时变量
acc_world_temp[i] = tracker->acc_world_lpf[i];
}
// 计算处理后加速度的水平模长
float acc_horizontal_mag = sqrtf(acc_world_temp[0] * acc_world_temp[0] +
acc_world_temp[1] * acc_world_temp[1]);
#if XTELL_TEST
debug2.acc_magnitude = acc_horizontal_mag;
#endif
// 应用死区，并积分
if (acc_horizontal_mag > ACC_DEAD_ZONE_THRESHOLD) {
tracker->velocity[0] += acc_world_temp[0] * dt;
tracker->velocity[1] += acc_world_temp[1] * dt;
}
// 更新速度和距离
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
tracker->velocity[1] * tracker->velocity[1]);
tracker->distance += tracker->speed * dt;
break;
case CONSTANT_SPEED:
//保持上次的速度不变。只更新距离
tracker->distance += tracker->speed * dt;
break;
case STATIC:
case WOBBLE:
// 速度清零，抑制漂移
memset(tracker->velocity, 0, sizeof(tracker->velocity));
tracker->speed = 0.0f;
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态
#if XTELL_TEST
debug2.acc_magnitude = 0;
#endif
break;
case FALLEN:
// TODO
break;
default:
break;
}
}
/**
* @brief 滑雪数据计算
*
* @param acc_data_buf 传入的三轴加速度数据
* @param gyr_data_buf 传入的三轴陀螺仪数据
* @param angle_data 传入的欧若拉角数据
* @return BLE_send_data_t 要发送给蓝牙的数据
*/
BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data) {
static int initialized = 0;
static float acc_data_g[3];
static float gyr_data_dps[3];
// const float delta_time = DELTA_TIME+0.01f;
// const float delta_time = DELTA_TIME + 0.005f;
const float delta_time = DELTA_TIME;
BLE_send_data_t BLE_send_data;
if (!initialized) {
skiing_tracker_init(&my_skiing_tracker);
initialized = 1;
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
}
#if ACC_RANGE==2
// 加速度 LSB to g
acc_data_g[0] = (float)acc_data_buf[0] / 16384.0f;
acc_data_g[1] = (float)acc_data_buf[1] / 16384.0f;
acc_data_g[2] = (float)acc_data_buf[2] / 16384.0f;
#endif
#if ACC_RANGE==4
// 加速度 LSB to g
acc_data_g[0] = (float)acc_data_buf[0] / 8192.0f;
acc_data_g[1] = (float)acc_data_buf[1] / 8192.0f;
acc_data_g[2] = (float)acc_data_buf[2] / 8192.0f;
#endif
#if ACC_RANGE==8
//±8g 4096
acc_data_g[0] = (float)acc_data_buf[0] / 4096.0f; //ax
acc_data_g[1] = (float)acc_data_buf[1] / 4096.0f; //ay
acc_data_g[2] = (float)acc_data_buf[2] / 4096.0f; //az
#endif
#if ACC_RANGE==16
//±16g 2048
acc_data_g[0] = (float)acc_data_buf[0] / 2048.0f; //ax
acc_data_g[1] = (float)acc_data_buf[1] / 2048.0f; //ay
acc_data_g[2] = (float)acc_data_buf[2] / 2048.0f; //az
#endif
// 陀螺仪 LSB to dps (度/秒)
// ±2000dps量程下，转换系数约为 0.061
gyr_data_dps[0] = (float)gyr_data_buf[0] * 0.061f;
gyr_data_dps[1] = (float)gyr_data_buf[1] * 0.061f;
gyr_data_dps[2] = (float)gyr_data_buf[2] * 0.061f;
skiing_tracker_update(&my_skiing_tracker, acc_data_g, gyr_data_dps, angle_data, delta_time);
BLE_send_data.skiing_state = my_skiing_tracker.state;
for (int i = 0; i < 3; i++) {
#ifdef XTELL_TEST
BLE_send_data.acc_data[i] = (short)(acc_data_g[i] * 9.8f) * 100; //cm/^s2
BLE_send_data.gyr_data[i] = (short)gyr_data_dps[i]; //dps
BLE_send_data.angle_data[i] = angle_data[i];
#else
BLE_send_data.acc_data[i] = (short)acc_data_buf[i]; //原始adc数据
BLE_send_data.gyr_data[i] = (short)gyr_data_buf[i]; //原始adc数据
BLE_send_data.angle_data[i] = angle_data[i];
#endif
}
BLE_send_data.speed_cms = (int)(my_skiing_tracker.speed * 100);
BLE_send_data.distance_cm = (int)(my_skiing_tracker.distance * 100);
// printf("Calculate the time interval =============== end\n");
return BLE_send_data;
}

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

@ -1,88 +0,0 @@
#ifndef SKIING_TRACKER_H
#define SKIING_TRACKER_H
#include "../xtell.h"
// 定义滑雪者可能的状态
typedef enum {
STATIC, // 静止或动态稳定：0
NO_CONSTANT_SPEED, // 正在滑雪，非匀速：1
CONSTANT_SPEED, // 正在滑雪，匀速：2
WOBBLE, // 正在原地旋转：3
WHEEL, // 转弯：4
FALLEN, // 已摔倒：5
TAKING_OFF, // 起跳冲击阶段：6
IN_AIR, // 空中失重阶段：7
LANDING, // 落地冲击阶段：8
STOP_DETECTION, // 停止检测：9
UNKNOWN // 未知状态：10
} skiing_state_t;
#define VARIANCE_BUFFER_SIZE 5 // 用于计算方差的数据窗口大小 (5个样本 @ 100Hz = 50ms)，减小延迟，提高实时性
#define DELTA_TIME 0.01f
// 追踪器数据结构体
typedef struct {
// 公开数据
float velocity[3]; // 当前速度 (x, y, z)，单位: m/s
float distance; // 总滑行距离，单位: m
float speed; // 当前速率 (标量)，单位: m/s
skiing_state_t state; // 当前滑雪状态
// 内部计算使用的私有成员
float acc_world[3]; // 在世界坐标系下的加速度
// 用于空中距离计算
float time_in_air; // 滞空时间计时器
float initial_velocity_on_takeoff[3]; // 起跳瞬间的速度向量
int airborne_entry_counter; // 进入空中状态的确认计数器
int grounded_entry_counter; // 落地确认计数器
// --- 内部计算使用的私有成员 ---
// 用于动态零速更新和旋转检测的缓冲区
float acc_buffer[VARIANCE_BUFFER_SIZE][3]; // 加速度数据窗口
float gyr_buffer[VARIANCE_BUFFER_SIZE][3]; // 角速度数据窗口
int buffer_index; // 缓冲区当前索引
int buffer_filled; // 缓冲区是否已填满的标志
// 用于高通滤波器（巴特沃斯一阶滤波器）的私有成员，以消除加速度的直流偏置
float acc_world_filtered[3]; //过滤过的
float acc_world_unfiltered_prev[3]; //上一次没过滤的
float acc_world_lpf[3]; // 经过低通滤波后的世界坐标系加速度
} skiing_tracker_t;
//ble发送的数据
typedef struct{ //__attribute__((packed)){ //该结构体取消内存对齐
char sensor_state;
char skiing_state;
int speed_cms; //求出的速度，cm/s
int distance_cm; //求出的距离，cm
short acc_data[3]; //三轴加速度, g
short gyr_data[3]; //三轴陀螺仪, dps
float angle_data[3]; //欧若拉角
}BLE_send_data_t;
typedef struct{
int acc_KS[3]; //卡尔曼后，LSB转换后的 三轴加速度数据（cm/s^2）
int gyr_KS_dps[3]; //卡尔曼后，LSB to dps 三轴陀螺仪数据
int angle_KS[3]; //卡尔曼后，计算得到的欧若拉角数据
}BLE_KS_send_data_t;
#ifdef XTELL_TEST
typedef struct{
float acc_variance; //三轴加速度方差之和
float gyr_variance; //三轴陀螺仪方差之和
float acc_magnitude; //三轴加速度模长
float gyr_magnitude; //三轴陀螺仪模长
}debug_t;
#endif
/**
* @brief 初始化滑雪追踪器
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
*/
void skiing_tracker_init(skiing_tracker_t *tracker);
BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data) ;
#endif // SKIING_TRACKER_H

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

@ -1,651 +0,0 @@
/*
*/
#include "skiing_tracker.h"
#include "../sensor/SC7U22.h"
#include <math.h>
#include <string.h>
#define G_ACCELERATION 9.81f
#define DEG_TO_RAD (3.14159265f / 180.0f)
#define ENABLE_XLOG 1
#ifdef xlog
#undef xlog
#endif
#if ENABLE_XLOG
#define xlog(format, ...) printf("[XT:%s] " format, __func__, ##__VA_ARGS__)
#else
#define xlog(format, ...) ((void)0)
#endif
// --- 静止检测 ---
//两个判断是否静止的必要条件：动态零速更新(ZUPT)阈值
// 加速方差阈值，提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
#define STOP_ACC_VARIANCE_THRESHOLD 0.2f
// 陀螺仪方差阈值
#define STOP_GYR_VARIANCE_THRESHOLD 5.0f
// 静止时候的陀螺仪模长
#define STOP_GYR_MAG_THRESHOLD 15
// --- --- ---
// --- 启动滑雪阈值 ---
// 加速度模长与重力的差值大于此值，认为开始运动；降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
#define START_ACC_MAG_THRESHOLD 1.0f //0.5、1
// 陀螺仪方差阈值，以允许启动瞬间的正常抖动，但仍能过滤掉混乱的、非滑雪的晃动。
#define START_GYR_VARIANCE_THRESHOLD 15.0f
// --- --- ---
// --- 滑雪过程 ---
//加速度 模长(不含重力），低于此值视为 在做匀速运动
#define SKIING_ACC_MAG_THRESHOLD 0.5f
//陀螺仪 模长，高于此值视为 摔倒了
#define FALLEN_GRY_MAG_THRESHOLD 2000.0f //未确定
// --- --- ---
// --- 原地旋转抖动 ---
// 加速度 方差 阈值。此值比 静止检测 阈值更宽松，
#define WOBBLE_ACC_VARIANCE_THRESHOLD 0.5f
// 加速度 模长 阈值
#define WOBBLE_ACC_MAG_THRESHOLD 1.0f
// 角速度 总模长 大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
#define ROTATION_GYR_MAG_THRESHOLD 30.0f
// --- --- ---
// --- 滑雪转弯动 ---
// 加速度 方差 阈值，大于此值，滑雪过程可能发生了急转弯
#define WHEEL_ACC_VARIANCE_THRESHOLD 7.0f
// 角速度 总模长 大于此值(度/秒)，认为滑雪过程中进行急转弯
#define WHEEL_GYR_MAG_THRESHOLD 500.0f //
// --- --- ---
// --- 跳跃 ---
// 加速度模长低于此值(g)，认为进入失重状态(IN_AIR)
#define AIRBORNE_ACC_MAG_LOW_THRESHOLD 0.4f
// 加速度模长高于此值(g)，认为发生落地冲击(LANDING)
#define LANDING_ACC_MAG_HIGH_THRESHOLD 3.5f
// 起跳加速度阈值(g)，用于进入TAKING_OFF状态
#define TAKEOFF_ACC_MAG_HIGH_THRESHOLD 1.8f
// 进入空中状态确认计数：需要连续3个采样点加速度低于阈值才判断为起跳
#define AIRBORNE_CONFIRM_COUNT 3
// 落地状态确认计数：加速度恢复到1g附近并持续2个采样点(20ms)则认为已落地
#define GROUNDED_CONFIRM_COUNT 2
// 最大滞空时间(秒)，超过此时间强制认为已落地，防止状态锁死
#define MAX_TIME_IN_AIR 12.5f
// --- --- ---
// --- 用于消除积分漂移的滤波器和阈值 ---
// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
// alpha = RC / (RC + dt)，参考RC电路而来，fc ≈ (1 - alpha) / (2 * π * dt)
#define HPF_ALPHA 0.999f
//0.995f： 0.08 Hz 的信号
//0.999f： 0.0159 Hz
// --- --- ---
// --- 低通滤波器 ---
// 低通滤波器系数 (alpha)。alpha 越小，滤波效果越强（更平滑），但延迟越大。
// alpha 推荐范围 0.7 ~ 0.95。可以从 0.85 开始尝试。
#define LPF_ALPHA 0.7f
// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
//参考：0.2f ~ 0.4f
#define ACC_DEAD_ZONE_THRESHOLD 0.05f
// --- 模拟摩擦力，进行速度衰减 ---
#define SPEED_ATTENUATION 1.0f //暂不模拟
BLE_KS_send_data_t KS_data;
static float quaternion_data[4];
#ifdef XTELL_TEST
debug_t debug1;
debug_t debug2;
#endif
static skiing_tracker_t my_skiing_tracker;
//////////////////////////////////////////////////////////////////////////////////////////////////
//实现
void clear_speed(void){
my_skiing_tracker.state = STATIC;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.speed = 0;
}
void start_detection(void){
my_skiing_tracker.state = STATIC;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.distance = 0;
my_skiing_tracker.speed = 0;
}
void stop_detection(void){
my_skiing_tracker.state = STOP_DETECTION;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.speed = 0;
}
/**
* @brief 初始化滑雪追踪器
*
* @param tracker
*/
void skiing_tracker_init(skiing_tracker_t *tracker)
{
if (!tracker) {
return;
}
// 使用memset一次性清零整个结构体，包括新增的缓冲区
memset(tracker, 0, sizeof(skiing_tracker_t));
tracker->state = STATIC;
}
/**
* @brief 当检测到落地时，计算空中的水平飞行距离并累加到总距离
*/
static void calculate_air_distance(skiing_tracker_t *tracker) {
float horizontal_speed_on_takeoff = sqrtf(
tracker->initial_velocity_on_takeoff[0] * tracker->initial_velocity_on_takeoff[0] +
tracker->initial_velocity_on_takeoff[1] * tracker->initial_velocity_on_takeoff[1]
);
float distance_in_air = horizontal_speed_on_takeoff * tracker->time_in_air;
tracker->distance += distance_in_air;
}
/**
* @brief 使用四元数直接从设备坐标系的加速度中移除重力分量
* @details 这种方法比使用欧拉角更精确、更稳定，且避免了万向节死锁。
* @param acc_device 输入：设备坐标系下的原始加速度 [x, y, z], 单位 m/s^2
* @param q 输入：表示姿态的四元数 [w, x, y, z]
* @param acc_linear_device 输出：设备坐标系下移除重力后的线性加速度 [x, y, z]
*/
void q_remove_gravity_with_quaternion(const float *acc_device, const float *q, float *acc_linear_device)
{
// 从四元数计算重力在设备坐标系下的投影
// G_device = R_transpose * G_world
// G_world = [0, 0, g]
// R_transpose 的第三列即为重力投影方向
float gx = 2.0f * (q[1] * q[3] - q[0] * q[2]);
float gy = 2.0f * (q[0] * q[1] + q[2] * q[3]);
float gz = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
// 从原始加速度中减去重力分量
acc_linear_device[0] = acc_device[0] - gx * G_ACCELERATION;
acc_linear_device[1] = acc_device[1] - gy * G_ACCELERATION;
acc_linear_device[2] = acc_device[2] - gz * G_ACCELERATION;
}
/**
* @brief 使用四元数将设备坐标系的线性加速度转换到世界坐标系
* @details 同样，此方法比使用欧拉角更优。
* @param acc_linear_device 输入：设备坐标系下的线性加速度 [x, y, z]
* @param q 输入：表示姿态的四元数 [w, x, y, z]
* @param acc_linear_world 输出：世界坐标系下的线性加速度 [x, y, z]
*/
void q_transform_to_world_with_quaternion(const float *acc_linear_device, const float *q, float *acc_linear_world)
{
// 这是 R_device_to_world * acc_linear_device 的展开形式
acc_linear_world[0] = (1.0f - 2.0f*q[2]*q[2] - 2.0f*q[3]*q[3]) * acc_linear_device[0] +
(2.0f*q[1]*q[2] - 2.0f*q[0]*q[3]) * acc_linear_device[1] +
(2.0f*q[1]*q[3] + 2.0f*q[0]*q[2]) * acc_linear_device[2];
acc_linear_world[1] = (2.0f*q[1]*q[2] + 2.0f*q[0]*q[3]) * acc_linear_device[0] +
(1.0f - 2.0f*q[1]*q[1] - 2.0f*q[3]*q[3]) * acc_linear_device[1] +
(2.0f*q[2]*q[3] - 2.0f*q[0]*q[1]) * acc_linear_device[2];
acc_linear_world[2] = (2.0f*q[1]*q[3] - 2.0f*q[0]*q[2]) * acc_linear_device[0] +
(2.0f*q[2]*q[3] + 2.0f*q[0]*q[1]) * acc_linear_device[1] +
(1.0f - 2.0f*q[1]*q[1] - 2.0f*q[2]*q[2]) * acc_linear_device[2];
// acc_linear_world[2] -= G_ACCELERATION;
}
/**
* @brief 计算缓冲区内三轴数据的方差之和
*
* @param buffer 传进来的三轴数据：陀螺仪/加速度
* @return float 返回方差和
*/
static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
{
float mean[3] = {0};
float variance[3] = {0};
// 计算均值
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
mean[0] += buffer[i][0];
mean[1] += buffer[i][1];
mean[2] += buffer[i][2];
}
mean[0] /= VARIANCE_BUFFER_SIZE;
mean[1] /= VARIANCE_BUFFER_SIZE;
mean[2] /= VARIANCE_BUFFER_SIZE;
// 计算方差
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
variance[0] += (buffer[i][0] - mean[0]) * (buffer[i][0] - mean[0]);
variance[1] += (buffer[i][1] - mean[1]) * (buffer[i][1] - mean[1]);
variance[2] += (buffer[i][2] - mean[2]) * (buffer[i][2] - mean[2]);
}
variance[0] /= VARIANCE_BUFFER_SIZE;
variance[1] /= VARIANCE_BUFFER_SIZE;
variance[2] /= VARIANCE_BUFFER_SIZE;
// 返回三轴方差之和，作为一个综合的稳定度指标
return variance[0] + variance[1] + variance[2];
}
/**
* @brief 摩擦力模拟，进行速度衰减
*
* @param tracker
*/
void forece_of_friction(skiing_tracker_t *tracker){
// 增加速度衰减，模拟摩擦力
tracker->velocity[0] *= SPEED_ATTENUATION;
tracker->velocity[1] *= SPEED_ATTENUATION;
tracker->velocity[2] = 0; // 垂直速度强制归零
}
/**
* @brief 状态机更新
*
* @param tracker 传入同步修改后传出
* @param acc_device_ms2 三轴加速度，m/s^2
* @param gyr_dps 三轴陀螺仪，dps
*/
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps)
{
// 缓冲区未填满时，不进行状态判断，默认为静止
if (!tracker->buffer_filled) {
tracker->state = STATIC;
return;
}
// --- 计算关键指标 ---
float acc_variance = calculate_variance(tracker->acc_buffer); // 计算加速度方差
float gyr_variance = calculate_variance(tracker->gyr_buffer); // 计算陀螺仪方差
float gyr_magnitude = sqrtf(gyr_dps[0]*gyr_dps[0] + gyr_dps[1]*gyr_dps[1] + gyr_dps[2]*gyr_dps[2]); //dps
float acc_magnitude = sqrtf(acc_device_ms2[0]*acc_device_ms2[0] + acc_device_ms2[1]*acc_device_ms2[1] + acc_device_ms2[2]*acc_device_ms2[2]); //m/s^s
float acc_magnitude_g = acc_magnitude / G_ACCELERATION; // 转换为g单位，用于跳跃判断
#ifdef XTELL_TEST
debug1.acc_variance =acc_variance;
debug1.gyr_variance =gyr_variance;
debug1.gyr_magnitude=gyr_magnitude;
debug1.acc_magnitude=fabsf(acc_magnitude - G_ACCELERATION);
#endif
// --- 状态机逻辑 (核心修改区域) ---
#if 0 //暂时不考虑空中
// 1. 空中/落地状态的后续处理
if (tracker->state == IN_AIR) {
// A. 检测巨大冲击 -> 落地
if (acc_magnitude_g > LANDING_ACC_MAG_HIGH_THRESHOLD) {
tracker->state = LANDING;
// B. 检测超时 -> 强制落地 (安全机制)
} else if (tracker->time_in_air > MAX_TIME_IN_AIR) {
tracker->state = LANDING;
// C. 检测恢复正常重力 (平缓落地)
} else if (acc_magnitude_g > 0.8f && acc_magnitude_g < 1.5f) {
tracker->grounded_entry_counter++;
if (tracker->grounded_entry_counter >= GROUNDED_CONFIRM_COUNT) {
tracker->state = LANDING;
}
} else {
tracker->grounded_entry_counter = 0;
}
return; // 在空中或刚切换到落地，结束本次状态判断
}
// 2. 严格的 "起跳->空中" 状态转换逻辑
// 只有当处于滑行状态时，才去检测起跳意图
if (tracker->state == NO_CONSTANT_SPEED || tracker->state == CONSTANT_SPEED || tracker->state == WHEEL) {
if (acc_magnitude_g > TAKEOFF_ACC_MAG_HIGH_THRESHOLD) {
tracker->state = TAKING_OFF;
tracker->airborne_entry_counter = 0; // 准备检测失重
return;
}
}
// 只有在TAKING_OFF状态下，才去检测是否进入失重
if (tracker->state == TAKING_OFF) {
if (acc_magnitude_g < AIRBORNE_ACC_MAG_LOW_THRESHOLD) {
tracker->airborne_entry_counter++;
if (tracker->airborne_entry_counter >= AIRBORNE_CONFIRM_COUNT) {
memcpy(tracker->initial_velocity_on_takeoff, tracker->velocity, sizeof(tracker->velocity));
tracker->time_in_air = 0;
tracker->state = IN_AIR;
tracker->airborne_entry_counter = 0;
tracker->grounded_entry_counter = 0;
return;
}
} else {
// 如果在起跳冲击后一段时间内没有失重，说明只是一个颠簸，恢复滑行
// 可以加一个小的超时计数器，这里为了简单先直接恢复
tracker->state = NO_CONSTANT_SPEED;
}
return; // 无论是否切换，都结束本次判断
}
#endif
// --- 静止判断 ---
if (acc_variance < STOP_ACC_VARIANCE_THRESHOLD && gyr_variance < STOP_GYR_VARIANCE_THRESHOLD && gyr_magnitude < STOP_GYR_MAG_THRESHOLD) {
tracker->state = STATIC;
return;
}
// --- 地面状态切换逻辑 ---
switch (tracker->state) {
case LANDING:
tracker->state = STATIC;
break;
case STATIC:
// 优先判断是否进入 WOBBLE 状态
// 条件：陀螺仪活动剧烈，但整体加速度变化不大（说明是原地转或晃）
if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) < WOBBLE_ACC_MAG_THRESHOLD) {
tracker->state = WOBBLE;
}
// 只有在陀螺仪和加速度都满足“前进”特征时，才启动
else if (gyr_variance > START_GYR_VARIANCE_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
}
break;
case WOBBLE:
// 从 WOBBLE 状态启动的条件应该和从 STATIC 启动一样严格
if (gyr_variance < START_GYR_VARIANCE_THRESHOLD * 2 && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
}
// 如果陀螺仪活动减弱，则可能恢复静止
else if (gyr_magnitude < ROTATION_GYR_MAG_THRESHOLD * 0.8f) { // 增加迟滞，避免抖动
// 不直接跳回STATIC，而是依赖下一轮的全局静止判断
}
break;
case NO_CONSTANT_SPEED: //非匀速状态
//暂时不考虑摔倒
// if (gyr_magnitude > FALLEN_GRY_MAG_THRESHOLD) {
// tracker->state = FALLEN; //摔倒
// } else
if (gyr_magnitude > WHEEL_GYR_MAG_THRESHOLD && acc_variance > WHEEL_ACC_VARIANCE_THRESHOLD) {
tracker->state = WHEEL; //转弯
} else if (fabsf(acc_magnitude - G_ACCELERATION) < SKIING_ACC_MAG_THRESHOLD) {
tracker->state = CONSTANT_SPEED; //匀速
}
break;
case CONSTANT_SPEED: //匀速状态
if (fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
}
//TODO：可以添加进入转弯或摔倒的判断
break;
case WHEEL:
// 从转弯状态，检查转弯是否结束
// 如果角速度和加速度方差都降下来了，就回到普通滑行状态
if (gyr_magnitude < WHEEL_GYR_MAG_THRESHOLD * 0.8f && acc_variance < WHEEL_ACC_VARIANCE_THRESHOLD * 0.8f) { // 乘以一个滞后系数避免抖动
tracker->state = NO_CONSTANT_SPEED;
}
break;
case FALLEN:
// TODO：回到 STATIC
break;
}
}
/**
* @brief 主更新函数
*
* @param tracker
* @param acc_g 三轴加速度，g
* @param gyr_dps 三轴陀螺仪，dps
* @param angle 欧若拉角
* @param dt 采样时间间隔，会用来积分求速度
*/
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt)
{
if (!tracker || !acc_g || !gyr_dps || !angle || dt <= 0) {
return;
}
if(my_skiing_tracker.state == STOP_DETECTION)
return;
// --- 数据预处理和缓冲 ---
float acc_device_ms2[3];
acc_device_ms2[0] = acc_g[0] * G_ACCELERATION;
acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
acc_device_ms2[2] = acc_g[2] * G_ACCELERATION;
// 将最新数据存入缓冲区
memcpy(tracker->acc_buffer[tracker->buffer_index], acc_device_ms2, sizeof(acc_device_ms2));
memcpy(tracker->gyr_buffer[tracker->buffer_index], gyr_dps, 3 * sizeof(float));
tracker->buffer_index++;
if (tracker->buffer_index >= VARIANCE_BUFFER_SIZE) {
tracker->buffer_index = 0;
tracker->buffer_filled = 1; // 标记缓冲区已满
}
// --- 更新状态机 ---
update_state_machine(tracker, acc_device_ms2, gyr_dps);
// --- 根据状态执行不同的计算逻辑 ---
switch (tracker->state) {
case TAKING_OFF:
tracker->speed = 0.0f;
break;
case IN_AIR:
// 在空中时，只累加滞空时间
tracker->time_in_air += dt;
break;
case LANDING:
// 刚落地，计算空中距离
calculate_air_distance(tracker);
// 清理速度和滤波器状态，为恢复地面追踪做准备
memset(tracker->velocity, 0, sizeof(tracker->velocity));
tracker->speed = 0;
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态
break;
case WHEEL:
case NO_CONSTANT_SPEED:
float linear_acc_device[3];
float linear_acc_world[3];
// 在设备坐标系下，移除重力，得到线性加速度
q_remove_gravity_with_quaternion(acc_device_ms2, quaternion_data, linear_acc_device);
// 将设备坐标系下的线性加速度，旋转到世界坐标系
q_transform_to_world_with_quaternion(linear_acc_device, quaternion_data, linear_acc_world);
// 将最终用于积分的加速度存入 tracker 结构体
memcpy(tracker->acc_no_g, linear_acc_world, sizeof(linear_acc_world));
float acc_world_temp[3]; // 临时变量存储当前周期的加速度
for (int i = 0; i < 2; i++) { // 只处理水平方向的 x 和 y 轴
// --- 核心修改：颠倒滤波器顺序为 HPF -> LPF ---
// 1. 高通滤波 (HPF) 先行: 消除因姿态误差导致的重力泄漏（直流偏置）
// HPF的瞬态响应会产生尖峰，这是正常的。
tracker->acc_world_filtered[i] = HPF_ALPHA * (tracker->acc_world_filtered[i] + tracker->acc_no_g[i] - tracker->acc_world_unfiltered_prev[i]);
tracker->acc_world_unfiltered_prev[i] = tracker->acc_no_g[i];
// 2. 低通滤波 (LPF) 殿后: 平滑掉HPF产生的尖峰和传感器自身的高频振动噪声。
// 这里使用 tracker->acc_world_filtered[i] 作为LPF的输入。
tracker->acc_world_lpf[i] = (1.0f - LPF_ALPHA) * tracker->acc_world_filtered[i] + LPF_ALPHA * tracker->acc_world_lpf[i];
// 将最终处理完的加速度值存入临时变量
acc_world_temp[i] = tracker->acc_world_lpf[i];
}
// 计算处理后加速度的水平模长
float acc_horizontal_mag = sqrtf(acc_world_temp[0] * acc_world_temp[0] +
acc_world_temp[1] * acc_world_temp[1]);
#if XTELL_TEST
debug2.acc_magnitude = acc_horizontal_mag;
#endif
// 应用死区，并积分
if (acc_horizontal_mag > ACC_DEAD_ZONE_THRESHOLD) {
tracker->velocity[0] += acc_world_temp[0] * dt;
tracker->velocity[1] += acc_world_temp[1] * dt;
}
// 更新速度和距离
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
tracker->velocity[1] * tracker->velocity[1]);
tracker->distance += tracker->speed * dt;
break;
case CONSTANT_SPEED:
//保持上次的速度不变。只更新距离
tracker->distance += tracker->speed * dt;
break;
case STATIC:
case WOBBLE:
// 速度清零，抑制漂移
memset(tracker->velocity, 0, sizeof(tracker->velocity));
tracker->speed = 0.0f;
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态
#if XTELL_TEST
debug2.acc_magnitude = 0;
#endif
break;
case FALLEN:
// TODO
break;
default:
break;
}
#if 1
float linear_acc_device[3];
float linear_acc_world[3];
float tmp_world_acc[3];
// 在设备坐标系下，移除重力，得到线性加速度
q_remove_gravity_with_quaternion(acc_device_ms2, quaternion_data, linear_acc_device);
// 将设备坐标系下的线性加速度，旋转到世界坐标系
q_transform_to_world_with_quaternion(linear_acc_device, quaternion_data, tmp_world_acc);
float all_world_mag = sqrtf(tmp_world_acc[0] * tmp_world_acc[0] +
tmp_world_acc[1] * tmp_world_acc[1] +
tmp_world_acc[2] * tmp_world_acc[2]);
static int count = 0;
if(count > 100){
xlog("===original(g): x %.2f, y %.2f, z %.2f===\n",acc_g[0],acc_g[1],acc_g[2]);
xlog("===world(m/s^2) no g: x %.2f, y %.2f, z %.2f, all %.2f===\n",tmp_world_acc[0],tmp_world_acc[1],tmp_world_acc[2],all_world_mag); //去掉重力加速度
xlog("===gyr(dps) : x %.2f, y %.2f, z %.2f===\n",gyr_dps[0],gyr_dps[1],gyr_dps[2]); //angle
xlog("===angle : x %.2f, y %.2f, z %.2f,===\n",angle[0],angle[1],angle[2]);
count = 0;
}
count++;
#endif
}
/**
* @brief 滑雪数据计算
*
* @param acc_data_buf 传入的三轴加速度数据
* @param gyr_data_buf 传入的三轴陀螺仪数据
* @param angle_data 传入的欧若拉角数据
* @return BLE_send_data_t 要发送给蓝牙的数据
*/
BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data, float* quaternion) {
static int initialized = 0;
static float acc_data_g[3];
static float gyr_data_dps[3];
if(quaternion != NULL){
memcpy(quaternion_data, quaternion, 4 * sizeof(float));
}
// const float delta_time = DELTA_TIME+0.01f;
// const float delta_time = DELTA_TIME + 0.005f;
const float delta_time = DELTA_TIME;
BLE_send_data_t BLE_send_data;
if (!initialized) {
skiing_tracker_init(&my_skiing_tracker);
initialized = 1;
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
}
#if ACC_RANGE==2
// 加速度 LSB to g
acc_data_g[0] = (float)acc_data_buf[0] / 16384.0f;
acc_data_g[1] = (float)acc_data_buf[1] / 16384.0f;
acc_data_g[2] = (float)acc_data_buf[2] / 16384.0f;
#endif
#if ACC_RANGE==4
// 加速度 LSB to g
acc_data_g[0] = (float)acc_data_buf[0] / 8192.0f;
acc_data_g[1] = (float)acc_data_buf[1] / 8192.0f;
acc_data_g[2] = (float)acc_data_buf[2] / 8192.0f;
#endif
#if ACC_RANGE==8
//±8g 4096
acc_data_g[0] = (float)acc_data_buf[0] / 4096.0f; //ax
acc_data_g[1] = (float)acc_data_buf[1] / 4096.0f; //ay
acc_data_g[2] = (float)acc_data_buf[2] / 4096.0f; //az
#endif
#if ACC_RANGE==16
//±16g 2048
acc_data_g[0] = (float)acc_data_buf[0] / 2048.0f; //ax
acc_data_g[1] = (float)acc_data_buf[1] / 2048.0f; //ay
acc_data_g[2] = (float)acc_data_buf[2] / 2048.0f; //az
#endif
// 陀螺仪 LSB to dps (度/秒)
// ±2000dps量程下，转换系数约为 0.061
gyr_data_dps[0] = (float)gyr_data_buf[0] * 0.061f;
gyr_data_dps[1] = (float)gyr_data_buf[1] * 0.061f;
gyr_data_dps[2] = (float)gyr_data_buf[2] * 0.061f;
skiing_tracker_update(&my_skiing_tracker, acc_data_g, gyr_data_dps, angle_data, delta_time);
BLE_send_data.skiing_state = my_skiing_tracker.state;
for (int i = 0; i < 3; i++) {
#ifdef XTELL_TEST
BLE_send_data.acc_data[i] = (short)(acc_data_g[i] * 9.8f) * 100; //cm/^s2
BLE_send_data.gyr_data[i] = (short)gyr_data_dps[i]; //dps
BLE_send_data.angle_data[i] = angle_data[i];
#else
BLE_send_data.acc_data[i] = (short)acc_data_buf[i]; //原始adc数据
BLE_send_data.gyr_data[i] = (short)gyr_data_buf[i]; //原始adc数据
BLE_send_data.angle_data[i] = angle_data[i];
#endif
}
BLE_send_data.speed_cms = (int)(my_skiing_tracker.speed * 100);
BLE_send_data.distance_cm = (int)(my_skiing_tracker.distance * 100);
// printf("Calculate the time interval =============== end\n");
return BLE_send_data;
}

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

@ -1,88 +0,0 @@
#ifndef SKIING_TRACKER_H
#define SKIING_TRACKER_H
#include "../xtell.h"
// 定义滑雪者可能的状态
typedef enum {
STATIC, // 静止或动态稳定：0
NO_CONSTANT_SPEED, // 正在滑雪，非匀速：1
CONSTANT_SPEED, // 正在滑雪，匀速：2
WOBBLE, // 正在原地旋转：3
WHEEL, // 转弯：4
FALLEN, // 已摔倒：5
TAKING_OFF, // 起跳冲击阶段：6
IN_AIR, // 空中失重阶段：7
LANDING, // 落地冲击阶段：8
STOP_DETECTION, // 停止检测：9
UNKNOWN // 未知状态：10
} skiing_state_t;
#define VARIANCE_BUFFER_SIZE 5 // 用于计算方差的数据窗口大小 (5个样本 @ 100Hz = 50ms)，减小延迟，提高实时性
#define DELTA_TIME 0.01f
// 追踪器数据结构体
typedef struct {
// 公开数据
float velocity[3]; // 当前速度 (x, y, z)，单位: m/s
float distance; // 总滑行距离，单位: m
float speed; // 当前速率 (标量)，单位: m/s
skiing_state_t state; // 当前滑雪状态
// 内部计算使用的私有成员
float acc_no_g[3]; // 去掉重力分量后的加速度
// 用于空中距离计算
float time_in_air; // 滞空时间计时器
float initial_velocity_on_takeoff[3]; // 起跳瞬间的速度向量
int airborne_entry_counter; // 进入空中状态的确认计数器
int grounded_entry_counter; // 落地确认计数器
// --- 内部计算使用的私有成员 ---
// 用于动态零速更新和旋转检测的缓冲区
float acc_buffer[VARIANCE_BUFFER_SIZE][3]; // 加速度数据窗口
float gyr_buffer[VARIANCE_BUFFER_SIZE][3]; // 角速度数据窗口
int buffer_index; // 缓冲区当前索引
int buffer_filled; // 缓冲区是否已填满的标志
// 用于高通滤波器（巴特沃斯一阶滤波器）的私有成员，以消除加速度的直流偏置
float acc_world_filtered[3]; //过滤过的
float acc_world_unfiltered_prev[3]; //上一次没过滤的
float acc_world_lpf[3]; // 经过低通滤波后的世界坐标系加速度
} skiing_tracker_t;
//ble发送的数据
typedef struct{ //__attribute__((packed)){ //该结构体取消内存对齐
char sensor_state;
char skiing_state;
int speed_cms; //求出的速度，cm/s
int distance_cm; //求出的距离，cm
short acc_data[3]; //三轴加速度, g
short gyr_data[3]; //三轴陀螺仪, dps
float angle_data[3]; //欧若拉角
}BLE_send_data_t;
typedef struct{
int acc_KS[3]; //卡尔曼后，LSB转换后的 三轴加速度数据（cm/s^2）
int gyr_KS_dps[3]; //卡尔曼后，LSB to dps 三轴陀螺仪数据
int angle_KS[3]; //卡尔曼后，计算得到的欧若拉角数据
}BLE_KS_send_data_t;
#ifdef XTELL_TEST
typedef struct{
float acc_variance; //三轴加速度方差之和
float gyr_variance; //三轴陀螺仪方差之和
float acc_magnitude; //三轴加速度模长
float gyr_magnitude; //三轴陀螺仪模长
}debug_t;
#endif
/**
* @brief 初始化滑雪追踪器
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
*/
void skiing_tracker_init(skiing_tracker_t *tracker);
BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data, float* quaternion);
#endif // SKIING_TRACKER_H

95
apps/earphone/xtell_Sensor/README.md
View File

@ -1,95 +0,0 @@
# 时间间隔 -- 软件模拟iic的情况下
目前测试代码如下：
```c
create_process(&test_id, "test",NULL, test, (int)(DELTA_TIME*1000));
```
对于test函数的调用时间设置的是10ms调用一次，test代码如下：
```c
void test(){
signed short acc_data_buf[3] = {0};
signed short gyr_data_buf[3] = {0};
signed short acc_gyro_input[6] = {0};
float Angle_output[3] = {0};
SL_SC7U22_RawData_Read(acc_data_buf,gyr_data_buf);
BLE_send_data = sensor_processing_task(acc_data_buf, gyr_data_buf);
//----省略-----
//一些ble数据发送
memset(&BLE_send_data, 0, sizeof(BLE_send_data_t));
memset(&data, 0, 50);
// xlog("end============\n");
}
```
sensor_processing_task当中就进行了计算，包括卡尔曼等，在timer设置成10ms的情况下，实际上test函数（或者是sensor_processing_task函数），距离上次调用到本次调用，实际间隔为40ms
计算距离不能直接采用timers设置的时间间隔作为dt来求距离，实际每次计算求速度的时间应该是40ms
# 11.13
代码主要文件夹：apps\earphone\xtell_Sensor
- apps\earphone\xtell_Sensor\send_data.c，‘ xtell_task_create ’ 函数，传感器计算程序逻辑开始位置，包括传感器读取数据的任务、蓝牙发送任务、速度距离计算任务
- apps\earphone\xtell_Sensor\calculate：目前只有计算传感器去除重力分量的代码
- 问题：存在漂移
- 水平的情况下三轴去掉重力分量，计算出来的结果, 误差有0.0x m/s^s
- original(g): x -0.02, y 0.00, z 1.00
device(m/s^2) no g: x -0.08, y -0.01, z -0.04, all 0.08
- 在板子任意倾斜角度下，去掉各轴重力分量的情况下，误差有0.3x m/s^s
- ===original(g): x -0.20, y -0.85, z 0.41===
- ===device(m/s^2) no g: x 0.06, y 0.31, z -0.10, all 0.33===
- apps\earphone\xtell_Sensor\sensor：传感器驱动，参与编译的为SC7U22.c和SC7U22.h
- apps\earphone\xtell_Sensor\A_hide：速度和距离计算代码
- 最新一版为apps\earphone\xtell_Sensor\A_hide\10\，水平距离测速2、3m误差
- 要使用只需要把代码复制粘贴到calculate文件夹下
# 11.18
去除重力分量后仍有误差：
- 数据对齐？
- 有没有丢数据？
- 重复定位的数据？
- 静态时的角度误差？
定时器1的回调函数（10ms调用一次）**A**：读取传感器数据，放进buff
定时器2的回调函数（5ms调用一次）**B**：读取buff的传感器数据，去除重力分离的计算
- **数据没有对齐**，A 的回调调用计数 > B 的回调调用计数
- **丢数据了**，A 读取传感器数据的回调函数中，打印了buff已满的log
- **重复定位**：移动后回到原先的位置，前后的计算得到的三轴角度相同
- **静态时的角度误差**：1°左右
- 定时器2不进行重力分离计算，只进行计数，也仍然有数据没有对齐和丢数据的情况
将读取传感器数据、去除重力分量计算放到同一个任务下，同步进行
- 数据没有丢失，数据也对齐了
- 在小倾斜的坡面下，去除重力分量后的总的加速度，**小于0.1m/s^2**
- 在大倾斜的坡面下（如旋转超过70°），去除重力分量后的总的加速度，在**0.4m/s^2上下**
- 貌似是角度越大，越接近方向锁，导致角度更容易漂移造错数据错误
采用四元数的方式做去除重力分量的计算：
- 将读取传感器数据、去除重力分量计算放到同一个任务下
- 在小倾斜的坡面下，去除重力分量后的总的加速度，低于**0.04m/s^2**
- 在大倾斜的坡面下（如旋转超过70°），去除重力分量后的总的加速度，在**0.1m/s^2上下**
- 大倾斜角度的误差要靠磁力计来消除（yaw无法通过加速度计来消除偏差）

166
apps/earphone/xtell_Sensor/ano/ano_protocol.c
View File

@ -1,166 +0,0 @@
/*
发送数据给上位机的,需要将log打印出口关闭
*/
#include "ano_protocol.h"
#include "asm/uart_dev.h"
#include "app_config.h"
// 定义协议常量
#define ANO_FRAME_HEADER 0xAA
#define ANO_TO_COMPUTER_ADDR 0xFF
// 用于保存 uart_dev_open 返回的句柄
static const uart_bus_t *ano_uart_dev = NULL;
/**
* @brief 计算并填充协议的校验和
* @param frame_buffer 指向数据帧缓冲区的指针
*/
static void ano_calculate_checksum(u8 *frame_buffer) {
#if TCFG_UART0_ENABLE==0
u8 sum_check = 0;
u8 add_check = 0;
// 数据长度在索引为 3 的位置
u8 data_len = frame_buffer[3];
// 需要计算校验和的总长度 = 帧头(1) + 地址(1) + ID(1) + 长度(1) + 数据(data_len)
u16 checksum_len = 4 + data_len;
for (u16 i = 0; i < checksum_len; i++) {
sum_check += frame_buffer[i];
add_check += sum_check;
}
// 将计算出的校验和填充到帧的末尾
frame_buffer[checksum_len] = sum_check;
frame_buffer[checksum_len + 1] = add_check;
#endif
}
/**
* @brief 初始化用于上位机通信的串口
*/
int ano_protocol_init(u32 baudrate) {
#if TCFG_UART0_ENABLE==0
// 防止重复初始化
if (ano_uart_dev) {
return 0;
}
struct uart_platform_data_t ut_arg = {0};
// TCFG_ONLINE_TX_PORT 和 TCFG_ONLINE_RX_PORT 通常在 app_config.h 中定义
ut_arg.tx_pin = TCFG_ONLINE_TX_PORT;
ut_arg.rx_pin = (u8)-1; // -1 表示不使用该引脚，因为我们只发送数据
ut_arg.baud = baudrate;
// 以下为接收相关配置，由于只发送，全部设为0或NULL
ut_arg.rx_cbuf = NULL;
ut_arg.rx_cbuf_size = 0;
ut_arg.frame_length = 0;
ut_arg.rx_timeout = 0;
ut_arg.isr_cbfun = NULL;
ano_uart_dev = (uart_bus_t *)uart_dev_open(&ut_arg);
if (ano_uart_dev == NULL) {
return -1;
}
#endif
return 0;
}
/**
* @brief 发送惯性传感器数据 (ID: 0x01)
*/
void ano_send_inertial_data(s16 acc_x, s16 acc_y, s16 acc_z,
s16 gyr_x, s16 gyr_y, s16 gyr_z,
u8 shock_sta) {
#if TCFG_UART0_ENABLE==0
if (ano_uart_dev == NULL) {
return; // 如果串口未初始化，则不执行任何操作
}
// 帧总长度 = 4(固定头) + 13(数据) + 2(校验) = 19 字节
u8 frame_buffer[19];
u8 data_idx = 4; // DATA区域从索引4开始
// 1. 填充帧头、地址、ID、长度
frame_buffer[0] = ANO_FRAME_HEADER;
frame_buffer[1] = ANO_TO_COMPUTER_ADDR;
frame_buffer[2] = 0x01; // 功能码 ID
frame_buffer[3] = 13; // 数据长度 LEN
// 2. 填充数据内容 (DATA)，注意小端模式 (低字节在前)
frame_buffer[data_idx++] = (u8)(acc_x & 0xFF);
frame_buffer[data_idx++] = (u8)(acc_x >> 8);
frame_buffer[data_idx++] = (u8)(acc_y & 0xFF);
frame_buffer[data_idx++] = (u8)(acc_y >> 8);
frame_buffer[data_idx++] = (u8)(acc_z & 0xFF);
frame_buffer[data_idx++] = (u8)(acc_z >> 8);
frame_buffer[data_idx++] = (u8)(gyr_x & 0xFF);
frame_buffer[data_idx++] = (u8)(gyr_x >> 8);
frame_buffer[data_idx++] = (u8)(gyr_y & 0xFF);
frame_buffer[data_idx++] = (u8)(gyr_y >> 8);
frame_buffer[data_idx++] = (u8)(gyr_z & 0xFF);
frame_buffer[data_idx++] = (u8)(gyr_z >> 8);
frame_buffer[data_idx++] = shock_sta;
// 3. 计算并填充校验和
ano_calculate_checksum(frame_buffer);
// 4. 通过串口发送整个数据帧
ano_uart_dev->write(frame_buffer, sizeof(frame_buffer));
#endif
}
/**
* @brief 发送飞控姿态数据 (ID: 0x03)
*
* @param rol
* @param pit
* @param yaw
* @param fusion_sta
*/
void ano_send_attitude_data(float rol, float pit, float yaw, u8 fusion_sta) {
#if TCFG_UART0_ENABLE==0
if (ano_uart_dev == NULL) {
return; // 如果串口未初始化，则不执行任何操作
}
// 帧总长度 = 4(固定头) + 7(数据) + 2(校验) = 13 字节
u8 frame_buffer[13];
u8 data_idx = 4; // DATA区域从索引4开始
// 1. 填充帧头、地址、ID、长度
frame_buffer[0] = ANO_FRAME_HEADER;
frame_buffer[1] = ANO_TO_COMPUTER_ADDR;
frame_buffer[2] = 0x03; // 功能码 ID
frame_buffer[3] = 7; // 数据长度 LEN
// 2. 转换浮点数为整数并填充 (DATA)，注意小端模式
s16 rol_int = (s16)(rol * 100);
s16 pit_int = (s16)(pit * 100);
s16 yaw_int = (s16)(yaw * 100);
frame_buffer[data_idx++] = (u8)(rol_int & 0xFF);
frame_buffer[data_idx++] = (u8)(rol_int >> 8);
frame_buffer[data_idx++] = (u8)(pit_int & 0xFF);
frame_buffer[data_idx++] = (u8)(pit_int >> 8);
frame_buffer[data_idx++] = (u8)(yaw_int & 0xFF);
frame_buffer[data_idx++] = (u8)(yaw_int >> 8);
frame_buffer[data_idx++] = fusion_sta;
// 3. 计算并填充校验和
ano_calculate_checksum(frame_buffer);
// 4. 通过串口发送整个数据帧
ano_uart_dev->write(frame_buffer, sizeof(frame_buffer));
#endif
}

37
apps/earphone/xtell_Sensor/ano/ano_protocol.h
View File

@ -1,37 +0,0 @@
#ifndef __ANO_PROTOCOL_H__
#define __ANO_PROTOCOL_H__
#include "system/includes.h"
/**
* @brief 初始化用于上位机通信的串口
*
* @param baudrate 波特率，例如 115200
* @return 0: 成功, -1: 失败
*/
int ano_protocol_init(u32 baudrate);
/**
* @brief 发送惯性传感器数据 (ID: 0x01)
* @param acc_x X轴加速度
* @param acc_y Y轴加速度
* @param acc_z Z轴加速度
* @param gyr_x X轴陀螺仪
* @param gyr_y Y轴陀螺仪
* @param gyr_z Z轴陀螺仪
* @param shock_sta 震动状态
*/
void ano_send_inertial_data(s16 acc_x, s16 acc_y, s16 acc_z,
s16 gyr_x, s16 gyr_y, s16 gyr_z,
u8 shock_sta);
/**
* @brief 发送飞控姿态数据 (ID: 0x03)
* @param rol 横滚角 (单位: 度)
* @param pit 俯仰角 (单位: 度)
* @param yaw 航向角 (单位: 度)
* @param fusion_sta 融合状态
*/
void ano_send_attitude_data(float rol, float pit, float yaw, u8 fusion_sta);
#endif // __ANO_PROTOCOL_H__

60
apps/earphone/xtell_Sensor/buffer/circle_buffer.c
View File

@ -1,60 +0,0 @@
#include "circle_buffer.h"
#include <string.h>
// 初始化环形缓冲区
void circle_buffer_init(circle_buffer_t *cb, void *buffer, u16 capacity, u16 element_size) {
cb->buffer = (u8 *)buffer;
cb->capacity = capacity;
cb->element_size = element_size;
cb->head = 0;
cb->tail = 0;
cb->size = 0;
}
// 向环形缓冲区写入一个元素
bool circle_buffer_write(circle_buffer_t *cb, const void *element) {
if (circle_buffer_is_full(cb)) {
return false; // 缓冲区已满
}
u8 *dest = cb->buffer + (cb->head * cb->element_size);
memcpy(dest, element, cb->element_size);
cb->head = (cb->head + 1) % cb->capacity;
cb->size++;
return true;
}
// 从环形缓冲区读取一个元素
bool circle_buffer_read(circle_buffer_t *cb, void *element) {
if (circle_buffer_is_empty(cb)) {
return false; // 缓冲区为空
}
u8 *src = cb->buffer + (cb->tail * cb->element_size);
memcpy(element, src, cb->element_size);
cb->tail = (cb->tail + 1) % cb->capacity;
cb->size--;
return true;
}
// 获取已用空间的大小（以元素为单位）
u16 circle_buffer_get_size(circle_buffer_t *cb) {
return cb->size;
}
// 获取剩余空间的大小（以元素为单位）
u16 circle_buffer_get_free_space(circle_buffer_t *cb) {
return cb->capacity - cb->size;
}
// 检查缓冲区是否已满
bool circle_buffer_is_full(circle_buffer_t *cb) {
return cb->size == cb->capacity;
}
// 检查缓冲区是否为空
bool circle_buffer_is_empty(circle_buffer_t *cb) {
return cb->size == 0;
}

70
apps/earphone/xtell_Sensor/buffer/circle_buffer.h
View File

@ -1,70 +0,0 @@
#ifndef CIRCLE_BUFFER_H
#define CIRCLE_BUFFER_H
#include "system/includes.h"
// 定义环形缓冲区的结构体
typedef struct {
u8 *buffer; // 缓冲区指针
u16 capacity; // 缓冲区总容量（以元素为单位）
u16 element_size; // 每个元素的大小（以字节为单位）
u16 head; // 头部指针（写入位置，以元素为单位）
u16 tail; // 尾部指针（读取位置，以元素为单位）
u16 size; // 当前已用大小（以元素为单位）
} circle_buffer_t;
/**
* @brief 初始化环形缓冲区
* @param cb 指向环形缓冲区结构体的指针
* @param buffer 外部提供的缓冲区内存
* @param capacity 缓冲区的总容量（以元素数量为单位）
* @param element_size 每个元素的大小（字节）
*/
void circle_buffer_init(circle_buffer_t *cb, void *buffer, u16 capacity, u16 element_size);
/**
* @brief 向环形缓冲区写入一个元素
* @param cb 指向环形缓冲区结构体的指针
* @param element 要写入的元素的指针
* @return 成功返回true，失败返回false
*/
bool circle_buffer_write(circle_buffer_t *cb, const void *element);
/**
* @brief 从环形缓冲区读取一个元素
* @param cb 指向环形缓冲区结构体的指针
* @param element 用于存放读取元素的缓冲区的指针
* @return 成功返回true，失败返回false
*/
bool circle_buffer_read(circle_buffer_t *cb, void *element);
/**
* @brief 获取环形缓冲区中已用空间的大小（以元素为单位）
* @param cb 指向环形缓冲区结构体的指针
* @return 已用空间的大小（元素数量）
*/
u16 circle_buffer_get_size(circle_buffer_t *cb);
/**
* @brief 获取环形缓冲区中剩余空间的大小（以元素为单位）
* @param cb 指向环形缓冲区结构体的指针
* @return 剩余空间的大小（元素数量）
*/
u16 circle_buffer_get_free_space(circle_buffer_t *cb);
/**
* @brief 检查缓冲区是否已满
* @param cb 指向环形缓冲区结构体的指针
* @return 如果已满返回true，否则返回false
*/
bool circle_buffer_is_full(circle_buffer_t *cb);
/**
* @brief 检查缓冲区是否为空
* @param cb 指向环形缓冲区结构体的指针
* @return 如果为空返回true，否则返回false
*/
bool circle_buffer_is_empty(circle_buffer_t *cb);
#endif // CIRCLE_BUFFER_H

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

@ -1,635 +0,0 @@
/*
*/
#include "skiing_tracker.h"
#include "../sensor/SC7U22.h"
#define G_ACCELERATION 9.81f
#define DEG_TO_RAD (3.14159265f / 180.0f)
#define ENABLE_XLOG 1
#ifdef xlog
#undef xlog
#endif
#if ENABLE_XLOG
#define xlog(format, ...) printf("[XT:%s] " format, __func__, ##__VA_ARGS__)
#else
#define xlog(format, ...) ((void)0)
#endif
// --- 静止检测 ---
//两个判断是否静止的必要条件：动态零速更新(ZUPT)阈值
// 加速方差阈值，提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
#define STOP_ACC_VARIANCE_THRESHOLD 0.2f
// 陀螺仪方差阈值
#define STOP_GYR_VARIANCE_THRESHOLD 5.0f
// 静止时候的陀螺仪模长
#define STOP_GYR_MAG_THRESHOLD 15
// --- --- ---
// --- 启动滑雪阈值 ---
// 加速度模长与重力的差值大于此值，认为开始运动；降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
#define START_ACC_MAG_THRESHOLD 1.0f //0.5、1
// 陀螺仪方差阈值，以允许启动瞬间的正常抖动，但仍能过滤掉混乱的、非滑雪的晃动。
#define START_GYR_VARIANCE_THRESHOLD 15.0f
// --- --- ---
// --- 滑雪过程 ---
//加速度 模长(不含重力），低于此值视为 在做匀速运动
#define SKIING_ACC_MAG_THRESHOLD 0.5f
//陀螺仪 模长，高于此值视为 摔倒了
#define FALLEN_GRY_MAG_THRESHOLD 2000.0f //未确定
// --- --- ---
// --- 原地旋转抖动 ---
// 加速度 方差 阈值。此值比 静止检测 阈值更宽松，
#define WOBBLE_ACC_VARIANCE_THRESHOLD 0.5f
// 加速度 模长 阈值
#define WOBBLE_ACC_MAG_THRESHOLD 1.0f
// 角速度 总模长 大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动
#define ROTATION_GYR_MAG_THRESHOLD 30.0f
// --- --- ---
// --- 滑雪转弯动 ---
// 加速度 方差 阈值，大于此值，滑雪过程可能发生了急转弯
#define WHEEL_ACC_VARIANCE_THRESHOLD 7.0f
// 角速度 总模长 大于此值(度/秒)，认为滑雪过程中进行急转弯
#define WHEEL_GYR_MAG_THRESHOLD 500.0f //
// --- --- ---
// --- 跳跃 ---
// 加速度模长低于此值(g)，认为进入失重状态(IN_AIR)
#define AIRBORNE_ACC_MAG_LOW_THRESHOLD 0.4f
// 加速度模长高于此值(g)，认为发生落地冲击(LANDING)
#define LANDING_ACC_MAG_HIGH_THRESHOLD 3.5f
// 起跳加速度阈值(g)，用于进入TAKING_OFF状态
#define TAKEOFF_ACC_MAG_HIGH_THRESHOLD 1.8f
// 进入空中状态确认计数：需要连续3个采样点加速度低于阈值才判断为起跳
#define AIRBORNE_CONFIRM_COUNT 3
// 落地状态确认计数：加速度恢复到1g附近并持续2个采样点(20ms)则认为已落地
#define GROUNDED_CONFIRM_COUNT 2
// 最大滞空时间(秒)，超过此时间强制认为已落地，防止状态锁死
#define MAX_TIME_IN_AIR 12.5f
// --- --- ---
// --- 用于消除积分漂移的滤波器和阈值 ---
// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
// alpha = RC / (RC + dt)，参考RC电路而来，fc ≈ (1 - alpha) / (2 * π * dt)
#define HPF_ALPHA 0.999f
//0.995f： 0.08 Hz 的信号
//0.999f： 0.0159 Hz
// --- --- ---
// --- 低通滤波器 ---
// 低通滤波器系数 (alpha)。alpha 越小，滤波效果越强（更平滑），但延迟越大。
// alpha 推荐范围 0.7 ~ 0.95。可以从 0.85 开始尝试。
#define LPF_ALPHA 0.7f
// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
//参考：0.2f ~ 0.4f
#define ACC_DEAD_ZONE_THRESHOLD 0.05f
// --- 模拟摩擦力，进行速度衰减 ---
#define SPEED_ATTENUATION 1.0f //暂不模拟
BLE_KS_send_data_t KS_data;
static float quaternion_data[4];
#ifdef XTELL_TEST
debug_t debug1;
debug_t debug2;
#endif
static skiing_tracker_t my_skiing_tracker;
//////////////////////////////////////////////////////////////////////////////////////////////////
//实现
void clear_speed(void){
my_skiing_tracker.state = STATIC;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.speed = 0;
}
void start_detection(void){
my_skiing_tracker.state = STATIC;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.distance = 0;
my_skiing_tracker.speed = 0;
}
void stop_detection(void){
my_skiing_tracker.state = STOP_DETECTION;
memset(my_skiing_tracker.velocity, 0, sizeof(my_skiing_tracker.velocity));
my_skiing_tracker.speed = 0;
}
/**
* @brief 初始化滑雪追踪器
*
* @param tracker
*/
void skiing_tracker_init(skiing_tracker_t *tracker)
{
if (!tracker) {
return;
}
// 使用memset一次性清零整个结构体，包括新增的缓冲区
memset(tracker, 0, sizeof(skiing_tracker_t));
tracker->state = STATIC;
}
/**
* @brief 当检测到落地时，计算空中的水平飞行距离并累加到总距离
*/
static void calculate_air_distance(skiing_tracker_t *tracker) {
float horizontal_speed_on_takeoff = sqrtf(
tracker->initial_velocity_on_takeoff[0] * tracker->initial_velocity_on_takeoff[0] +
tracker->initial_velocity_on_takeoff[1] * tracker->initial_velocity_on_takeoff[1]
);
float distance_in_air = horizontal_speed_on_takeoff * tracker->time_in_air;
tracker->distance += distance_in_air;
}
/**
* @brief 使用四元数直接从设备坐标系的加速度中移除重力分量
* @details 这种方法比使用欧拉角更精确、更稳定，且避免了万向节死锁。
* @param acc_device 输入：设备坐标系下的原始加速度 [x, y, z], 单位 m/s^2
* @param q 输入：表示姿态的四元数 [w, x, y, z]
* @param acc_linear_device 输出：设备坐标系下移除重力后的线性加速度 [x, y, z]
*/
void q_remove_gravity_with_quaternion(const float *acc_device, const float *q, float *acc_linear_device)
{
// 从四元数计算重力在设备坐标系下的投影
// G_device = R_transpose * G_world
// G_world = [0, 0, g]
// R_transpose 的第三列即为重力投影方向
float gx = 2.0f * (q[1] * q[3] - q[0] * q[2]);
float gy = 2.0f * (q[0] * q[1] + q[2] * q[3]);
float gz = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
// 从原始加速度中减去重力分量
acc_linear_device[0] = acc_device[0] - gx * G_ACCELERATION;
acc_linear_device[1] = acc_device[1] - gy * G_ACCELERATION;
acc_linear_device[2] = acc_device[2] - gz * G_ACCELERATION;
}
/**
* @brief 使用四元数将设备坐标系的线性加速度转换到世界坐标系
* @details 同样，此方法比使用欧拉角更优。
* @param acc_linear_device 输入：设备坐标系下的线性加速度 [x, y, z]
* @param q 输入：表示姿态的四元数 [w, x, y, z]
* @param acc_linear_world 输出：世界坐标系下的线性加速度 [x, y, z]
*/
void q_transform_to_world_with_quaternion(const float *acc_linear_device, const float *q, float *acc_linear_world)
{
// 这是 R_device_to_world * acc_linear_device 的展开形式
acc_linear_world[0] = (1.0f - 2.0f*q[2]*q[2] - 2.0f*q[3]*q[3]) * acc_linear_device[0] +
(2.0f*q[1]*q[2] - 2.0f*q[0]*q[3]) * acc_linear_device[1] +
(2.0f*q[1]*q[3] + 2.0f*q[0]*q[2]) * acc_linear_device[2];
acc_linear_world[1] = (2.0f*q[1]*q[2] + 2.0f*q[0]*q[3]) * acc_linear_device[0] +
(1.0f - 2.0f*q[1]*q[1] - 2.0f*q[3]*q[3]) * acc_linear_device[1] +
(2.0f*q[2]*q[3] - 2.0f*q[0]*q[1]) * acc_linear_device[2];
acc_linear_world[2] = (2.0f*q[1]*q[3] - 2.0f*q[0]*q[2]) * acc_linear_device[0] +
(2.0f*q[2]*q[3] + 2.0f*q[0]*q[1]) * acc_linear_device[1] +
(1.0f - 2.0f*q[1]*q[1] - 2.0f*q[2]*q[2]) * acc_linear_device[2];
// acc_linear_world[2] -= G_ACCELERATION;
}
/**
* @brief 计算缓冲区内三轴数据的方差之和
*
* @param buffer 传进来的三轴数据：陀螺仪/加速度
* @return float 返回方差和
*/
static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
{
float mean[3] = {0};
float variance[3] = {0};
// 计算均值
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
mean[0] += buffer[i][0];
mean[1] += buffer[i][1];
mean[2] += buffer[i][2];
}
mean[0] /= VARIANCE_BUFFER_SIZE;
mean[1] /= VARIANCE_BUFFER_SIZE;
mean[2] /= VARIANCE_BUFFER_SIZE;
// 计算方差
for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
variance[0] += (buffer[i][0] - mean[0]) * (buffer[i][0] - mean[0]);
variance[1] += (buffer[i][1] - mean[1]) * (buffer[i][1] - mean[1]);
variance[2] += (buffer[i][2] - mean[2]) * (buffer[i][2] - mean[2]);
}
variance[0] /= VARIANCE_BUFFER_SIZE;
variance[1] /= VARIANCE_BUFFER_SIZE;
variance[2] /= VARIANCE_BUFFER_SIZE;
// 返回三轴方差之和，作为一个综合的稳定度指标
return variance[0] + variance[1] + variance[2];
}
/**
* @brief 摩擦力模拟，进行速度衰减
*
* @param tracker
*/
void forece_of_friction(skiing_tracker_t *tracker){
// 增加速度衰减，模拟摩擦力
tracker->velocity[0] *= SPEED_ATTENUATION;
tracker->velocity[1] *= SPEED_ATTENUATION;
tracker->velocity[2] = 0; // 垂直速度强制归零
}
/**
* @brief 状态机更新
*
* @param tracker 传入同步修改后传出
* @param acc_device_ms2 三轴加速度，m/s^2
* @param gyr_dps 三轴陀螺仪，dps
*/
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps)
{
// 缓冲区未填满时，不进行状态判断，默认为静止
if (!tracker->buffer_filled) {
tracker->state = STATIC;
return;
}
// --- 计算关键指标 ---
float acc_variance = calculate_variance(tracker->acc_buffer); // 计算加速度方差
float gyr_variance = calculate_variance(tracker->gyr_buffer); // 计算陀螺仪方差
float gyr_magnitude = sqrtf(gyr_dps[0]*gyr_dps[0] + gyr_dps[1]*gyr_dps[1] + gyr_dps[2]*gyr_dps[2]); //dps
float acc_magnitude = sqrtf(acc_device_ms2[0]*acc_device_ms2[0] + acc_device_ms2[1]*acc_device_ms2[1] + acc_device_ms2[2]*acc_device_ms2[2]); //m/s^s
float acc_magnitude_g = acc_magnitude / G_ACCELERATION; // 转换为g单位，用于跳跃判断
#ifdef XTELL_TEST
debug1.acc_variance =acc_variance;
debug1.gyr_variance =gyr_variance;
debug1.gyr_magnitude=gyr_magnitude;
debug1.acc_magnitude=fabsf(acc_magnitude - G_ACCELERATION);
#endif
// --- 状态机逻辑 (核心修改区域) ---
#if 0 //暂时不考虑空中
// 1. 空中/落地状态的后续处理
if (tracker->state == IN_AIR) {
// A. 检测巨大冲击 -> 落地
if (acc_magnitude_g > LANDING_ACC_MAG_HIGH_THRESHOLD) {
tracker->state = LANDING;
// B. 检测超时 -> 强制落地 (安全机制)
} else if (tracker->time_in_air > MAX_TIME_IN_AIR) {
tracker->state = LANDING;
// C. 检测恢复正常重力 (平缓落地)
} else if (acc_magnitude_g > 0.8f && acc_magnitude_g < 1.5f) {
tracker->grounded_entry_counter++;
if (tracker->grounded_entry_counter >= GROUNDED_CONFIRM_COUNT) {
tracker->state = LANDING;
}
} else {
tracker->grounded_entry_counter = 0;
}
return; // 在空中或刚切换到落地，结束本次状态判断
}
// 2. 严格的 "起跳->空中" 状态转换逻辑
// 只有当处于滑行状态时，才去检测起跳意图
if (tracker->state == NO_CONSTANT_SPEED || tracker->state == CONSTANT_SPEED || tracker->state == WHEEL) {
if (acc_magnitude_g > TAKEOFF_ACC_MAG_HIGH_THRESHOLD) {
tracker->state = TAKING_OFF;
tracker->airborne_entry_counter = 0; // 准备检测失重
return;
}
}
// 只有在TAKING_OFF状态下，才去检测是否进入失重
if (tracker->state == TAKING_OFF) {
if (acc_magnitude_g < AIRBORNE_ACC_MAG_LOW_THRESHOLD) {
tracker->airborne_entry_counter++;
if (tracker->airborne_entry_counter >= AIRBORNE_CONFIRM_COUNT) {
memcpy(tracker->initial_velocity_on_takeoff, tracker->velocity, sizeof(tracker->velocity));
tracker->time_in_air = 0;
tracker->state = IN_AIR;
tracker->airborne_entry_counter = 0;
tracker->grounded_entry_counter = 0;
return;
}
} else {
// 如果在起跳冲击后一段时间内没有失重，说明只是一个颠簸，恢复滑行
// 可以加一个小的超时计数器，这里为了简单先直接恢复
tracker->state = NO_CONSTANT_SPEED;
}
return; // 无论是否切换，都结束本次判断
}
#endif
// --- 静止判断 ---
if (acc_variance < STOP_ACC_VARIANCE_THRESHOLD && gyr_variance < STOP_GYR_VARIANCE_THRESHOLD && gyr_magnitude < STOP_GYR_MAG_THRESHOLD) {
tracker->state = STATIC;
return;
}
// --- 地面状态切换逻辑 ---
switch (tracker->state) {
case LANDING:
tracker->state = STATIC;
break;
case STATIC:
// 优先判断是否进入 WOBBLE 状态
// 条件：陀螺仪活动剧烈，但整体加速度变化不大（说明是原地转或晃）
if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) < WOBBLE_ACC_MAG_THRESHOLD) {
tracker->state = WOBBLE;
}
// 只有在陀螺仪和加速度都满足“前进”特征时，才启动
else if (gyr_variance > START_GYR_VARIANCE_THRESHOLD && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
}
break;
case WOBBLE:
// 从 WOBBLE 状态启动的条件应该和从 STATIC 启动一样严格
if (gyr_variance < START_GYR_VARIANCE_THRESHOLD * 2 && fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
}
// 如果陀螺仪活动减弱，则可能恢复静止
else if (gyr_magnitude < ROTATION_GYR_MAG_THRESHOLD * 0.8f) { // 增加迟滞，避免抖动
// 不直接跳回STATIC，而是依赖下一轮的全局静止判断
}
break;
case NO_CONSTANT_SPEED: //非匀速状态
//暂时不考虑摔倒
// if (gyr_magnitude > FALLEN_GRY_MAG_THRESHOLD) {
// tracker->state = FALLEN; //摔倒
// } else
if (gyr_magnitude > WHEEL_GYR_MAG_THRESHOLD && acc_variance > WHEEL_ACC_VARIANCE_THRESHOLD) {
tracker->state = WHEEL; //转弯
} else if (fabsf(acc_magnitude - G_ACCELERATION) < SKIING_ACC_MAG_THRESHOLD) {
tracker->state = CONSTANT_SPEED; //匀速
}
break;
case CONSTANT_SPEED: //匀速状态
if (fabsf(acc_magnitude - G_ACCELERATION) > START_ACC_MAG_THRESHOLD) {
tracker->state = NO_CONSTANT_SPEED;
}
//TODO：可以添加进入转弯或摔倒的判断
break;
case WHEEL:
// 从转弯状态，检查转弯是否结束
// 如果角速度和加速度方差都降下来了，就回到普通滑行状态
if (gyr_magnitude < WHEEL_GYR_MAG_THRESHOLD * 0.8f && acc_variance < WHEEL_ACC_VARIANCE_THRESHOLD * 0.8f) { // 乘以一个滞后系数避免抖动
tracker->state = NO_CONSTANT_SPEED;
}
break;
case FALLEN:
// TODO：回到 STATIC
break;
}
}
/**
* @brief 主更新函数
*
* @param tracker
* @param acc_g 三轴加速度，g
* @param gyr_dps 三轴陀螺仪，dps
* @param angle 欧若拉角
* @param dt 采样时间间隔，会用来积分求速度
*/
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt)
{
if (!tracker || !acc_g || !gyr_dps || !angle || dt <= 0) {
return;
}
if(my_skiing_tracker.state == STOP_DETECTION)
return;
// --- 数据预处理和缓冲 ---
float acc_device_ms2[3];
acc_device_ms2[0] = acc_g[0] * G_ACCELERATION;
acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
acc_device_ms2[2] = acc_g[2] * G_ACCELERATION;
// 将最新数据存入缓冲区
memcpy(tracker->acc_buffer[tracker->buffer_index], acc_device_ms2, sizeof(acc_device_ms2));
memcpy(tracker->gyr_buffer[tracker->buffer_index], gyr_dps, 3 * sizeof(float));
tracker->buffer_index++;
if (tracker->buffer_index >= VARIANCE_BUFFER_SIZE) {
tracker->buffer_index = 0;
tracker->buffer_filled = 1; // 标记缓冲区已满
}
// --- 更新状态机 ---
update_state_machine(tracker, acc_device_ms2, gyr_dps);
// --- 根据状态执行不同的计算逻辑 ---
switch (tracker->state) {
case TAKING_OFF:
tracker->speed = 0.0f;
break;
case IN_AIR:
// 在空中时，只累加滞空时间
tracker->time_in_air += dt;
break;
case LANDING:
// 刚落地，计算空中距离
calculate_air_distance(tracker);
// 清理速度和滤波器状态，为恢复地面追踪做准备
memset(tracker->velocity, 0, sizeof(tracker->velocity));
tracker->speed = 0;
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态
break;
case WHEEL:
case NO_CONSTANT_SPEED:
float linear_acc_device[3];
float linear_acc_world[3];
// 在设备坐标系下，移除重力，得到线性加速度
q_remove_gravity_with_quaternion(acc_device_ms2, quaternion_data, linear_acc_device);
// 将设备坐标系下的线性加速度，旋转到世界坐标系
q_transform_to_world_with_quaternion(linear_acc_device, quaternion_data, linear_acc_world);
// 将最终用于积分的加速度存入 tracker 结构体
memcpy(tracker->acc_no_g, linear_acc_world, sizeof(linear_acc_world));
float acc_world_temp[3]; // 临时变量存储当前周期的加速度
for (int i = 0; i < 2; i++) { // 只处理水平方向的 x 和 y 轴
// --- 核心修改：颠倒滤波器顺序为 HPF -> LPF ---
// 1. 高通滤波 (HPF) 先行: 消除因姿态误差导致的重力泄漏（直流偏置）
// HPF的瞬态响应会产生尖峰，这是正常的。
tracker->acc_world_filtered[i] = HPF_ALPHA * (tracker->acc_world_filtered[i] + tracker->acc_no_g[i] - tracker->acc_world_unfiltered_prev[i]);
tracker->acc_world_unfiltered_prev[i] = tracker->acc_no_g[i];
// 2. 低通滤波 (LPF) 殿后: 平滑掉HPF产生的尖峰和传感器自身的高频振动噪声。
// 这里使用 tracker->acc_world_filtered[i] 作为LPF的输入。
tracker->acc_world_lpf[i] = (1.0f - LPF_ALPHA) * tracker->acc_world_filtered[i] + LPF_ALPHA * tracker->acc_world_lpf[i];
// 将最终处理完的加速度值存入临时变量
acc_world_temp[i] = tracker->acc_world_lpf[i];
}
// 计算处理后加速度的水平模长
float acc_horizontal_mag = sqrtf(acc_world_temp[0] * acc_world_temp[0] +
acc_world_temp[1] * acc_world_temp[1]);
#if XTELL_TEST
debug2.acc_magnitude = acc_horizontal_mag;
#endif
// 应用死区，并积分
if (acc_horizontal_mag > ACC_DEAD_ZONE_THRESHOLD) {
tracker->velocity[0] += acc_world_temp[0] * dt;
tracker->velocity[1] += acc_world_temp[1] * dt;
}
// 更新速度和距离
tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
tracker->velocity[1] * tracker->velocity[1]);
tracker->distance += tracker->speed * dt;
break;
case CONSTANT_SPEED:
//保持上次的速度不变。只更新距离
tracker->distance += tracker->speed * dt;
break;
case STATIC:
case WOBBLE:
// 速度清零，抑制漂移
memset(tracker->velocity, 0, sizeof(tracker->velocity));
tracker->speed = 0.0f;
memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
memset(tracker->acc_world_lpf, 0, sizeof(tracker->acc_world_lpf)); // 清理新增的LPF状态
#if XTELL_TEST
debug2.acc_magnitude = 0;
#endif
break;
case FALLEN:
// TODO
break;
default:
break;
}
#if 1
float linear_acc_device[3];
float linear_acc_world[3];
float tmp_world_acc[3];
// 在设备坐标系下，移除重力，得到线性加速度
q_remove_gravity_with_quaternion(acc_device_ms2, quaternion_data, linear_acc_device);
// 将设备坐标系下的线性加速度，旋转到世界坐标系
q_transform_to_world_with_quaternion(linear_acc_device, quaternion_data, tmp_world_acc);
float all_world_mag = sqrtf(tmp_world_acc[0] * tmp_world_acc[0] +
tmp_world_acc[1] * tmp_world_acc[1] +
tmp_world_acc[2] * tmp_world_acc[2]);
static int count = 0;
if(count > 100){
xlog("===original(g): x %.2f, y %.2f, z %.2f===\n",acc_g[0],acc_g[1],acc_g[2]);
xlog("===world(m/s^2) no g: x %.2f, y %.2f, z %.2f, all %.2f===\n",tmp_world_acc[0],tmp_world_acc[1],tmp_world_acc[2],all_world_mag); //去掉重力加速度
xlog("===gyr(dps) : x %.2f, y %.2f, z %.2f===\n",gyr_dps[0],gyr_dps[1],gyr_dps[2]); //angle
xlog("===angle : x %.2f, y %.2f, z %.2f,===\n",angle[0],angle[1],angle[2]);
xlog("===speed(cm/s): %d\n",(int)(tracker->speed*100) );
count = 0;
}
count++;
#endif
}
/**
* @brief 滑雪数据计算
*
* @param acc_data_buf 传入的三轴加速度数据
* @param gyr_data_buf 传入的三轴陀螺仪数据
* @param angle_data 传入的欧若拉角数据
* @return 速度cm/s
*/
uint16_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data, float* quaternion) {
static int initialized = 0;
static float acc_data_g[3];
static float gyr_data_dps[3];
if(quaternion != NULL){
memcpy(quaternion_data, quaternion, 4 * sizeof(float));
}
// const float delta_time = DELTA_TIME+0.01f;
// const float delta_time = DELTA_TIME + 0.005f;
const float delta_time = DELTA_TIME;
if (!initialized) {
skiing_tracker_init(&my_skiing_tracker);
initialized = 1;
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
}
#if ACC_RANGE==2
// 加速度 LSB to g
acc_data_g[0] = (float)acc_data_buf[0] / 16384.0f;
acc_data_g[1] = (float)acc_data_buf[1] / 16384.0f;
acc_data_g[2] = (float)acc_data_buf[2] / 16384.0f;
#endif
#if ACC_RANGE==4
// 加速度 LSB to g
acc_data_g[0] = (float)acc_data_buf[0] / 8192.0f;
acc_data_g[1] = (float)acc_data_buf[1] / 8192.0f;
acc_data_g[2] = (float)acc_data_buf[2] / 8192.0f;
#endif
#if ACC_RANGE==8
//±8g 4096
acc_data_g[0] = (float)acc_data_buf[0] / 4096.0f; //ax
acc_data_g[1] = (float)acc_data_buf[1] / 4096.0f; //ay
acc_data_g[2] = (float)acc_data_buf[2] / 4096.0f; //az
#endif
#if ACC_RANGE==16
//±16g 2048
acc_data_g[0] = (float)acc_data_buf[0] / 2048.0f; //ax
acc_data_g[1] = (float)acc_data_buf[1] / 2048.0f; //ay
acc_data_g[2] = (float)acc_data_buf[2] / 2048.0f; //az
#endif
// 陀螺仪 LSB to dps (度/秒)
// ±2000dps量程下，转换系数约为 0.061
gyr_data_dps[0] = (float)gyr_data_buf[0] * 0.061f;
gyr_data_dps[1] = (float)gyr_data_buf[1] * 0.061f;
gyr_data_dps[2] = (float)gyr_data_buf[2] * 0.061f;
skiing_tracker_update(&my_skiing_tracker, acc_data_g, gyr_data_dps, angle_data, delta_time);
return (uint16_t)(my_skiing_tracker.speed * 100);
}

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

@ -1,83 +0,0 @@
#ifndef SKIING_TRACKER_H
#define SKIING_TRACKER_H
#include "../xtell.h"
#include <math.h>
#include <string.h>
// 定义滑雪者可能的状态
typedef enum {
STATIC, // 静止或动态稳定：0
NO_CONSTANT_SPEED, // 正在滑雪，非匀速：1
CONSTANT_SPEED, // 正在滑雪，匀速：2
WOBBLE, // 正在原地旋转：3
WHEEL, // 转弯：4
FALLEN, // 已摔倒：5
TAKING_OFF, // 起跳冲击阶段：6
IN_AIR, // 空中失重阶段：7
LANDING, // 落地冲击阶段：8
STOP_DETECTION, // 停止检测：9
UNKNOWN // 未知状态：10
} skiing_state_t;
#define VARIANCE_BUFFER_SIZE 5 // 用于计算方差的数据窗口大小 (5个样本 @ 100Hz = 50ms)，减小延迟，提高实时性
#define DELTA_TIME 0.01f
// 追踪器数据结构体
typedef struct {
// 公开数据
float velocity[3]; // 当前速度 (x, y, z)，单位: m/s
float distance; // 总滑行距离，单位: m
float speed; // 当前速率 (标量)，单位: m/s
skiing_state_t state; // 当前滑雪状态
// 内部计算使用的私有成员
float acc_no_g[3]; // 去掉重力分量后的加速度
// 用于空中距离计算
float time_in_air; // 滞空时间计时器
float initial_velocity_on_takeoff[3]; // 起跳瞬间的速度向量
int airborne_entry_counter; // 进入空中状态的确认计数器
int grounded_entry_counter; // 落地确认计数器
// --- 内部计算使用的私有成员 ---
// 用于动态零速更新和旋转检测的缓冲区
float acc_buffer[VARIANCE_BUFFER_SIZE][3]; // 加速度数据窗口
float gyr_buffer[VARIANCE_BUFFER_SIZE][3]; // 角速度数据窗口
int buffer_index; // 缓冲区当前索引
int buffer_filled; // 缓冲区是否已填满的标志
// 用于高通滤波器（巴特沃斯一阶滤波器）的私有成员，以消除加速度的直流偏置
float acc_world_filtered[3]; //过滤过的
float acc_world_unfiltered_prev[3]; //上一次没过滤的
float acc_world_lpf[3]; // 经过低通滤波后的世界坐标系加速度
} skiing_tracker_t;
typedef struct{
int acc_KS[3]; //卡尔曼后，LSB转换后的 三轴加速度数据（cm/s^2）
int gyr_KS_dps[3]; //卡尔曼后，LSB to dps 三轴陀螺仪数据
int angle_KS[3]; //卡尔曼后，计算得到的欧若拉角数据
}BLE_KS_send_data_t;
#ifdef XTELL_TEST
typedef struct{
float acc_variance; //三轴加速度方差之和
float gyr_variance; //三轴陀螺仪方差之和
float acc_magnitude; //三轴加速度模长
float gyr_magnitude; //三轴陀螺仪模长
}debug_t;
#endif
/**
* @brief 初始化滑雪追踪器
*
* @param tracker 指向 skiing_tracker_t 结构体的指针
*/
void skiing_tracker_init(skiing_tracker_t *tracker);
uint16_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data, float* quaternion);
#endif // SKIING_TRACKER_H

37
apps/earphone/xtell_Sensor/example/example.c
View File

@ -1,37 +0,0 @@
//////////////////////////////////////////////////////////////////////////////////////////////////
//START -- 宏定义
#define ENABLE_XLOG 1
#ifdef xlog
#undef xlog
#endif
#if ENABLE_XLOG
#define xlog(format, ...) printf("[%s] " format, __func__, ##__VA_ARGS__)
#else
#define xlog(format, ...) ((void)0)
#endif
//END -- 宏定义
//////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////
//START -- 变量定义
//END -- 变量定义
//////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////
//START -- 函数定义
//END -- 函数定义
//////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////
//实现

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

@ -1,458 +0,0 @@
#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 <math.h>
#include <string.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/SC7U22.h"
#include "./buffer/circle_buffer.h"
#include "btstack/avctp_user.h"
#include "calculate/skiing_tracker.h"
#include "xtell.h"
#include "./ano/ano_protocol.h"
#include "./sensor/MMC56.h"
#include "./sensor/BMP280.h"
#include "./sensor/AK8963.h"
#include "asm/rtc.h"
#include "system/timer.h"
#include "adv_time_stamp_setting.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 SENSOR_DATA_BUFFER_SIZE 1000 // 定义缓冲区可以存储XXX个sensor_data_t元素
//
///////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////
//START -- 函数定义
void send_data_to_ble_client(const u8* data, u16 length);
extern void create_process(u16* pid, const char* name, void *priv, void (*func)(void *priv), u32 msec);
extern void close_process(u16* pid,char* name);
void start_collect_fuc(void);
void BLE_send_fuc(void);
//END -- 函数定义
//////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////
//START -- 变量定义
static u32 timer_offset_ms = 0;
typedef struct {
// -- 六轴 --
signed short SC7U22_data[6];
// -- 磁力计 --
uint8_t mmc5603nj_buffer[9];
// -- 速度 --
uint16_t speed_cms;
// -- 气压计 --
int16_t temperature;
uint32_t pressure;
// -- 左/右腿 --
uint8_t foot_state; //1：左脚；2：右脚
// -- 时间 --
u32 timestamp_ms;
} BLE_send_data_t;
static int count = 0;
// --- 环形缓冲区 ---
static circle_buffer_t BLE_send_buff; // 环形缓冲区管理结构体
BLE_send_data_t BLE_send_data[SENSOR_DATA_BUFFER_SIZE];
// -- 任务id --
u16 SC7U22_calibration_id;
u16 start_collect_fuc_id;
u16 BLE_send_fuc_id;
static u8 stop_ble_send_fuc_id;
//END -- 变量定义
//////////////////////////////////////////////////////////////////////////////////////////////////
// 重置计时器
void reset_ms_timer(void) {
timer_offset_ms = sys_timer_get_ms();
xlog("Timer has been reset.\n");
}
// 获取从上次重置后经过的毫秒数
u32 get_ms_timer(void) {
return sys_timer_get_ms() - timer_offset_ms;
}
// ----------------------------------------------------------------------------------------------------------------
// --------------------------------------------定时器回调函数----------------------------------------------------------
/**
* @brief 六轴静态校准
*
*/
void SC7U22_static_calibration(void){
signed short acc_data_buf[3];
signed short gyr_data_buf[3];
float angle[3];
float quaternion_output[3];
static signed short combined_raw_data[6];
static int calibration_done = 0;
char status = 0;
static first_set_flag = 0;
if(first_set_flag == 0){
first_set_flag = 1;
set_SC7U22_Error_Flag(0);
}
SL_SC7U22_RawData_Read(acc_data_buf,gyr_data_buf);
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, angle,NULL, 0, quaternion_output);
if(status == 1){ //校准完成
extern u16 SC7U22_calibration_id;
extern u8 SC7U22_init;
first_set_flag = 0;
SC7U22_init = 0x11;
close_process(&SC7U22_calibration_id, "SC7U22_calibration");
u8 send2_1[5] = {0xBB,0xBE,0x02,0x00,0x00};
send2_1[4] = SC7U22_init;
send_data_to_ble_client(&send2_1,5);
}
if(count > 100){
count = 0;
char log_buffer[100];
// snprintf( log_buffer, sizeof(log_buffer),"status:%d\n",status);
// send_data_to_ble_client(&log_buffer,strlen(log_buffer));
xlog("status:%d\n", status);
xlog("RawData:AX=%d,AY=%d,AZ=%d,GX=%d,GY=%d,GZ=%d\r\n",combined_raw_data[0],combined_raw_data[1],combined_raw_data[2],combined_raw_data[3],combined_raw_data[4],combined_raw_data[5]);
uint8_t send[5] = {0xBB, 0xBE, 0x02, 0x00, 0x12};
send_data_to_ble_client(&send,5); //正在校验中
}
count++;
}
/**
* @brief 开始采集传感器数据和计算速度
*
*/
void start_collect_fuc(void){
// xlog("=======sensor_read_data START\n");
static signed short combined_raw_data[6];
static int initialized = 0;
static int calibration_done = 0;
char status = 0;
BLE_send_data_t BLE_send_data_tmp;
uint8_t mmc5603nj_buffer[9];
signed short acc_data_buf[3];
signed short gyr_data_buf[3];
float angle[3];
float quaternion_output[3];
float temperature = 0;
float pressure = 0;
// -- 读数据 --
SL_SC7U22_RawData_Read(acc_data_buf,gyr_data_buf);
mmc5603nj_read_origin_data(mmc5603nj_buffer);
// bmp280_read_originanl_data(&BLE_send_data_tmp.adc_P, &BLE_send_data_tmp.adc_T);
bmp280_read_data(&temperature, &pressure);
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
// -- 四元数 --
status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, angle,NULL, 0, quaternion_output);
// -- 速度计算 --
memcpy(acc_data_buf, &combined_raw_data[0], 3 * sizeof(signed short));
memcpy(gyr_data_buf, &combined_raw_data[3], 3 * sizeof(signed short));
uint16_t speed = sensor_processing_task(acc_data_buf,gyr_data_buf,angle, quaternion_output);
// -- 数据包装进结构体 --
memcpy(&BLE_send_data_tmp.SC7U22_data[0], acc_data_buf, 3 * sizeof(signed short));
memcpy(&BLE_send_data_tmp.SC7U22_data[3], gyr_data_buf, 3 * sizeof(signed short));
memcpy(BLE_send_data_tmp.mmc5603nj_buffer, mmc5603nj_buffer, 9);
BLE_send_data_tmp.temperature = (int16_t)(temperature * 1000.0f);
BLE_send_data_tmp.pressure = (int32_t)(pressure * 1000.0f);
BLE_send_data_tmp.speed_cms = speed;
extern u8 foot_init;
BLE_send_data_tmp.foot_state = foot_init;
BLE_send_data_tmp.timestamp_ms = get_ms_timer();
// -- 放进缓冲区 --
if(circle_buffer_is_full(&BLE_send_buff) == 0){
circle_buffer_write(&BLE_send_buff, &BLE_send_data_tmp);
}
// xlog("=======sensor_read_data END\n");
}
/**
* @brief ble数据发送函数
*
*/
void BLE_send_fuc(void){
BLE_send_data_t data_to_send;
if (circle_buffer_is_empty(&BLE_send_buff) == 0) {
circle_buffer_read(&BLE_send_buff, &data_to_send);
} else {
// 缓冲区为空，直接返回
return;
}
// --- 封装并发送六轴传感器数据 ---
{
// 协议定义: 包头(2) + 长度(1) + 类型(1) + 数据(12) = 16字节
const uint8_t IMU_PACKET_LEN = 16;
const uint8_t IMU_PAYLOAD_LEN = 13; // 类型(1) + 数据(12)
const uint8_t IMU_TYPE = 0x01;
uint8_t imu_packet[IMU_PACKET_LEN];
// 填充包头
imu_packet[0] = 0xBB;
imu_packet[1] = 0xBE;
imu_packet[2] = IMU_PAYLOAD_LEN;
imu_packet[3] = IMU_TYPE;
// 拷贝六轴数据
// memcpy(&imu_packet[4], data_to_send.SC7U22_data, sizeof(data_to_send.SC7U22_data));
for (int i = 0; i < 6; i++) {
// SC7U22_data[i] 是一个 signed short (2字节)
// 将其低字节放在前面
imu_packet[4 + i * 2] = (uint8_t)(data_to_send.SC7U22_data[i] & 0xFF);
// 将其高字节放在后面
imu_packet[4 + i * 2 + 1] = (uint8_t)((data_to_send.SC7U22_data[i] >> 8) & 0xFF);
}
// xlog("imu %d\n",data_to_send.SC7U22_data[0]);
// xlog("imu_packet: 0x%x 0x%x 0x%x\n",imu_packet[4],imu_packet[5],imu_packet[6]);
send_data_to_ble_client(&imu_packet, IMU_PACKET_LEN);
}
// --- 封装并发送磁力计数据 ---
{
// 协议定义: 包头(2) + 长度(1) + 类型(1) + 数据(9) = 13字节
const uint8_t MAG_PACKET_LEN = 13;
const uint8_t MAG_PAYLOAD_LEN = 10; // 类型(1) + 数据(9)
const uint8_t MAG_TYPE = 0x02;
uint8_t mag_packet[MAG_PACKET_LEN];
// 填充包头
mag_packet[0] = 0xBB;
mag_packet[1] = 0xBE;
mag_packet[2] = MAG_PAYLOAD_LEN;
mag_packet[3] = MAG_TYPE;
// 拷贝磁力计数据
// memcpy(&mag_packet[4], data_to_send.mmc5603nj_buffer, sizeof(data_to_send.mmc5603nj_buffer));
for (int i = 0; i < 9; i++) {
mag_packet[4 + i] = data_to_send.mmc5603nj_buffer[i];
}
// xlog("mag: 0x%x 0x%x 0x%x\n",mag_packet[4],mag_packet[5],mag_packet[6]);
send_data_to_ble_client(&mag_packet, MAG_PACKET_LEN);
}
// --- 封装并发送压力机计数据 ---
{
// 协议定义: 包头(2) + 长度(1) + 类型(1) + 数据(6) = 10字节
const uint8_t PT_PACKET_LEN = 10;
const uint8_t PT_PAYLOAD_LEN = 7; // 类型(1) + 数据(6)
const uint8_t PT_TYPE = 0x03;
uint8_t pt_packet[PT_PACKET_LEN];
// 填充包头
pt_packet[0] = 0xBB;
pt_packet[1] = 0xBE;
pt_packet[2] = PT_PAYLOAD_LEN;
pt_packet[3] = PT_TYPE;
// 直接发送 int16_t 的二进制补码
pt_packet[4] = (uint8_t)(data_to_send.temperature & 0xFF); // 低字节
pt_packet[5] = (uint8_t)((data_to_send.temperature >> 8) & 0xFF); // 高字节
// 气压 (保持不变)
pt_packet[6] = (uint8_t)(data_to_send.pressure & 0xFF);
pt_packet[7] = (uint8_t)((data_to_send.pressure >> 8) & 0xFF);
pt_packet[8] = (uint8_t)((data_to_send.pressure >> 16) & 0xFF);
pt_packet[9] = (uint8_t)((data_to_send.pressure >> 24) & 0xFF);
send_data_to_ble_client(&pt_packet, PT_PACKET_LEN);
}
// --- 封装并发送速度数据 ---
{
// 协议定义: 包头(2) + 长度(1) + 类型(1) + 数据(2) = 6字节
const uint8_t SPEED_PACKET_LEN = 6;
const uint8_t SPEED_PAYLOAD_LEN = 3; // 类型(1) + 数据(2)
const uint8_t SPEED_TYPE = 0x04;
uint8_t speed_packet[SPEED_PACKET_LEN];
// 填充包头
speed_packet[0] = 0xBB;
speed_packet[1] = 0xBE;
// 填充长度
speed_packet[2] = SPEED_PAYLOAD_LEN;
// 填充类型
speed_packet[3] = SPEED_TYPE;
// 小端模式
speed_packet[4] = (uint8_t)(data_to_send.speed_cms & 0xFF); // 低字节
speed_packet[5] = (uint8_t)((data_to_send.speed_cms >> 8) & 0xFF); // 高字节
send_data_to_ble_client(&speed_packet, SPEED_PACKET_LEN);
}
// --- 封装并发送数据 ---
{
// 协议定义: 包头(2) + 长度(1) + 类型(1) + 数据(5) = 9字节
const uint8_t OTHER_PACKET_LEN = 9;
const uint8_t OTHER_PAYLOAD_LEN = 6; // 类型(1) + 数据(5)
const uint8_t OTHER_TYPE = 0x05;
uint8_t oher_packet[OTHER_PACKET_LEN];
// 填充包头
oher_packet[0] = 0xBB;
oher_packet[1] = 0xBE;
// 填充长度
oher_packet[2] = OTHER_PAYLOAD_LEN;
// 填充类型
oher_packet[3] = OTHER_TYPE;
// 小端模式
oher_packet[4] = (uint8_t)data_to_send.foot_state; // 数据来源
oher_packet[5] = (uint8_t)((data_to_send.timestamp_ms >> 0) & 0xFF); // LSB
oher_packet[6] = (uint8_t)((data_to_send.timestamp_ms >> 8) & 0xFF);
oher_packet[7] = (uint8_t)((data_to_send.timestamp_ms >> 16) & 0xFF);
oher_packet[8] = (uint8_t)((data_to_send.timestamp_ms >> 24) & 0xFF); // MSB
send_data_to_ble_client(&oher_packet, OTHER_PACKET_LEN);
}
}
void stop_BLE_send_fuc(void){
if (circle_buffer_is_empty(&BLE_send_buff)) {
close_process(&BLE_send_fuc_id,"BLE_send_fuc");
close_process(&stop_ble_send_fuc_id,"stop_BLE_send_fuc");
}
}
// ------------------------------------------------------------------------------------------------------
// ------------------------------------------------------------------------------------------------------
/**
* @brief 六轴静态校验
*
*/
void start_calibration(void){
create_process(&SC7U22_calibration_id,"SC7U22_calibration",NULL,SC7U22_static_calibration,10);
}
void stop_calibration(void){
close_process(&SC7U22_calibration_id, "SC7U22_calibration");
}
/**
* @brief 开始采集传感器数据并通过ble发送
*
*/
void start_clloct(void){
reset_ms_timer();
create_process(&start_collect_fuc_id,"start_collect",NULL,start_collect_fuc,10);
create_process(&BLE_send_fuc_id,"BLE_send_fuc",NULL,BLE_send_fuc,1);
}
/**
* @brief 停止采集和ble发送
*
*/
void stop_clloct(void){
close_process(&start_collect_fuc_id,"start_collect");
create_process(&stop_ble_send_fuc_id,"stop_BLE_send_fuc",NULL,stop_BLE_send_fuc,1000); //等缓冲区内容发送完，才停止ble发送任务
}
/**
* @brief 初始化，在app_main.c的app_main函数被调用
*
*/
void xtell_task_create(void){
#if TCFG_GSENOR_USER_IIC_TYPE
int ret = hw_iic_init(0);
xlog("init iic result:%d\n", ret); //返回0成功
#else
int ret = soft_iic_init(0);
int num_chars_written = snprintf(log_buffer_1, sizeof(log_buffer_1),"init iic: %d\n", ret);
#endif
// MPU9250_Mag_Init();
//iic总线设备扫描
// extern void i2c_scanner_probe(void);
// i2c_scanner_probe();
xlog("xtell_task_create\n");
circle_buffer_init(&BLE_send_buff, BLE_send_data, SENSOR_DATA_BUFFER_SIZE, sizeof(BLE_send_data_t));
}
//////////////////////////////////////////////////////////////////////////////
//test
//
void bmp280_test(void){
}
void gsensor_test(void){
#if 1
if(count >= 5){
xlog("==============time============\n");
u32 current_time = get_ms_timer();
xlog("Time since last reset: %u ms\n", current_time);
// 为了演示，我们在这里调用重置
if (current_time > 5000) { // 比如超过5秒就重置一次
reset_ms_timer();
}
count = 0;
}
count++;
#endif
}

133
apps/earphone/xtell_Sensor/sensor/AK8963.c
View File

@ -1,133 +0,0 @@
#include "AK8963.h"
#include "math.h"
#include "os/os_api.h"
#include "../xtell.h"
#include "printf.h"
// 用于存放从Fuse ROM读取的磁力计灵敏度校准值
static float mag_asa_x = 1.0f;
static float mag_asa_y = 1.0f;
static float mag_asa_z = 1.0f;
// 磁力计在16-bit分辨率下的转换因子 (单位: uT/LSB)
#define MAG_RAW_TO_UT_FACTOR (4912.0f / 32760.0f)
/**
* @brief 初始化MPU9250的磁力计AK8963
* @return 0: 成功, 1: MPU9250连接失败, 2: AK8963连接失败
*/
u8 MPU9250_Mag_Init(void) {
u8 temp_data[3];
// --- 检查 MPU9250 连接并复位 ---
_gravity_sensor_get_ndata(MPU9250_ADDR_R, MPU9250_WHO_AM_I, temp_data, 1);
if (temp_data[0] != 0x71 && temp_data[0] != 0x73) {
printf("MPU9250 comm failed, read ID: 0x%X\n", temp_data[0]);
return 1;
}
printf("MPU9250 get id:0x%X\n", temp_data[0]);
gravity_sensor_command(MPU9250_ADDR_W, MPU9250_PWR_MGMT_1, 0x80); // 软复位
os_time_dly(10); // 等待复位完成
gravity_sensor_command(MPU9250_ADDR_W, MPU9250_PWR_MGMT_1, 0x01); // 退出睡眠，选择时钟源
os_time_dly(2);
// --- 强制复位 I2C Master 模块并开启旁路 ---
gravity_sensor_command(MPU9250_ADDR_W, MPU9250_USER_CTRL, 0x20);
os_time_dly(1);
gravity_sensor_command(MPU9250_ADDR_W, MPU9250_USER_CTRL, 0x00);
os_time_dly(1);
gravity_sensor_command(MPU9250_ADDR_W, MPU9250_INT_PIN_CFG, 0x02);
os_time_dly(2);
// --- 再次验证 AK8963 连接 ---
_gravity_sensor_get_ndata(AK8963_ADDR_R, AK8963_WIA, temp_data, 1);
if (temp_data[0] != 0x48) {
printf("AK8963 comm failed after final attempt, read ID: 0x%X\n", temp_data[0]);
return 2;
}
printf("AK8963 get id: 0x%X\n", temp_data[0]);
// ------------------ 配置 AK8963 ------------------
// Power-down模式
gravity_sensor_command(AK8963_ADDR_W, AK8963_CNTL1, 0x00);
os_time_dly(1);
// Fuse ROM access模式
gravity_sensor_command(AK8963_ADDR_W, AK8963_CNTL1, 0x0F);
os_time_dly(1);
_gravity_sensor_get_ndata(AK8963_ADDR_R, AK8963_ASAX, temp_data, 3);
// 计算校准系数
mag_asa_x = (float)(temp_data[0] - 128) / 256.0f + 1.0f;
mag_asa_y = (float)(temp_data[1] - 128) / 256.0f + 1.0f;
mag_asa_z = (float)(temp_data[2] - 128) / 256.0f + 1.0f;
// 再次进入Power-down模式
gravity_sensor_command(AK8963_ADDR_W, AK8963_CNTL1, 0x00);
os_time_dly(1);
// 设置工作模式：16-bit分辨率，100Hz连续测量模式 (0x16)
gravity_sensor_command(AK8963_ADDR_W, AK8963_CNTL1, 0x16);
os_time_dly(1);
printf("AK8963 configured successfully.\n");
return 0; // 初始化成功
}
/**
* @brief 读取磁力计的三轴原始数据
* @param mx, my, mz - 用于存放X, Y, Z轴数据的指针 (int16_t类型)
* @return 0: 成功, 1: 数据未就绪, 2: 数据溢出
*/
u8 MPU9250_Read_Mag_Raw(int16_t *mx, int16_t *my, int16_t *mz) {
u8 read_buf[7];
// 检查数据是否准备好 (使用8位读地址)
_gravity_sensor_get_ndata(AK8963_ADDR_R, AK8963_ST1, read_buf, 1);
if (!(read_buf[0] & 0x01)) {
return 1; // 数据未就绪
}
// 连续读取7个字节 (使用8位读地址)
_gravity_sensor_get_ndata(AK8963_ADDR_R, AK8963_HXL, read_buf, 7);
// 检查数据是否溢出
if (read_buf[6] & 0x08) {
return 2; // 数据溢出
}
// 组合数据
*mx = (int16_t)((read_buf[1] << 8) | read_buf[0]);
*my = (int16_t)((read_buf[3] << 8) | read_buf[2]);
*mz = (int16_t)((read_buf[5] << 8) | read_buf[4]);
return 0; // 读取成功
}
/**
* @brief 读取磁力计的三轴数据，并转换为uT(微特斯拉) (此函数内部逻辑不变)
* @param mx, my, mz - 用于存放X, Y, Z轴数据的指针 (float类型)
* @return 0: 成功, 1: 数据未就绪, 2: 数据溢出
*/
u8 MPU9250_Read_Mag_uT(float *mx, float *my, float *mz) {
int16_t raw_mx, raw_my, raw_mz;
u8 status = MPU9250_Read_Mag_Raw(&raw_mx, &raw_my, &raw_mz);
if (status != 0) {
return status;
}
// 应用灵敏度校准，并转换为uT单位
*mx = (float)raw_mx * mag_asa_x * MAG_RAW_TO_UT_FACTOR;
*my = (float)raw_my * mag_asa_y * MAG_RAW_TO_UT_FACTOR;
*mz = (float)raw_mz * mag_asa_z * MAG_RAW_TO_UT_FACTOR;
return 0;
}

46
apps/earphone/xtell_Sensor/sensor/AK8963.h
View File

@ -1,46 +0,0 @@
// mpu9250_mag.h
#ifndef __MPU9250_MAG_H
#define __MPU9250_MAG_H
#include "stdint.h" // 假设你有标准整数类型，u8 对应 uint8_t
#include "gSensor/gSensor_manage.h"
//==================================================================================
// MPU9250 和 AK8963 的 I2C 地址 (已转换为8位格式)
//==================================================================================
// MPU9250的7位地址是 0x68(接地)
#define MPU9250_ADDR_7BIT 0x69
#define MPU9250_ADDR_W (MPU9250_ADDR_7BIT << 1 | 0) // 8位写地址: 0xD0
#define MPU9250_ADDR_R (MPU9250_ADDR_7BIT << 1 | 1) // 8位读地址: 0xD1
// AK8963磁力计的7位地址是 0x0C
#define AK8963_ADDR_7BIT 0x0C
#define AK8963_ADDR_W (AK8963_ADDR_7BIT << 1 | 0) // 8位写地址: 0x18
#define AK8963_ADDR_R (AK8963_ADDR_7BIT << 1 | 1) // 8位读地址: 0x19
//==================================================================================
// MPU9250 相关寄存器 (用于开启旁路模式)
//==================================================================================
#define MPU9250_WHO_AM_I 0x75
#define MPU9250_INT_PIN_CFG 0x37
#define MPU9250_USER_CTRL 0x6A
#define MPU9250_PWR_MGMT_1 0x6B
//==================================================================================
// AK8963 磁力计相关寄存器
//==================================================================================
#define AK8963_WIA 0x00
#define AK8963_ST1 0x02
#define AK8963_HXL 0x03
#define AK8963_ST2 0x09
#define AK8963_CNTL1 0x0A
#define AK8963_ASAX 0x10
u8 MPU9250_Mag_Init(void);
u8 MPU9250_Read_Mag_Raw(int16_t *mx, int16_t *my, int16_t *mz);
u8 MPU9250_Read_Mag_uT(float *mx, float *my, float *mz);
#endif // __MPU9250_MAG_H

211
apps/earphone/xtell_Sensor/sensor/BMP280.c
View File

@ -1,211 +0,0 @@
/*
气压计
*/
#include "BMP280.h"
#include <string.h>
#include "os/os_api.h"
#include "gSensor/gSensor_manage.h"
/*==================================================================================*/
/* BMP280 内部定义 */
/*==================================================================================*/
// 存储校准参数的静态全局变量
static uint16_t t1;
static int16_t t2, t3;
static uint16_t p1;
static int16_t p2, p3, p4, p5, p6, p7, p8, p9;
static int32_t t_fine;
/*==================================================================================*/
/* 封装的底层I2C读写函数 */
/*==================================================================================*/
/**
* @brief 写入单个字节到BMP280寄存器
*/
static uint8_t bmp280_write_reg(uint8_t reg, uint8_t data) {
gravity_sensor_command(BMP_IIC_WRITE_ADDRESS, reg, data);
return 0;
}
/**
* @brief 从BMP280读取多个字节
*/
static uint8_t bmp280_read_regs(uint8_t reg, uint8_t *buf, uint16_t len) {
return _gravity_sensor_get_ndata(BMP_IIC_READ_ADDRESS, reg, buf, len);
}
/*==================================================================================*/
/* 核心算法 */
/*==================================================================================*/
/**
* @brief 温度补偿计算
* @param adc_T - 原始温度数据
* @return 补偿后的温度值 (单位: °C)
*/
static float compensate_temperature(int32_t adc_T) {
float var1, var2, temperature;
var1 = (((float)adc_T) / 16384.0f - ((float)t1) / 1024.0f) * ((float)t2);
var2 = ((((float)adc_T) / 131072.0f - ((float)t1) / 8192.0f) *
(((float)adc_T) / 131072.0f - ((float)t1) / 8192.0f)) *
((float)t3);
t_fine = (int32_t)(var1 + var2);
temperature = (var1 + var2) / 5120.0f;
if (temperature < -40.0f) return -40.0f;
if (temperature > 85.0f) return 85.0f;
return temperature;
}
/**
* @brief 气压补偿计算
* @param adc_P - 原始气压数据
* @return 补偿后的气压值 (单位: Pa)
*/
static float compensate_pressure(int32_t adc_P) {
float var1, var2, pressure;
var1 = ((float)t_fine / 2.0f) - 64000.0f;
var2 = var1 * var1 * ((float)p6) / 32768.0f;
var2 = var2 + var1 * ((float)p5) * 2.0f;
var2 = (var2 / 4.0f) + (((float)p4) * 65536.0f);
var1 = (((float)p3) * var1 * var1 / 524288.0f + ((float)p2) * var1) / 524288.0f;
var1 = (1.0f + var1 / 32768.0f) * ((float)p1);
if (var1 == 0.0f) {
return 0; // 避免除以零
}
pressure = 1048576.0f - (float)adc_P;
pressure = (pressure - (var2 / 4096.0f)) * 6250.0f / var1;
var1 = ((float)p9) * pressure * pressure / 2147483648.0f;
var2 = pressure * ((float)p8) / 32768.0f;
pressure = pressure + (var1 + var2 + ((float)p7)) / 16.0f;
if (pressure < 30000.0f) return 30000.0f;
if (pressure > 110000.0f) return 110000.0f;
return pressure;
}
/*==================================================================================*/
/* 外部接口函数实现 */
/*==================================================================================*/
uint8_t bmp280_init(void) {
uint8_t id;
uint8_t calib_data[24];
// 1. 检查芯片ID
if (bmp280_read_regs(BMP280_REG_ID, &id, 1) == 0) {
printf("bmp280 get id error:%d\n",id );
return 1; // I2C读取失败
}
if (id != 0x58) {
printf("bmp280 check diff:%d\n",id );
return 1; // ID不匹配
}
printf("bmp280 get id:0%X\n",id );
// 2. 软复位
bmp280_write_reg(BMP280_REG_RESET, 0xB6);
os_time_dly(10); // 等待复位完成
// 3. 一次性读取所有校准参数
if (bmp280_read_regs(BMP280_REG_CALIB_START, calib_data, 24) == 0) {
return 2; // 读取校准数据失败
}
// 4. 解析校准参数
t1 = (uint16_t)(((uint16_t)calib_data[1] << 8) | calib_data[0]);
t2 = (int16_t)(((int16_t)calib_data[3] << 8) | calib_data[2]);
t3 = (int16_t)(((int16_t)calib_data[5] << 8) | calib_data[4]);
p1 = (uint16_t)(((uint16_t)calib_data[7] << 8) | calib_data[6]);
p2 = (int16_t)(((int16_t)calib_data[9] << 8) | calib_data[8]);
p3 = (int16_t)(((int16_t)calib_data[11] << 8) | calib_data[10]);
p4 = (int16_t)(((int16_t)calib_data[13] << 8) | calib_data[12]);
p5 = (int16_t)(((int16_t)calib_data[15] << 8) | calib_data[14]);
p6 = (int16_t)(((int16_t)calib_data[17] << 8) | calib_data[16]);
p7 = (int16_t)(((int16_t)calib_data[19] << 8) | calib_data[18]);
p8 = (int16_t)(((int16_t)calib_data[21] << 8) | calib_data[20]);
p9 = (int16_t)(((int16_t)calib_data[23] << 8) | calib_data[22]);
// 5. 配置传感器 (推荐设置: 正常模式，高精度)
// t_standby=0.5ms, filter=16, spi_en=0
uint8_t config_reg = (0 << 5) | (4 << 2) | (0 << 0);
bmp280_write_reg(BMP280_REG_CONFIG, config_reg);
// osrs_t=x2, osrs_p=x16, mode=normal
uint8_t ctrl_meas_reg = (2 << 5) | (5 << 2) | (3 << 0);
bmp280_write_reg(BMP280_REG_CTRL_MEAS, ctrl_meas_reg);
os_time_dly(10); // 等待配置生效
printf("bmp280 init success\n");
return 0; // 初始化成功
}
/**
* @brief 获取转换后的温度和压力数据
*
* @param temperature 传出，温度
* @param pressure 传出，压力
* @return uint8_t
*/
uint8_t bmp280_read_data(float *temperature, float *pressure) {
uint8_t data[6];
int32_t adc_P, adc_T;
// printf("==========debug1===========\n");
// 一次性读取6个字节的温度和气压原始数据
if (bmp280_read_regs(BMP280_REG_PRESS_MSB, data, 6) == 0) {
printf("bmp280:read data error\n");
return 1; // 读取失败
}
// printf("==========debug2===========\n");
// 组合原始数据 (20位)
adc_P = (int32_t)((((uint32_t)(data[0])) << 12) | (((uint32_t)(data[1])) << 4) | (((uint32_t)(data[2])) >> 4));
adc_T = (int32_t)((((uint32_t)(data[3])) << 12) | (((uint32_t)(data[4])) << 4) | (((uint32_t)(data[5])) >> 4));
// 如果没有数据，直接返回错误 (ADC读数为0x80000是未测量状态)
if (adc_T == 0x80000 || adc_P == 0x80000) {
*temperature = 0.0f;
*pressure = 0.0f;
printf("bmp280:no data\n");
return 1;
}
// printf("==========debug3===========\n");
// 进行补偿计算
*temperature = compensate_temperature(adc_T);
*pressure = compensate_pressure(adc_P);
return 0; // 成功
}
/**
* @brief 获取该气压计的原始adc数据
*
* @param adc_P 传出，气压
* @param adc_T 传出，温度
*/
void bmp280_read_originanl_data(int* adc_P, int* adc_T){
uint8_t data[6];
// 一次性读取6个字节的温度和气压原始数据
if (bmp280_read_regs(BMP280_REG_PRESS_MSB, data, 6) != 0) {
return; // 读取失败
}
// 组合原始数据 (20位)
adc_P = (int32_t)((((uint32_t)(data[0])) << 12) | (((uint32_t)(data[1])) << 4) | (((uint32_t)(data[2])) >> 4));
adc_T = (int32_t)((((uint32_t)(data[3])) << 12) | (((uint32_t)(data[4])) << 4) | (((uint32_t)(data[5])) >> 4));
}

54
apps/earphone/xtell_Sensor/sensor/BMP280.h
View File

@ -1,54 +0,0 @@
#ifndef BMP280_DRIVER_H
#define BMP280_DRIVER_H
#include <stdint.h>
#define BMP_PULL_UP 0 //外部是否接的上拉
// I2C 从设备地址
#if BMP_PULL_UP == 1 //外部接的高
#define BMP_IIC_7BIT_ADDRESS 0x76 //7位,外部接高为0x77
#define BMP_IIC_WRITE_ADDRESS (BMP_IIC_7BIT_ADDRESS<<1) //8位地址
#define BMP_IIC_READ_ADDRESS (BMP_IIC_WRITE_ADDRESS | 0x01)
#else
#define BMP_IIC_7BIT_ADDRESS 0x77 //7位,外部接低为0x76
#define BMP_IIC_WRITE_ADDRESS (BMP_IIC_7BIT_ADDRESS<<1) //8位地址
#define BMP_IIC_READ_ADDRESS (BMP_IIC_WRITE_ADDRESS | 0x01)
#endif
// BMP280 寄存器地址
#define BMP280_REG_CALIB_START 0x88
#define BMP280_REG_ID 0xD0
#define BMP280_REG_RESET 0xE0
#define BMP280_REG_STATUS 0xF3
#define BMP280_REG_CTRL_MEAS 0xF4
#define BMP280_REG_CONFIG 0xF5
#define BMP280_REG_PRESS_MSB 0xF7
/**
* @brief 初始化BMP280传感器
* @return 0: 成功, 1: 芯片ID错误, 2: 读取校准参数失败
* @note 此函数会完成ID检查、软复位、读取校准参数，并设置传感器为连续测量模式。
*/
uint8_t bmp280_init(void);
/**
* @brief 从BMP280读取温度和气压数据
* @param[out] temperature - 指向浮点数变量的指针，用于存储温度值 (单位: °C)
* @param[out] pressure - 指向浮点数变量的指针，用于存储气压值 (单位: Pa)
* @return 0: 成功, 1: 读取数据失败
*/
uint8_t bmp280_read_data(float *temperature, float *pressure);
/**
* @brief 获取该气压计的原始adc数据
*
* @param adc_P 传出，气压
* @param adc_T 传出，温度
*/
void bmp280_read_originanl_data(int* adc_P, int* adc_T);
#endif // BMP280_DRIVER_H

283
apps/earphone/xtell_Sensor/sensor/LIS2DH12.c
View File

@ -1,283 +0,0 @@
// LIS2DH12驱动 - 由Kilo Code注释
#include "gSensor/gSensor_manage.h"
#include "app_config.h"
#include "math.h"
#include "LIS2DH12.h"
#include "colorful_lights/colorful_lights.h"
#include <string.h> // 用于 memcpy
//////////////////////////////////////////////////////////////////////////////////////////////////
//START -- 宏定义
#define ENABLE_XLOG 1
#ifdef xlog
#undef xlog
#endif
#if ENABLE_XLOG
#define xlog(format, ...) printf("[%s] " format, __func__, ##__VA_ARGS__)
#else
#define xlog(format, ...) ((void)0)
#endif
// --- 运动检测核心参数 ---
#define SAMPLE_COUNT 6 // 定义静止状态检测所需的样本数量
#define THRESHOLD 50.00f // 定义静止状态检测的阈值（三轴数据方差），值越大，对微小抖动的容忍度越高
#define LPF_ALPHA 0.95f // 低通滤波系数，越接近1，滤波效果越强，重力估算越平滑
#define DEADZONE_MSS 0.2f // 加速度死区阈值 (m/s^2)，低于此值的线性加速度被视为噪声并忽略
// --- 原有业务逻辑宏定义 ---
#define STATIC_MAX_TIME 60*5*5 // 传感器静止最大时间，单位 200ms
#define DORMANCY_MAX_TIME 60*5 // 休眠检测时间，单位 200ms
// --- I2C地址定义 ---
#define LIS2DH12_W_ADDR 0x32
#define LIS2DH12_R_ADDR 0x33
// --- IIC 寄存器地址宏定义 ---
#define LIS2DH12_WHO_AM_I 0x01 //0F
#define LIS2DH12_CTRL_REG1 0x20
#define LIS2DH12_CTRL_REG4 0x23
#define LIS2DH12_CTRL_REG5 0x24
#define LIS2DH12_OUT_X_L 0x28
#define LIS2DH12_FIFO_CTRL_REG 0x2E
#define LIS2DH12_SRC_REG 0x2F
//END -- 宏定义
//////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////
//START -- 变量定义
u8 dormancy_flag = 0; // 休眠标识
u8 dormancy_ago_moedl = 0; // 记录休眠前灯效
u16 gsensor_static_flag; // 记录传感器静止的时间,单位 200ms
axis_info_t current_data[32]; // 用于存储从FIFO读取的原始传感器数据
//运动数据全局变量
static motion_data_t motion_data = {{0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}}; // 存储最终计算出的速度和距离
static axis_info_xtell gravity_vector = {0.0f, 0.0f, -GRAVITY_EARTH}; // 存储估算出的重力向量，初始假设Z轴朝下
static bool sensor_is_stable = false; // 传感器是否静止的标志
static axis_info_xtell linear_accel_global = {0.0f, 0.0f, 0.0f}; // 存储移除重力后的线性加速度，用于日志打印
static axis_info_xtell zero_g_offset = {0.0f, 0.0f, 0.0f}; // 存储开机校准测得的零点偏移量
u8 gsensor_alarm;
axis_info_xtell gsensor_xtell; // 存储is_sensor_stable计算出的平均值
//END -- 变量定义
//////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////
//START -- 函数定义
//END -- 函数定义
//////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////
//实现
// --- I2C底层函数封装 ---
static u32 SL_MEMS_i2cRead(u8 addr, u8 reg, u8 len, u8 *buf) {
return _gravity_sensor_get_ndata(addr, reg, buf, len);
}
static u8 SL_MEMS_i2cWrite(u8 addr, u8 reg, u8 data) {
gravity_sensor_command(addr, reg, data);
return 0;
}
// 检查传感器ID，确认设备是否正常连接
char LIS2DH12_Check() {
u8 reg_value = 0;
SL_MEMS_i2cRead(LIS2DH12_R_ADDR, LIS2DH12_WHO_AM_I, 1, &reg_value);
if (reg_value == 0x6A) { //0x33
return 0x01;
}
return 0x00;
}
// 从传感器FIFO读取一批原始数据
void LIS2DH12_read_data(axis_info_t *sl_accel) {
u8 fifo_src = 0;
u8 samples_available = 0;
u8 data[192];
s16 raw_x,raw_y,raw_z;
SL_MEMS_i2cRead(LIS2DH12_R_ADDR, LIS2DH12_SRC_REG, 1, &fifo_src);
samples_available = fifo_src & 0x1F;
if (samples_available == 0) return;
SL_MEMS_i2cRead(LIS2DH12_R_ADDR, LIS2DH12_OUT_X_L | 0x80, samples_available * 6, data);
for (u8 i = 0; i < samples_available; i++) {
// 数据处理方式与 +/-8g 普通模式(10位) 匹配
raw_x = (int16_t)((data[i * 6 + 1] << 8) | data[i * 6]) >> 6;
raw_y = (int16_t)((data[i * 6 + 3] << 8) | data[i * 6 + 2]) >> 6;
raw_z = (int16_t)((data[i * 6 + 5] << 8) | data[i * 6 + 4]) >> 6;
sl_accel[i].x = raw_x;
sl_accel[i].y = raw_y;
sl_accel[i].z = raw_z;
}
}
// 开机校准函数：测量传感器的静态零点偏移
void LIS2DH12_calibrate() {
xlog("开始传感器校准...\n");
axis_info_t cal_data[32];
long x_sum = 0, y_sum = 0;
const int num_samples = 32;
delay_2ms(100); // 等待约200ms，让FIFO填满数据
LIS2DH12_read_data(cal_data);
for (int i = 0; i < num_samples; i++) {
x_sum += cal_data[i].x;
y_sum += cal_data[i].y;
}
zero_g_offset.x = (float)x_sum / num_samples;
zero_g_offset.y = (float)y_sum / num_samples;
zero_g_offset.z = 0; // Z轴主要受重力影响，不进行校准
xlog("校准完成. X轴偏移: %.2f, Y轴偏移: %.2f\n", zero_g_offset.x, zero_g_offset.y);
}
// 初始化并配置LIS2DH12传感器
u8 LIS2DH12_Config(void) {
if (LIS2DH12_Check() != 1) {
xlog("LIS2DH12 I2C error\n");
return -1;
}
// 统一配置: 25Hz采样率, +/-8g量程, 普通模式(10位)
SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_CTRL_REG1, 0x37); // 25 Hz ODR
SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_CTRL_REG4, 0x20); // +/-8g, BDU enabled
SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_CTRL_REG5, 0x40); // 使能FIFO
SL_MEMS_i2cWrite(LIS2DH12_W_ADDR, LIS2DH12_FIFO_CTRL_REG, 0x80); // 流模式
// 执行开机校准
LIS2DH12_calibrate();
xlog("LIS2DH12 I2C success\n");
return 0;
}
// 判断传感器是否处于静止状态
bool is_sensor_stable(axis_info_t *accel_data, int sample_count) {
float mean_x = 0, mean_y = 0, mean_z = 0;
float variance_x = 0, variance_y = 0, variance_z = 0;
if (sample_count <= 1) return true;
// 1. 计算均值
for (int i = 0; i < sample_count; i++) {
mean_x += accel_data[i].x;
mean_y += accel_data[i].y;
mean_z += accel_data[i].z;
}
mean_x /= sample_count;
mean_y /= sample_count;
mean_z /= sample_count;
gsensor_xtell.x = mean_x;
gsensor_xtell.y = mean_y;
gsensor_xtell.z = mean_z;
// 2. 计算方差
for (int i = 0; i < sample_count; i++) {
variance_x += (accel_data[i].x - mean_x) * (accel_data[i].x - mean_x);
variance_y += (accel_data[i].y - mean_y) * (accel_data[i].y - mean_y);
variance_z += (accel_data[i].z - mean_z) * (accel_data[i].z - mean_z);
}
variance_x /= (sample_count - 1);
variance_y /= (sample_count - 1);
variance_z /= (sample_count - 1);
// 3. 如果方差大于阈值，则认为在运动
if (variance_x > THRESHOLD || variance_y > THRESHOLD || variance_z > THRESHOLD) {
return false;
}
return true;
}
// 获取当前的总加速度（包含重力），单位 m/s^2
axis_info_xtell get_current_accel_mss(void) {
axis_info_xtell accel_mss;
// 灵敏度 @ +/-8g 普通模式 (10-bit) = 12 mg/LSB
const float sensitivity_g_per_lsb = 0.012f;
// 在转换前，先减去校准测得的零点偏移
accel_mss.x = ((float)gsensor_xtell.x - zero_g_offset.x) * sensitivity_g_per_lsb * GRAVITY_EARTH;
accel_mss.y = ((float)gsensor_xtell.y - zero_g_offset.y) * sensitivity_g_per_lsb * GRAVITY_EARTH;
accel_mss.z = (float)gsensor_xtell.z * sensitivity_g_per_lsb * GRAVITY_EARTH;
return accel_mss;
}
// 获取计算好的运动数据（速度和距离）
void get_motion_data(motion_data_t *data) {
if (data) {
memcpy(data, &motion_data, sizeof(motion_data_t));
}
}
// 获取移除重力后的线性加速度
axis_info_xtell get_linear_accel_mss(void) {
return linear_accel_global;
}
// 核心计算任务，由定时器周期性调用
void xtell_i2c_test() {
// 1. 读取一批最新的传感器数据
LIS2DH12_read_data(current_data);
// 2. 判断传感器当前是否静止
sensor_is_stable = is_sensor_stable(current_data, SAMPLE_COUNT);
// 3. 获取校准和转换后的总加速度 (m/s^2)
axis_info_xtell current_accel_mss = get_current_accel_mss();
// 4. 使用低通滤波器估算重力向量
gravity_vector.x = LPF_ALPHA * gravity_vector.x + (1.0f - LPF_ALPHA) * current_accel_mss.x;
gravity_vector.y = LPF_ALPHA * gravity_vector.y + (1.0f - LPF_ALPHA) * current_accel_mss.y;
gravity_vector.z = LPF_ALPHA * gravity_vector.z + (1.0f - LPF_ALPHA) * current_accel_mss.z;
// 5. 从总加速度中减去重力，得到线性加速度
linear_accel_global.x = current_accel_mss.x - gravity_vector.x;
linear_accel_global.y = current_accel_mss.y - gravity_vector.y;
linear_accel_global.z = current_accel_mss.z - gravity_vector.z;
// 6. 应用死区：忽略过小的加速度值（噪声）
if (fabsf(linear_accel_global.x) < DEADZONE_MSS) linear_accel_global.x = 0.0f;
if (fabsf(linear_accel_global.y) < DEADZONE_MSS) linear_accel_global.y = 0.0f;
if (fabsf(linear_accel_global.z) < DEADZONE_MSS) linear_accel_global.z = 0.0f;
// 7. 积分线性加速度，得到速度
motion_data.velocity.x += linear_accel_global.x * SAMPLING_PERIOD_S;
motion_data.velocity.y += linear_accel_global.y * SAMPLING_PERIOD_S;
motion_data.velocity.z += linear_accel_global.z * SAMPLING_PERIOD_S;
// 8. 如果传感器静止，重置速度和距离以消除漂移
if (sensor_is_stable) {
motion_data.velocity.x = 0.0f;
motion_data.velocity.y = 0.0f;
motion_data.velocity.z = 0.0f;
motion_data.distance.x = 0.0f;
motion_data.distance.y = 0.0f;
motion_data.distance.z = 0.0f;
}
// 9. 积分速度，得到距离
motion_data.distance.x += motion_data.velocity.x * SAMPLING_PERIOD_S;
motion_data.distance.y += motion_data.velocity.y * SAMPLING_PERIOD_S;
motion_data.distance.z += motion_data.velocity.z * SAMPLING_PERIOD_S;
// 10. 计算并打印总的移动距离（可选，用于调试）
float total_distance_magnitude = sqrtf(motion_data.distance.x * motion_data.distance.x +
motion_data.distance.y * motion_data.distance.y +
motion_data.distance.z * motion_data.distance.z);
// xlog("Total distance traveled: %.2f m\n", total_distance_magnitude);
}

59
apps/earphone/xtell_Sensor/sensor/LIS2DH12.h
View File

@ -1,59 +0,0 @@
#ifndef LIS2DH12_H
#define LIS2DH12_H
#include "gSensor/gSensor_manage.h"
#include "le_rcsp_adv_module.h"
// --- 物理常量定义 ---
#define GRAVITY_EARTH 9.80665f // 地球重力加速度 (m/s^2)
#define SAMPLING_PERIOD_S 0.2f // 采样周期 (对应于200ms的定时器)
// --- 数据结构定义 ---
// 三轴数据结构体 (可用于加速度、速度、距离)
typedef struct {
float x;
float y;
float z;
} axis_info_xtell;
// 运动数据结构体，包含速度和距离
typedef struct {
axis_info_xtell velocity; // 速度 (m/s)
axis_info_xtell distance; // 距离 (m)
} motion_data_t;
// --- API 函数声明 ---
/**
* @brief 初始化并配置LIS2DH12传感器
* @return 0 表示成功, -1 表示失败
*/
unsigned char LIS2DH12_Config(void);
/**
* @brief 核心计算任务，应由定时器周期性调用
*/
void xtell_i2c_test(void);
// --- 数据获取函数声明 ---
/**
* @brief 获取计算好的运动数据（速度和距离）
* @param data 指向 motion_data_t 结构体的指针，用于存放结果
*/
void get_motion_data(motion_data_t *data);
/**
* @brief 获取当前的总加速度（包含重力），单位 m/s^2
* @return axis_info_xtell 包含x,y,z轴总加速度的结构体
*/
axis_info_xtell get_current_accel_mss(void);
/**
* @brief 获取当前移除重力后的线性加速度，单位 m/s^2
* @return axis_info_xtell 包含x,y,z轴线性加速度的结构体
*/
axis_info_xtell get_linear_accel_mss(void);
#endif

259
apps/earphone/xtell_Sensor/sensor/MMC56.c
View File

@ -1,259 +0,0 @@
#include "MMC56.h"
#include "math.h"
#include "os/os_api.h"
#include "../xtell.h"
#include "gSensor/gSensor_manage.h"
#include "printf.h"
#define CALIBRATION_TIME 20000 //校准持续时间 ms
#define SAMPLE_INTERVAL 100 //校准采样间隔
// 用于跟踪当前是否处于连续测量模式
static uint8_t g_continuous_mode_enabled = 0;
mmc5603nj_cal_data_t cal_data; //校准数据
static void mmc5603nj_write_reg(uint8_t reg, uint8_t data) {
gravity_sensor_command(MMC_IIC_WRITE_ADDRESS, reg, data);
}
static uint32_t mmc5603nj_read_regs(uint8_t reg, uint8_t *buf, uint8_t len) {
return _gravity_sensor_get_ndata(MMC_IIC_READ_ADDRESS, reg, buf, len);
}
// 外部接口函数实现
uint8_t mmc5603nj_get_pid(void) {
uint8_t pid = 0;
mmc5603nj_read_regs(MMC_PID, &pid, 1);
return pid;
}
int mmc5603nj_init(void) {
// ID
if ( mmc5603nj_get_pid() != 0x10) {
printf("MMC5603NJ init failed: wrong Product ID (read: 0x%X)\n", mmc5603nj_get_pid());
// return 0;
}
// 软件复位
mmc5603nj_write_reg(MMC_INCTRL1, 0x80); // SW_RESET bit
os_time_dly(20); // 等待复位完成
// 设置20位分辨率 (BW[1:0] = 11)
// 同时确保所有轴都使能 (X/Y/Z_inhibit = 0)
mmc5603nj_write_reg(MMC_INCTRL1, 0x03);
os_time_dly(1);
// 设置内部控制寄存器2
// CMM_EN = 1 (使能连续模式功能)
// HPOWER = 1 (高功耗模式，更稳定)
mmc5603nj_write_reg(MMC_INCTRL2, 0x90); // 0b10010000
// 设置自动SET/RESET功能
// AUTO_SR_EN = 1
mmc5603nj_write_reg(MMC_INCTRL0, 0x20); // 0b00100000
g_continuous_mode_enabled = 0;
printf("MMC5603NJ initialized successfully.\n");
mmc5603nj_enable_continuous_mode(0x04);
return 1;
}
void mmc5603nj_start_calibration(void){
printf("\n--- Magnetometer Calibration Start ---\n");
printf("Slowly rotate the device in all directions (like drawing a 3D '8')...\n");
printf("Calibration will last for 20 seconds.\n\n");
printf("will start after 5 seconds\n\n");
os_time_dly(500);
// 初始化最大最小值
// 使用一个临时变量来读取数据，避免干扰read函数的正常逻辑
mmc5603nj_mag_data_t temp_mag_data;
// 首次读取以获取初始值
mmc5603nj_read_mag_data(&temp_mag_data); // 首次读取不应用校准
float max_x = temp_mag_data.x;
float min_x = temp_mag_data.x;
float max_y = temp_mag_data.y;
float min_y = temp_mag_data.y;
float max_z = temp_mag_data.z;
float min_z = temp_mag_data.z;
uint32_t start_time = os_time_get(); // 假设os_time_get()返回毫秒级时间戳
int samples = 0;
int over = CALIBRATION_TIME/SAMPLE_INTERVAL;
while (samples <= over) {
// 读取原始磁力计数据
mmc5603nj_read_mag_data(&temp_mag_data);
// 更新最大最小值
if (temp_mag_data.x > max_x) max_x = temp_mag_data.x;
if (temp_mag_data.x < min_x) min_x = temp_mag_data.x;
if (temp_mag_data.y > max_y) max_y = temp_mag_data.y;
if (temp_mag_data.y < min_y) min_y = temp_mag_data.y;
if (temp_mag_data.z > max_z) max_z = temp_mag_data.z;
if (temp_mag_data.z < min_z) min_z = temp_mag_data.z;
samples++;
os_time_dly(SAMPLE_INTERVAL / 10);
}
// 检查数据范围是否合理，防止传感器未动或故障
if ((max_x - min_x < 0.1f) || (max_y - min_y < 0.1f) || (max_z - min_z < 0.1f)) {
printf("\n--- Calibration Failed ---\n");
printf("Device might not have been rotated enough.\n");
printf("X range: %.2f, Y range: %.2f, Z range: %.2f\n", max_x - min_x, max_y - min_y, max_z - min_z);
return;
}
// 计算硬磁偏移 (椭球中心)
cal_data.offset_x = (max_x + min_x) / 2.0f;
cal_data.offset_y = (max_y + min_y) / 2.0f;
cal_data.offset_z = (max_z + min_z) / 2.0f;
printf("\n--- Calibration Complete ---\n");
printf("Collected %d samples.\n", samples);
printf("Offsets (Gauss):\n");
printf(" X: %.4f\n", cal_data.offset_x);
printf(" Y: %.4f\n", cal_data.offset_y);
printf(" Z: %.4f\n", cal_data.offset_z);
printf("Please save these values and apply them in your code.\n\n");
}
void mmc5603nj_enable_continuous_mode(uint8_t rate) {
// 在连续模式下，ODR寄存器必须被设置
mmc5603nj_write_reg(MMC_ODR, rate); //要设置频率
// mmc5603nj_set_data_rate(0x04);
// 启用连续模式 (INCTRL2的CMM_EN位已在init中设置)
// 只需要设置 INCTRL0 的 CMM_FREQ_EN 位
mmc5603nj_write_reg(MMC_INCTRL0, 0xA0); // 0b10100000 (CMM_FREQ_EN=1, AUTO_SR_EN=1)
g_continuous_mode_enabled = 1;
}
void mmc5603nj_disable_continuous_mode(void) {
// 禁用连续模式
mmc5603nj_write_reg(MMC_INCTRL0, 0x20); // 恢复到仅使能 AUTO_SR_EN 的状态
g_continuous_mode_enabled = 0;
}
float mmc5603nj_get_temperature(void) {
uint8_t status = 0;
uint8_t temp_raw = 0;
uint8_t timeout = 20;
// 触发一次温度测量
mmc5603nj_write_reg(MMC_INCTRL0, 0x02); // TAKE_MEAS_T
// 等待测量完成
do {
os_time_dly(10);
mmc5603nj_read_regs(MMC_STATUS1, &status, 1);
timeout--;
} while ((status & 0x80) == 0 && timeout > 0);
if (timeout == 0) {
printf("Error: Temperature measurement timeout!\n");
return -273.15f; // 返回一个绝对零度的错误值
}
mmc5603nj_read_regs(MMC_TOUT, &temp_raw, 1);
return ((float)temp_raw * 0.8f) - 75.0f;
}
void mmc5603nj_read_mag_data(mmc5603nj_mag_data_t *mag_data) {
uint8_t buffer[9];
if (g_continuous_mode_enabled) {
// 连续模式下，只需检查数据是否就绪
uint8_t status = 0;
mmc5603nj_read_regs(MMC_STATUS1, &status, 1);
if ((status & 0x40) == 0) { // Meas_M_done bit
// 数据未就绪，可以选择返回或等待，这里我们直接返回旧数据
return;
}
} else {
// 单次测量模式
uint8_t status = 0;
uint8_t timeout = 20;
// 触发一次带自动SET/RESET的磁场测量
mmc5603nj_write_reg(MMC_INCTRL0, 0x21); // 0b00100001 (TAKE_MEAS_M=1, AUTO_SR_EN=1)
// 等待测量完成
do {
os_time_dly(10);
mmc5603nj_read_regs(MMC_STATUS1, &status, 1);
timeout--;
} while ((status & 0x40) == 0 && timeout > 0);
if (timeout == 0) {
printf("Error: Magnetic measurement timeout!\n");
mag_data->x = mag_data->y = mag_data->z = 0.0f;
return;
}
}
// 读取9个字节的原始数据
mmc5603nj_read_regs(MMC_XOUT0, buffer, 9);
// 解析数据 (20位分辨率)
int32_t raw_x = ((uint32_t)buffer[0] << 12) | ((uint32_t)buffer[1] << 4) | ((uint32_t)buffer[6] & 0x0F);
int32_t raw_y = ((uint32_t)buffer[2] << 12) | ((uint32_t)buffer[3] << 4) | ((uint32_t)buffer[6] >> 4);
int32_t raw_z = ((uint32_t)buffer[4] << 12) | ((uint32_t)buffer[5] << 4) | ((uint32_t)buffer[8] & 0x0F);
// 应用偏置和灵敏度进行转换
mag_data->x = ((float)raw_x - 524288.0f) / 16384.0f;
mag_data->y = ((float)raw_y - 524288.0f) / 16384.0f;
mag_data->z = ((float)raw_z - 524288.0f) / 16384.0f;
//减去偏移
mag_data->x -= cal_data.offset_x;
mag_data->y -= cal_data.offset_y;
mag_data->z -= cal_data.offset_z;
}
void mmc5603nj_read_origin_data(uint8_t *buffer) {
if (g_continuous_mode_enabled) {
// 连续模式下，只需检查数据是否就绪
uint8_t status = 0;
mmc5603nj_read_regs(MMC_STATUS1, &status, 1);
if ((status & 0x40) == 0) { // Meas_M_done bit
// 数据未就绪，可以选择返回或等待，这里我们直接返回旧数据
return;
}
} else {
// 单次测量模式
uint8_t status = 0;
uint8_t timeout = 20;
// 触发一次带自动SET/RESET的磁场测量
mmc5603nj_write_reg(MMC_INCTRL0, 0x21); // 0b00100001 (TAKE_MEAS_M=1, AUTO_SR_EN=1)
// 等待测量完成
do {
os_time_dly(10);
mmc5603nj_read_regs(MMC_STATUS1, &status, 1);
timeout--;
} while ((status & 0x40) == 0 && timeout > 0);
if (timeout == 0) {
printf("Error: Magnetic measurement timeout!\n");
return;
}
}
// 读取9个字节的原始数据
mmc5603nj_read_regs(MMC_XOUT0, buffer, 9);
}

103
apps/earphone/xtell_Sensor/sensor/MMC56.h
View File

@ -1,103 +0,0 @@
#ifndef MMC5603NJ_DRIVER_H
#define MMC5603NJ_DRIVER_H
#include <stdint.h>
//该芯片的iic地址是固定的, 没法通过外部上下拉来改变
#define BMP_IIC_7BIT_ADDRESS 0x30 //0110000 手册第12页
//8位地址:
#define MMC_IIC_WRITE_ADDRESS (BMP_IIC_7BIT_ADDRESS <<1) // 0x60 : 01100000
#define MMC_IIC_READ_ADDRESS (MMC_IIC_WRITE_ADDRESS | 0x01) // 0x61 : 01100001
// 寄存器地址定义 -- 数据手册第6页
#define MMC_XOUT0 0x00
#define MMC_XOUT1 0x01
#define MMC_YOUT0 0x02
#define MMC_YOUT1 0x03
#define MMC_ZOUT0 0x04
#define MMC_ZOUT1 0x05
#define MMC_XOUT2 0x06
#define MMC_YOUT2 0x07
#define MMC_ZOUT2 0x08
#define MMC_TOUT 0x09
#define MMC_STATUS1 0x18
#define MMC_ODR 0x1A
#define MMC_INCTRL0 0x1B
#define MMC_INCTRL1 0x1C
#define MMC_INCTRL2 0x1D
#define MMC_ST_X_TH 0x1E
#define MMC_ST_Y_TH 0x1F
#define MMC_ST_Z_TH 0x20
#define MMC_ST_X 0x27
#define MMC_ST_Y 0x28
#define MMC_ST_Z 0x29
#define MMC_PID 0x39
// 定义一个结构体来存放三轴磁场数据（原始数据）
typedef struct {
float x;
float y;
float z;
} mmc5603nj_original_data_t;
// 定义一个结构体来存放三轴磁场数据（单位：高斯 Gauss）
typedef struct {
float x;
float y;
float z;
} mmc5603nj_mag_data_t;
// 定义一个结构体来存放磁力计的硬磁偏移校准数据
typedef struct {
float offset_x;
float offset_y;
float offset_z;
} mmc5603nj_cal_data_t;
/**
* @brief 初始化MMC5603NJ传感器
* 该函数会对传感器进行软件复位，并检查设备ID。
* @return 0 表示成功, -1 表示失败 (设备ID不匹配).
*/
int mmc5603nj_init(void);
/**
* @brief 设置传感器的数据输出速率 (ODR - Output Data Rate)
* @param rate 速率值，具体含义请参考datasheet ODR寄存器说明。
*/
void mmc5603nj_set_data_rate(uint8_t rate);
/**
* @brief 启用连续测量模式
*/
void mmc5603nj_enable_continuous_mode(uint8_t rate);
/**
* @brief 禁用连续测量模式
*/
void mmc5603nj_disable_continuous_mode(void);
/**
* @brief 获取产品ID
* @return 产品的ID值，对于MMC5603NJ，应为0x10.
*/
uint8_t mmc5603nj_get_pid(void);
/**
* @brief 读取传感器的温度
* @return 温度值 (单位: 摄氏度 °C).
*/
float mmc5603nj_get_temperature(void);
/**
* @brief 读取三轴磁场数据
* 此函数会根据当前是连续模式还是单次模式来读取数据。
* @param mag_data 指向 mmc5603nj_mag_data_t 结构体的指针，用于存放结果。
*/
void mmc5603nj_read_mag_data(mmc5603nj_mag_data_t *mag_data);
#endif // MMC5603NJ_DRIVER_H

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

File diff suppressed because it is too large Load Diff

143
apps/earphone/xtell_Sensor/sensor/SC7U22.h
View File

@ -1,143 +0,0 @@
/**************************************************
Copyright (c) 2022 Silan MEMS. All Rights Reserved.
@Silan MEMS Sensor Product Line
@Code Author:Zhou Min
**************************************************/
#ifndef __SCU722_H__
#define __SCU722_H__
#include "gSensor/gSensor_manage.h"
#include "printf.h"
#include "MMC56.h"
//是否使能串口打印调试
#define SL_Sensor_Algo_Release_Enable 0x00
//是否开启FIFO模式，默认STREAM模式
#define SL_SC7U22_FIFO_ENABLE 0x00
/***使用前请根据实际情况配置以下参数******/
/**SC7U22的SDO 接地: 0****************/
/**SC7U22的SDO 接电源:1****************/
#define SL_SC7U22_SDO_VDD_GND 0
/*****************************************/
/***使用前请根据实际IIC地址配置参数***/
/**SC7U22的IIC 接口地址为 7bits: 0****/
/**SC7U22的IIC 接口地址为 8bits: 1****/
#define SL_SC7U22_IIC_7BITS_8BITS 1
/*****************************************/
#if SL_SC7U22_SDO_VDD_GND==0
#define SL_SC7U22_IIC_7BITS_ADDR 0x18
#define SL_SC7U22_IIC_8BITS_WRITE_ADDR 0x30
#define SL_SC7U22_IIC_8BITS_READ_ADDR 0x31
#else
#define SL_SC7U22_IIC_7BITS_ADDR 0x19
#define SL_SC7U22_IIC_8BITS_WRITE_ADDR 0x32
#define SL_SC7U22_IIC_8BITS_READ_ADDR 0x33
#endif
#if SL_SC7U22_IIC_7BITS_8BITS==0
#define SL_SC7U22_IIC_ADDRESS SL_SC7U22_IIC_7BITS_ADDR
#else
#define SL_SC7U22_IIC_WRITE_ADDRESS SL_SC7U22_IIC_8BITS_WRITE_ADDR
#define SL_SC7U22_IIC_READ_ADDRESS SL_SC7U22_IIC_8BITS_READ_ADDR
#endif
unsigned char SL_SC7U22_I2c_Spi_Write(unsigned char sl_spi_iic, unsigned char reg, unsigned char dat);
unsigned char SL_SC7U22_I2c_Spi_Read(unsigned char sl_spi_iic, unsigned char reg, unsigned short len, unsigned char* buf);
/*************I2C通信检测函数******************/
unsigned char SL_SC7U22_Check(void);
/*************函数返回值*****************/
/**return : 1 IIC通信正常,IC正常**************/
/**return : 0 IIC通信异常,IC异常**********/
/*************传感器初始化函数*******************/
unsigned char SL_SC7U22_Config(void);
/*************函数返回值*****************/
/**return : 1 IIC通信正常,IC正常*************/
/**return : 0; IIC通信异常,IC异常*********/
/*************SC7U22 Sensor Time**************/
unsigned int SL_SC7U22_TimeStamp_Read(void);
/*************函数返回值*****************/
/**return : 内部传感器时间***************/
#if SL_SC7U22_FIFO_ENABLE ==0x00
/******实时读取数据寄存器数据，相当于从400Hz的FIFO中取出数据******/
void SL_SC7U22_RawData_Read(signed short* acc_data_buf, signed short* gyr_data_buf);
/************* 输入XYZ三轴数据存放的地址*****************/
/************* *acc_data_buf: ACC数据***********************/
/************* *gyr_data_buf: GYR数据***********************/
#else
/******实时读取数据寄存器FIFO数据******/
unsigned short SL_SC7U22_FIFO_Read(signed short* accx_buf, signed short* accy_buf, signed short* accz_buf, signed short* gyrx_buf, signed short* gyry_buf, signed short* gyrz_buf);
/*************输入XYZ三轴数据首地址**************************/
/*************accx_buf[0]: ACC_X的第一个数据**************/
/*************accy_buf[0]: ACC_Y的第一个数据**************/
/*************accz_buf[0]: ACC_Z的第一个数据**************/
/*************gyrx_buf[0]: GYR_X的第一个数据**************/
/*************gyry_buf[0]: GYR_Y的第一个数据**************/
/*************gyrz_buf[0]: GYR_Z的第一个数据**************/
/****************函数返回值****************************/
/**return : len 表示数组长度*************************/
#endif
/*********进入传感器关闭模式*************/
unsigned char SL_SC7U22_POWER_DOWN(void);
/**0: 关闭模式失败***********************/
/**1: 关闭模式成功***********************/
/*********SC7U22 RESET***************/
unsigned char SL_SC7U22_SOFT_RESET(void);
/**0: 成功*****************************/
/**1: 失败**************************/
/*************GSensor and GyroSensor开启和关闭函数*********/
unsigned char SL_SC7U22_Open_Close_SET(unsigned char acc_enable,unsigned char gyro_enable);
/**acc_enable: 0=关闭ACC Sensor; 1=开启ACC Sensor*********/
/**gyro_enable: 0=关闭GYRO Sensor; 1=开启GYRO Sensor*******/
/**return: 0=设置失败，1=设置成功**************************/
/*********进入睡眠模式并开启中断函数*************/
unsigned char SL_SC7U22_IN_SLEEP_SET(unsigned char acc_odr,unsigned char vth,unsigned char tth,unsigned char int_io);
/**acc_odr: 12/25/50**************************************/
/**vth: 运动检测，阈值参数****************************/
/**tth: 运动检测，持续时间阈值，小于该时间则过滤**********/
/**int_io: 1=INT1, 2=INT2*********************************/
/**return: 0=设置失败，1=设置成功**************************/
/*********进入唤醒模式，设置参数并关闭中断函数***********/
unsigned char SL_SC7U22_WakeUp_SET(unsigned char odr_mode,unsigned char acc_range,unsigned char acc_hp_en,unsigned short gyro_range,unsigned char gyro_hp_en);
/**odr_mode: 25HZ/50Hz/100Hz/200Hz ACC+GYRO***************/
/**acc_range: ±2G/±4G/±8G/±16G*****************************/
/**acc_hp_en: 0=关闭高性能模式;1=开启*****/
/**gyro_range: ±125dps/±250dps/±500dps/±1000dps/±2000dps***/
/**gyro_hp_en: 0=关闭高性能模式;1=开启高性能模式; ********/
/**return: 0=设置失败，1=设置成功**************************/
/*********SC7U22 Angle Cauculate***************/
unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en,signed short *acc_gyro_input,float *Angle_output, unsigned char yaw_rst);
/**in calibration_en: 1=enable 0=disable***********************/
/**in/out acc_gyro_input[0]: ACC-X*****************************/
/**in/out acc_gyro_input[1]: ACC-Y*****************************/
/**in/out acc_gyro_input[2]: ACC-Z*****************************/
/**in/out acc_gyro_input[3]: GYR-X*****************************/
/**in/out acc_gyro_input[4]: GYR-Y*****************************/
/**in/out acc_gyro_input[5]: GYR-Z*****************************/
/**output Angle_output[0]: Pitch*****************************/
/**output Angle_output[1]: Roll******************************/
/**output Angle_output[2]: Yaw*******************************/
/**input yaw_rst: reset yaw value***************************/
void set_SC7U22_Error_Flag(char flag);
unsigned char Original_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst);
unsigned char SIX_SL_SC7U22_Angle_Output(unsigned char auto_calib_start, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst);
unsigned char Q_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, const mmc5603nj_mag_data_t *mag_data_input, unsigned char yaw_rst, float *quaternion_output);
unsigned char get_calibration_state(void);
/**寄存器宏定义*******************************/
#define SC7U22_WHO_AM_I 0x01
#endif // __SCU722_H__

927
apps/earphone/xtell_Sensor/sensor/SC7U22_Ma.c
View File

@ -1,927 +0,0 @@
#include "SC7U22.h"
#include "math.h"
#include "os/os_api.h"
#if SL_Sensor_Algo_Release_Enable==0x00
#include "printf.h"
#endif
//I2C SPI选择
//#define SL_SC7U22_SPI_EN_I2C_DISABLE 0x00 //需要配合SL_SPI_IIC_INTERFACE使用
#define SL_SPI_IIC_INTERFACE 0x01 //需要配合SL_SC7A22H_SPI_EN_I2C_DISABLE 使用
//是否使能原始数据高通滤波
#define SL_SC7U22_RAWDATA_HPF_ENABLE 0x00
//中断默认电平
#define SL_SC7U22_INT_DEFAULT_LEVEL 0x01
//SDO 是否上拉
#define SL_SC7U22_SDO_PullUP_ENABLE 0x01
//AOI中断是否唤醒
#define SL_SC7U22_AOI_Wake_Up_ENABLE 0x00
//FIFO_STREAM模式//FIFO_WTM模式
//#define SL_SC7U22_FIFO_STREAM_WTM 0x01//0X00=STREAM MODE 0X01=FIFO MODE
#define SL_SC7U22_IIC_DELAY_US 5
static u32 SL_MEMS_i2cRead(u8 addr, u8 reg, u8 len, u8 *buf) {
return _gravity_sensor_get_ndata(addr, reg, buf, len);
}
static u8 SL_MEMS_i2cWrite(u8 addr, u8 reg, u8 data) {
gravity_sensor_command(addr, reg, data);
return 0;
}
unsigned char SL_SC7U22_I2c_Spi_Write(unsigned char sl_spi_iic, unsigned char reg, unsigned char dat)
{
if (sl_spi_iic == 1) {
SL_MEMS_i2cWrite(SL_SC7U22_IIC_8BITS_WRITE_ADDR, reg, dat);
return 0;
}
// SPI not implemented
return 1; // 失败
}
unsigned char SL_SC7U22_I2c_Spi_Read(unsigned char sl_spi_iic, unsigned char reg, unsigned short len, unsigned char* buf)
{
if (sl_spi_iic == 1) {
return SL_MEMS_i2cRead(SL_SC7U22_IIC_8BITS_READ_ADDR, reg, len, buf);
}
// SPI not implemented
return 0; // 失败
}
static void sl_delay(unsigned char sl_i)
{
os_time_dly(sl_i);
}
unsigned char SL_SC7U22_Check(void)
{
unsigned char reg_value=0;
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, SC7U22_WHO_AM_I, 1, &reg_value);
#if SL_Sensor_Algo_Release_Enable==0x00
printf("0x%x=0x%x\r\n",SC7U22_WHO_AM_I,reg_value);
#endif
if(reg_value==0x6A)
return 0x01;//SC7U22
else
return 0x00;//通信异常
}
unsigned char SL_SC7U22_Config(void)
{
unsigned char Check_Flag=0;
unsigned char reg_value=0;
#if SL_SPI_IIC_INTERFACE==0x00 //SPI
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x90
sl_delay(1);
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x4A, 0x66);
sl_delay(1);
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x83);//goto 0x6F
sl_delay(1);
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x6F, 0x04);//I2C disable
sl_delay(1);
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x6F
sl_delay(1);
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x4A, 0x00);
sl_delay(1);
#endif
Check_Flag=SL_SC7U22_Check();
// Check_Flag= SL_SC7U22_SOFT_RESET();
// Check_Flag=1;//强制初始化
#if SL_Sensor_Algo_Release_Enable==0x00
printf("SL_SC7U22_Check=0x%x\r\n",Check_Flag);
#endif
if(Check_Flag==1)
{
Check_Flag= SL_SC7U22_POWER_DOWN();
}
#if SL_Sensor_Algo_Release_Enable==0x00
printf("SL_SC7U22_POWER_DOWN=0x%x\r\n",Check_Flag);
#endif
if(Check_Flag==1)
{
Check_Flag= SL_SC7U22_SOFT_RESET();
}
#if SL_Sensor_Algo_Release_Enable==0x00
printf("SL_SC7U22_SOFT_RESET=0x%x\r\n",Check_Flag);
#endif
if(Check_Flag==1)
{
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00
os_time_dly(1);//10ms
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, 0x0E);//PWR_CTRL ENABLE ACC+GYR+TEMP
os_time_dly(1);//10ms
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0x08);//ACC_CONF 0x08=100Hz
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x41, 0x01);//ACC_RANGE ±4G
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0x88);//GYR_CONF 0x88=100Hz
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, 0x00);//GYR_RANGE 2000dps
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x04, 0x50);//COM_CFG
#if SL_SC7U22_RAWDATA_HPF_ENABLE ==0x01
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x83);//goto 0x83
sl_delay(1);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x26, 1, &reg_value);
reg_value=reg_value|0xA0;
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x26, reg_value);//HPF_CFG rawdata hpf
#endif
#if SL_SC7U22_AOI_Wake_Up_ENABLE==0x01
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x30, 0x2A);//XYZ-ENABLE
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x32, 0x01);//VTH
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x33, 0x01);//TTH
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x3F, 0x30);//HPF FOR AOI1&AOI2
#endif
#if SL_SC7U22_FIFO_ENABLE==0x01
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1E,0x1D);//
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1D,0x00);//
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1D,0x20);//
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1C,0x37);//
#endif
#if SL_SC7U22_SDO_PullUP_ENABLE ==0x01
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x8C);//goto 0x8C
sl_delay(1);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x30, 1, &reg_value);
reg_value=reg_value&0xFE;//CS PullUP_enable
reg_value=reg_value&0xFD;//SDO PullUP_enable
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x30, reg_value);
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x00);//goto 0x00
os_time_dly(1);
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x00);//goto 0x00
os_time_dly(1);
#else
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x8C);//goto 0x8C
sl_delay(1);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x30, 1, &reg_value);
reg_value=reg_value&0xFE;//CS PullUP_enable
reg_value=reg_value|0x02;//SDO PullUP_disable
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x30, reg_value);
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x00);//goto 0x00
sl_delay(1);
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x00);//goto 0x00
sl_delay(1);
#endif
return 1;
}
else
return 0;
}
//读取时间戳
unsigned int SL_SC7U22_TimeStamp_Read(void)
{
unsigned char time_data[3];
unsigned int time_stamp;
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x18, 1, &time_data[0]);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x19, 1, &time_data[1]);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x20, 1, &time_data[2]);
time_stamp=(unsigned int)(time_data[0]<<16|time_data[1]<<8|time_data[2]);
return time_stamp;
}
#if SL_SC7U22_FIFO_ENABLE ==0x00
//100Hz 10ms read once
void SL_SC7U22_RawData_Read(signed short * acc_data_buf,signed short * gyr_data_buf)
{
unsigned char raw_data[12];
unsigned char drdy_satus=0x00;
unsigned short drdy_cnt=0;
while((drdy_satus&0x03)!=0x03)//acc+gyro
// while((drdy_satus&0x01)!=0x01)//acc
{
drdy_satus=0x00;
sl_delay(1);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x0B, 1, &drdy_satus);
drdy_cnt++;
if(drdy_cnt>30000) break;
}
#if SL_Sensor_Algo_Release_Enable==0x00
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x30, 1, &drdy_satus);
// printf("RawData:0x40=%x\r\n",drdy_satus);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x40, 1, &drdy_satus);
// printf("RawData:0x40=%x\r\n",drdy_satus);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x06, 1, &drdy_satus);
// printf("RawData:0x06=%x\r\n",drdy_satus);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x07, 1, &drdy_satus);
// printf("RawData:0x07=%x\r\n",drdy_satus);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x7D, 1, &drdy_satus);
// printf("RawData:0x7D=%x\r\n",drdy_satus);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x31, 1, &drdy_satus);
// printf("RawData:0x31=%x\r\n",drdy_satus);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x02, 1, &drdy_satus);
// printf("RawData:0x02=%x\r\n",drdy_satus);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x03, 1, &drdy_satus);
// printf("RawData:0x03=%x\r\n",drdy_satus);
#endif
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x0C, 12, &raw_data[0]);
acc_data_buf[0] =(signed short)((((unsigned char)raw_data[0])* 256) + ((unsigned char)raw_data[1]));//ACCX-16位
acc_data_buf[1] =(signed short)((((unsigned char)raw_data[2])* 256) + ((unsigned char)raw_data[3]));//ACCY-16位
acc_data_buf[2] =(signed short)((((unsigned char)raw_data[4])* 256) + ((unsigned char)raw_data[5]));//ACCZ-16位
gyr_data_buf[0] =(signed short)((((unsigned char)raw_data[6])* 256) + ((unsigned char)raw_data[7]));//GYRX-16位
gyr_data_buf[1] =(signed short)((((unsigned char)raw_data[8])* 256) + ((unsigned char)raw_data[9]));//GYRY-16位
gyr_data_buf[2] =(signed short)((((unsigned char)raw_data[10])* 256) + ((unsigned char)raw_data[11]));//GYRZ-16位
#if SL_Sensor_Algo_Release_Enable==0x00
printf("RawData:AX=%d,AY=%d,AZ=%d,GX=%d,GY=%d,GZ=%d\r\n",acc_data_buf[0],acc_data_buf[1],acc_data_buf[2],gyr_data_buf[0],gyr_data_buf[1],gyr_data_buf[2]);
#endif
}
#else
#if SL_Sensor_Algo_Release_Enable==0x00
#define SL_SC7U22_WAIT_FIFO_LEN_ENABLE 0x00//0x01
#else
#define SL_SC7U22_WAIT_FIFO_LEN_ENABLE 0x00
#endif
unsigned char Acc_FIFO_Num;
unsigned char Gyr_FIFO_Num;
unsigned char SL_SC7U22_FIFO_DATA[1024];
unsigned short SL_SC7U22_FIFO_Read(signed short *accx_buf,signed short *accy_buf,signed short *accz_buf,signed short *gyrx_buf,signed short *gyry_buf,signed short *gyrz_buf)
{
int16_t Acc_x = 0, Acc_y = 0, Acc_z = 0;
int16_t Gyr_x = 0, Gyr_y = 0, Gyr_z = 0;
unsigned char fifo_num1=0;
unsigned char fifo_num2=0;
unsigned short fifo_num=0;
unsigned short fifo_len=0;
unsigned short temp = 0;
unsigned short i = 0 ;
unsigned char header[2];
unsigned short j;
#if SL_Sensor_Algo_Release_Enable==0x00 //user can set to zero
#if SL_SC7U22_WAIT_FIFO_LEN_ENABLE==0x00
while((fifo_num1&0x20)!=0x20)
{
sl_delay(200);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x1F,1,&fifo_num1);
}
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x1F,1,&fifo_num1);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x20,1,&fifo_num2);
if((fifo_num1&0x10)==0x10)
{
fifo_num=2048;
}
else
{
fifo_num=(fifo_num1&0x0F)*256+fifo_num2;
}
#else
while(fifo_num2<194)//32
{
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x1F,1,&fifo_num1);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x20,1,&fifo_num2);
sl_delay(20);
fifo_wait++;
if(fifo_wait>30000) break;
}
fifo_wait=0;
fifo_num=fifo_num2;
#endif
#else
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x1F,1,&fifo_num1);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x20,1,&fifo_num2);
if((fifo_num1&0x10)==0x10)
{
fifo_num=2048;
}
else
{
fifo_num=(fifo_num1&0x0F)*256+fifo_num2;
}
#endif
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x21, fifo_num*2, SL_SC7U22_FIFO_DATA);//读取FIFO数据 BYTE NUM
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1D, 0x00);//BY PASS MODE
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1D, 0x20);//Stream MODE
printf("SC7U22_FIFO_NUM1:%d\n",fifo_num);
#if SL_Sensor_Algo_Release_Enable==0x00
// printf("0x1F:0x%x 0x20:0x%x\n",fifo_num1,fifo_num2);
// printf("SC7U22_FIFO_NUM1:%d\n",fifo_num);
#endif
fifo_len=0;
i = 0;
Acc_FIFO_Num=0;
Gyr_FIFO_Num=0;
while(i < fifo_num*2)
{
//header process 1
header[0] = SL_SC7U22_FIFO_DATA[i + 0];
header[1] = SL_SC7U22_FIFO_DATA[i + 1];
i = i + 2;
//timestamp process 2
if(header[1] & 0x80)
{
i = i + 4;//every frame include the timestamp, 4 bytes
}
//acc process 3
if(header[0] & 0x04)
{
accx_buf[Acc_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 0] * 256 + SL_SC7U22_FIFO_DATA[i + 1])) ;
accy_buf[Acc_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 2] * 256 + SL_SC7U22_FIFO_DATA[i + 3])) ;
accz_buf[Acc_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 4] * 256 + SL_SC7U22_FIFO_DATA[i + 5])) ;
printf("AccNum : %d ,Acc_x : %4d, Acc_y : %4d, Acc_z : %4d,\r\n",Acc_FIFO_Num, accx_buf[Acc_FIFO_Num], accy_buf[Acc_FIFO_Num], accz_buf[Acc_FIFO_Num]);
i = i + 6;
Acc_FIFO_Num++;
}
//gyro process 3
if(header[0] & 0x02)
{
gyrx_buf[Gyr_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 0] * 256 + SL_SC7U22_FIFO_DATA[i + 1])) ;
gyry_buf[Gyr_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 2] * 256 + SL_SC7U22_FIFO_DATA[i + 3])) ;
gyrz_buf[Gyr_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 4] * 256 + SL_SC7U22_FIFO_DATA[i + 5])) ;
printf("GyrNum : %d, Gyr_x : %4d, Gyr_y : %4d, Gyr_z : %4d,\r\n",Gyr_FIFO_Num, gyrx_buf[Gyr_FIFO_Num], gyry_buf[Gyr_FIFO_Num], gyrz_buf[Gyr_FIFO_Num]);
i = i + 6;
Gyr_FIFO_Num++;
}
//temperature process 1
if(header[0] & 0x01)
{
i = i + 2;
}
}
return fifo_len;
}
#endif
unsigned char SL_SC7U22_POWER_DOWN(void)
{
unsigned char SL_Read_Reg = 0xff;
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00
sl_delay(20);
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, 0x00);//POWER DOWN
sl_delay(200);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x7D, 1,&SL_Read_Reg);
if(SL_Read_Reg==0x00) return 1;
else return 0;
}
unsigned char SL_SC7U22_SOFT_RESET(void)
{
unsigned char SL_Read_Reg = 0xff;
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00
os_time_dly(1);
#if SL_Sensor_Algo_Release_Enable==0x00
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x04, 1,&SL_Read_Reg);
printf("SL_SC7U22_SOFT_RESET1 0x04=0x%x\r\n",SL_Read_Reg);
SL_Read_Reg = 0xff;
#endif
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x04, 0x10);//BOOT
#if SL_Sensor_Algo_Release_Enable==0x00
#endif
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x4A, 0xA5);//SOFT_RESET
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x4A, 0xA5);//SOFT_RESET
os_time_dly(20);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x04, 1,&SL_Read_Reg);
#if SL_Sensor_Algo_Release_Enable==0x00
printf("SL_SC7U22_SOFT_RESET2 0x08=0x%x\r\n",SL_Read_Reg);
#endif
if(SL_Read_Reg==0x50) return 1;
else return 0;
}
/****acc_enable ==0 close acc;acc_enable ==1 open acc******/
/****gyro_enable==0 close acc;gyro_enable==1 open acc******/
unsigned char SL_SC7U22_Open_Close_SET(unsigned char acc_enable,unsigned char gyro_enable)
{
unsigned char SL_Read_Reg = 0xff;
unsigned char SL_Read_Check= 0xff;
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00
sl_delay(1);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x7D, 1,&SL_Read_Reg);
if(acc_enable==0)
{
SL_Read_Reg=SL_Read_Reg&0xFB;//Bit.ACC_EN=0
}
else if(acc_enable==1)
{
SL_Read_Reg=SL_Read_Reg|0x04;//Bit.ACC_EN=1
}
if(gyro_enable==0)
{
SL_Read_Reg=SL_Read_Reg&0xFD;//Bit.GYR_EN=0
}
else if(gyro_enable==1)
{
SL_Read_Reg=SL_Read_Reg|0x02;//Bit.GYR_EN=1
}
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, SL_Read_Reg);//PWR_CTRL ENABLE ACC+GYR+TEMP
sl_delay(5);//5ms
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, SL_Read_Reg);//PWR_CTRL ENABLE ACC+GYR+TEMP
sl_delay(20);//10ms
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x7D, 1,&SL_Read_Check);
if(SL_Read_Reg!=SL_Read_Check)
{
#if SL_Sensor_Algo_Release_Enable==0x00
printf("SL_Read_Reg=0x%x SL_Read_Check=0x%x\r\n",SL_Read_Reg,SL_Read_Check);
#endif
return 0;
}
return 1;
}
/*******开启中断******/
unsigned char SL_SC7U22_IN_SLEEP_SET(unsigned char acc_odr,unsigned char vth,unsigned char tth,unsigned char int_io)
{
unsigned char SL_Read_Reg = 0xff;
unsigned char SL_Acc_Odr_Reg = 0xff;
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00
sl_delay(1);
if(int_io==1)
{
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x06, 0x02);//AOI1-INT1
}
else if(int_io==2)
{
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x08, 0x02);//AOI1-INT2
}
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x04, 1, &SL_Read_Reg);
#if SL_SC7U22_INT_DEFAULT_LEVEL ==0x01
SL_Read_Reg=SL_Read_Reg|0x04;
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x04, SL_Read_Reg);//defalut high level&& push-pull
#else
reg_value=reg_value&0xDF;
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x06, SL_Read_Reg);//defalut low level&& push-pull
#endif
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x30, 0x2A);//AIO1-Enable
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x32, vth);//VTH
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x33, tth);//TTH
if(acc_odr==12)
{
SL_Acc_Odr_Reg=0x05;
}
else if(acc_odr==25)
{
SL_Acc_Odr_Reg=0x06;
}
else if(acc_odr==50)
{
SL_Acc_Odr_Reg=0x07;
}
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, SL_Acc_Odr_Reg);//ACC_CONF
os_time_dly(1);//5ms
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, 0x04);//acc open and gyro close
os_time_dly(1);//5ms
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, 0x04);//acc open and gyro close
sl_delay(200);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x7D, 1,&SL_Read_Reg);
if(SL_Read_Reg!=0x04)
{
#if SL_Sensor_Algo_Release_Enable==0x00
printf("SL_Read_Reg=0x%x 0x04\r\n",SL_Read_Reg);
#endif
return 0;
}
return 1;
}
/*******ODR SET:25 50 100 200******************/
/*******acc range:2 4 8 16*********************/
/*******gyro range:125 250 500 1000 2000*******/
/*******acc_hp_en: 0=disable 1=enable**********/
/*******gyro_hp_en:0=disable 1=enable**********/
unsigned char SL_SC7U22_WakeUp_SET(unsigned char odr_mode,unsigned char acc_range,unsigned char acc_hp_en,unsigned short gyro_range,unsigned char gyro_hp_en)
{
unsigned char SL_Odr_Reg = 0x00;
unsigned char SL_acc_mode_Reg = 0x00;
unsigned char SL_gyro_mode_Reg = 0x00;
unsigned char SL_acc_range_Reg = 0x00;
unsigned char SL_gyro_range_Reg = 0x00;
unsigned char SL_Read_Check = 0xff;
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00
sl_delay(1);
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, 0x06);//PWR_CTRL ENABLE ACC+GYR
sl_delay(5);
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, 0x06);//PWR_CTRL ENABLE ACC+GYR
sl_delay(200);
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x30, 0x00);//AIO1-disable
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x32, 0xff);//vth
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x33, 0xff);//tth
if(odr_mode==25)
{
SL_Odr_Reg=0x06;
}
else if(odr_mode==50)
{
SL_Odr_Reg=0x07;
}
else if(odr_mode==100)
{
SL_Odr_Reg=0x08;
}
else if(odr_mode==200)
{
SL_Odr_Reg=0x09;
}
if(acc_hp_en==1)
SL_acc_mode_Reg=0x80;
SL_acc_mode_Reg=SL_acc_mode_Reg|SL_Odr_Reg;
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, SL_acc_mode_Reg);//ACC_CONF
if(gyro_hp_en==1)
SL_gyro_mode_Reg=0x40;
else if(gyro_hp_en==2)
SL_gyro_mode_Reg=0x80;
else if(gyro_hp_en==3)
SL_gyro_mode_Reg=0xC0;
SL_gyro_mode_Reg=SL_gyro_mode_Reg|SL_Odr_Reg;
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, SL_gyro_mode_Reg);//GYR_CONF
if(acc_range==2)
{
SL_acc_range_Reg=0x00;
}
else if(acc_range==4)
{
SL_acc_range_Reg=0x01;
}
else if(acc_range==8)
{
SL_acc_range_Reg=0x02;
}
else if(acc_range==16)
{
SL_acc_range_Reg=0x03;
}
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x41, SL_acc_range_Reg);//ACC_RANGE
if(gyro_range==2000)
{
SL_gyro_range_Reg=0x00;
}
else if(gyro_range==1000)
{
SL_gyro_range_Reg=0x01;
}
else if(gyro_range==500)
{
SL_gyro_range_Reg=0x02;
}
else if(gyro_range==250)
{
SL_gyro_range_Reg=0x03;
}
else if(gyro_range==125)
{
SL_gyro_range_Reg=0x04;
}
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, SL_gyro_range_Reg);//GYR_RANGE 2000dps
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, SL_gyro_range_Reg);//GYR_RANGE 2000dps
#if SL_Sensor_Algo_Release_Enable==0x00
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x40, 1, &SL_Read_Check);
// printf("RawData:0x40=%x\r\n",SL_Read_Check);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x41, 1, &SL_Read_Check);
// printf("RawData:0x41=%x\r\n",SL_Read_Check);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x42, 1, &SL_Read_Check);
// printf("RawData:0x42=%x\r\n",SL_Read_Check);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x43, 1, &SL_Read_Check);
// printf("RawData:0x43=%x\r\n",SL_Read_Check);
#endif
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x43, 1,&SL_Read_Check);
if(SL_Read_Check!=SL_gyro_range_Reg)
{
#if SL_Sensor_Algo_Release_Enable==0x00
printf("SL_Read_Check=0x%x SL_gyro_range_Reg=0x%x\r\n",SL_Read_Check,SL_gyro_range_Reg);
#endif
return 0;
}
return 1;
}
#if SL_SC7U22_FIFO_ENABLE ==0x00
// =================================================================================================
// Madgwick AHRS 滤波器相关变量和函数
// -------------------------------------------------------------------------------------------------
// 定义常量
#define sampleFreq 100.0f // 传感器采样频率 (Hz)，必须与实际的传感器数据更新频率一致
#define betaDef 0.1f // 算法的比例增益 beta，影响加速度计修正陀螺仪的权重
// 全局变量
static volatile float beta = betaDef; // 算法增益 beta
static volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // 表示姿态的四元数 (w, x, y, z)
/**
* @brief 快速计算 1/sqrt(x)
* @param x 输入的浮点数
* @return 1/sqrt(x) 的近似值
*/
static float invSqrt(float x) {
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y)); // 牛顿迭代法，提高精度
return y;
}
/**
* @brief Madgwick AHRS 姿态更新函数 (IMU版本)
* @details 该函数融合了陀螺仪和加速度计的数据，计算出表示设备姿态的四元数。
* 1. 使用陀螺仪数据积分，得到一个初步的姿态估计（预测）。
* 2. 使用加速度计数据（当设备处于静止或低速运动时，加速度计主要测量重力）来修正这个估计（修正）。
* 3. 通过梯度下降法找到一个最优的旋转，使得在当前姿态下，重力向量的方向与加速度计测量的方向最接近。
* @param gx, gy, gz 陀螺仪三轴角速度 (单位: rad/s)
* @param ax, ay, az 加速度计三轴加速度 (单位: g)
*/
static void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
float recipNorm;
float s0, s1, s2, s3;
float qDot1, qDot2, qDot3, qDot4;
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
float dt = 1.0f / sampleFreq; // 采样时间间隔
// --- 1. 陀螺仪积分：计算四元数的变化率 ---
// 姿态运动学的基本方程，描述了姿态如何随角速度变化。
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
// --- 2. 加速度计修正 ---
// 仅当加速度计读数有效时（即模长不为0）才进行修正，防止计算NaN。
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// 将加速度计读数归一化，得到单位向量
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// 预先计算一些重复使用的值，提高效率
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_4q0 = 4.0f * q0;
_4q1 = 4.0f * q1;
_4q2 = 4.0f * q2;
_8q1 = 8.0f * q1;
_8q2 = 8.0f * q2;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
// --- 梯度下降法：计算修正量 ---
// 目标函数 f(q, a) = [2(q1q3 - q0q2) - ax, 2(q0q1 + q2q3) - ay, 2(0.5 - q1^2 - q2^2) - az]^T
// s0, s1, s2, s3 是目标函数 f 对四元数 q 的雅可比矩阵 J 与 f 的乘积。
// 这个结果代表了误差函数的梯度方向，用于修正四元数的变化率。
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // 归一化梯度
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// --- 应用修正量 ---
// 将计算出的修正量（梯度）从陀螺仪积分结果中减去，beta是修正的权重。
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}
// --- 3. 积分：更新四元数 ---
// 使用一阶龙格-库塔法（即欧拉法）进行积分，得到新的四元数。
q0 += qDot1 * dt;
q1 += qDot2 * dt;
q2 += qDot3 * dt;
q3 += qDot4 * dt;
// --- 4. 归一化四元数 ---
// 保持四元数的模长为1，防止由于计算误差导致的累积漂移。
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}
// =================================================================================================
// --- 静态校准相关变量 ---
static unsigned char SL_SC7U22_Error_Flag=0;
static unsigned char SL_SC7U22_Error_cnt=0;
static unsigned char SL_SC7U22_Error_cnt2=0;
static signed short Temp_Accgyro[6] ={0};
static signed short Error_Accgyro[6]={0};
static signed int Sum_Avg_Accgyro[6] ={0};
static float yaw_offset = 0.0f;
static signed short SL_GetAbsShort(signed short v_Val_s16r)
{
if(v_Val_s16r==(-32768))
return 32767;
return (v_Val_s16r < 0) ? -v_Val_s16r : v_Val_s16r;
}
/**
* @brief 姿态角解算函数
* @details
* 该函数主要完成两项工作：
* 1. 静态校准：在初始阶段，检测传感器是否处于静止状态。如果是，则计算加速度计和陀螺仪的零点偏移（误差），用于后续的数据补偿。
* 2. 姿态解算：使用 Madgwick 滤波器融合经过校准后的加速度计和陀螺仪数据，计算出物体的俯仰角（Pitch）、横滚角（Roll）和偏航角（Yaw）。
*
* @param calibration_en 传入：外部校准使能标志。如果为0，则强制认为已经校准完成。
* @param acc_gyro_input 传入和传出：包含6轴原始数据的数组指针，顺序为 [ACC_X, ACC_Y, ACC_Z, GYR_X, GYR_Y, GYR_Z]。该函数会对其进行原地修改，填充为校准后的数据。
* @param Angle_output 传出：滤波后的结果，顺序为 [Pitch, Roll, Yaw]。
* @param yaw_rst 传入：Yaw轴重置标志。如果为1，则将Yaw角清零。
*
* @return
* - 0: 正在进行静态校准。
* - 1: 姿态角计算成功。
* - 2: 校准未完成，无法进行计算。
*/
unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst)
{
unsigned short acc_gyro_delta[2];
unsigned char sl_i = 0;
// 如果外部强制使能校准，则将标志位置1
if (calibration_en == 0) {
SL_SC7U22_Error_Flag = 1;
}
// =================================================================================
// 步骤 1: 静态校准 (与原版逻辑相同)
// ---------------------------------------------------------------------------------
if (SL_SC7U22_Error_Flag == 0) {
// 计算当前数据与上一帧数据的差值，用于判断是否静止
acc_gyro_delta[0] = 0;
acc_gyro_delta[1] = 0;
for (sl_i = 0; sl_i < 3; sl_i++) {
acc_gyro_delta[0] += SL_GetAbsShort(acc_gyro_input[sl_i] - Temp_Accgyro[sl_i]);
acc_gyro_delta[1] += SL_GetAbsShort(acc_gyro_input[3 + sl_i] - Temp_Accgyro[3 + sl_i]);
}
// 保存当前数据，用于下一帧比较
for (sl_i = 0; sl_i < 6; sl_i++) {
Temp_Accgyro[sl_i] = acc_gyro_input[sl_i];
}
// 判断是否处于静止状态：加速度变化量、陀螺仪变化量、各轴加速度值都在一个很小的范围内
// 假设1g = 8192 (对应 +/-4g 量程)
if ((acc_gyro_delta[0] / 8 < 80) && (acc_gyro_delta[1] < 20) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 8192) < 3000)) { //acc<80mg gyro<20 lsb
if (SL_SC7U22_Error_cnt < 200) {
SL_SC7U22_Error_cnt++; // 静止计数器累加
}
} else {
SL_SC7U22_Error_cnt = 0; // 如果发生移动，则清空静止计数器
}
// 如果静止时间足够长（这里是190个采样周期，约1.9秒）
if (SL_SC7U22_Error_cnt > 190) {
// 开始累加50个采样点的数据
for (sl_i = 0; sl_i < 6; sl_i++) {
Sum_Avg_Accgyro[sl_i] += acc_gyro_input[sl_i];
}
SL_SC7U22_Error_cnt2++;
if (SL_SC7U22_Error_cnt2 > 49) {
// 累加满50个点后，计算平均值
SL_SC7U22_Error_Flag = 1; // 标记校准完成
SL_SC7U22_Error_cnt2 = 0;
SL_SC7U22_Error_cnt = 0;
for (sl_i = 0; sl_i < 6; sl_i++) {
Sum_Avg_Accgyro[sl_i] = Sum_Avg_Accgyro[sl_i] / 50;
}
// 计算零点偏移：理想值 - 实际平均值
// 加速度Z轴的理想值是8192，对应1g（假设量程为±4g）
Error_Accgyro[0] = 0 - Sum_Avg_Accgyro[0];
Error_Accgyro[1] = 0 - Sum_Avg_Accgyro[1];
Error_Accgyro[2] = 8192 - Sum_Avg_Accgyro[2];
Error_Accgyro[3] = 0 - Sum_Avg_Accgyro[3];
Error_Accgyro[4] = 0 - Sum_Avg_Accgyro[4];
Error_Accgyro[5] = 0 - Sum_Avg_Accgyro[5];
#if SL_Sensor_Algo_Release_Enable == 0x00
printf("AVG_Recode AX:%d,AY:%d,AZ:%d,GX:%d,GY:%d,GZ:%d\r\n", Sum_Avg_Accgyro[0], Sum_Avg_Accgyro[1], Sum_Avg_Accgyro[2], Sum_Avg_Accgyro[3], Sum_Avg_Accgyro[4], Sum_Avg_Accgyro[5]);
printf("Error_Recode AX:%d,AY:%d,AZ:%d,GX:%d,GY:%d,GZ:%d\r\n", Error_Accgyro[0], Error_Accgyro[1], Error_Accgyro[2], Error_Accgyro[3], Error_Accgyro[4], Error_Accgyro[5]);
#endif
}
} else {
// 如果在累加过程中发生移动，则重新开始
SL_SC7U22_Error_cnt2 = 0;
for (sl_i = 0; sl_i < 6; sl_i++) {
Sum_Avg_Accgyro[sl_i] = 0;
}
}
return 0; // 返回0，表示正在校准
}
// =================================================================================
// 步骤 2: 姿态解算 (Madgwick)
// ---------------------------------------------------------------------------------
if (SL_SC7U22_Error_Flag == 1) { // 确认已经校准完成
// --- 2.1 数据预处理 ---
// 应用零点偏移补偿
for (sl_i = 0; sl_i < 6; sl_i++) {
Temp_Accgyro[sl_i] = acc_gyro_input[sl_i] + Error_Accgyro[sl_i];
}
#if 1 // 将校准后的数据写回输入数组
for (sl_i = 0; sl_i < 6; sl_i++) {
acc_gyro_input[sl_i] = Temp_Accgyro[sl_i];
}
#endif
// --- 2.2 转换数据单位 ---
// 将校准后的传感器原始值 (LSB) 转换为 Madgwick 算法所需的物理单位。
// 加速度: LSB -> g (重力加速度)。转换系数 = 量程 / (2^15)。假设 +/-4g 量程, 系数 = 4 / 32768 = 1/8192。
float ax = (float)Temp_Accgyro[0] / 8192.0f;
float ay = (float)Temp_Accgyro[1] / 8192.0f;
float az = (float)Temp_Accgyro[2] / 8192.0f;
// 角速度: LSB -> rad/s (弧度/秒)。转换系数 = (量程 * PI) / (180 * 2^15)。
// 假设 +/-2000dps 量程, 系数 = (2000 * 3.14159) / (180 * 32768) ≈ 0.001064
float gx = (float)Temp_Accgyro[3] * 0.001064f;
float gy = (float)Temp_Accgyro[4] * 0.001064f;
float gz = (float)Temp_Accgyro[5] * 0.001064f;
// --- 2.3 调用 Madgwick 更新函数 ---
// 将处理好的物理单位数据传入滤波器，更新姿态四元数。
MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);
// --- 2.4 将四元数转换为欧拉角 ---
// 欧拉角（Pitch, Roll, Yaw）更直观，便于使用。转换公式如下。
// 转换结果单位为度 (乘以 180/PI ≈ 57.29578)。
float yaw, pitch, roll;
// Roll (横滚角，绕x轴旋转)
roll = atan2f(2.0f * (q0 * q1 + q2 * q3), 1.0f - 2.0f * (q1 * q1 + q2 * q2)) * 57.29578f;
// Pitch (俯仰角，绕y轴旋转)
float sinp = 2.0f * (q0 * q2 - q3 * q1);
if (fabsf(sinp) >= 1)
pitch = copysignf(3.14159265f / 2, sinp) * 57.29578f; // 防止万向节死锁，当sinp接近+/-1时，直接赋+/-90度
else
pitch = asinf(sinp) * 57.29578f;
// Yaw (偏航角，绕z轴旋转)
yaw = atan2f(2.0f * (q0 * q3 + q1 * q2), 1.0f - 2.0f * (q2 * q2 + q3 * q3)) * 57.29578f;
// --- 2.5 处理Yaw轴重置 ---
// Yaw角无法通过加速度计校正，会随时间漂移。提供一个重置机制，将当前Yaw角作为新的零点。
if (yaw_rst) {
yaw_offset = yaw;
}
// --- 2.6 输出最终角度 ---
// 将计算出的欧拉角存入输出数组。
Angle_output[0] = pitch;
Angle_output[1] = roll;
Angle_output[2] = yaw - yaw_offset; // 输出减去偏移量的相对Yaw角
return 1; // 返回1，表示计算成功
}
return 2; // 校准未完成，返回错误状态
}
#endif

181
apps/earphone/xtell_Sensor/sensor/WF282A.c
View File

@ -1,181 +0,0 @@
/*
气压计 - WF282A
*/
#include "wf282a.h"
#include <math.h>
#include <stdint.h> // 推荐使用标准类型
#include "gSensor/gSensor_manage.h"
/*==================================================================================*/
/* WF282A 内部定义 */
/*==================================================================================*/
// 存储校准系数的静态全局变量
static int16_t c0, c1, c01, c11, c20, c21, c30;
static int32_t c00, c10;
/*==================================================================================*/
/* 封装的底层I2C读写函数 */
/*==================================================================================*/
/**
* @brief 写入单个字节到WF282A寄存器
*/
static void wf282a_write_reg(uint8_t reg, uint8_t data) {
gravity_sensor_command(WF_IIC_WRITE_ADDRESS, reg, data);
}
/**
* @brief 从WF282A读取多个字节
*/
static uint32_t wf282a_read_regs(uint8_t reg, uint8_t *buf, uint8_t len) {
return _gravity_sensor_get_ndata(WF_IIC_READ_ADDRESS, reg, buf, len);
}
/*==================================================================================*/
/* 内部辅助函数 */
/*==================================================================================*/
/**
* @brief 从缓冲区中解析所有校准系数
* @param buf 包含从寄存器0x10开始读取的18个字节的校准数据
*/
static void parse_calibration_data(const uint8_t *buf) {
// c0 (12-bit)
c0 = ((int16_t)buf[0] << 4) | (buf[1] >> 4);
if (c0 & (1 << 11)) c0 |= 0xF000;
// c1 (12-bit)
c1 = (((int16_t)buf[1] & 0x0F) << 8) | buf[2];
if (c1 & (1 << 11)) c1 |= 0xF000;
// c00 (20-bit)
c00 = ((int32_t)buf[3] << 12) | ((int32_t)buf[4] << 4) | (buf[5] >> 4);
if (c00 & (1 << 19)) c00 |= 0xFFF00000;
// c10 (20-bit)
c10 = (((int32_t)buf[5] & 0x0F) << 16) | ((int32_t)buf[6] << 8) | buf[7];
if (c10 & (1 << 19)) c10 |= 0xFFF00000;
// c01, c11, c20, c21, c30 (16-bit)
c01 = (int16_t)((uint16_t)buf[8] << 8 | buf[9]);
c11 = (int16_t)((uint16_t)buf[10] << 8 | buf[11]);
c20 = (int16_t)((uint16_t)buf[12] << 8 | buf[13]);
c21 = (int16_t)((uint16_t)buf[14] << 8 | buf[15]);
c30 = (int16_t)((uint16_t)buf[16] << 8 | buf[17]);
}
/**
* @brief 获取原始温度值 (ADC)
*/
static int32_t Get_Traw() {
uint8_t buff[3];
int32_t Traw;
// 从 MSB 寄存器 WF_TMP_B2 (0x03) 开始连续读取3个字节
wf282a_read_regs(WF_TMP_B2, buff, 3);
// buff[0] = B2 (MSB), buff[1] = B1, buff[2] = B0 (LSB)
Traw = (int32_t)buff[0] << 16 | (int32_t)buff[1] << 8 | (int32_t)buff[2];
// 24位二进制补码转32位
if (Traw & (1 << 23)) {
Traw |= 0xFF000000;
}
return Traw;
}
/**
* @brief 获取原始气压值 (ADC)
*/
static int32_t Get_Praw() {
uint8_t buff[3];
int32_t Praw;
// 从 MSB 寄存器 WF_PRS_B2 (0x00) 开始连续读取3个字节
wf282a_read_regs(WF_PRS_B2, buff, 3);
// buff[0] = B2 (MSB), buff[1] = B1, buff[2] = B0 (LSB)
Praw = (int32_t)buff[0] << 16 | (int32_t)buff[1] << 8 | (int32_t)buff[2];
// 24位二进制补码转32位
if (Praw & (1 << 23)) {
Praw |= 0xFF000000;
}
return Praw;
}
/*==================================================================================*/
/* 4. 外部接口函数实现 */
/*==================================================================================*/
uint8_t WF_Init() {
uint8_t calib_buf[18];
uint8_t check_cfg;
// 1. 配置传感器工作模式
// 推荐配置：压力8次过采样，温度1次过采样，测量速率16Hz
wf282a_write_reg(WF_PRS_CFG, (PM_RATE_16 << 4) | PM_PRC_8);
wf282a_write_reg(WF_TMP_CFG, (TMP_RATE_16 << 4) | TMP_PRC_1 | TMP_INT_SENSOR);
wf282a_write_reg(WF_MEAS_CFG, 0x07); // 启动连续压力和温度测量
wf282a_write_reg(WF_CFG_REG, 0x00); // 无中断或FIFO移位配置
// 2. 一次性读取所有校准系数 (从0x10到0x21，共18字节)
if (wf282a_read_regs(COEF_C0, calib_buf, 18) != 0) {
return 2; // 读取校准数据失败
}
parse_calibration_data(calib_buf);
// 3. 检查配置是否写入成功
wf282a_read_regs(WF_MEAS_CFG, &check_cfg, 1);
if (check_cfg != 0x07) {
return 1; // 错误
} else {
return 0; // 成功
}
}
void WF_Sleep() {
wf282a_write_reg(WF_MEAS_CFG, 0x00); // 待机模式
}
void WF_Wakeup() {
wf282a_write_reg(WF_MEAS_CFG, 0x07); // 恢复连续测量
}
uint8_t WF_GetID() {
uint8_t id;
wf282a_read_regs(WF_ID_REG, &id, 1);
return id;
}
float WF_Temperature_Calculate() {
float Traw_sc;
int32_t Traw = Get_Traw();
Traw_sc = (float)Traw / KT; // 缩放原始温度值
return (float)c0 * 0.5f + (float)c1 * Traw_sc;
}
float WF_Pressure_Calculate() {
float Traw_sc, Praw_sc, Pcomp;
int32_t Traw = Get_Traw();
int32_t Praw = Get_Praw();
Traw_sc = (float)Traw / KT; // 缩放原始温度值
Praw_sc = (float)Praw / KP; // 缩放原始压力值
// 公式: 手册给出
Pcomp = (float)c00
+ Praw_sc * ((float)c10 + Praw_sc * ((float)c20 + Praw_sc * (float)c30))
+ Traw_sc * (float)c01
+ Traw_sc * Praw_sc * ((float)c11 + Praw_sc * (float)c21);
return Pcomp;
}
float WF_Altitude_Calculate() {
float pressure_pa = WF_Pressure_Calculate();
// 使用标准大气压公式计算海拔
// P = P0 * (1 - L*h / T0)^(g*M / (R*L))
// 简化公式: h = 44330 * (1 - (P/P0)^(1/5.255))
// 1/5.255 ≈ 0.1903
if (pressure_pa <= 0) {
return 0.0f; // 避免无效计算
}
return 44330.0f * (1.0f - powf(pressure_pa / 101325.0f, 0.1902949f));
}

148
apps/earphone/xtell_Sensor/sensor/WF282A.h
View File

@ -1,148 +0,0 @@
#ifndef _WF282A_H_
#define _WF282A_H_
#include <stdint.h> // 使用标准整数类型
// 标定值
#define KT 524288.0f
#define KP 1572864.0f
#define WF_PULL_UP 1 //外部是否接的上拉
// I2C 从设备地址
#if WF_PULL_UP == 1 //外部接的高
#define WF_IIC_7BIT_ADDRESS 0x77 //7位,外部接高为0x77
#define WF_IIC_WRITE_ADDRESS (WF_IIC_7BIT_ADDRESS<<1) //8位地址
#define WF_IIC_READ_ADDRESS (WF_IIC_WRITE_ADDRESS | 0x01)
#else
#define WF_IIC_7BIT_ADDRESS 0x76 //7位,外部接低为0x76
#define WF_IIC_WRITE_ADDRESS (WF_IIC_7BIT_ADDRESS<<1) //8位地址
#define WF_IIC_READ_ADDRESS (WF_IIC_WRITE_ADDRESS | 0x01)
#endif
#define WF_CHIP_ID 0X10
// 寄存器映射
// 压力数据
#define WF_PRS_B2 0x00
#define WF_PRS_B1 0x01
#define WF_PRS_B0 0x02
// 温度数据
#define WF_TMP_B2 0x03
#define WF_TMP_B1 0x04
#define WF_TMP_B0 0x05
// 配置寄存器
#define WF_PRS_CFG 0x06
#define WF_TMP_CFG 0x07
#define WF_MEAS_CFG 0x08
#define WF_CFG_REG 0x09
#define WF_INT_STS 0x0A
#define WF_FIFO_STS 0x0B
#define WF_RESET_REG 0x0C
// ID寄存器
#define WF_ID_REG 0x0D
// 校准系数寄存器
#define COEF_C0 0x10
#define COEF_C0_C1 0x11
#define COEF_C1 0x12
#define COEF_C00_H 0x13
#define COEF_C00_L 0x14
#define COEF_C00_C10 0x15
#define COEF_C10_M 0x16
#define COEF_C10_L 0x17
#define COEF_C01_H 0x18
#define COEF_C01_L 0x19
#define COEF_C11_H 0x1A
#define COEF_C11_L 0x1B
#define COEF_C20_H 0x1C
#define COEF_C20_L 0x1D
#define COEF_C21_H 0x1E
#define COEF_C21_L 0x1F
#define COEF_C30_H 0x20
#define COEF_C30_L 0x21
// --- 配置宏 ---
// 压力配置 (PRS_CFG[6:4]) - 测量速率
#define PM_RATE_1 0x00 // 1 次/秒
#define PM_RATE_2 0x01 // 2 次/秒
#define PM_RATE_4 0x02 // 4 次/秒
#define PM_RATE_8 0x03 // 8 次/秒
#define PM_RATE_16 0x04 // 16 次/秒
#define PM_RATE_32 0x05 // 32 次/秒
#define PM_RATE_64 0x06 // 64 次/秒
#define PM_RATE_128 0x07 // 128 次/秒
// 压力配置 (PRS_CFG[3:0]) - 过采样率
#define PM_PRC_1 0x00 // 1 次 (单次)
#define PM_PRC_2 0x01 // 2 次 (低功耗)
#define PM_PRC_4 0x02 // 4 次
#define PM_PRC_8 0x03 // 8 次 (标准)
#define PM_PRC_16 0x04 // 16 次 (需要移位)
#define PM_PRC_32 0x05 // 32 次 (需要移位)
#define PM_PRC_64 0x06 // 64 次 (高精度, 需要移位)
#define PM_PRC_128 0x07 // 128 次 (需要移位)
// 温度配置 (TMP_CFG[7]) - 传感器源
#define TMP_EXT_SENSOR 0x80 // 使用外部传感器
#define TMP_INT_SENSOR 0x00 // 使用内部传感器
// 温度配置 (TMP_CFG[6:4]) - 测量速率
#define TMP_RATE_1 0x00 // 1 次/秒
#define TMP_RATE_2 0x01 // 2 次/秒
#define TMP_RATE_4 0x02 // 4 次/秒
#define TMP_RATE_8 0x03 // 8 次/秒
#define TMP_RATE_16 0x04 // 16 次/秒
#define TMP_RATE_32 0x05 // 32 次/秒
#define TMP_RATE_64 0x06 // 64 次/秒
#define TMP_RATE_128 0x07 // 128 次/秒
// 温度配置 (TMP_CFG[3:0]) - 过采样率
#define TMP_PRC_1 0x00 // 1 次
#define TMP_PRC_2 0x01 // 2 次
#define TMP_PRC_4 0x02 // 4 次
#define TMP_PRC_8 0x03 // 8 次
#define TMP_PRC_16 0x04 // 16 次
#define TMP_PRC_32 0x05 // 32 次
#define TMP_PRC_64 0x06 // 64 次
#define TMP_PRC_128 0x07 // 128 次
/**
* @brief 初始化WF282A传感器
* @return 0: 成功, 1: 失败
*/
uint8_t WF_Init(void);
/**
* @brief 使传感器进入休眠/待机模式
*/
void WF_Sleep(void);
/**
* @brief 唤醒传感器，开始连续测量
*/
void WF_Wakeup(void);
/**
* @brief 获取传感器芯片ID
* @return 芯片ID (应为 0x10)
*/
uint8_t WF_GetID(void);
/**
* @brief 计算并返回当前海拔高度
* @return 海拔高度 (单位: 米)
*/
float WF_Altitude_Calculate(void);
/**
* @brief 计算并返回补偿后的压力值
* @return 压力 (单位: Pa)
*/
float WF_Pressure_Calculate(void);
/**
* @brief 计算并返回补偿后的温度值
* @return 温度 (单位: °C)
*/
float WF_Temperature_Calculate(void);
#endif // _WF282A_H_

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

@ -1,10 +0,0 @@
#ifndef XTELL_H
#define XTELL_H
#include "system/includes.h"
// #define KS_BLE 1
#define XTELL_TEST 1
#define ACC_RANGE 16 //g，加速度满量程:2、4、8、16
#endif

176
apps/earphone/xtell_Sensor/xtell_app_main.c
View File

@ -1,176 +0,0 @@
#include "system/includes.h"
/*#include "btcontroller_config.h"*/
#include "btstack/btstack_task.h"
#include "app_config.h"
#include "app_action.h"
#include "asm/pwm_led.h"
#include "tone_player.h"
#include "gpio.h"
#include "app_main.h"
#include "asm/charge.h"
#include "update.h"
#include "app_power_manage.h"
#include "app_charge.h"
#include "bt_profile_cfg.h"
#include "dev_manager/dev_manager.h"
#include "update_loader_download.h"
#include "avctp_user.h"
#include "debug.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 LOG_TAG_CONST APP
#define LOG_TAG "[APP]"
#define LOG_ERROR_ENABLE
#define LOG_DEBUG_ENABLE
#define LOG_INFO_ENABLE
/* #define LOG_DUMP_ENABLE */
#define LOG_CLI_ENABLE
//
///////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////
//变量
extern APP_VAR app_var;
u16 close_BL_number=0;
//
//////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////
//函数定义
extern void timer_2ms_handler();
extern void app_var_init(void);
void app_earphone_play_voice_file(const char *name);
void clr_wdt(void);
extern void check_power_on_key(void);
extern int cpu_reset_by_soft();
extern int audio_dec_init();
extern int audio_enc_init();
//
//////////////////////////////////////////////////////////////////////////////////////////////////
/*充电拔出,CPU软件复位, 不检测按键，直接开机*/
static void app_poweron_check(int update)
{
if (!update && cpu_reset_by_soft()) {
app_var.play_poweron_tone = 0;
return;
}
}
void create_process(u16* pid,char* name, void *priv, void (*func)(void *priv), u32 msec){
xlog("1 name=%s, pid =%d\n",name,*pid);
if (*pid != 0) return;
*pid = sys_timer_add(priv, func, msec);
xlog("2 name=%s, pid =%d\n",name,*pid);
}
void close_process(u16* pid,char* name){
xlog("name=%s,pid =%d\n",name,*pid);
if (*pid == 0) return;
sys_timer_del(*pid);
*pid = 0;
}
void close_BL(){
/**开机默认关闭 经典蓝牙 */
// close_BL_flag++;
xlog("xtell Classic Bluetooth off\n");
user_send_cmd_prepare(USER_CTRL_DISCONNECTION_HCI, 0, NULL); //断开此时经典蓝牙的连接，经典蓝牙还是可以被发现
delay_2ms(50);
user_send_cmd_prepare(USER_CTRL_WRITE_SCAN_DISABLE, 0, NULL); //关闭蓝牙可发现,已连接时不能操作
delay_2ms(50);
user_send_cmd_prepare(USER_CTRL_WRITE_CONN_DISABLE, 0, NULL); //关闭蓝牙可连接，
// sys_timer_del(close_BL_number); //删除定时器任务
close_process(&close_BL_number,__func__);
}
void xtell_set_ble_name(char* name){
}
extern u32 timer_get_ms(void);
void xtell_app_main()
{
int update = 0;
u32 addr = 0, size = 0;
struct intent it;
xlog("==============xtell_app_main start================\n");
log_info("app_main\n");
app_var.start_time = timer_get_ms();
if (!UPDATE_SUPPORT_DEV_IS_NULL()) {
update = update_result_deal();
}
app_var_init();
if (get_charge_online_flag()) {
#if(TCFG_SYS_LVD_EN == 1)
vbat_check_init();
#endif
xlog("==============idle================\n");
init_intent(&it);
it.name = "idle";
it.action = ACTION_IDLE_MAIN;
start_app(&it);
} else {
xlog("==============handler start================\n");
check_power_on_voltage();
app_poweron_check(update);
init_intent(&it);
it.name = "handler";
it.action = ACTION_EARPHONE_MAIN;
start_app(&it);
xlog("==============handler end================\n");
}
//////////////////////////////////////////////////
//开机必须延时关闭经典蓝牙，不然底层代码会再次把蓝牙 打开
// create_process(&close_BL_number, "close_BL",NULL, close_BL, 3000);
u8 mac_data[6];
extern void rcsp_adv_fill_mac_addr(u8 *mac_addr_buf);
rcsp_adv_fill_mac_addr(mac_data); //读取MAC地址
xlog("xtell BT mac data:%x:%x:%x:%x:%x:%x",mac_data[0],mac_data[1],mac_data[2],mac_data[3],mac_data[4],mac_data[5]);
user_send_cmd_prepare(USER_CTRL_WRITE_SCAN_ENABLE, 0, NULL); //打开蓝牙可发现,已连接时不能操作
delay_2ms(50);
user_send_cmd_prepare(USER_CTRL_WRITE_CONN_ENABLE, 0, NULL); //打开蓝牙可连接
delay_2ms(50);
extern void xtell_task_create(void);
xtell_task_create();
xlog("==============xtell_app_end================\n");
}

581
apps/earphone/xtell_Sensor/xtell_handler.c
View File

@ -1,581 +0,0 @@
#include "system/includes.h"
#include "media/includes.h"
#include "tone_player.h"
#include "earphone.h"
#include "app_config.h"
#include "app_action.h"
#include "app_task.h"
#include "btstack/avctp_user.h"
#include "btstack/btstack_task.h"
#include "btctrler/btctrler_task.h"
#include "btstack/frame_queque.h"
#include "user_cfg.h"
// #include "aec_user.h"
#include "classic/hci_lmp.h"
#include "bt_common.h"
#include "bt_ble.h"
#include "bt_tws.h"
#include "pbg_user.h"
#include "btstack/bluetooth.h"
#include "colorful_lights/colorful_lights.h"
#include "app_chargestore.h"
#include "jl_kws/jl_kws_api.h"
#include "asm/charge.h"
#include "app_charge.h"
#include "ui_manage.h"
#include "app_chargestore.h"
#include "app_umidigi_chargestore.h"
#include "app_testbox.h"
#include "app_online_cfg.h"
#include "app_main.h"
#include "app_power_manage.h"
#include "gSensor/gSensor_manage.h"
#include "key_event_deal.h"
#include "classic/tws_api.h"
#include "asm/pwm_led.h"
#include "ir_sensor/ir_manage.h"
#include "in_ear_detect/in_ear_manage.h"
#include "vol_sync.h"
#include "bt_background.h"
#include "default_event_handler.h"
#include "debug.h"
#include "system/event.h"
#include "./ano/ano_protocol.h"
#include "./sensor/MMC56.h"
#include "./sensor/BMP280.h"
#include "./sensor/AK8963.h"
#include "./calculate/skiing_tracker.h"
///////////////////////////////////////////////////////////////////////////////////////////////////
//宏定义
#define LOG_TAG_CONST EARPHONE
#define LOG_TAG "[EARPHONE]"
#define LOG_ERROR_ENABLE
#define LOG_DEBUG_ENABLE
#define xlog_ENABLE
#if(USE_DMA_UART_TEST) //使用dm串口测试时不能同时打开
#define MY_SNIFF_EN 0
#else
#define MY_SNIFF_EN 1 //默认打开
#endif
#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
//
///////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////
//变量
extern u8 init_ok;
extern u8 sniff_out;
unsigned char xtell_bl_state=0; //存放经典蓝牙的连接状态，0断开，1是连接
u8 bt_newname =0;
unsigned char xt_ble_new_name[9] = "CM-33333";
static u16 play_poweron_ok_timer_id = 0;
// -- 初始化标志位 --
u8 SC7U22_init = 0x10; //六轴是否初始化
u8 MMC5603nj_init = 0x20; //地磁是否初始化
u8 BMP280_init = 0x30; //气压计初始化
u8 foot_init = 0x40; //数据来源初始化：左脚0x41 or 右脚0x42
// -- 线程id --
u16 gsensor_test_id = 0;
//
///////////////////////////////////////////////////////////////////////////////////////////////////
extern int bt_hci_event_handler(struct bt_event *bt);
extern void SC7U22_static_calibration(void);
extern void create_process(u16* pid, const char* name, void *priv, void (*func)(void *priv), u32 msec);
extern void close_process(u16* pid,char* name);
extern void start_collect_fuc(void);
extern void BLE_send_fuc(void);
extern void start_calibration(void);
extern void start_clloct(void);
extern void stop_clloct(void);
extern void set_foot_state(u8 state);
extern void stop_calibration(void);
///////////////////////////////////////////////////////////////////////////////////////////////////
/*
* 模式状态机, 通过start_app()控制状态切换
*/
/* extern int audio_mic_init(); */
static int state_machine(struct application *app, enum app_state state, struct intent *it){
int error = 0;
static u8 tone_player_err = 0;
xlog("bt_state_machine=%d\n", state);
switch (state) {
case APP_STA_CREATE:
xlog("APP_STA_CREATE\n");
/* set_adjust_conn_dac_check(0); */
break;
case APP_STA_START:
xlog("APP_STA_START\n");
if (!it) {
xlog("APP_STA_START:it none\n");
break;
}
switch (it->action) {
case ACTION_EARPHONE_MAIN:
xlog("ble init\n");
/*
* handler 初始化
*/
clk_set("sys", BT_NORMAL_HZ);
u32 sys_clk = clk_get("sys");
bt_pll_para(TCFG_CLOCK_OSC_HZ, sys_clk, 0, 0);
/* bredr_set_dut_enble(1, 1); */
bt_function_select_init();
bredr_handle_register();
EARPHONE_STATE_INIT();
btstack_init();
sys_auto_shut_down_enable();
bt_sniff_feature_init();
sys_auto_sniff_controle(MY_SNIFF_EN, NULL);
app_var.dev_volume = -1;
break;
case ACTION_A2DP_START: //蓝牙音频传输协议
xlog("ACTION_A2DP_START\n");
break;
case ACTION_BY_KEY_MODE:
xlog("ACTION_BY_KEY_MODE\n");
break;
case ACTION_TONE_PLAY:
xlog("ACTION_TONE_PLAY\n");
// STATUS *p_tone = get_tone_config();
// tone_play_index(p_tone->bt_init_ok, 1);
break;
case ACTION_DO_NOTHING:
xlog("ACTION_DO_NOTHING\n");
break;
}
break;
case APP_STA_PAUSE:
xlog("APP_STA_PAUSE\n");
break;
case APP_STA_RESUME:
xlog("APP_STA_RESUME\n");
//恢复前台运行
sys_auto_shut_down_disable();
sys_key_event_enable();
break;
case APP_STA_STOP:
xlog("APP_STA_STOP\n");
break;
case APP_STA_DESTROY:
xlog("APP_STA_DESTROY\n");
r_printf("APP_STA_DESTROY\n");
if (!app_var.goto_poweroff_flag) {
bt_app_exit(NULL);
}
break;
}
xlog("state machine error\n");
return error;
}
///////////////////////////////////////////////////////////////////////////////////////////////////
//handle
void le_user_app_send_event(size_t command, unsigned char* data, size_t size)
{
// 中断->事件
static unsigned char buffer[512];
if(data && size && size <= sizeof(buffer)) {
// 拷贝到缓存，避免转发事件的时候，地址发送改变。
memcpy(buffer, data, size);
struct sys_event event;
event.type = SYS_APP_USER_EVENT;
event.u.app.command = command;
event.u.app.buffer = buffer;
event.u.app.size = size;
sys_event_notify(&event);
}
}
void le_user_app_event_handler(struct sys_event* event){
switch (event->type) {
// 打印接收到的数据
printf("BLE data\n");
put_buf(event->u.app.buffer, event->u.app.size);
case SYS_APP_USER_EVENT:
if (event->u.app.buffer[0] == 0xBE && event->u.app.buffer[1] == 0xBB) {
if(event->u.app.buffer[2] == 0x01){ //后面的数据长度 1
switch (event->u.app.buffer[3]){
case 0x01:
// extern void gsensor_test(void);
// create_process(&gsensor_test_id,"gsensor_test",NULL,gsensor_test,1000);
xlog("ota_test");
break;
case 0xff: //测试
u8 device_buff[10];
u8 founds = 0;
extern void i2c_scanner_probe(u8* device_addr, u8* found_number);
i2c_scanner_probe(device_buff,&founds);
for(int i = 0;i < founds;i++){
send_data_to_ble_client(&device_buff,founds);
}
break;
default:
break;
}
}else if(event->u.app.buffer[2] == 0x02){ //后面数据长度为2
switch (event->u.app.buffer[3]){ //数据包类型
case 0x00: //数据包类型为：指定传感器初始化
u8 send2_0[5] = {0xBB,0xBE,0x02,0x00,0x00};
if(event->u.app.buffer[4] == 0x01){ //六轴
stop_calibration();
if (SL_SC7U22_Config() == 0) {
send2_0[4] = 0x00; //初始化失败
SC7U22_init = 0x10;
send_data_to_ble_client(&send2_0,5);
return;
}
start_calibration();
}else if(event->u.app.buffer[4] == 0x02){ //地磁
if(mmc5603nj_init() == 0){
MMC5603nj_init = 0x20;
send2_0[4] = MMC5603nj_init; //地磁初始化失败
send_data_to_ble_client(&send2_0,5);
return;
}
MMC5603nj_init = 0x21;
send2_0[4] = MMC5603nj_init; //地磁初始化成功
send_data_to_ble_client(&send2_0,5);
}else if(event->u.app.buffer[4] == 0x03){ //气压计初始化
if(bmp280_init() != 0){
//初始化失败
BMP280_init = 0x30;
send2_0[4] = BMP280_init;
send_data_to_ble_client(&send2_0,5);
return;
}
BMP280_init = 0x31;
send2_0[4] = BMP280_init; //气压计初始化成功
send_data_to_ble_client(&send2_0,5);
}
break;
case 0x01: //设置传感器采集对象：左脚or右脚
u8 send2_1[9] = {0xBB,0xBE,0x06,0x05,0x00,0x00,0x00,0x00,0x00};
if(event->u.app.buffer[4] == 0x01){ //设定数据来源是左脚
foot_init = 0x41;
}else if(event->u.app.buffer[4] == 0x02){//设定数据来源是右脚
foot_init = 0x42;
}
send2_1[4] = foot_init;
send_data_to_ble_client(&send2_1,9);
break;
case 0x02: //数据包类型为：获取指定传感器初始化状态
u8 send2_2[5] = {0xBB,0xBE,0x02,0x00,0x00};
if(event->u.app.buffer[4] == 0x01){ //六轴
send2_2[4] = SC7U22_init;
}else if(event->u.app.buffer[4] == 0x02){ //地磁
send2_2[4] = MMC5603nj_init;
}else if(event->u.app.buffer[4] == 0x03){ //气压计
send2_2[4] = BMP280_init;
}
send_data_to_ble_client(&send2_2,5);
break;
case 0x03: //开始/停止滑雪计算
if(event->u.app.buffer[4] == 0x01){ //开始滑雪计算
if(SC7U22_init == 0x10 || MMC5603nj_init == 0x20 || BMP280_init == 0x30){ //传感器未进行初始化
u8 send2_3[5] = {0xBB,0xBE,0x02,0x00,0x00};
send_data_to_ble_client(&send2_3,5);
return;
}
start_clloct();
}else if(event->u.app.buffer[4] == 0x02){ //停止滑雪计算
stop_clloct();
}
break;
}
}
}
break;
default:
xlog("%d\n",event->type);
break;
}
}
static void play_poweron_ok_timer(void *priv)
{
app_var.wait_timer_do = 0;
log_d("\n-------play_poweron_ok_timer-------\n", priv);
if (is_dac_power_off()) {
#if TCFG_USER_TWS_ENABLE
bt_tws_poweron();
#else
bt_wait_connect_and_phone_connect_switch(0);
#endif
return;
}
app_var.wait_timer_do = sys_timeout_add(priv, play_poweron_ok_timer, 100);
}
static void play_bt_connect_dly(void *priv)
{
app_var.wait_timer_do = 0;
log_d("\n-------play_bt_connect_dly-------\n", priv);
if (!app_var.goto_poweroff_flag) {
STATUS *p_tone = get_tone_config();
tone_play_index(p_tone->bt_connect_ok, 1);
}
}
static int bt_connction_status_event_handler(struct bt_event *bt)
{
STATUS *p_tone = get_tone_config();
u8 *phone_number = NULL;
switch (bt->event) {
case BT_STATUS_INIT_OK:
/*
* 蓝牙初始化完成
*/
xlog("BT_STATUS_INIT_OK\n");
init_ok = 1;
__set_sbc_cap_bitpool(38);
#if (TCFG_USER_BLE_ENABLE)
if (BT_MODE_IS(BT_BQB)) {
ble_bqb_test_thread_init();
} else {
#if !TCFG_WIRELESS_MIC_ENABLE
bt_ble_init();
#endif
}
#endif
bt_init_ok_search_index();
#if TCFG_TEST_BOX_ENABLE
testbox_set_bt_init_ok(1);
#endif
#if ((CONFIG_BT_MODE == BT_BQB)||(CONFIG_BT_MODE == BT_PER))
bt_wait_phone_connect_control(1);
#else
if (is_dac_power_off()) {
bt_wait_connect_and_phone_connect_switch(0);
} else {
app_var.wait_timer_do = sys_timeout_add(NULL, play_poweron_ok_timer, 100);
}
#endif
/*if (app_var.play_poweron_tone) {
tone_play_index(p_tone->power_on, 1);
}*/
break;
case BT_STATUS_SECOND_CONNECTED:
clear_current_poweron_memory_search_index(0);
case BT_STATUS_FIRST_CONNECTED:
xlog("BT_STATUS_CONNECTED\n");
xtell_bl_state = 1; //蓝牙连接成功 置1
if(strcmp(xt_ble_new_name,"CM-11111") != 0){
//蓝牙连接成功
bt_newname =1;
u8 temp[5]={0xBB,0xBE,0x02,0x04,0x00};
temp[4] = xtell_bl_state; //经典蓝牙连接状态
send_data_to_ble_client(&temp,5);
}
earphone_change_pwr_mode(PWR_DCDC15, 3000);
sys_auto_shut_down_disable();
ui_update_status(STATUS_BT_CONN); //单台在此处设置连接状态,对耳的连接状态需要同步，在bt_tws.c中去设置
/* tone_play(TONE_CONN); */
/*os_time_dly(40); // for test*/
xlog("tone status:%d\n", tone_get_status());
if (get_call_status() == BT_CALL_HANGUP) {
if (app_var.phone_dly_discon_time) {
sys_timeout_del(app_var.phone_dly_discon_time);
app_var.phone_dly_discon_time = 0;
} else {
app_var.wait_timer_do = sys_timeout_add(NULL, play_bt_connect_dly, 1600);
/* tone_play_index(p_tone->bt_connect_ok, 1); */
}
}
/*int timeout = 5000 + rand32() % 10000;
sys_timeout_add(NULL, connect_phone_test, timeout);*/
break;
case BT_STATUS_FIRST_DISCONNECT:
case BT_STATUS_SECOND_DISCONNECT:
xlog("BT_STATUS_DISCONNECT\n");
xtell_bl_state = 0; //断开蓝牙 清0
//蓝牙断开连接
if(bt_newname){ //已经改成新蓝牙名字，断开才播报
bt_newname=0;
u8 temp[5]={0xBB,0xBE,0x02,0x04,0x00};
temp[4] = xtell_bl_state; //经典蓝牙连接状态
send_data_to_ble_client(&temp,5);
}
if (app_var.goto_poweroff_flag) {
/*关机不播断开提示音*/
/*关机时不改UI*/
break;
}
// bt_discon_dly_handle(NULL);
break;
//phone status deal
case BT_STATUS_PHONE_INCOME:
break;
case BT_STATUS_PHONE_OUT:
break;
case BT_STATUS_PHONE_ACTIVE:
break;
case BT_STATUS_PHONE_HANGUP:
break;
case BT_STATUS_PHONE_NUMBER:
break;
case BT_STATUS_INBAND_RINGTONE: //铃声
break;
case BT_STATUS_CALL_VOL_CHANGE:
break;
case BT_STATUS_SNIFF_STATE_UPDATE:
xlog(" BT_STATUS_SNIFF_STATE_UPDATE %d\n", bt->value); //0退出SNIFF
if (bt->value == 0) {
sniff_out = 1;
sys_auto_sniff_controle(MY_SNIFF_EN, bt->args);
} else {
sys_auto_sniff_controle(0, bt->args);
}
break;
case BT_STATUS_LAST_CALL_TYPE_CHANGE:
break;
case BT_STATUS_CONN_A2DP_CH:
case BT_STATUS_CONN_HFP_CH:
if ((!is_1t2_connection()) && (get_current_poweron_memory_search_index(NULL))) { //回连下一个device
if (get_esco_coder_busy_flag()) {
clear_current_poweron_memory_search_index(0);
} else {
user_send_cmd_prepare(USER_CTRL_START_CONNECTION, 0, NULL);
}
}
break;
case BT_STATUS_PHONE_MANUFACTURER:
break;
case BT_STATUS_VOICE_RECOGNITION:
break;
case BT_STATUS_AVRCP_INCOME_OPID:
xlog("BT_STATUS_AVRCP_INCOME_OPID:%d\n", bt->value);
break;
default:
xlog(" BT STATUS DEFAULT\n");
break;
}
return 0;
}
static int event_handler(struct application *app, struct sys_event *event)
{
le_user_app_event_handler(event);
if (SYS_EVENT_REMAP(event)) {
g_printf("****SYS_EVENT_REMAP**** \n");
return 0;
}
switch (event->type) {
case SYS_KEY_EVENT:
break;
case SYS_BT_EVENT:
/*
* 蓝牙事件处理
*/
if ((u32)event->arg == SYS_BT_EVENT_TYPE_CON_STATUS) {
printf("in event_handler:bt_connction_status_event_handler");
bt_connction_status_event_handler(&event->u.bt);
} else if ((u32)event->arg == SYS_BT_EVENT_TYPE_HCI_STATUS) {
bt_hci_event_handler(&event->u.bt);
}
break;
case SYS_DEVICE_EVENT:
/*
* 系统设备事件处理
*/
if ((u32)event->arg == DEVICE_EVENT_FROM_CHARGE) {
} else if ((u32)event->arg == DEVICE_EVENT_FROM_POWER) {
return app_power_event_handler(&event->u.dev);
}
#if TCFG_UMIDIGI_BOX_ENABLE
else if ((u32)event->arg == DEVICE_EVENT_UMIDIGI_CHARGE_STORE) {
app_umidigi_chargestore_event_handler(&event->u.umidigi_chargestore);
}
#endif
#if TCFG_TEST_BOX_ENABLE
else if ((u32)event->arg == DEVICE_EVENT_TEST_BOX) {
app_testbox_event_handler(&event->u.testbox);
}
#endif
break;
default:
return false;
}
SYS_EVENT_HANDLER_SPECIFIC(event);
#ifdef CONFIG_BT_BACKGROUND_ENABLE
if (app) {
default_event_handler(event);
}
#endif
return false;
}
///////////////////////////////////////////////////////////////////////////////////////////////////
static const struct application_operation app_handler_ops = {
.state_machine = state_machine,
.event_handler = event_handler,
};
/*
* 注册earphone模式
*/
REGISTER_APPLICATION(app_handler) = {
.name = "handler",
.action = ACTION_EARPHONE_MAIN,
.ops = &app_handler_ops,
.state = APP_STA_DESTROY,
};