3

.vscode/settings.json

@@ -20,6 +20,8 @@
         "bluetooth.h": "c",
         "SCU722.C": "cpp",
         "math.h": "c",
-        "avctp_user.h": "c"
+        "avctp_user.h": "c",
+        "string.h": "c",
+        "dev_manager.h": "c"
     }
 }

apps/earphone/xtell_Sensor/A_hide/1/1/skiing_tracker.c

@@ -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;
+    }
+}

apps/earphone/xtell_Sensor/A_hide/1/1/skiing_tracker.h

@@ -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

apps/earphone/xtell_Sensor/A_hide/1/2/skiing_tracker.c

@@ -0,0 +1,207 @@
+#include "skiing_tracker.h"
+#include <math.h>   // 使用 sqrtf, fabsf, atan2f
+#include <string.h> // 使用 memset
+#include "system/includes.h"
+#include "btstack/btstack_task.h"
+#include "app_config.h"
+#include "app_action.h"
+#include "asm/pwm_led.h"
+#include "tone_player.h"
+#include "ui_manage.h"
+#include "gpio.h"
+#include "app_main.h"
+#include "asm/charge.h"
+#include "update.h"
+#include "app_power_manage.h"
+#include "audio_config.h"
+#include "app_charge.h"
+#include "bt_profile_cfg.h"
+#include "dev_manager/dev_manager.h"
+#include "update_loader_download.h"
+#include "../sensor/LIS2DH12.h"
+#include "../buffer/circle_buffer.h"
+#include "btstack/avctp_user.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 SAMPLE_RATE_HZ 100 // 传感器采样率，100Hz
+#define DT (1.0f / SAMPLE_RATE_HZ) // 每次处理的时间间隔
+#define TASK_PERIOD_MS (1000 / SAMPLE_RATE_HZ) // 任务执行周期, 10ms for 100Hz
+Skiing_State_t skiing_state = {0};
+
+// --- 用于BLE发送的数据包结构 ---
+typedef struct {
+    float speed_ms;
+    float distance_m;
+} sensor_ble_packet_t;
+
+/**
+ * @brief  处理滑雪数据，计算速度和距离
+ * @param  acc_gyro_input  传入：6轴原始数据 [AX, AY, AZ, GX, GY, GZ]
+ * @param  dt              传入：两次调用之间的时间间隔（秒），例如0.01s for 100Hz
+ * @retval None
+ */
+void SL_SC7U22_Process_Skiing_Data(signed short* acc_gyro_input, float dt)
+{
+    // =================================================================================
+    // 步骤 1: 姿态解算和静态校准
+    // =================================================================================
+    float angles_deg[3] = {0}; // [Pitch, Roll, Yaw] in degrees
+
+    // 调用您现有的姿态解算函数
+    // 假设 calibration_en = 1, yaw_rst = 0
+    unsigned char angle_status = SL_SC7U22_Angle_Output(1, acc_gyro_input, angles_deg, 0);
+
+    if (angle_status != 1) {
+        // 如果校准未完成或计算失败，则不进行后续处理
+        skiing_state.is_calibrated = 0;
+        return;
+    }
+    skiing_state.is_calibrated = 1;
+
+    // 将角度转换为弧度，用于后续三角函数计算
+    skiing_state.angles_rad[0] = angles_deg[0] * DEG_TO_RAD; // Pitch
+    skiing_state.angles_rad[1] = angles_deg[1] * DEG_TO_RAD; // Roll
+    skiing_state.angles_rad[2] = angles_deg[2] * DEG_TO_RAD; // Yaw
+
+    // =================================================================================
+    // 步骤 2: 检测滑雪者是否静止 (这是漂移抑制的关键)
+    // =================================================================================
+    // 静止检测逻辑：
+    // 1. 加速度的模长约等于1g
+    // 2. 角速度的模长非常小
+    long long accel_mag_sq = (long long)acc_gyro_input[0] * acc_gyro_input[0] +
+                             (long long)acc_gyro_input[1] * acc_gyro_input[1] +
+                             (long long)acc_gyro_input[2] * acc_gyro_input[2];
+
+    long long gyro_mag_sq = (long long)acc_gyro_input[3] * acc_gyro_input[3] +
+                            (long long)acc_gyro_input[4] * acc_gyro_input[4] +
+                            (long long)acc_gyro_input[5] * acc_gyro_input[5];
+
+    // --- 这些阈值需要根据实际测试进行精细调整 ---
+    const float G_SQ_UPPER = (G_ACCEL * 1.05f) * (G_ACCEL * 1.05f); // (1.05g)^2
+    const float G_SQ_LOWER = (G_ACCEL * 0.95f) * (G_ACCEL * 0.95f); // (0.95g)^2
+    const long long GYRO_MAG_SQ_THRESH = 300; // 对应约 1 deg/s 的抖动
+    const int STILLNESS_DURATION = 100; // 需要持续100个采样点（1秒 @ 100Hz）才确认为静止
+
+    if (accel_mag_sq > G_SQ_LOWER && accel_mag_sq < G_SQ_UPPER && gyro_mag_sq < GYRO_MAG_SQ_THRESH) {
+        if (skiing_state.stillness_counter < STILLNESS_DURATION) {
+            skiing_state.stillness_counter++;
+        }
+    } else {
+        skiing_state.stillness_counter = 0;
+        skiing_state.is_still = 0;
+    }
+
+    if (skiing_state.stillness_counter >= STILLNESS_DURATION) {
+        skiing_state.is_still = 1;
+    }
+
+    // 如果检测到静止，则重置速度（零速更新）
+    if (skiing_state.is_still) {
+        skiing_state.velocity[0] = 0.0f;
+        skiing_state.velocity[1] = 0.0f;
+        skiing_state.velocity[2] = 0.0f;
+        skiing_state.speed = 0.0f;
+        return; // 静止时，不需要进行积分
+    }
+
+    // =================================================================================
+    // 步骤 3: 坐标转换与重力补偿
+    // =================================================================================
+    float accel_body_g[3]; // 机体坐标系下的加速度，单位 g
+    accel_body_g[0] = (float)acc_gyro_input[0] / G_ACCEL;
+    accel_body_g[1] = (float)acc_gyro_input[1] / G_ACCEL;
+    accel_body_g[2] = (float)acc_gyro_input[2] / G_ACCEL;
+
+    // 使用旋转矩阵将机体坐标系加速度(Ab)转换到世界坐标系(An)
+    // An = C_b^n * Ab
+    // C_b^n 是从机体(b)到导航(n)的旋转矩阵，由姿态角决定
+    float sp = sinf(skiing_state.angles_rad[0]); // sin(pitch)
+    float cp = cosf(skiing_state.angles_rad[0]); // cos(pitch)
+    float sr = sinf(skiing_state.angles_rad[1]); // sin(roll)
+    float cr = cosf(skiing_state.angles_rad[1]); // cos(roll)
+    float sy = sinf(skiing_state.angles_rad[2]); // sin(yaw)
+    float cy = cosf(skiing_state.angles_rad[2]); // cos(yaw)
+
+    // 世界坐标系下的加速度（单位g），此时仍包含重力
+    float accel_nav_g[3];
+    accel_nav_g[0] = cy*cr*accel_body_g[0] + (cy*sp*sr - sy*cr)*accel_body_g[1] + (cy*sp*cr + sy*sr)*accel_body_g[2];
+    accel_nav_g[1] = sy*cr*accel_body_g[0] + (sy*sp*sr + cy*cr)*accel_body_g[1] + (sy*sp*cr - cy*sr)*accel_body_g[2];
+    accel_nav_g[2] = -sr*accel_body_g[0] +      cp*sr*accel_body_g[1] +      cp*cr*accel_body_g[2];
+
+    // 减去重力分量。在世界坐标系下，重力总是指向Z轴负方向（假设Z轴向上），大小为1g
+    // 如果您的世界坐标系Z轴向下，则应为 accel_nav_g[2] -= 1.0f;
+    accel_nav_g[2] += 1.0f;
+
+    // 将线性加速度从 g 转换为 m/s^2
+    float linear_accel_ms2[3];
+    const float G_IN_MS2 = 9.80665f;
+    linear_accel_ms2[0] = accel_nav_g[0] * G_IN_MS2;
+    linear_accel_ms2[1] = accel_nav_g[1] * G_IN_MS2;
+    linear_accel_ms2[2] = accel_nav_g[2] * G_IN_MS2;
+
+
+    // =================================================================================
+    // 步骤 4: 积分计算速度和距离
+    // =================================================================================
+    // 积分得到速度
+    skiing_state.velocity[0] += linear_accel_ms2[0] * dt;
+    skiing_state.velocity[1] += linear_accel_ms2[1] * dt;
+    skiing_state.velocity[2] += linear_accel_ms2[2] * dt;
+
+    // 计算标量速度
+    skiing_state.speed = sqrtf(skiing_state.velocity[0] * skiing_state.velocity[0] +
+                               skiing_state.velocity[1] * skiing_state.velocity[1] +
+                               skiing_state.velocity[2] * skiing_state.velocity[2]);
+
+    // 积分得到距离
+    skiing_state.distance += skiing_state.speed * dt;
+}
+
+
+
+// 传感器数据采集与处理任务
+void sensor_processing_task(signed short * acc_data_buf,signed short * gyr_data_buf) {
+    signed short acc_gyro_data[6];
+    acc_gyro_data[0] = acc_data_buf[0];
+    acc_gyro_data[1] = acc_data_buf[1];
+    acc_gyro_data[2] = acc_data_buf[2];
+    acc_gyro_data[3] = gyr_data_buf[0];
+    acc_gyro_data[4] = gyr_data_buf[1];
+    acc_gyro_data[5] = gyr_data_buf[2];
+
+    // 调用滑雪数据处理函数，传入原始数据和时间间隔DT
+    SL_SC7U22_Process_Skiing_Data(acc_gyro_data, DT);
+
+    // 处理和输出结果
+    if (skiing_state.is_calibrated) {
+        sensor_ble_packet_t packet;
+        packet.speed_ms = skiing_state.speed; 
+        packet.distance_m = skiing_state.distance;
+
+        static int print_counter = 0;
+        if (++print_counter >= 10) {
+            print_counter = 0;
+            xlog(": %.2f m/s, Dist: %.2f m, Still: %d",
+                   packet.speed_ms, packet.distance_m, skiing_state.is_still);
+        }
+
+    } else {
+        // 如果还未校准完成，可以打印提示信息
+        static int calib_print_counter = 0;
+        if (++calib_print_counter >= 100) { // 每1秒打印一次
+            calib_print_counter = 0;
+            xlog("Calibrating... Please keep the device still.");
+        }
+    }
+}

