重力分量去除后仍有偏差
This commit is contained in:
596
apps/earphone/xtell_Sensor/A_hide/10/skiing_tracker.c
Normal file
596
apps/earphone/xtell_Sensor/A_hide/10/skiing_tracker.c
Normal file
@ -0,0 +1,596 @@
|
||||
/*
|
||||
|
||||
*/
|
||||
#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
Normal file
88
apps/earphone/xtell_Sensor/A_hide/10/skiing_tracker.h
Normal file
@ -0,0 +1,88 @@
|
||||
#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
|
||||
583
apps/earphone/xtell_Sensor/A_hide/9/skiing_tracker.c
Normal file
583
apps/earphone/xtell_Sensor/A_hide/9/skiing_tracker.c
Normal file
@ -0,0 +1,583 @@
|
||||
/*
|
||||
|
||||
*/
|
||||
#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
Normal file
88
apps/earphone/xtell_Sensor/A_hide/9/skiing_tracker.h
Normal file
@ -0,0 +1,88 @@
|
||||
#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
|
||||
@ -31,8 +31,25 @@ sensor_processing_task当中就进行了计算，包括卡尔曼等，在timer
|
||||
计算距离不能直接采用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文件夹下
|
||||
|
||||
|
||||
|
||||
@ -6,77 +6,20 @@
|
||||
#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)
|
||||
|
||||
// --- 静止检测 ---
|
||||
//两个判断是否静止的必要条件：动态零速更新(ZUPT)阈值
|
||||
// 加速方差阈值，提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
|
||||
#define STOP_ACC_VARIANCE_THRESHOLD 0.2f
|
||||
// 陀螺仪方差阈值
|
||||
#define STOP_GYR_VARIANCE_THRESHOLD 5.0f
|
||||
// 静止时候的陀螺仪模长
|
||||
#define STOP_GYR_MAG_THRESHOLD 15
|
||||
// --- --- ---
|
||||
|
||||
// --- 启动滑雪阈值 ---
|
||||
// 加速度模长与重力的差值大于此值，认为开始运动；降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
|
||||
#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 2000.0f //未确定
|
||||
// --- --- ---
|
||||
|
||||
// --- 原地旋转抖动 ---
|
||||
// 用于原地旋转判断的加速度方差阈值。此值比 静止检测 阈值更宽松，
|
||||
// 以允许原地旋转时身体的正常晃动，但仍能与真实滑行时的剧烈加速度变化区分开。
|
||||
#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 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.999
|
||||
//0.995： 0.08 Hz 的信号
|
||||
//0.999： 0.0159 Hz
|
||||
// --- --- ---
|
||||
|
||||
// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
|
||||
// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
|
||||
#define ACC_DEAD_ZONE_THRESHOLD 0.05f
|
||||
|
||||
// --- 模拟摩擦力，进行速度衰减 ---
|
||||
#define SPEED_ATTENUATION 1.0f //暂不模拟
|
||||
BLE_KS_send_data_t KS_data;
|
||||
|
||||
#ifdef XTELL_TEST
|
||||
@ -135,41 +78,54 @@ static void calculate_air_distance(skiing_tracker_t *tracker) {
|
||||
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;
|
||||
|
||||
/**
|
||||
* @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);
|
||||
|
||||
float ax = acc_device[0];
|
||||
float ay = acc_device[1];
|
||||
float az = acc_device[2];
|
||||
// 世界坐标系下的重力矢量
|
||||
const float g_world[3] = {0.0f, 0.0f, G_ACCELERATION};
|
||||
|
||||
// 使用经过验证的、正确的身体坐标系到世界坐标系的旋转矩阵 (基于 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;
|
||||
// 计算旋转矩阵 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 计算缓冲区内三轴数据的方差之和
|
||||
*
|
||||
@ -205,157 +161,7 @@ static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
|
||||
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]);
|
||||
float acc_magnitude = sqrtf(acc_device_ms2[0]*acc_device_ms2[0] + acc_device_ms2[1]*acc_device_ms2[1] + acc_device_ms2[2]*acc_device_ms2[2]);
|
||||
float acc_magnitude_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:
|
||||
//不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;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
@ -381,237 +187,30 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d
|
||||
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));
|
||||
#if 1 //测试禁止状态下陀螺仪的三轴加速度，去掉重力分量后是否正常
|
||||
float tmp_device_acc[3];
|
||||
extern void remove_gravity_in_device_frame(const float *acc_device, const float *angle, float *acc_linear_device);
|
||||
remove_gravity_in_device_frame(acc_device_ms2,angle,tmp_device_acc);
|
||||
|
||||
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 1
|
||||
// --- 根据状态执行不同的计算逻辑 ---
|
||||
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));
|
||||
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;
|
||||
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->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:
|
||||
// 速度清零，抑制漂移
|
||||
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));
|
||||
break;
|
||||
case ROTATING:
|
||||
tracker->speed = 0.0f;
|
||||
break;
|
||||
case FALLEN:
|
||||
// TODO
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
#else
|
||||
// 坐标转换 & 移除重力
|
||||
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){ //匀速
|
||||
|
||||
//保持上次的速度不变。只更新距离
|
||||
tracker->distance += tracker->speed * dt;
|
||||
|
||||
}else if(tracker->state == WHEEL){ //转弯
|
||||
//暂时：保持上次的速度不变。只更新距离
|
||||
tracker->distance += tracker->speed * dt;
|
||||
}else{
|
||||
// 在静止或旋转状态下，速度已经在状态机内部被清零
|
||||
// 额外增加速度衰减，模拟摩擦力，进一步抑制漂移
|
||||
tracker->velocity[0] *= SPEED_ATTENUATION;
|
||||
tracker->velocity[1] *= SPEED_ATTENUATION;
|
||||
tracker->velocity[2] = 0; // 垂直速度强制归零
|
||||
// 计算处理后加速度的水平模长
|
||||
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]);
|
||||
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("===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
|
||||
|
||||
}
|
||||
|
||||
|
||||
#if 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 = Original_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 = Original_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];
|
||||
BLE_send_data.Angle[i] = final_angle_data[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;
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
/**
|
||||
* @brief 滑雪数据计算
|
||||
@ -638,6 +237,14 @@ BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short*
|
||||
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;
|
||||
@ -645,6 +252,13 @@ BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short*
|
||||
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
|
||||
@ -660,23 +274,22 @@ BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short*
|
||||
|
||||
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");
|
||||
// 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;
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -7,14 +7,14 @@ typedef enum {
|
||||
STATIC, // 静止或动态稳定：0
|
||||
NO_CONSTANT_SPEED, // 正在滑雪，非匀速：1
|
||||
CONSTANT_SPEED, // 正在滑雪，匀速：2
|
||||
ROTATING, // 正在原地旋转：
|
||||
WHEEL, // 转弯：3
|
||||
FALLEN, // 已摔倒：4
|
||||
TAKING_OFF, // 起跳冲击阶段：5
|
||||
IN_AIR, // 空中失重阶段：6
|
||||
LANDING, // 落地冲击阶段：7
|
||||
STOP_DETECTION, // 停止检测：8
|
||||
UNKNOWN // 未知状态：9
|
||||
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)，减小延迟，提高实时性
|
||||
@ -48,6 +48,8 @@ typedef struct {
|
||||
// 用于高通滤波器（巴特沃斯一阶滤波器）的私有成员，以消除加速度的直流偏置
|
||||
float acc_world_filtered[3]; //过滤过的
|
||||
float acc_world_unfiltered_prev[3]; //上一次没过滤的
|
||||
|
||||
float acc_world_lpf[3]; // 经过低通滤波后的世界坐标系加速度
|
||||
} skiing_tracker_t;
|
||||
|
||||
//ble发送的数据
|
||||
|
||||
@ -280,14 +280,25 @@ void sensor_read_data(){
|
||||
memcpy(&combined_raw_data[3], tmp.gyr_data, 3 * sizeof(signed short));
|
||||
|
||||
if (!calibration_done) { //第1次启动，开启零漂检测
|
||||
status = SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0);
|
||||
// status = SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0);
|
||||
status = Original_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0);
|
||||
|
||||
// static count = 0;
|
||||
// if(count > 100){
|
||||
// count = 0;
|
||||
// xlog("SL_SC7U22_Angle_Output status:%d",status);
|
||||
// xlog("RawData:AX=%d,AY=%d,AZ=%d,GX=%d,GY=%d,GZ=%d\r\n",tmp.acc_data[0],tmp.acc_data[1],tmp.acc_data[2],tmp.gyr_data[0],tmp.gyr_data[1],tmp.gyr_data[2]);
|
||||
// }
|
||||
// count++;
|
||||
|
||||
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, tmp.angle, 0);
|
||||
// status = SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
|
||||
status = Original_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
|
||||
memcpy(tmp.acc_data, &combined_raw_data[0], 3 * sizeof(signed short));
|
||||
memcpy(tmp.gyr_data, &combined_raw_data[3], 3 * sizeof(signed short));
|
||||
circle_buffer_write(&sensor_read, &tmp);
|
||||
@ -320,15 +331,22 @@ void calculate_data(){
|
||||
static int count = 0;
|
||||
extern char xt_Check_Flag;
|
||||
void BLE_send_data(){
|
||||
// xlog("=======start\n");
|
||||
if(circle_buffer_is_empty(&sensor_send)){
|
||||
// xlog("sensor_send_buffer: send buffer empty\n");
|
||||
return;
|
||||
}
|
||||
#ifdef XTELL_TEST
|
||||
// #if 0
|
||||
BLE_send_data_t tmp;
|
||||
circle_buffer_read(&sensor_send, &tmp);
|
||||
|
||||
if(count >=50){
|
||||
if(count >=100){
|
||||
// extern debug_t debug2;
|
||||
// xlog("s %d, %dcm/s, %dcm\n",tmp.skiing_state, tmp.speed_cms, tmp.distance_cm);
|
||||
// xlog("Acc:%d, %d, %d\n",tmp.acc_data[0],tmp.acc_data[1],tmp.acc_data[2]);
|
||||
// xlog("Gyr:%d, %d, %d\n", tmp.gyr_data[0],tmp.gyr_data[1],tmp.gyr_data[2]);
|
||||
// xlog("debug2.acc_magnitude:%.2f\n", debug2.acc_magnitude);
|
||||
|
||||
int num_chars_written;
|
||||
count = 0;
|
||||
@ -396,7 +414,7 @@ void BLE_send_data(){
|
||||
// xlog("GYR:%d, %d, %d\n", tmp.gyr_data[0],tmp.gyr_data[1],tmp.gyr_data[2]);
|
||||
}
|
||||
count++;
|
||||
|
||||
// xlog("=======end\n");
|
||||
#else
|
||||
#endif
|
||||
}
|
||||
@ -488,7 +506,8 @@ void xtell_task_create(void){
|
||||
gpio_set_pull_up(IO_PORTE_05,1);
|
||||
gpio_direction_output(IO_PORTE_05,1);
|
||||
|
||||
os_time_dly(10);
|
||||
// os_time_dly(10);
|
||||
delay_2ms(10);
|
||||
|
||||
SL_SC7U22_Config();
|
||||
// extern u8 LIS2DH12_Config(void);
|
||||
@ -511,7 +530,7 @@ void xtell_task_create(void){
|
||||
|
||||
create_process(&sensor_read_data_id, "read",NULL, sensor_read_data, 10);
|
||||
|
||||
create_process(&calculate_data_id, "calculate",NULL, calculate_data, 5);
|
||||
create_process(&calculate_data_id, "calculate",NULL, calculate_data, 6);
|
||||
|
||||
create_process(&ble_send_data_id, "send",NULL, BLE_send_data, 1);
|
||||
|
||||
|
||||
@ -129,8 +129,18 @@ unsigned char SL_SC7U22_Config(void)
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0xBC);//ACC_CON 高性能模式，1600Hz -- lmx
|
||||
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0xBB);//ACC_CON 高性能模式，800Hz -- lmx
|
||||
|
||||
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x41, 0x01);//ACC_RANGE 01：±4G
|
||||
#if ACC_RANGE==2
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x41, 0x00);//ACC_RANGE 00：±2G
|
||||
#endif
|
||||
#if ACC_RANGE==4
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x41, 0x01);//ACC_RANGE 01：±4G
|
||||
#endif
|
||||
#if ACC_RANGE==8
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x41, 0x02);//ACC_RANGE 02：±8G
|
||||
#endif
|
||||
#if ACC_RANGE==16
|
||||
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x41, 0x03);//ACC_RANGE 03：±16G
|
||||
#endif
|
||||
|
||||
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0x86);//GYR_CONF 0x87=50Hz 0x86=25Hz
|
||||
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0x8C);//GYR_CONF 1600Hz -- lmx
|
||||
@ -748,7 +758,15 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short
|
||||
}
|
||||
|
||||
// 判断是否处于静止状态：加速度变化量、陀螺仪变化量、各轴加速度值都在一个很小的范围内
|
||||
#if (ACC_RANGE == 2)
|
||||
if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 16384) < 3000)) { //acc<160mg gyro<40 lsb
|
||||
#elif (ACC_RANGE == 4)
|
||||
if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 8192) < 3000)) { //acc<160mg gyro<40 lsb
|
||||
#elif (ACC_RANGE == 8)
|
||||
if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 4096) < 3000)) { //acc<160mg gyro<40 lsb
|
||||
#elif (ACC_RANGE == 16)
|
||||
if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 2048) < 3000)) { //acc<160mg gyro<40 lsb
|
||||
#endif
|
||||
// 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++; // 静止计数器累加
|
||||
@ -776,7 +794,22 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short
|
||||
// 计算零点偏移：理想值 - 实际平均值
|
||||
Error_Accgyro[0] = 0 - Sum_Avg_Accgyro[0];
|
||||
Error_Accgyro[1] = 0 - Sum_Avg_Accgyro[1];
|
||||
#if ACC_RANGE==2
|
||||
Error_Accgyro[2] = 16384 - Sum_Avg_Accgyro[2];
|
||||
#endif
|
||||
|
||||
#if ACC_RANGE==4
|
||||
Error_Accgyro[2] = 8192 - Sum_Avg_Accgyro[2];
|
||||
#endif
|
||||
|
||||
#if ACC_RANGE==8
|
||||
Error_Accgyro[2] = 4096 - Sum_Avg_Accgyro[2];
|
||||
#endif
|
||||
|
||||
#if ACC_RANGE==16
|
||||
Error_Accgyro[2] = 2048 - Sum_Avg_Accgyro[2];
|
||||
#endif
|
||||
|
||||
Error_Accgyro[3] = 0 - Sum_Avg_Accgyro[3];
|
||||
Error_Accgyro[4] = 0 - Sum_Avg_Accgyro[4];
|
||||
Error_Accgyro[5] = 0 - Sum_Avg_Accgyro[5];
|
||||
@ -811,6 +844,13 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short
|
||||
#endif
|
||||
// --- 2.2 使用加速度计计算姿态角 ---
|
||||
// 将加速度原始值转换为归一化的重力分量
|
||||
#if ACC_RANGE==2
|
||||
//2g：
|
||||
angle_acc[0] = (float)Temp_Accgyro[0] / 16384; //ax
|
||||
angle_acc[1] = (float)Temp_Accgyro[1] / 16384; //ay
|
||||
angle_acc[2] = (float)Temp_Accgyro[2] / 16384; //az
|
||||
#endif
|
||||
|
||||
#if ACC_RANGE==4
|
||||
//4g：
|
||||
angle_acc[0] = (float)Temp_Accgyro[0] / 8192; //ax
|
||||
@ -818,6 +858,12 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short
|
||||
angle_acc[2] = (float)Temp_Accgyro[2] / 8192; //az
|
||||
#endif
|
||||
|
||||
#if ACC_RANGE==8
|
||||
angle_acc[0] = (float)Temp_Accgyro[0] / 4096; //ax
|
||||
angle_acc[1] = (float)Temp_Accgyro[1] / 4096; //ay
|
||||
angle_acc[2] = (float)Temp_Accgyro[2] / 4096; //az
|
||||
#endif
|
||||
|
||||
#if ACC_RANGE==16
|
||||
//±16g 2048
|
||||
angle_acc[0] = (float)Temp_Accgyro[0] / 2048; //ax
|
||||
@ -951,63 +997,68 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short
|
||||
|
||||
return 2; // 校准未完成，返回错误状态
|
||||
}
|
||||
|
||||
// =================================================================================
|
||||
// 关键参数定义 - 需要根据实际情况进行调整
|
||||
// ---------------------------------------------------------------------------------
|
||||
// 滤波系数 α (alpha)，决定了对陀螺仪和加速度计的信任程度。
|
||||
// α 越大，越相信陀螺仪，动态响应越好，但对漂移的修正越慢。
|
||||
// α 越小，越相信加速度计，对漂移的修正越快，但对运动加速度越敏感。
|
||||
// 典型值范围：0.95 ~ 0.99
|
||||
#define FILTER_ALPHA 0.98f
|
||||
|
||||
// 采样时间间隔 SAMPLE_INTERVAL (单位：秒)
|
||||
// 必须与此函数的频率严格对应。例如，100Hz 对应 0.01f。
|
||||
#define SAMPLE_INTERVAL 0.01f
|
||||
// =================================================================================
|
||||
/**
|
||||
* @brief 不带卡尔曼的欧若拉数据输出，以及消除零点漂移后的六轴数据
|
||||
* @brief 姿态角解算函数 (基于一阶互补滤波)
|
||||
* @details
|
||||
* 该函数主要完成两项工作：
|
||||
* 1. 静态校准：在初始阶段，检测传感器是否处于静止状态。如果是，则计算加速度计和陀螺仪的零点偏移（误差），用于后续的数据补偿。
|
||||
* 2. 姿态解算：使用一阶互补滤波器融合经过校准后的加速度计和陀螺仪数据，计算出物体的俯仰角（Pitch）、横滚角（Roll）和偏航角（Yaw）。
|
||||
*
|
||||
* @param calibration_en
|
||||
* @param acc_gyro_input
|
||||
* @param Angle_output
|
||||
* @param yaw_rst
|
||||
* @return unsigned char
|
||||
* @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 Original_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;
|
||||
float angle_acc[3] = {0};
|
||||
float gyro_val[3] = {0};
|
||||
unsigned char Original_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst)
|
||||
{
|
||||
// --- 静态变量和宏定义 ---
|
||||
const float dt = 0.01f; // 假设采样周期为 10ms (100Hz)
|
||||
// 互补滤波器系数. alpha 越大, 越相信陀螺仪的积分, 动态响应越好, 但长期会漂移.
|
||||
// (1-alpha) 越大, 越相信加速度计的角度, 静态越准, 但动态响应差且易受运动干扰.
|
||||
// 典型值在 0.95 ~ 0.98 之间.
|
||||
const float FILTER_ALPHA = 0.98f;
|
||||
|
||||
// 如果外部强制使能校准，则将标志位置1
|
||||
unsigned char sl_i = 0;
|
||||
float angle_acc[3] = {0}; // [Pitch_acc, Roll_acc, unused]
|
||||
float gyro_dps[3] = {0}; // [Roll_rate, Pitch_rate, Yaw_rate]
|
||||
|
||||
// 如果外部强制禁用校准，则将标志位置1
|
||||
if (calibration_en == 0) {
|
||||
SL_SC7U22_Error_Flag = 1;
|
||||
}
|
||||
|
||||
// =================================================================================
|
||||
// 步骤 1: 静态校准 (此部分完全保留，不做任何修改)
|
||||
// 步骤 1: 静态校准 (此部分逻辑完全保留)
|
||||
// ---------------------------------------------------------------------------------
|
||||
if (SL_SC7U22_Error_Flag == 0) {
|
||||
// ... (省略与原代码完全相同的部分以节省篇幅) ...
|
||||
// 计算当前数据与上一帧数据的差值，用于判断是否静止
|
||||
unsigned short acc_gyro_delta[2];
|
||||
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];
|
||||
}
|
||||
// 判断是否处于静止状态
|
||||
#if (ACC_RANGE == 2)
|
||||
if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 16384) < 3000)) {
|
||||
#elif (ACC_RANGE == 4)
|
||||
if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 8192) < 3000)) {
|
||||
#elif (ACC_RANGE == 8)
|
||||
if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 4096) < 3000)) {
|
||||
#elif (ACC_RANGE == 16)
|
||||
if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 2048) < 3000)) {
|
||||
#endif
|
||||
if (SL_SC7U22_Error_cnt < 200) SL_SC7U22_Error_cnt++;
|
||||
} else {
|
||||
SL_SC7U22_Error_cnt = 0;
|
||||
}
|
||||
// 如果静止时间足够长
|
||||
if (SL_SC7U22_Error_cnt > 190) {
|
||||
for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] += acc_gyro_input[sl_i];
|
||||
SL_SC7U22_Error_cnt2++;
|
||||
@ -1018,7 +1069,15 @@ unsigned char Original_SL_SC7U22_Angle_Output(unsigned char calibration_en, sign
|
||||
for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] = Sum_Avg_Accgyro[sl_i] / 50;
|
||||
Error_Accgyro[0] = 0 - Sum_Avg_Accgyro[0];
|
||||
Error_Accgyro[1] = 0 - Sum_Avg_Accgyro[1];
|
||||
#if ACC_RANGE==2
|
||||
Error_Accgyro[2] = 16384 - Sum_Avg_Accgyro[2];
|
||||
#elif ACC_RANGE==4
|
||||
Error_Accgyro[2] = 8192 - Sum_Avg_Accgyro[2];
|
||||
#elif ACC_RANGE==8
|
||||
Error_Accgyro[2] = 4096 - Sum_Avg_Accgyro[2];
|
||||
#elif ACC_RANGE==16
|
||||
Error_Accgyro[2] = 2048 - Sum_Avg_Accgyro[2];
|
||||
#endif
|
||||
Error_Accgyro[3] = 0 - Sum_Avg_Accgyro[3];
|
||||
Error_Accgyro[4] = 0 - Sum_Avg_Accgyro[4];
|
||||
Error_Accgyro[5] = 0 - Sum_Avg_Accgyro[5];
|
||||
@ -1033,7 +1092,7 @@ unsigned char Original_SL_SC7U22_Angle_Output(unsigned char calibration_en, sign
|
||||
}
|
||||
|
||||
// =================================================================================
|
||||
// 步骤 2: 姿态解算 (使用一阶互补滤波)
|
||||
// 步骤 2: 姿态解算 (一阶互补滤波)
|
||||
// ---------------------------------------------------------------------------------
|
||||
if (SL_SC7U22_Error_Flag == 1) { // 确认已经校准完成
|
||||
// --- 2.1 数据预处理 ---
|
||||
@ -1043,49 +1102,63 @@ unsigned char Original_SL_SC7U22_Angle_Output(unsigned char calibration_en, sign
|
||||
}
|
||||
|
||||
// --- 2.2 使用加速度计计算姿态角 (Pitch 和 Roll) ---
|
||||
// 注意：这里使用了atan2f来计算Roll角，它比atanf更稳定，能自动处理所有象限，避免了后续的if判断。
|
||||
float acc_x = (float)Temp_Accgyro[0];
|
||||
float acc_y = (float)Temp_Accgyro[1];
|
||||
float acc_z = (float)Temp_Accgyro[2];
|
||||
|
||||
// Pitch = arcsin(ax / g)
|
||||
angle_acc[0] = asinf(acc_x / sqrtf(acc_x*acc_x + acc_y*acc_y + acc_z*acc_z)) * 57.29578f;
|
||||
// 使用 atan2 来计算角度，避免奇点问题和象限问题，更稳健
|
||||
// Pitch = arctan(-ax / sqrt(ay^2 + az^2))
|
||||
angle_acc[0] = atan2f(-acc_x, sqrtf(acc_y*acc_y + acc_z*acc_z)) * 57.29578f;
|
||||
// Roll = arctan(ay / az)
|
||||
angle_acc[1] = atan2f(acc_y, acc_z) * 57.29578f;
|
||||
|
||||
// --- 2.3 转换陀螺仪数据单位 ---
|
||||
// 将陀螺仪原始值（LSB）转换为角速度（度/秒）
|
||||
gyro_val[0] = Temp_Accgyro[4] * 0.061; // GYR-Y -> Pitch 速率
|
||||
gyro_val[1] = Temp_Accgyro[3] * 0.061; // GYR-X -> Roll 速率
|
||||
gyro_val[2] = Temp_Accgyro[5] * 0.061; // GYR-Z -> Yaw 速率
|
||||
// **注意轴向对应关系**
|
||||
gyro_dps[0] = Temp_Accgyro[3] * 0.061f; // GYR-X -> Roll 速率
|
||||
gyro_dps[1] = Temp_Accgyro[4] * 0.061f; // GYR-Y -> Pitch 速率
|
||||
gyro_dps[2] = Temp_Accgyro[5] * 0.061f; // GYR-Z -> Yaw 速率
|
||||
|
||||
// =================================================================================
|
||||
// 步骤 2.4: 一阶互补滤波
|
||||
// 公式: angle = alpha * (angle + gyro_rate * dt) + (1 - alpha) * acc_angle
|
||||
// ---------------------------------------------------------------------------------
|
||||
// Pitch 轴融合
|
||||
Angle_output[0] = FILTER_ALPHA * (Angle_output[0] + gyro_val[0] * SAMPLE_INTERVAL) + (1.0f - FILTER_ALPHA) * angle_acc[0];
|
||||
// Pitch 轴融合 (Angle_output[0] 对应 Pitch)
|
||||
Angle_output[0] = FILTER_ALPHA * (Angle_output[0] + gyro_dps[1] * dt) + (1.0f - FILTER_ALPHA) * angle_acc[0];
|
||||
|
||||
// Roll 轴融合
|
||||
Angle_output[1] = FILTER_ALPHA * (Angle_output[1] + gyro_val[1] * SAMPLE_INTERVAL) + (1.0f - FILTER_ALPHA) * angle_acc[1];
|
||||
// Roll 轴融合 (Angle_output[1] 对应 Roll)
|
||||
Angle_output[1] = FILTER_ALPHA * (Angle_output[1] + gyro_dps[0] * dt) + (1.0f - FILTER_ALPHA) * angle_acc[1];
|
||||
|
||||
/************** Yaw 轴计算 **************/
|
||||
// Yaw角无法通过加速度计（重力）来校正，因此只能使用陀螺仪进行简单积分。
|
||||
// 如果需要精确的Yaw角，必须引入磁力计进行修正。
|
||||
if (yaw_rst == 1) {
|
||||
Angle_output[2] = 0; // 如果有复位信号，则清零
|
||||
}
|
||||
|
||||
// 增加一个简单的阈值，当角速度较小时，认为没有转动，以减少漂移
|
||||
if (SL_GetAbsShort(Temp_Accgyro[5]) > 8) {
|
||||
Angle_output[2] += gyro_val[2] * SAMPLE_INTERVAL;
|
||||
if (fabsf(gyro_dps[2]) > 0.1f) { // 使用转换后的dps值判断更直观
|
||||
Angle_output[2] += gyro_dps[2] * dt;
|
||||
}
|
||||
|
||||
// 保持Yaw角在 -180 到 180 度之间 (可选，但推荐)
|
||||
if (Angle_output[2] > 180.0f) {
|
||||
Angle_output[2] -= 360.0f;
|
||||
} else if (Angle_output[2] < -180.0f) {
|
||||
Angle_output[2] += 360.0f;
|
||||
}
|
||||
|
||||
// --- 将校准后的数据写回输入数组 (可选，根据你的需求保留或移除) ---
|
||||
#if 1
|
||||
for (sl_i = 0; sl_i < 6; sl_i++) {
|
||||
acc_gyro_input[sl_i] = Temp_Accgyro[sl_i];
|
||||
}
|
||||
#endif
|
||||
|
||||
return 1; // 返回1，表示计算成功
|
||||
}
|
||||
|
||||
return 2; // 校准未完成，返回错误状态
|
||||
|
||||
}
|
||||
|
||||
unsigned char get_calibration_state(void){
|
||||
return SL_SC7U22_Error_Flag;
|
||||
}
|
||||
|
||||
@ -4,6 +4,6 @@
|
||||
// #define KS_BLE 1
|
||||
#define XTELL_TEST 1
|
||||
|
||||
#define ACC_RANGE 4 //g，加速度满量程
|
||||
#define ACC_RANGE 16 //g，加速度满量程:2、4、8、16
|
||||
|
||||
#endif
|
||||
@ -149,7 +149,7 @@ void xtell_app_main()
|
||||
//////////////////////////////////////////////////
|
||||
|
||||
//开机必须延时关闭经典蓝牙，不然底层代码会再次把蓝牙 打开
|
||||
create_process(&close_BL_number, "close_BL",NULL, close_BL, 3000);
|
||||
// create_process(&close_BL_number, "close_BL",NULL, close_BL, 3000);
|
||||
|
||||
|
||||
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -6364,7 +6364,7 @@ objs/apps/earphone/xtell_Sensor/send_data.c.o
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,sensor_read_data,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,circle_buffer_is_full,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,SL_SC7U22_RawData_Read,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,SL_SC7U22_Angle_Output,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,Original_SL_SC7U22_Angle_Output,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,printf,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,circle_buffer_write,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,calculate_data,pl
|
||||
@ -6380,7 +6380,7 @@ objs/apps/earphone/xtell_Sensor/send_data.c.o
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,gpio_set_direction,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,gpio_set_pull_up,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,gpio_direction_output,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,os_time_dly,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,delay_2ms,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,SL_SC7U22_Config,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,circle_buffer_init,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,create_process,l
|
||||
@ -6434,8 +6434,9 @@ objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o
|
||||
-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,asinf,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,atanf,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,Original_SL_SC7U22_Angle_Output,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,sqrtf,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,atan2f,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,sqrtf,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,fabsf,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,get_calibration_state,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,gravity_sensor_command,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,_gravity_sensor_get_ndata,l
|
||||
@ -6480,17 +6481,18 @@ objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o
|
||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,start_detection,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,stop_detection,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,skiing_tracker_init,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,remove_gravity_in_device_frame,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,cosf,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,sinf,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,forece_of_friction,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,skiing_tracker_update,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,sqrtf,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,sensor_processing_task,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,fabsf,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,cosf,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,sinf,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,puts,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,debug2,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,KS_data,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,debug1,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,debug2,pl
|
||||
cpu/br28/liba/cpu.a.llvm.19376.crc16.c
|
||||
-r=cpu/br28/liba/cpu.a.llvm.19376.crc16.c,__crc16_mutex_init,pl
|
||||
-r=cpu/br28/liba/cpu.a.llvm.19376.crc16.c,os_mutex_create,l
|
||||
|
||||
332604
cpu/br28/tools/sdk.lst
332604
cpu/br28/tools/sdk.lst
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Reference in New Issue
Block a user