编译通过

apps/earphone/xtell_Sensor/sensor/SCU722.c

@@ -656,7 +656,9 @@ float angle0[3] = {0, 0, 0}, angle_dot0[3] = {0, 0, 0}; // 姿态角的估计值
 // Q_gyro:  过程噪声协方差，表示陀螺仪偏置（bias）的不确定性。
 // R_angle: 测量噪声协方差，表示通过加速度计计算出的角度测量值的不确定性。值越小，表示越相信加速度计的测量结果。
 // dt:      采样时间间隔（单位：秒），这里是10ms (0.01s)，对应100Hz的采样率。
-float Q_angle = 0.0003, Q_gyro = 0.001, R_angle = 0.005, dt = 0.01;
+//float Q_angle=0.0003, Q_gyro=0.001, R_angle=0.005, dt=0.005;//5ms ST
+//float Q_angle=0.00001, Q_gyro=0.00001, R_angle=0.005, dt=0.0025;//5ms ST
+float Q_angle = 0.0003, Q_gyro = 0.001, R_angle = 0.005, dt = 0.01; //10ms
 
 // --- 协方差矩阵 P ---
 // P矩阵表示系统状态估计的不确定性程度。它是一个2x2的矩阵：
@@ -704,10 +706,10 @@ signed int    Sum_Avg_Accgyro[6] ={0};
  * 1.  静态校准：在初始阶段，检测传感器是否处于静止状态。如果是，则计算加速度计和陀螺仪的零点偏移（误差），用于后续的数据补偿。
  * 2.  姿态解算：使用卡尔曼滤波器融合经过校准后的加速度计和陀螺仪数据，计算出物体的俯仰角（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，则将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，则将Yaw角清零。
  *
  * @return
  * - 0: 正在进行静态校准。
@@ -945,3 +947,73 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short
 
 
 #endif
+
+
+/*
+//-----------------------------------------
+调用示例-by_lmx：
+
+
+//1.
+// 定义用于存放传感器数据的缓冲区
+static signed short acc_raw_data[3];    // [0]: acc_x, [1]: acc_y, [2]: acc_z
+static signed short gyr_raw_data[3];    // [0]: gyr_x, [1]: gyr_y, [2]: gyr_z
+static signed short combined_raw_data[6]; // 用于合并 acc 和 gyr 数据
+static float final_angle_data[3];      // [0]: Pitch, [1]: Roll, [2]: Yaw
+
+// 传感器数据处理任务
+void sensor_processing_task(void *priv)
+{
+    while (1) {
+        // 4. 周期性调用读取函数，获取原始数据
+        SL_SC7U22_RawData_Read(acc_raw_data, gyr_raw_data);
+
+        // 5. 合并加速度和角速度数据到一个数组中
+        // SL_SC7U22_Angle_Output 函数需要一个包含6个元素的数组作为输入
+        memcpy(&combined_raw_data[0], acc_raw_data, sizeof(acc_raw_data));
+        memcpy(&combined_raw_data[3], gyr_raw_data, sizeof(gyr_raw_data));
+
+        // 6. 调用姿态解算函数
+        // 参数: (校准使能, 输入的6轴数据, 输出的角度数据, Yaw轴复位标志)
+        // calibration_en = 1: 让函数内部自动管理校准过程
+        // yaw_rst = 0:      不复位Yaw角
+        unsigned char status = SL_SC7U22_Angle_Output(1, combined_raw_data, final_angle_data, 0);
+
+        // 7. 检查函数返回的状态
+        if (status == 1) {
+            // 计算成功，final_angle_data 中的数据有效
+            printf("Pitch: %.2f, Roll: %.2f, Yaw: %.2f\n", final_angle_data[0], final_angle_data[1], final_angle_data[2]);
+        } else if (status == 0) {
+            // 传感器正在进行静态校准，此时角度数据可能不准确
+            printf("Sensor is calibrating...\n");
+        } else {
+            // status == 2, 表示校准未完成或发生错误
+            printf("Angle calculation error or calibration not finished.\n");
+        }
+
+        // 延时一段时间，例如10ms (对应100Hz)
+        os_time_dly(1);
+    }
+}
+
+// 应用程序主入口或初始化函数
+void app_main()
+{
+    // ... 其他初始化代码 ...
+
+    // 2. 调用初始化函数来配置SCU722传感器
+    unsigned char init_success = SL_SC7U22_Config();
+
+    if (init_success) {
+        printf("SCU722 初始化成功!\n");
+        // 3. 创建一个任务来周期性地读取和处理数据
+        task_create(sensor_processing_task, NULL, "sensor_task");
+    } else {
+        printf("SCU722 初始化失败!\n");
+    }
+
+    // ...
+}
+
+//-----------------------------------------
+*/