apps/earphone/xtell_Sensor/A_hide/1/2/skiing_tracker.h

@@ -0,0 +1,28 @@
+#ifndef SKIING_TRACKER_H
+#define SKIING_TRACKER_H
+
+#include "system/includes.h"
+// 在您的文件顶部添加
+#define G_ACCEL 8192.0f // 传感器在静止时Z轴的理论读数，对应1g。根据您的代码，±8G量程下，1g ≈ 32768 / 4 = 8192 LSB
+#define DEG_TO_RAD 0.0174532925f // 角度转弧度 M_PI / 180.0
+
+typedef struct {
+    // 输出结果
+    float velocity[3];      // 速度 (m/s) 在世界坐标系 [Vx, Vy, Vz]
+    float speed;            // 标量速度 (m/s)
+    float distance;         // 总滑行距离 (m)
+
+    // 内部状态
+    unsigned char is_calibrated;
+    unsigned char is_still;
+    int stillness_counter;
+
+    // 姿态
+    float angles_rad[3];    // 姿态角 [Pitch, Roll, Yaw] in radians
+
+} Skiing_State_t;
+
+
+
+void SL_SC7U22_Process_Skiing_Data(signed short* acc_gyro_input, float dt);
+#endif // SKIING_TRACKER_H

apps/earphone/xtell_Sensor/calculate/skiing_tracker.c

