重力分量去除后仍有偏差
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);
|
||||
|
||||
// 计算处理后加速度的水平模长
|
||||
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++;
|
||||
|
||||
tracker->buffer_index++;
|
||||
if (tracker->buffer_index >= VARIANCE_BUFFER_SIZE) {
|
||||
tracker->buffer_index = 0;
|
||||
tracker->buffer_filled = 1; // 标记缓冲区已满
|
||||
}
|
||||
#endif
|
||||
|
||||
// --- 更新状态机 ---
|
||||
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; // 垂直速度强制归零
|
||||
}
|
||||
|
||||
#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,16 +331,23 @@ 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;
|
||||
char* division = "==========\n";
|
||||
@ -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];
|
||||
Error_Accgyro[2] = 8192 - Sum_Avg_Accgyro[2];
|
||||
#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 不带卡尔曼的欧若拉数据输出，以及消除零点漂移后的六轴数据
|
||||
*
|
||||
* @param calibration_en
|
||||
* @param acc_gyro_input
|
||||
* @param Angle_output
|
||||
* @param yaw_rst
|
||||
* @return unsigned char
|
||||
* @brief 姿态角解算函数 (基于一阶互补滤波)
|
||||
* @details
|
||||
* 该函数主要完成两项工作：
|
||||
* 1. 静态校准：在初始阶段，检测传感器是否处于静止状态。如果是，则计算加速度计和陀螺仪的零点偏移（误差），用于后续的数据补偿。
|
||||
* 2. 姿态解算：使用一阶互补滤波器融合经过校准后的加速度计和陀螺仪数据，计算出物体的俯仰角（Pitch）、横滚角（Roll）和偏航角（Yaw）。
|
||||
*
|
||||
* @param calibration_en 传入：外部校准使能标志。如果为0，则强制认为已经校准完成。
|
||||
* @param acc_gyro_input 传入和传出：包含6轴原始数据的数组指针，顺序为 [ACC_X, ACC_Y, ACC_Z, GYR_X, GYR_Y, GYR_Z]。该函数会对其进行原地修改，填充为校准后的数据。
|
||||
* @param Angle_output 传出：滤波后的结果，顺序为 [Pitch, Roll, Yaw]。
|
||||
* @param yaw_rst 传入：Yaw轴重置标志。如果为1，则将Yaw角清零。
|
||||
*
|
||||
* @return
|
||||
* - 0: 正在进行静态校准。
|
||||
* - 1: 姿态角计算成功。
|
||||
* - 2: 校准未完成，无法进行计算。
|
||||
*/
|
||||
unsigned char 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 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;
|
||||
|
||||
unsigned char sl_i = 0;
|
||||
float angle_acc[3] = {0};
|
||||
float gyro_val[3] = {0};
|
||||
float angle_acc[3] = {0}; // [Pitch_acc, Roll_acc, unused]
|
||||
float gyro_dps[3] = {0}; // [Roll_rate, Pitch_rate, Yaw_rate]
|
||||
|
||||
// 如果外部强制使能校准，则将标志位置1
|
||||
// 如果外部强制禁用校准，则将标志位置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];
|
||||
@ -1031,9 +1090,9 @@ unsigned char Original_SL_SC7U22_Angle_Output(unsigned char calibration_en, sign
|
||||
}
|
||||
return 0; // 返回0，表示正在校准
|
||||
}
|
||||
|
||||
|
||||
// =================================================================================
|
||||
// 步骤 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];
|
||||
|
||||
/************** Yaw 轴计算**************/
|
||||
// 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);
|
||||
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user