From ebca849be32ecb84aaee0064eddcf03fa9d7965e Mon Sep 17 00:00:00 2001 From: lmx Date: Tue, 18 Nov 2025 17:27:06 +0800 Subject: [PATCH] cun --- .../A_hide/11_test/skiing_tracker.c | 377 ++++++++++++ .../A_hide/11_test/skiing_tracker.h | 88 +++ apps/earphone/xtell_Sensor/README.md | 10 + .../xtell_Sensor/calculate/skiing_tracker.c | 541 +++++++++++++----- .../xtell_Sensor/calculate/skiing_tracker.h | 4 +- apps/earphone/xtell_Sensor/send_data.c | 37 +- apps/earphone/xtell_Sensor/sensor/SC7U22.c | 230 +++++++- apps/earphone/xtell_Sensor/sensor/SC7U22.h | 1 + 8 files changed, 1123 insertions(+), 165 deletions(-) create mode 100644 apps/earphone/xtell_Sensor/A_hide/11_test/skiing_tracker.c create mode 100644 apps/earphone/xtell_Sensor/A_hide/11_test/skiing_tracker.h diff --git a/apps/earphone/xtell_Sensor/A_hide/11_test/skiing_tracker.c b/apps/earphone/xtell_Sensor/A_hide/11_test/skiing_tracker.c new file mode 100644 index 0000000..43aa73f --- /dev/null +++ b/apps/earphone/xtell_Sensor/A_hide/11_test/skiing_tracker.c @@ -0,0 +1,377 @@ +/* + 使用四元数求角度和去掉重力分量 +*/ +#include "skiing_tracker.h" +#include "../sensor/SC7U22.h" +#include +#include + +#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; +} + diff --git a/apps/earphone/xtell_Sensor/A_hide/11_test/skiing_tracker.h b/apps/earphone/xtell_Sensor/A_hide/11_test/skiing_tracker.h new file mode 100644 index 0000000..b1021f2 --- /dev/null +++ b/apps/earphone/xtell_Sensor/A_hide/11_test/skiing_tracker.h @@ -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 \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/README.md b/apps/earphone/xtell_Sensor/README.md index aaede93..85bbea1 100644 --- a/apps/earphone/xtell_Sensor/README.md +++ b/apps/earphone/xtell_Sensor/README.md @@ -53,3 +53,13 @@ sensor_processing_task当中就进行了计算，包括卡尔曼等，在timer - 要使用只需要把代码复制粘贴到calculate文件夹下 + +# 11.18 + +去除重力分量后仍有误差： + +- 数据对齐？ +- 有没有丢数据？ +- 重复定位的数据？ +- 静态时的角度误差？ + diff --git a/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c b/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c index 8741260..1e84dec 100644 --- a/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c +++ b/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c @@ -6,22 +6,86 @@ #include #include -#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; +// --- 静止检测 --- +//两个判断是否静止的必要条件：动态零速更新(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; +static float quaternion_data[4]; #ifdef XTELL_TEST debug_t debug1; @@ -79,108 +143,56 @@ 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 在设备坐标系下，从原始加速度数据中移除重力分量 + * @brief 使用四元数直接从设备坐标系的加速度中移除重力分量 + * @details 这种方法比使用欧拉角更精确、更稳定，且避免了万向节死锁。 * @param acc_device 输入：设备坐标系下的原始加速度 [x, y, z], 单位 m/s^2 - * @param angle 输入：姿态角 [pitch, roll, yaw]，单位: 度 + * @param q 输入：表示姿态的四元数 [w, x, y, z] * @param acc_linear_device 输出：设备坐标系下移除重力后的线性加速度 [x, y, z] */ -void remove_gravity_in_device_frame(const float *acc_device, const float *angle, float *acc_linear_device) +void q_remove_gravity_with_quaternion(const float *acc_device, const float *q, 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 轴 + // 从四元数计算重力在设备坐标系下的投影 + // 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]; - 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]; + // 从原始加速度中减去重力分量 + 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 计算缓冲区内三轴数据的方差之和 * @@ -216,7 +228,170 @@ 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]); //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 * 2 && 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; + } + + +} /** @@ -242,36 +417,100 @@ 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; - #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]); + // 将最新数据存入缓冲区 + 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)); - - 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; - + 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: + q_transform_to_world_with_quaternion(acc_device_ms2, quaternion_data, 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; } - count++; - - #endif } @@ -285,12 +524,16 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d * @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) { +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; @@ -339,21 +582,21 @@ 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; } diff --git a/apps/earphone/xtell_Sensor/calculate/skiing_tracker.h b/apps/earphone/xtell_Sensor/calculate/skiing_tracker.h index 5188afa..750180d 100644 --- a/apps/earphone/xtell_Sensor/calculate/skiing_tracker.h +++ b/apps/earphone/xtell_Sensor/calculate/skiing_tracker.h @@ -30,7 +30,7 @@ typedef struct { skiing_state_t state; // 当前滑雪状态 // 内部计算使用的私有成员 - float acc_world[3]; // 在世界坐标系下的加速度 + float acc_no_g[3]; // 去掉重力分量后的加速度 // 用于空中距离计算 float time_in_air; // 滞空时间计时器 @@ -84,5 +84,5 @@ typedef struct{ */ 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) ; +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 \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/send_data.c b/apps/earphone/xtell_Sensor/send_data.c index c700a85..570b877 100644 --- a/apps/earphone/xtell_Sensor/send_data.c +++ b/apps/earphone/xtell_Sensor/send_data.c @@ -173,12 +173,13 @@ void test(){ -#define SENSOR_DATA_BUFFER_SIZE 100 // 定义缓冲区可以存储100个sensor_data_t元素 +#define SENSOR_DATA_BUFFER_SIZE 500 // 定义缓冲区可以存储100个sensor_data_t元素 static circle_buffer_t sensor_read; // 环形缓冲区管理结构体 typedef struct { signed short acc_data[3]; signed short gyr_data[3]; float angle[3]; //pitch roll yaw + float quaternion_output[4]; } sensor_data_t; static sensor_data_t sensor_read_buffer[SENSOR_DATA_BUFFER_SIZE]; // 存放sensor读到的数据 @@ -187,7 +188,8 @@ static BLE_send_data_t sensor_send_buffer[SENSOR_DATA_BUFFER_SIZE]; // 存放ble static u8 mutex1 = 0; static u8 mutex2 = 0; - +static int count_test1 = 0; +static int count_test2 = 0; /** * @brief //读取传感器的数据放进缓冲区 * @@ -238,6 +240,8 @@ void sensor_read_data(){ memcpy(tmp.gyr_data, &combined_raw_data[3], 3 * sizeof(signed short)); if(mutex1 == 0){ mutex1 = 1; + // count_test1++; + // xlog("count_test_1: %d\n",count_test1); circle_buffer_write(&sensor_read, &tmp); mutex1 = 0; } @@ -264,8 +268,9 @@ void calculate_data(){ }else{ return; } + - BLE_send_data_t data_by_calculate = sensor_processing_task(tmp.acc_data, tmp.gyr_data,tmp.angle); + BLE_send_data_t data_by_calculate = sensor_processing_task(tmp.acc_data, tmp.gyr_data,tmp.angle,tmp.quaternion_output); @@ -455,10 +460,12 @@ void sensor_measure(void){ static int initialized = 0; static int calibration_done = 0; char status = 0; - if(circle_buffer_is_full(&sensor_read)){ - // xlog("sensor_read_data: read buffer full\n"); - return; + + if(count_test1 >= 100){ + count_test1 = 0; + xlog("count_test1\n"); } + count_test1++; static sensor_data_t tmp; SL_SC7U22_RawData_Read(tmp.acc_data,tmp.gyr_data); @@ -467,9 +474,10 @@ void sensor_measure(void){ 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 = SIX_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0); // status = Original_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0); + status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0, tmp.quaternion_output); int count = 0; if(count > 100){ @@ -487,15 +495,18 @@ void sensor_measure(void){ } } 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 = SIX_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0); // status = Original_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0); + status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0, tmp.quaternion_output); memcpy(tmp.acc_data, &combined_raw_data[0], 3 * sizeof(signed short)); memcpy(tmp.gyr_data, &combined_raw_data[3], 3 * sizeof(signed short)); - BLE_send_data_t data_by_calculate = sensor_processing_task(tmp.acc_data, tmp.gyr_data,tmp.angle); + BLE_send_data_t data_by_calculate = sensor_processing_task(tmp.acc_data, tmp.gyr_data,tmp.angle, tmp.quaternion_output); extern void ano_send_attitude_data(float rol, float pit, float yaw, uint8_t fusion_sta) ; ano_send_attitude_data(tmp.angle[0],tmp.angle[1],tmp.angle[2], 1); } + + // xlog("=======sensor_read_data END\n"); } @@ -533,7 +544,7 @@ void xtell_task_create(void){ ano_protocol_init(115200); - // create_process(&calculate_data_id, "calculate",NULL, sensor_measure, 10); + create_process(&calculate_data_id, "calculate",NULL, sensor_measure, 10); circle_buffer_init(&sensor_read, sensor_read_buffer, SENSOR_DATA_BUFFER_SIZE, sizeof(sensor_data_t)); @@ -545,11 +556,11 @@ void xtell_task_create(void){ xlog("SkiingTracker_Init\n"); - create_process(&sensor_read_data_id, "read",NULL, sensor_read_data, 10); + // create_process(&sensor_read_data_id, "read",NULL, sensor_read_data, 10); - create_process(&calculate_data_id, "calculate",NULL, calculate_data, 6); + // create_process(&calculate_data_id, "calculate",NULL, calculate_data, 4); - create_process(&ble_send_data_id, "send",NULL, BLE_send_data, 1); + // create_process(&ble_send_data_id, "send",NULL, BLE_send_data, 1); #if 0 hw_iic_init_result = ret; diff --git a/apps/earphone/xtell_Sensor/sensor/SC7U22.c b/apps/earphone/xtell_Sensor/sensor/SC7U22.c index f6083d1..771c379 100644 --- a/apps/earphone/xtell_Sensor/sensor/SC7U22.c +++ b/apps/earphone/xtell_Sensor/sensor/SC7U22.c @@ -129,6 +129,8 @@ 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, 0x40, 0xA8);//ACC_CON 高性能模式，100Hz，平均数4 -- lmx + #if ACC_RANGE==2 SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x41, 0x00);//ACC_RANGE 00：±2G #endif @@ -144,8 +146,9 @@ unsigned char SL_SC7U22_Config(void) // 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 - SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0xAC);//GYR_CONF 1600Hz -- lmx + // SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0xAC);//GYR_CONF 1600Hz -- lmx // SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0xAB);//GYR_CONF 800Hz -- lmx + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0xE8);//GYR_CONF 100Hz, 噪声优化开启,4个平均一次 -- lmx SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, 0x00);//GYR_RANGE 2000dps SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, 0x00);//GYR_RANGE 2000dps @@ -997,6 +1000,10 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short return 2; // 校准未完成，返回错误状态 } + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// +// /** * @brief 姿态角解算函数 (基于一阶互补滤波) * @details @@ -1164,6 +1171,227 @@ unsigned char get_calibration_state(void){ } +///////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// 如果没有定义 PI，请取消下面的注释 +// #define PI 3.14159265358979323846f + +// ================================================================================================= +// Mahony AHRS (Attitude and Heading Reference System) 相关变量定义 +// Mahony滤波器是一种高效的互补滤波器，它使用四元数来表示姿态，从而避免了万向节死锁问题。 +// 它通过一个PI控制器来校正陀ar螺仪的积分漂移。 +// ------------------------------------------------------------------------------------------------- +// --- 滤波器参数 --- +// Kp: 比例增益，决定了加速度计数据校正陀螺仪的权重。值越大，对加速度计的响应越快，但对运动加速度更敏感。 +// Ki: 积分增益，决定了用于校正陀螺仪静态漂移的权重。 +// Q_dt: 采样时间间隔（单位：秒），这里是10ms (0.01s)，对应100Hz的采样率。 +const float Kp = 2.0f; +const float Ki = 0.005f; +const float Q_dt = 0.01f; + +// --- 状态变量 --- +// 四元数 (Quaternion)，表示当前的姿态。初始化为 (1, 0, 0, 0)，代表初始姿态为水平。 +static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; +// 陀螺仪积分误差项，用于补偿静态漂移 +static float exInt = 0.0f, eyInt = 0.0f, ezInt = 0.0f; +// ================================================================================================= + + + + +/** +* @brief 姿态角解算函数 (基于四元数和Mahony滤波器) +* @details +* 该函数主要完成两项工作： +* 1. 静态校准：在初始阶段，检测传感器是否处于静止状态。如果是，则计算加速度计和陀螺仪的零点偏移（误差），用于后续的数据补偿。 +* 2. 姿态解算：使用基于四元数的Mahony互补滤波器融合经过校准后的加速度计和陀螺仪数据，计算出物体的俯仰角（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，则将整个姿态滤波器状态重置。 +* +* @return +* - 0: 正在进行静态校准。 +* - 1: 姿态角计算成功。 +* - 2: 校准未完成，无法进行计算。 +*/ +unsigned char Q_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst, float *quaternion_output) +{ + unsigned char sl_i = 0; + + // 如果外部强制禁用校准，则将标志位置1 + if (calibration_en == 0) { + SL_SC7U22_Error_Flag = 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++; + if (SL_SC7U22_Error_cnt2 > 49) { + SL_SC7U22_Error_Flag = 1; + SL_SC7U22_Error_cnt2 = 0; + SL_SC7U22_Error_cnt = 0; + 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]; + // xlog("AVG_Recode AX:%d,AY:%d,AZ:%d,GX:%d,GY:%d,GZ:%d\r\n", Sum_Avg_Accgyro[0], Sum_Avg_Accgyro[1], Sum_Avg_Accgyro[2], Sum_Avg_Accgyro[3], Sum_Avg_Accgyro[4], Sum_Avg_Accgyro[5]); + // xlog("Error_Recode AX:%d,AY:%d,AZ:%d,GX:%d,GY:%d,GZ:%d\r\n", Error_Accgyro[0], Error_Accgyro[1], Error_Accgyro[2], Error_Accgyro[3], Error_Accgyro[4], Error_Accgyro[5]); + } + } else { + SL_SC7U22_Error_cnt2 = 0; + for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] = 0; + } + return 0; // 返回0，表示正在校准 + } + + // ================================================================================= + // 步骤 2: 姿态解算 (Mahony AHRS) + // --------------------------------------------------------------------------------- + if (SL_SC7U22_Error_Flag == 1) { // 确认已经校准完成 + + // --- 2.1 Yaw轴/姿态重置 --- + // 注意：重置yaw会重置整个姿态滤波器，使设备回到初始水平姿态 + if (yaw_rst == 1) { + q0 = 1.0f; q1 = 0.0f; q2 = 0.0f; q3 = 0.0f; + exInt = 0.0f; eyInt = 0.0f; ezInt = 0.0f; + } + + // --- 2.2 数据预处理 --- + // 应用零点偏移补偿 + for (sl_i = 0; sl_i < 6; sl_i++) { + Temp_Accgyro[sl_i] = acc_gyro_input[sl_i] + Error_Accgyro[sl_i]; + } + + // 将校准后的数据写回输入数组 (可选) +#if 1 + for (sl_i = 0; sl_i < 6; sl_i++) { + acc_gyro_input[sl_i] = Temp_Accgyro[sl_i]; + } +#endif + + // 获取校准后的数据 + float ax = (float)Temp_Accgyro[0]; + float ay = (float)Temp_Accgyro[1]; + float az = (float)Temp_Accgyro[2]; + // 将陀螺仪数据从 LSB 转换为弧度/秒 (rad/s) + // 转换系数 0.061 ≈ 2000dps / 32768 LSB; PI/180 ≈ 0.01745 + float gx = (float)Temp_Accgyro[3] * 0.061f * 0.0174533f; // Roll rate + float gy = (float)Temp_Accgyro[4] * 0.061f * 0.0174533f; // Pitch rate + float gz = (float)Temp_Accgyro[5] * 0.061f * 0.0174533f; // Yaw rate + + // --- 2.3 Mahony算法核心 --- + float norm; + float vx, vy, vz; // 估计的重力向量 + float ex, ey, ez; // 加速度计测量和估计的重力向量之间的误差 + + // 归一化加速度计测量值，得到单位重力向量 + norm = sqrtf(ax * ax + ay * ay + az * az); + if (norm > 0.0f) { // 防止除以零 + ax = ax / norm; + ay = ay / norm; + az = az / norm; + + // 根据当前姿态(四元数)估计重力向量的方向 + vx = 2.0f * (q1 * q3 - q0 * q2); + vy = 2.0f * (q0 * q1 + q2 * q3); + vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3; + + // 计算测量值与估计值之间的误差（叉积） + ex = (ay * vz - az * vy); + ey = (az * vx - ax * vz); + ez = (ax * vy - ay * vx); + + // 积分误差项，用于消除陀螺仪的静态漂移 + exInt = exInt + ex * Ki * Q_dt; + eyInt = eyInt + ey * Ki * Q_dt; + ezInt = ezInt + ez * Ki * Q_dt; + + // 使用PI控制器校正陀螺仪的测量值 + gx = gx + Kp * ex + exInt; + gy = gy + Kp * ey + eyInt; + gz = gz + Kp * ez + ezInt; + } + + // 使用校正后的角速度更新四元数 (一阶毕卡法) + q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * 0.5f * Q_dt; + q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * 0.5f * Q_dt; + q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * 0.5f * Q_dt; + q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * 0.5f * Q_dt; + + // 归一化四元数，保持其单位长度 + norm = sqrtf(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 = q0 / norm; + q1 = q1 / norm; + q2 = q2 / norm; + q3 = q3 / norm; + + // --- 2.4 将四元数转换为欧拉角 (Pitch, Roll, Yaw) --- + // 转换公式将四元数转换为 ZYX 顺序的欧拉角 + // 结果单位为弧度，乘以 57.29578f 转换为度 + + // Pitch (绕Y轴旋转) + Angle_output[0] = asinf(-2.0f * (q1 * q3 - q0 * q2)) * 57.29578f; + + // Roll (绕X轴旋转) + Angle_output[1] = atan2f(2.0f * (q0 * q1 + q2 * q3), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * 57.29578f; + + // Yaw (绕Z轴旋转) - 注意：无磁力计的6轴IMU，Yaw角会随时间漂移 + Angle_output[2] = atan2f(2.0f * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.29578f; + + + if (quaternion_output != NULL) { + quaternion_output[0] = q0; // w + quaternion_output[1] = q1; // x + quaternion_output[2] = q2; // y + quaternion_output[3] = q3; // z + } + + return 1; // 返回1，表示计算成功 + } + + return 2; // 校准未完成，返回错误状态 +} + #endif /////////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/apps/earphone/xtell_Sensor/sensor/SC7U22.h b/apps/earphone/xtell_Sensor/sensor/SC7U22.h index 71bfa4b..60f05e3 100644 --- a/apps/earphone/xtell_Sensor/sensor/SC7U22.h +++ b/apps/earphone/xtell_Sensor/sensor/SC7U22.h @@ -132,6 +132,7 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en,signed short * unsigned char Original_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst); unsigned char SIX_SL_SC7U22_Angle_Output(unsigned char auto_calib_start, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst); +unsigned char Q_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst, float *quaternion_output); unsigned char get_calibration_state(void); /**寄存器宏定义*******************************/ #define SC7U22_WHO_AM_I 0x01