cun

apps/earphone/xtell_Sensor/calculate/skiing_tracker.c

@@ -174,6 +174,9 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d
         extern mmc5603nj_cal_data_t cal_data; 
         xlog("cal_data:X: %.4f, Y: %.4f, Z: %.4f\n", cal_data.offset_x,cal_data.offset_y,cal_data.offset_z);
         count = 0;
+        char send_tmp[1];
+        send_tmp[0] = (char)all_world_mag;
+        send_data_to_ble_client(&send_tmp,1);
         
     }
     count++;

apps/earphone/xtell_Sensor/send_data.c

@@ -481,7 +481,8 @@ void sensor_measure(void){
         // 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,&mag_data, 0, tmp.quaternion_output);
+        // status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle,&mag_data, 0, tmp.quaternion_output);
+        status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle,NULL, 0, tmp.quaternion_output);
 
         if(count > 100){
             count = 0;
@@ -502,7 +503,9 @@ void sensor_measure(void){
         // 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,&mag_data, 0, tmp.quaternion_output);
+        // status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle,&mag_data, 0, tmp.quaternion_output);
+        status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle,NULL, 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, tmp.quaternion_output);
@@ -540,9 +543,6 @@ void xtell_task_create(void){
     
     #endif
     
-    gpio_set_direction(IO_PORTE_05,0);          //设置PE5 输出模式
-    gpio_set_pull_up(IO_PORTE_05,1);
-    gpio_direction_output(IO_PORTE_05,1);
 
     // os_time_dly(10);
     // delay_2ms(10);
@@ -577,9 +577,7 @@ void xtell_task_create(void){
     circle_buffer_init(&sensor_send, sensor_send_buffer, SENSOR_DATA_BUFFER_SIZE, sizeof(BLE_send_data_t));
 
 
-    //初始化滑雪追踪器
+
-    // SkiingTracker_Init(&skiing_data); 
-    xlog("SkiingTracker_Init\n");
 
 
     // create_process(&sensor_read_data_id, "read",NULL, sensor_read_data, 10);

apps/earphone/xtell_Sensor/sensor/MMC56.c

@@ -6,6 +6,9 @@
 #include "gSensor/gSensor_manage.h"
 #include "printf.h"
 
+#define CALIBRATION_TIME 20000  //校准持续时间 ms
+#define SAMPLE_INTERVAL  100    //校准采样间隔
+
 // 用于跟踪当前是否处于连续测量模式
 static uint8_t g_continuous_mode_enabled = 0;
 mmc5603nj_cal_data_t cal_data;  //校准数据
@@ -27,7 +30,7 @@ uint8_t mmc5603nj_get_pid(void) {
 
 int mmc5603nj_init(void) {
     //  ID
-    if (mmc5603nj_get_pid() != 0x80) {
+    if ( mmc5603nj_get_pid() != 0x10) {
         printf("MMC5603NJ init failed: wrong Product ID (read: 0x%X)\n", mmc5603nj_get_pid());
         return -1;
     }
@@ -55,17 +58,16 @@ int mmc5603nj_init(void) {
 
     mmc5603nj_enable_continuous_mode(0x04);
 
-    
+    return 0;
+}
+
+void mmc5603nj_start_calibration(void){
     printf("\n--- Magnetometer Calibration Start ---\n");
     printf("Slowly rotate the device in all directions (like drawing a 3D '8')...\n");
     printf("Calibration will last for 20 seconds.\n\n");
     printf("will start after 5 seconds\n\n");
     os_time_dly(500);
 
-    // 定义校准时长和采样间隔
-    const uint32_t calibration_duration_ms = 20000; // 20秒
-    const uint32_t sample_interval_ms = 100;        // 每100ms采样一次
-
     // 初始化最大最小值
     // 使用一个临时变量来读取数据，避免干扰read函数的正常逻辑
     mmc5603nj_mag_data_t temp_mag_data;
@@ -81,7 +83,7 @@ int mmc5603nj_init(void) {
 
     uint32_t start_time = os_time_get(); // 假设os_time_get()返回毫秒级时间戳
     int samples = 0;
-    int over = calibration_duration_ms/sample_interval_ms;
+    int over = CALIBRATION_TIME/SAMPLE_INTERVAL;
 
     while (samples <= over) {
         // 读取原始磁力计数据
@@ -98,7 +100,7 @@ int mmc5603nj_init(void) {
         if (temp_mag_data.z < min_z) min_z = temp_mag_data.z;
         
         samples++;
-        os_time_dly(sample_interval_ms / 10); // os_time_dly的参数通常是ticks (1 tick = 10ms)
+        os_time_dly(SAMPLE_INTERVAL / 10); 
     }
 
     // 检查数据范围是否合理，防止传感器未动或故障
@@ -106,7 +108,7 @@ int mmc5603nj_init(void) {
         printf("\n--- Calibration Failed ---\n");
         printf("Device might not have been rotated enough.\n");
         printf("X range: %.2f, Y range: %.2f, Z range: %.2f\n", max_x - min_x, max_y - min_y, max_z - min_z);
-        return -1;
+        return;
     }
 
     // 计算硬磁偏移 (椭球中心)
@@ -121,8 +123,6 @@ int mmc5603nj_init(void) {
     printf("  Y: %.4f\n", cal_data.offset_y);
     printf("  Z: %.4f\n", cal_data.offset_z);
     printf("Please save these values and apply them in your code.\n\n");
-
-    return 0;
 }
 
 

apps/earphone/xtell_Sensor/sensor/SC7U22.c

@@ -1182,7 +1182,7 @@ unsigned char get_calibration_state(void){
 // Kp: 比例增益，决定了加速度计数据校正陀螺仪的权重。值越大，对加速度计的响应越快，但对运动加速度更敏感。
 // Ki: 积分增益，决定了用于校正陀螺仪静态漂移的权重。
 // Q_dt: 采样时间间隔（单位：秒），这里是10ms (0.01s)，对应100Hz的采样率。
-#define HAVE_MAG 1
+#define HAVE_MAG 0
 #if HAVE_MAG == 0
 // -- 无地磁 --
 const float Kp = 2.0f;
@@ -1190,8 +1190,8 @@ const float Ki = 0.005f;
 const float Q_dt = 0.01f;
 #else
 // -- 有地磁 --
-const float Kp = 0.3f;
+const float Kp = 2.0f;
-const float Ki = 0.001f;
+const float Ki = 0.005f;
 const float Q_dt = 0.01f;
 #endif
 
@@ -1233,7 +1233,7 @@ float         Temp_Mag[3] = {0.0f, 0.0f, 0.0f};
 */
 unsigned char Q_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, const mmc5603nj_mag_data_t* _mag_data_input, unsigned char yaw_rst, float *quaternion_output)
 {
-#if 1  //有地磁置1
+#if 0  //有地磁置1
     unsigned char sl_i = 0;
     // 如果外部强制禁用校准，则将标志位置1
     if (calibration_en == 0) {
@@ -1312,8 +1312,6 @@ unsigned char Q_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed shor
                 // Error_Mag_f[0] = 0.0f - (float)Sum_Avg_Mag_f[0];
                 // Error_Mag_f[1] = 0.0f - (float)Sum_Avg_Mag_f[1];
                 // Error_Mag_f[2] = 0.0f - (float)Sum_Avg_Mag_f[2];
-                // 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;
@@ -1583,10 +1581,30 @@ unsigned char Q_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed shor
             eyInt = eyInt + ey * Ki * Q_dt;
             ezInt = ezInt + ez * Ki * Q_dt;
 
+            float kp_dynamic = Kp; // 默认使用全局Kp
+
+            // // 计算重力向量与Z轴的夹角余弦值
+            // // 当设备接近水平时，abs_az_component 接近 1
+            // // 当设备接近垂直时，abs_az_component 接近 0
+            // float abs_az_component = fabsf(az); 
+
+            // // 设置一个阈值，比如当与水平面的夹角大于75度时 (cos(75) approx 0.26)
+            // // 就开始降低Kp
+            // if (abs_az_component < 0.26f) {
+            //     // 线性降低Kp，或者直接使用一个较小的值
+            //     // 越接近垂直，az越小，Kp也越小
+            //     kp_dynamic = Kp * (abs_az_component / 0.26f); 
+            // }
+
             // 使用PI控制器校正陀螺仪的测量值
-            gx = gx + Kp * ex + exInt;
+            gx += kp_dynamic * ex + exInt;
-            gy = gy + Kp * ey + eyInt;
+            gy += kp_dynamic * ey + eyInt;
-            gz = gz + Kp * ez + ezInt;
+            gz += kp_dynamic * ez + ezInt;
+
+            
+            // gx = gx + Kp * ex + exInt;
+            // gy = gy + Kp * ey + eyInt;
+            // gz = gz + Kp * ez + ezInt;
         }
 
         // 使用校正后的角速度更新四元数 (一阶毕卡法)

apps/earphone/xtell_Sensor/xtell_handler.c

@@ -49,6 +49,7 @@
 #include "./sensor/MMC56.h"
 #include "./sensor/BMP280.h"
 #include "./sensor/AK8963.h"
+#include "./calculate/skiing_tracker.h"
 ///////////////////////////////////////////////////////////////////////////////////////////////////
 //宏定义
 #define LOG_TAG_CONST       EARPHONE
@@ -220,8 +221,10 @@ void le_user_app_event_handler(struct sys_event* event){
                             extern void sensor_measure(void);
                             static int test_id;
                             SL_SC7U22_Config();
+                            static skiing_tracker_t skiing_data;
+                            skiing_tracker_init(&skiing_data); 
                             create_process(&test_id, "test",NULL, sensor_measure, 10);
-                            send_tmp = "start_detection\n";
+                            send_tmp = "The test has begun.\n";
                             send_data_to_ble_client(send_tmp,strlen(send_tmp));
                             break;
                         case 0x03: