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

存档

Browse Source
This commit is contained in:
lmx
2025-11-18 10:15:00 +08:00
parent b621ef7e44
commit d0d9c0a630
46 changed files with 172875 additions and 173101 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -79,7 +79,62 @@ static void calculate_air_distance(skiing_tracker_t *tracker) {
}
/**
* @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
@ -88,9 +143,9 @@ static void calculate_air_distance(skiing_tracker_t *tracker) {
*/
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 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);
@ -189,20 +244,30 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d
#if 1 //测试禁止状态下陀螺仪的三轴加速度，去掉重力分量后是否正常
float tmp_device_acc[3];
float tmp_world_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);
transform_acc_to_world_frame(acc_device_ms2,angle,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]),tmp_world_acc; //去掉重力加速度
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++;