@@ -1,153 +1,213 @@
 #include "skiing_tracker.h"
-#include <math.h>   // 使用 sqrtf, fabsf, atan2f
-#include <string.h> // 使用 memset
+#include "../sensor/SC7U22.h" // 包含传感器驱动头文件以调用姿态解算函数
+#include <math.h>
+#include <string.h>
 
-// ======================= 用户可配置参数 =======================
+#define G_ACCELERATION 9.81f      // 重力加速度 (m/s^2)
+#define DEG_TO_RAD (3.14159265f / 180.0f)
 
-// IMU的采样率 (Hz)
-#define SAMPLE_RATE             100.0f
-#define DT                      (1.0f / SAMPLE_RATE)
+// --- 状态检测阈值 ---
+// 判断是否静止的加速度阈值 (m/s^2)。当加速度的模长减去重力后，小于此值，则认为可能静止。
+#define STATIC_ACC_THRESHOLD 1.0f
+// 连续多少帧满足静止条件才确认为静止状态
+#define STATIC_FRAMES_REQUIRED 50 // 累加，超过这个数加速度仍变化不大，说明近似静止
 
-// 传感器灵敏度配置 (必须与硬件配置匹配)
-// 加速度计量程: ±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;
+/**
+ * @brief 初始化滑雪追踪器
+ */
+void skiing_tracker_init(skiing_tracker_t *tracker)
+{
+    if (!tracker) {
+        return;
+    }
+    memset(tracker, 0, sizeof(skiing_tracker_t));
+    tracker->state = SKIING_STATE_STATIC; // 初始状态为静止
 }
 
-// 简单的低通滤波器
-static float low_pass_filter(float new_input, float prev_output, float alpha) {
-    return alpha * new_input + (1.0f - alpha) * prev_output;
+/**
+ * @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;
+
+    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];
+
+    // 旋转矩阵（简化版，仅用Pitch和Roll）
+    // Z-Y-X 顺序旋转
+    acc_world[0] = cr * ax + sr * sp * ay + sr * cp * az;
+    acc_world[1] = cp * ay - sp * az;
+    acc_world[2] = -sr * ax + cr * sp * ay + cr * cp * az;
 }
 
-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;
+
+/**
+ * @brief 更新滑雪状态机
+ */
+static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device)
+{
+    // 计算当前加速度的模长
+    float acc_magnitude = sqrtf(acc_device[0] * acc_device[0] + acc_device[1] * acc_device[1] + acc_device[2] * acc_device[2]);
+
+    // 状态判断逻辑
+    switch (tracker->state) {
+        case SKIING_STATE_STATIC:
+            // 如果加速度变化较大，则切换到滑雪状态
+            if (fabsf(acc_magnitude - G_ACCELERATION) > STATIC_ACC_THRESHOLD * 2.0f) { // 使用一个更大的阈值来启动
+                tracker->state = SKIING_STATE_SKIING;
+                tracker->static_frames_count = 0;
+            }
+            break;
+
+        case SKIING_STATE_SKIING:
+            // 检测是否进入静止状态 (零速更新 ZUPT)
+            if (fabsf(acc_magnitude - G_ACCELERATION) < STATIC_ACC_THRESHOLD) {
+                tracker->static_frames_count++;
+                if (tracker->static_frames_count >= STATIC_FRAMES_REQUIRED) {
+                    tracker->state = SKIING_STATE_STATIC;
+                    // 进入静止状态，强制将速度清零以消除漂移
+                    memset(tracker->velocity, 0, sizeof(tracker->velocity));
+                    tracker->speed = 0.0f;
+                }
+            } else {
+                // 如果在运动，则重置静止计数器
+                tracker->static_frames_count = 0;
+            }
+            break;
+        
+        // 可以在此添加摔倒等其他状态的判断
+        case SKIING_STATE_FALLEN:
+            // TODO: 添加从摔倒状态恢复的逻辑
+            break;
+
+        default:
+            tracker->state = SKIING_STATE_UNKNOWN;
+            break;
+    }
 }
 
-// 核心更新函数
-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");
+/**
+ * @brief 主更新函数
+ */
+void skiing_tracker_update(skiing_tracker_t *tracker, float *acc, float *angle, float dt)
+{
+    if (!tracker || !acc || !angle || dt <= 0) {
+        return;
     }
 
-    // 2. 状态机处理
-    switch (tracker->state) {
-        case STATE_UNCALIBRATED:
-            if (tracker->still_counter > STILL_SAMPLES_FOR_CALIBRATION) {
-                tracker->state = STATE_CALIBRATING;
-                reset_calibration(tracker);
-            }
-            break;
+    // 将原始g单位的加速度转换为 m/s^2
+    float acc_device_ms2[3];
+    acc_device_ms2[0] = acc[0] * G_ACCELERATION;
+    acc_device_ms2[1] = acc[1] * G_ACCELERATION;
+    acc_device_ms2[2] = acc[2] * G_ACCELERATION;
 
-        case STATE_CALIBRATING:
-            if (is_moving) { // 如果在校准时移动了，则校准失败，返回未校准状态
-                tracker->state = STATE_UNCALIBRATED;
-                reset_calibration(tracker);
-                break;
-            }
+    // 更新状态机
+    update_state_machine(tracker, acc_device_ms2);
 
-            // 累加采样数据
-            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->state == SKIING_STATE_SKIING) {
+        // 坐标系转换: 首先，利用姿态角(Pitch, Roll)将传感器测得的总加速度（运动加速度+重力）
+        // 从不断变化的“设备坐标系”转换到一个固定的“世界坐标系”。在这个世界坐标系里，Z轴永远垂直于地面指向上方。
+        // 执行坐标系转换
+        transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world);
+        // 转换完成后，重力就变成了一个恒定的、方向沿Z轴向下的矢量。
 
-            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));
+        // 在世界坐标系下，移除Z轴上的重力分量
+        tracker->acc_world[2] -= G_ACCELERATION;
 
