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-03 18:48:15 +08:00
parent 97e85df2f8
commit 671730a351
96 changed files with 170886 additions and 186935 deletions
Download Patch File Download Diff File Expand all files Collapse all files

153
apps/earphone/xtell_Sensor/calculate/skiing_tracker.c Normal file
View File

@ -0,0 +1,153 @@
#include "skiing_tracker.h"
#include <math.h> // 使用 sqrtf, fabsf, atan2f
#include <string.h> // 使用 memset
// ======================= 用户可配置参数 =======================
// IMU的采样率 (Hz)
#define SAMPLE_RATE 100.0f
#define DT (1.0f / SAMPLE_RATE)
// 传感器灵敏度配置 (必须与硬件配置匹配)
// 加速度计量程: ±8G -> 1G = 32768 / 8 = 4096 LSB
#define ACCEL_SENSITIVITY 4096.0f // LSB/g
#define GRAVITY_MSS 9.80665f // 标准重力加速度 (m/s^2)
// 陀螺仪灵敏度 (2000dps)
#define GYRO_SENSITIVITY 16.4f // LSB/(deg/s)
// 状态检测阈值
#define MOTION_ACCEL_THRESHOLD (ACCEL_SENSITIVITY * 0.3f) // 加速度变化超过0.3g认为在运动
#define MOTION_GYRO_THRESHOLD (GYRO_SENSITIVITY * 90.0f) // 角速度超过xx dps认为在运动
#define STILL_SAMPLES_FOR_CALIBRATION 100 // 连续静止1秒 (100个点) 开始校准
#define CALIBRATION_SAMPLE_COUNT 50 // 用于平均的校准样本数 (0.5秒)
#define MOTION_SAMPLES_TO_START_SKIING 10 // 连续运动0.1秒开始滑行
#define STILL_SAMPLES_TO_STOP_SKIING 20 // 连续静止0.2秒停止滑行
// 角度转弧度
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#define RAD_TO_DEG(rad) ((rad) * 180.0f / M_PI)
// ======================= 内部实现 =======================
void SkiingTracker_Init(SkiingTracker* tracker) {
memset(tracker, 0, sizeof(SkiingTracker));
tracker->state = STATE_UNCALIBRATED;
}
// 简单的低通滤波器
static float low_pass_filter(float new_input, float prev_output, float alpha) {
return alpha * new_input + (1.0f - alpha) * prev_output;
}
static void reset_calibration(SkiingTracker* tracker) {
tracker->calib_samples_count = 0;
tracker->calib_acc_sum[0] = 0;
tracker->calib_acc_sum[1] = 0;
tracker->calib_acc_sum[2] = 0;
}
// 核心更新函数
void SkiingTracker_Update(SkiingTracker* tracker, signed short* raw_accel, signed short* raw_gyro) {
// 运动状态检测
// 使用原始数据进行判断，避免校准误差影响
float acc_mag = sqrtf((float)raw_accel[0] * raw_accel[0] + (float)raw_accel[1] * raw_accel[1] + (float)raw_accel[2] * raw_accel[2]);
float gyro_mag = sqrtf((float)raw_gyro[0] * raw_gyro[0] + (float)raw_gyro[1] * raw_gyro[1] + (float)raw_gyro[2] * raw_gyro[2]);
int is_moving = (fabsf(acc_mag - ACCEL_SENSITIVITY) > MOTION_ACCEL_THRESHOLD) || (gyro_mag > MOTION_GYRO_THRESHOLD);
if (is_moving) {
tracker->still_counter = 0;
tracker->motion_counter++;
printf("===motion count===\n");
} else {
tracker->motion_counter = 0;
tracker->still_counter++;
printf("===still count===\n");
}
// 2. 状态机处理
switch (tracker->state) {
case STATE_UNCALIBRATED:
if (tracker->still_counter > STILL_SAMPLES_FOR_CALIBRATION) {
tracker->state = STATE_CALIBRATING;
reset_calibration(tracker);
}
break;
case STATE_CALIBRATING:
if (is_moving) { // 如果在校准时移动了，则校准失败，返回未校准状态
tracker->state = STATE_UNCALIBRATED;
reset_calibration(tracker);
break;
}
// 累加采样数据
tracker->calib_acc_sum[0] += raw_accel[0];
tracker->calib_acc_sum[1] += raw_accel[1];
tracker->calib_acc_sum[2] += raw_accel[2];
tracker->calib_samples_count++;
if (tracker->calib_samples_count >= CALIBRATION_SAMPLE_COUNT) {
// 校准完成，计算平均重力矢量
tracker->static_gravity[0] = (short)(tracker->calib_acc_sum[0] / CALIBRATION_SAMPLE_COUNT);
tracker->static_gravity[1] = (short)(tracker->calib_acc_sum[1] / CALIBRATION_SAMPLE_COUNT);
tracker->static_gravity[2] = (short)(tracker->calib_acc_sum[2] / CALIBRATION_SAMPLE_COUNT);
// 计算坡度（可选，用于显示）
float horiz_g = sqrtf((float)tracker->static_gravity[0] * tracker->static_gravity[0] + (float)tracker->static_gravity[1] * tracker->static_gravity[1]);
float vert_g = fabsf((float)tracker->static_gravity[2]);
tracker->slope_angle_deg = RAD_TO_DEG(atan2f(horiz_g, vert_g));
tracker->state = STATE_READY;
}
break;
case STATE_READY:
if (tracker->motion_counter > MOTION_SAMPLES_TO_START_SKIING) {
tracker->state = STATE_SKIING;
}
break;
case STATE_SKIING:
if (tracker->still_counter > STILL_SAMPLES_TO_STOP_SKIING) {
tracker->state = STATE_STOPPED;
tracker->velocity = 0.0f; // 零速更新
tracker->forward_accel = 0.0f;
break;
}
// 计算线性加速度 (重力抵消)
// 假设传感器的X轴指向滑雪板前进方向
long linear_accel_x_lsb = (long)raw_accel[0] - tracker->static_gravity[0];
// 转换为 m/s^2
float current_accel_mss = (float)linear_accel_x_lsb / ACCEL_SENSITIVITY * GRAVITY_MSS;
// 低通滤波以平滑加速度
tracker->forward_accel = low_pass_filter(current_accel_mss, tracker->forward_accel, 0.3f);
// 积分计算速度和距离 (梯形积分)
float prev_velocity = tracker->velocity;
tracker->velocity += tracker->forward_accel * DT;
// 物理约束：速度不能为负（不能往坡上滑）
if (tracker->velocity < 0) {
tracker->velocity = 0;
}
tracker->distance += (prev_velocity + tracker->velocity) / 2.0f * DT;
break;
case STATE_STOPPED:
// 在停止状态下，如果再次检测到运动，则重新进入滑行状态
if (tracker->motion_counter > MOTION_SAMPLES_TO_START_SKIING) {
tracker->state = STATE_SKIING;
}
// 如果长时间静止，返回未校准状态，以应对更换雪道的情况
// if (tracker->still_counter > 3000) { // e.g., 30 seconds
// tracker->state = STATE_UNCALIBRATED;
// }
break;
}
}

