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

cun

Browse Source
This commit is contained in:
lmx
2025-11-18 17:27:06 +08:00
parent d0d9c0a630
commit ebca849be3
8 changed files with 1123 additions and 165 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

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

88
apps/earphone/xtell_Sensor/A_hide/11_test/skiing_tracker.h Normal file
View 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, float* quaternion);
#endif // SKIING_TRACKER_H