-                tracker->state = STATE_READY;
-            }
-            break;
+        // 积分计算速度 (v = v0 + a*t)
+        tracker->velocity[0] += tracker->acc_world[0] * dt;
+        tracker->velocity[1] += tracker->acc_world[1] * dt;
+        tracker->velocity[2] += tracker->acc_world[2] * dt; // 垂直方向速度也计算在内
 
-        case STATE_READY:
-            if (tracker->motion_counter > MOTION_SAMPLES_TO_START_SKIING) {
-                tracker->state = STATE_SKIING;
-            }
-            break;
+        // 计算当前速率
+        tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] + 
+                               tracker->velocity[1] * tracker->velocity[1] + 
+                               tracker->velocity[2] * tracker->velocity[2]);
 
-        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;
-            }
+        // 积分计算距离 (d = d0 + v*t)
+        tracker->distance += tracker->speed * dt;
+    }
+}
 
-            // 计算线性加速度 (重力抵消)
-            // 假设传感器的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;
+// 传感器数据采集与处理任务
+void sensor_processing_task(signed short * acc_data_buf,signed short * gyr_data_buf) {
+    // --- 1. 定义静态变量 ---
+    static skiing_tracker_t my_skiing_tracker;
+    static int initialized = 0;
+    static int calibration_done = 0; // 新增：用于标记一次性静态校准是否完成
 
-            // 物理约束：速度不能为负（不能往坡上滑）
-            if (tracker->velocity < 0) {
-                tracker->velocity = 0;
-            }
-            tracker->distance += (prev_velocity + tracker->velocity) / 2.0f * DT;
-            break;
+    static signed short combined_raw_data[6];
+    static float final_angle_data[3];
+    static float calibrated_acc_g[3];
 
-        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;
+    // sensor_processing_task的调用频率, dt = 0.001s
+    const float delta_time = 0.01f;
+
+    // --- 2. 初始化 ---
+    if (!initialized) {
+        skiing_tracker_init(&my_skiing_tracker);
+        initialized = 1;
+        printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
+    }
+
+    // --- 3. 数据处理 ---
+    //合并加速度和陀螺仪数据
+    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 (get_calibration_state() == 0) { //正在校准
+        //领票校准
+        status = 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 {
+        status = SL_SC7U22_Angle_Output(0, combined_raw_data, final_angle_data, 0);
+    }
+
+    // c. 检查姿态解算是否成功
+    if (status == 1) {
+        // 将校准后的加速度数据从 LSB (原始值) 转换为 g (重力单位)
+        // ±8g量程下，8192 LSB 对应 1g
+        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;
+
+        skiing_tracker_update(&my_skiing_tracker, calibrated_acc_g, final_angle_data, delta_time);
+
+        static count = 0;
+        if(count < 10){  
+            count++;
+            return;
+        }else{
+            count = 0;
+        }
+        printf("State: %d, Speed: %.2f m/s, Distance: %.2f m\n",
+               my_skiing_tracker.state,
+               my_skiing_tracker.speed,
+               my_skiing_tracker.distance);
+
+    } else if (status == 0) {
+        // 传感器正在进行静态校准
+        // printf("Sensor is calibrating...\n");
+    } else {
+        // status == 2, 表示校准失败或发生错误
+        // printf("Angle calculation error or calibration not finished.\n");
     }
 }

apps/earphone/xtell_Sensor/calculate/skiing_tracker.h

@@ -1,53 +1,41 @@
 #ifndef SKIING_TRACKER_H
 #define SKIING_TRACKER_H
 
-// 定义滑雪者的运动状态
+// 定义滑雪者可能的状态
 typedef enum {
-    STATE_UNCALIBRATED, // 未校准，等待在斜坡上静止
-    STATE_CALIBRATING,  // 正在校准重力矢量
-    STATE_READY,        // 校准完成，准备滑行
-    STATE_SKIING,       // 正在滑行
-    STATE_STOPPED       // 在斜坡上中途停止
-} MotionState;
+    SKIING_STATE_STATIC,        // 静止
+    SKIING_STATE_SKIING,        // 正在滑雪
+    SKIING_STATE_FALLEN,        // 已摔倒
+    SKIING_STATE_UNKNOWN        // 未知状态
+} skiing_state_t;
 
-// 存储所有运动学数据
+// 追踪器数据结构体
 typedef struct {
-    // 最终输出
-    float velocity;         // 沿斜坡方向的速度 (m/s)
-    float distance;         // 沿斜坡方向的滑行距离 (m)
-    float slope_angle_deg;  // 动态计算出的坡度 (度)
+    float velocity[3];          // 当前速度 (x, y, z)，单位: m/s
+    float distance;             // 总滑行距离，单位: m
+    float speed;                // 当前速率 (标量)，单位: m/s
+    skiing_state_t state;       // 当前滑雪状态
 
-    // 内部状态变量
-    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;
+    // 私有成员，用于内部计算
+    int static_frames_count;    // 用于判断静止状态的帧计数器
+    float acc_world[3];         // 在世界坐标系下的加速度
+} skiing_tracker_t;
 
 /**
  * @brief 初始化滑雪追踪器
- * @param tracker 指向 SkiingTracker 实例的指针
+ *
+ * @param tracker 指向 skiing_tracker_t 结构体的指针
  */