53
apps/earphone/xtell_Sensor/calculate/skiing_tracker.h Normal file
View File

@ -0,0 +1,53 @@
#ifndef SKIING_TRACKER_H
#define SKIING_TRACKER_H
// 定义滑雪者的运动状态
typedef enum {
STATE_UNCALIBRATED, // 未校准，等待在斜坡上静止
STATE_CALIBRATING, // 正在校准重力矢量
STATE_READY, // 校准完成，准备滑行
STATE_SKIING, // 正在滑行
STATE_STOPPED // 在斜坡上中途停止
} MotionState;
// 存储所有运动学数据
typedef struct {
// 最终输出
float velocity; // 沿斜坡方向的速度 (m/s)
float distance; // 沿斜坡方向的滑行距离 (m)
float slope_angle_deg; // 动态计算出的坡度 (度)
// 内部状态变量
MotionState state; // 当前运动状态
// 校准相关
short static_gravity[3]; // 校准后得到的静态重力矢量 (LSB)
long calib_acc_sum[3]; // 用于计算平均值的累加器
int calib_samples_count; // 校准采样计数
// 运动检测
int motion_counter;
int still_counter; // 静止状态计数器
// 物理量
float forward_accel; // 沿滑行方向的加速度 (m/s^2)
} SkiingTracker;
/**
* @brief 初始化滑雪追踪器
* @param tracker 指向 SkiingTracker 实例的指针
*/
void SkiingTracker_Init(SkiingTracker* tracker);
/**
* @brief 处理IMU数据，自动校准并计算速度和距离
* @details 这是核心处理函数，应在每次获取新的IMU数据后调用。
*
* @param tracker 指向 SkiingTracker 实例的指针
* @param raw_accel 未经校准的原始加速度数据 [X, Y, Z]，单位是 LSB
* @param raw_gyro 未经校准的原始陀螺仪数据 [X, Y, Z]，单位是 LSB
*/
void SkiingTracker_Update(SkiingTracker* tracker, signed short* raw_accel, signed short* raw_gyro);
#endif // SKIING_TRACKER_H