-void SkiingTracker_Init(SkiingTracker* tracker);
+void skiing_tracker_init(skiing_tracker_t *tracker);
 
 /**
- * @brief 处理IMU数据，自动校准并计算速度和距离
- * @details 这是核心处理函数，应在每次获取新的IMU数据后调用。
+ * @brief 处理传感器数据并更新滑雪状态
  *
- * @param tracker 指向 SkiingTracker 实例的指针
- * @param raw_accel        未经校准的原始加速度数据 [X, Y, Z]，单位是 LSB
- * @param raw_gyro         未经校准的原始陀螺仪数据 [X, Y, Z]，单位是 LSB
+ * @param tracker 指向 skiing_tracker_t 结构体的指针
+ * @param acc 校准后的加速度数据 [x, y, z]，单位: g (1g = 9.8m/s^2)
+ * @param angle 姿态角数据 [pitch, roll, yaw]，单位: 度
+ * @param dt 采样时间间隔，单位: 秒 (s)
  */
-void SkiingTracker_Update(SkiingTracker* tracker, signed short* raw_accel, signed short* raw_gyro);
+void skiing_tracker_update(skiing_tracker_t *tracker, float *acc, float *angle, float dt);
 
 #endif // SKIING_TRACKER_H

apps/earphone/xtell_Sensor/send_data.c

@@ -57,8 +57,9 @@ static circle_buffer_t sensor_cb;
 
 
 //--- test ---
-static SkiingTracker skiing_data;
-static char ble_send_data[50];
+// 全局变量
+u16 gsensor_id=0;
+u16 test_id=0;
 //END -- 变量定义
 //////////////////////////////////////////////////////////////////////////////////////////////////
 
@@ -68,97 +69,45 @@ void send_sensor_data_task(void) {
     // printf("xtell_ble_send\n");
 }
 
+void test(){
 
-void sensor_test(){
+    signed short acc_data_buf[3] = {0};
+    signed short gyr_data_buf[3] = {0};
 
-    // signed short acc_raw[3], gyr_raw[3];
-
-    // // 读取原始数据
-    // SL_SC7U22_RawData_Read(acc_raw, gyr_raw);
-
-    // // 将原始数据送入追踪器进行处理
-    // SkiingTracker_Update(&skiing_data, acc_raw, gyr_raw);
-    
-    // // c. 打印状态和结果
-    const char* state_str = "UNKNOWN";
-    switch(skiing_data.state) {
-        case STATE_UNCALIBRATED: state_str = "UNCALIBRATED"; break;
-        case STATE_CALIBRATING:  state_str = "CALIBRATING..."; break;
-        case STATE_READY:        state_str = "READY"; break;
-        case STATE_SKIING:       state_str = "SKIING"; break;
-        case STATE_STOPPED:      state_str = "STOPPED"; break;
+    signed short acc_gyro_input[6] = {0};
+    float Angle_output[3] = {0};
+    extern void SL_SC7U22_RawData_Read(signed short * acc_data_buf,signed short * gyr_data_buf);
+    SL_SC7U22_RawData_Read(acc_data_buf,gyr_data_buf);
+    acc_gyro_input[0] = acc_data_buf[0];
+    acc_gyro_input[1] = acc_data_buf[1];
+    acc_gyro_input[2] = acc_data_buf[2];
+    acc_gyro_input[3] = gyr_data_buf[0];
+    acc_gyro_input[4] = gyr_data_buf[1];
+    acc_gyro_input[5] = gyr_data_buf[2];
+    // extern unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst);
+    // SL_SC7U22_Angle_Output(0,acc_gyro_input,Angle_output,1);
+    static int count = 0;
+    if(count >=100){ //1s打印1次
+        count = 0;
+        xlog("ACC_X:%d, ACC_Y:%d, ACC_Z:%d, GYR_X:%.d, GYR_Y:%d, GYR_Z:%d", 
+            acc_gyro_input[0],acc_gyro_input[1],acc_gyro_input[2],acc_gyro_input[3],acc_gyro_input[4],acc_gyro_input[5]
+            );
+        // xlog("Pitch:%.2f, Roll:%.2f, Yaw:%.2f\n",
+        //     Angle_output[0],Angle_output[1],Angle_output[2]
+        //     );
     }
+    count++;
 
-    // // 转换为 km/h
-    // float velocity_kmh = skiing_data.velocity * 3.6f;
-    
-    // printf("State: %s, Slope: %.1f deg, Vel: %.2f km/h, Dist: %.2f m\n",
-    //         state_str,
-    //         skiing_data.slope_angle_deg,
-    //         velocity_kmh,
-    //         skiing_data.distance);
-    
-
-
-
-    signed short acc_raw[3], gyr_raw[3];
-
-    // 读取原始数据
-    SL_SC7U22_RawData_Read(acc_raw, gyr_raw);
-    
-    // 将原始数据送入追踪器进行处理
-    SkiingTracker_Update(&skiing_data, acc_raw, gyr_raw);
-    
-    // 清空发送缓冲区
-    memset(ble_send_data, 0, sizeof(ble_send_data));
-
-    // 准备要填充的数据
-    // 状态
-    char state_code = (char)skiing_data.state;
-
-    // 坡度 (舍弃小数)
-    char slope_angle = (char)skiing_data.slope_angle_deg;
-    
-    // 速度 km/h (舍弃小数)
-    float velocity_kmh_float = skiing_data.velocity * 3.6f;
-    // 使用 uint16_t 来存储速度，因为它可能超过255
-    uint16_t velocity_kmh_int = (uint16_t)velocity_kmh_float;
-
-    // 距离 (m)
-    // 使用 uint32_t 来存储距离，因为它会变得很大
-    uint32_t distance_int = (uint32_t)skiing_data.distance;
-
-    // 按顺序填充到 ble_send_data 数组中
-    int index = 0;
-
-    // 字节 0: 状态码 (1 byte)
-    ble_send_data[index++] = state_code;
-
-    // 字节 1: 坡度 (1 byte)
-    ble_send_data[index++] = slope_angle;
-
-    // 字节 2-3: 速度 (2 bytes, 小端模式)
-    // 将 16 位的速度拆分成两个 8 位的 char
-    ble_send_data[index++] = (char)(velocity_kmh_int & 0xFF);         // 低8位
-    ble_send_data[index++] = (char)((velocity_kmh_int >> 8) & 0xFF);  // 高8位
-
-    // 字节 4-7: 距离 (4 bytes, 小端模式)
-    // 将 32 位的距离拆分成四个 8 位的 char
-    ble_send_data[index++] = (char)(distance_int & 0xFF);             // 最低8位
-    ble_send_data[index++] = (char)((distance_int >> 8) & 0xFF);
-    ble_send_data[index++] = (char)((distance_int >> 16) & 0xFF);
-    ble_send_data[index++] = (char)((distance_int >> 24) & 0xFF);     // 最高8位
-    
-    send_data_to_ble_client(ble_send_data, index);
-
-    printf("State: %s, Slope: %.1f deg, Vel: %.2f km/h, Dist: %.2f m\n",
-        state_str,
-        skiing_data.slope_angle_deg,
-        velocity_kmh_float,
-        skiing_data.distance);
-    
+    void sensor_processing_task(signed short * acc_data_buf,signed short * gyr_data_buf);
+    sensor_processing_task(acc_data_buf, gyr_data_buf);
 }
-u16 test_id=0;
+void gsensor_test(){
+
+    sys_timer_del(gsensor_id);
+
+}
+
+
 void xtell_task_create(void){
 
     // int ret = hw_iic_init(0);
@@ -181,9 +130,10 @@ void xtell_task_create(void){
 
 
     //初始化滑雪追踪器
-    // SkiingTracker_Init(&skiing_data);
+    // SkiingTracker_Init(&skiing_data); 
     xlog("SkiingTracker_Init\n");
 
-    create_process(&test_id, "test",NULL, sensor_test, 10);
-
+    // create_process(&gsensor_id, "gsensor",NULL, gsensor_test, 1000);
+    create_process(&test_id, "test",NULL, test, 10);
+    
 }

apps/earphone/xtell_Sensor/sensor/SC7U22.c

@@ -4,7 +4,7 @@
 #include "os/os_api.h"
 
 
-#define ENABLE_XLOG 1
+#define ENABLE_XLOG 0
 #ifdef xlog
 #undef xlog
 #endif
@@ -733,7 +733,8 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short
         }
 
         // 判断是否处于静止状态：加速度变化量、陀螺仪变化量、各轴加速度值都在一个很小的范围内
-        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 ((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
+        // 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++; // 静止计数器累加
             }
@@ -926,6 +927,9 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short
     return 2; // 校准未完成，返回错误状态
 }
 
+unsigned char get_calibration_state(void){
+	return SL_SC7U22_Error_Flag;
+}
 
 
 #endif

apps/earphone/xtell_Sensor/sensor/SC7U22.h

@@ -130,7 +130,7 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en,signed short *
 /**output Angle_output[2]:   Yaw*******************************/
 /**input  yaw_rst:   reset yaw value***************************/
 
-
+unsigned char get_calibration_state(void);
 /**寄存器宏定义*******************************/
 #define SC7U22_WHO_AM_I         0x01
 

cpu/br28/tools/sdk.elf.resolution.txt

@@ -6337,11 +6337,12 @@ objs/apps/earphone/xtell_Sensor/xtell_handler.c.o
 -r=objs/apps/earphone/xtell_Sensor/xtell_handler.c.o,sniff_out,l
 objs/apps/earphone/xtell_Sensor/send_data.c.o
 -r=objs/apps/earphone/xtell_Sensor/send_data.c.o,send_sensor_data_task,pl
--r=objs/apps/earphone/xtell_Sensor/send_data.c.o,sensor_test,pl
+-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,test,pl
 -r=objs/apps/earphone/xtell_Sensor/send_data.c.o,SL_SC7U22_RawData_Read,l
--r=objs/apps/earphone/xtell_Sensor/send_data.c.o,SkiingTracker_Update,l
--r=objs/apps/earphone/xtell_Sensor/send_data.c.o,send_data_to_ble_client,l
 -r=objs/apps/earphone/xtell_Sensor/send_data.c.o,printf,l
+-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,sensor_processing_task,l
+-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,gsensor_test,pl
+-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,sys_timer_del,l
 -r=objs/apps/earphone/xtell_Sensor/send_data.c.o,xtell_task_create,pl
 -r=objs/apps/earphone/xtell_Sensor/send_data.c.o,soft_iic_init,l
 -r=objs/apps/earphone/xtell_Sensor/send_data.c.o,gpio_set_direction,l
@@ -6350,6 +6351,7 @@ objs/apps/earphone/xtell_Sensor/send_data.c.o
 -r=objs/apps/earphone/xtell_Sensor/send_data.c.o,SL_SC7U22_Config,l
 -r=objs/apps/earphone/xtell_Sensor/send_data.c.o,circle_buffer_init,l
 -r=objs/apps/earphone/xtell_Sensor/send_data.c.o,create_process,l
+-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,gsensor_id,pl
 -r=objs/apps/earphone/xtell_Sensor/send_data.c.o,test_id,pl
 objs/apps/earphone/xtell_Sensor/buffer/circle_buffer.c.o
 -r=objs/apps/earphone/xtell_Sensor/buffer/circle_buffer.c.o,circle_buffer_init,pl
@@ -6383,7 +6385,6 @@ objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o
 -r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,SL_SC7U22_I2c_Spi_Write,pl
 -r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,SL_SC7U22_I2c_Spi_Read,pl
 -r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,SL_SC7U22_Check,pl
--r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,printf,l
 -r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,SL_SC7U22_Config,pl
 -r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,SL_SC7U22_POWER_DOWN,pl
 -r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,SL_SC7U22_SOFT_RESET,pl
@@ -6396,6 +6397,7 @@ objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o
 -r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,SL_SC7U22_Angle_Output,pl
 -r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,asinf,l
 -r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,atanf,l
+-r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,get_calibration_state,pl
 -r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,gravity_sensor_command,l
 -r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,_gravity_sensor_get_ndata,l
 -r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,angle,pl
@@ -6432,11 +6434,16 @@ objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o
 -r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,Error_Accgyro,pl
 -r=objs/apps/earphone/xtell_Sensor/sensor/SC7U22.c.o,Sum_Avg_Accgyro,pl
 objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o
--r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,SkiingTracker_Init,pl
--r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,SkiingTracker_Update,pl
+-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,skiing_tracker_init,pl
+-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,skiing_tracker_update,pl
 -r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,sqrtf,l
+-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,sensor_processing_task,pl
+-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,printf,l
+-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,get_calibration_state,l
+-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,SL_SC7U22_Angle_Output,l
 -r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,fabsf,l
--r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,atan2f,l
+-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,cosf,l
+-r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,sinf,l
 -r=objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o,puts,l
 cpu/br28/liba/cpu.a.llvm.19376.crc16.c
 -r=cpu/br28/liba/cpu.a.llvm.19376.crc16.c,__crc16_mutex_init,pl

objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.d

@@ -1,15 +1,133 @@
 objs/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c.o: \
   apps/earphone/xtell_Sensor/calculate/skiing_tracker.c \
   apps/earphone/xtell_Sensor/calculate/skiing_tracker.h \
-  C:/JL/pi32/pi32v2-include\math.h C:/JL/pi32/pi32v2-include\sys/reent.h \
-  C:/JL/pi32/pi32v2-include\_ansi.h C:/JL/pi32/pi32v2-include\newlib.h \
+  apps/earphone/xtell_Sensor/calculate/../sensor/SC7U22.h \
+  apps/common/device\gSensor/gSensor_manage.h \
+  include_lib/system/generic\printf.h \
+  include_lib/system/generic/typedef.h \
+  include_lib/driver/cpu/br28\asm/cpu.h \
+  include_lib/driver/cpu/br28\asm/br28.h \
+  include_lib/driver/cpu/br28\asm/io_omap.h \
+  include_lib/driver/cpu/br28\asm/io_imap.h \
+  include_lib/driver/cpu/br28\asm/csfr.h \
+  include_lib/driver/cpu/br28\asm/cache.h \
+  include_lib/driver/cpu/br28\asm/irq.h \
+  include_lib/driver/cpu/br28\asm/hwi.h \
+  include_lib/system\generic/printf.h include_lib\system/generic/log.h \
+  include_lib\system/generic/printf.h \
+  include_lib/system\generic/errno-base.h \
+  C:/JL/pi32/pi32v2-include\string.h C:/JL/pi32/pi32v2-include/_ansi.h \
+  C:/JL/pi32/pi32v2-include\newlib.h \
   C:/JL/pi32/pi32v2-include\sys/config.h \
   C:/JL/pi32/pi32v2-include\machine/ieeefp.h \
   C:/JL/pi32/pi32v2-include\sys/features.h \
+  C:/JL/pi32/pi32v2-include\sys/reent.h \
   C:/JL/pi32/pi32v2-include\sys/_types.h \
   C:/JL/pi32/pi32v2-include\machine/_types.h \
   C:/JL/pi32/pi32v2-include\machine/_default_types.h \
   C:/JL/pi32/pi32v2-include\sys/lock.h \
-  C:/JL/pi32/pi32v2-include\string.h \
   C:/JL/pi32/pi32v2-include\sys/cdefs.h \
-  C:/JL/pi32/pi32v2-include\sys/string.h
+  C:/JL/pi32/pi32v2-include\sys/string.h \
+  C:/JL/pi32/pi32v2-include\strings.h \
+  C:/JL/pi32/pi32v2-include\sys/types.h \
+  C:/JL/pi32/pi32v2-include\sys/_stdint.h \
+  C:/JL/pi32/pi32v2-include\machine/types.h include_lib\system/malloc.h \
+  include_lib/system/generic\cpu.h \
+  include_lib/driver/cpu/br28\asm/iic_hw.h \
+  include_lib/driver/cpu/br28\asm/iic_soft.h include_lib/system\timer.h \
+  include_lib/system/generic/list.h apps/earphone/include\app_config.h \
+  apps/earphone/board/br28\board_config.h include_lib\media/audio_def.h \
+  apps/earphone/board/br28/board_jl701n_demo_cfg.h \
+  apps/earphone/board/br28/board_jl701n_demo_global_build_cfg.h \
+  apps/common/device/usb\usb_std_class_def.h \
+  apps/earphone/board/br28/board_jl701n_btemitter_cfg.h \
+  apps/earphone/board/br28/board_jl701n_btemitter_global_build_cfg.h \
+  apps/earphone/board/br28/board_jl701n_anc_cfg.h \
+  apps/earphone/board/br28/board_jl701n_anc_global_build_cfg.h \
+  apps/earphone/board/br28/board_jl7016g_hybrid_cfg.h \
+  apps/earphone/board/br28/board_jl7016g_hybrid_global_build_cfg.h \
+  apps/earphone/board/br28/board_jl7018f_demo_cfg.h \
+  apps/earphone/board/br28/board_jl7018f_demo_global_build_cfg.h \
+  apps/common/device/usb\usb_common_def.h \
+  include_lib/btctrler\btcontroller_mode.h \
+  apps/earphone/include/user_cfg_id.h \
+  apps/common/config/include\bt_profile_cfg.h \
+  include_lib/btctrler\btcontroller_modules.h \
+  include_lib/btctrler/hci_transport.h include_lib/btctrler/ble/hci_ll.h \
+  C:/JL/pi32/pi32v2-include\stdint.h \
+  C:/JL/pi32/pi32v2-include\sys/_intsup.h \
+  C:/JL/pi32/pi32v2-include\stdlib.h \
+  C:/JL/pi32/pi32v2-include\machine/stdlib.h \
+  C:/JL/pi32/pi32v2-include\alloca.h \
+  include_lib/btctrler/classic/hci_lmp.h include_lib/system\event.h \
+  include_lib/system/generic/rect.h include_lib\system/includes.h \
+  include_lib/system/init.h include_lib/system/spinlock.h \
+  include_lib/system/generic\irq.h include_lib/system/task.h \
+  include_lib/system/os/os_api.h include_lib/system\os/os_cpu.h \
+  include_lib/system/generic\jiffies.h include_lib/system\os/os_error.h \
+  include_lib/system\os/os_type.h include_lib/system\os/ucos_ii.h \
+  include_lib/system\os/os_cfg.h include_lib/system\os/os_api.h \
+  include_lib/system/wait.h include_lib/system/app_core.h \
+  include_lib/system/app_msg.h include_lib/system/database.h \
+  include_lib/system/fs/fs.h include_lib/system\generic/ioctl.h \
+  include_lib/system\generic/atomic.h include_lib\system/sys_time.h \
+  include_lib/system/fs/fs_file_name.h include_lib/system/fs/sdfile.h \
+  include_lib/system/power_manage.h include_lib/system/syscfg_id.h \
+  include_lib/system/bank_switch.h include_lib/system/generic/includes.h \
+  include_lib/system/generic/ascii.h include_lib/system/generic/gpio.h \
+  include_lib/driver/cpu/br28\asm/gpio.h \
+  include_lib/system/generic/version.h include_lib/system/generic/lbuf.h \
+  include_lib/system/generic/lbuf_lite.h \
+  include_lib/system/generic/circular_buf.h \
+  include_lib/system/generic/index.h \
+  include_lib/system/generic/debug_lite.h \
+  include_lib/system/device/includes.h \
+  include_lib/system/device/device.h \
+  include_lib/system\device/ioctl_cmds.h \
+  include_lib/system/device/key_driver.h \
+  include_lib/system/device/iokey.h include_lib/system/device/irkey.h \
+  include_lib/system/device/adkey.h \
+  include_lib/driver/cpu/br28\asm/adc_api.h \
+  include_lib/system/device/slidekey.h \
+  include_lib/system/device/touch_key.h \
+  include_lib/driver/cpu/br28\asm/plcnt.h \
+  include_lib/system/device/rdec_key.h \
+  include_lib/driver/cpu/br28\asm/rdec.h \
+  include_lib/driver/cpu/br28\asm/includes.h \
+  include_lib/driver/cpu/br28\asm/crc16.h \
+  include_lib/driver/cpu/br28\asm/clock.h \
+  include_lib/driver/cpu/br28\asm/clock_hw.h \
+  include_lib/driver/cpu/br28\asm/clock_define.h \
+  include_lib/driver/cpu/br28\asm/uart.h \
+  include_lib/driver\device/uart.h \
+  include_lib/driver/cpu/br28\asm/uart_dev.h \
+  include_lib/driver/cpu/br28\asm/spiflash.h \
+  include_lib/driver\device/spiflash.h \
+  include_lib/driver/cpu/br28\asm/power_interface.h \
+  include_lib/driver/cpu/br28\asm/power/p33.h \
+  include_lib/driver/cpu/br28/asm/power/p33_sfr.h \
+  include_lib/driver/cpu/br28/asm/power/p33_app.h \
+  include_lib/driver/cpu/br28/asm/power/p33_io_app.h \
+  include_lib/driver/cpu/br28/asm/power/rtc_app.h \
+  include_lib/driver/cpu/br28\asm/power/p11.h \
+  include_lib/driver/cpu/br28/asm/power/p11_csfr.h \
+  include_lib/driver/cpu/br28/asm/power/p11_sfr.h \
+  include_lib/driver/cpu/br28/asm/power/p11_io_omap.h \
+  include_lib/driver/cpu/br28/asm/power/p11_io_imap.h \
+  include_lib/driver/cpu/br28/asm/power/p11_app.h \
+  include_lib/driver/cpu/br28/asm/power/p11.h \
+  include_lib/driver/cpu/br28\asm/power/power_api.h \
+  include_lib/driver/cpu/br28\asm/power/power_port.h \
+  include_lib/driver/cpu/br28\asm/power/power_wakeup.h \
+  include_lib/driver/cpu/br28\asm/power/power_reset.h \
+  include_lib/driver/cpu/br28\asm/power/power_compat.h \
+  include_lib/driver/cpu/br28\asm/power/lp_ipc.h \
+  include_lib/driver/cpu/br28/asm/power/m2p_msg.h \
+  include_lib/driver/cpu/br28/asm/power/p2m_msg.h \
+  include_lib/driver/cpu/br28\asm/efuse.h \
+  include_lib/driver/cpu/br28\asm/wdt.h \
+  include_lib/driver/cpu/br28\asm/debug.h \
+  include_lib/driver/cpu/br28\asm/timer.h \
+  include_lib/driver/cpu/br28\asm/rtc.h \
+  include_lib/driver\device/sdio_host_init.h \
+  C:/JL/pi32/pi32v2-include\math.h