From bdadd5de1ec8acdf0f0c29563b149264a1fc69de Mon Sep 17 00:00:00 2001 From: lmx Date: Fri, 21 Nov 2025 10:53:47 +0800 Subject: [PATCH] cun --- .../xtell_Sensor/calculate/skiing_tracker.c | 3 ++ apps/earphone/xtell_Sensor/send_data.c | 14 ++++---- apps/earphone/xtell_Sensor/sensor/MMC56.c | 22 ++++++------ apps/earphone/xtell_Sensor/sensor/SC7U22.c | 36 ++++++++++++++----- apps/earphone/xtell_Sensor/xtell_handler.c | 5 ++- 5 files changed, 51 insertions(+), 29 deletions(-) diff --git a/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c b/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c index 2e46e37..c26f9ea 100644 --- a/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c +++ b/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++; diff --git a/apps/earphone/xtell_Sensor/send_data.c b/apps/earphone/xtell_Sensor/send_data.c index bc9bba8..3d04e00 100644 --- a/apps/earphone/xtell_Sensor/send_data.c +++ b/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); diff --git a/apps/earphone/xtell_Sensor/sensor/MMC56.c b/apps/earphone/xtell_Sensor/sensor/MMC56.c index bacd250..1d528af 100644 --- a/apps/earphone/xtell_Sensor/sensor/MMC56.c +++ b/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; } diff --git a/apps/earphone/xtell_Sensor/sensor/SC7U22.c b/apps/earphone/xtell_Sensor/sensor/SC7U22.c index 3f08a9e..0be8e81 100644 --- a/apps/earphone/xtell_Sensor/sensor/SC7U22.c +++ b/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 Ki = 0.001f; +const float Kp = 2.0f; +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; - gy = gy + Kp * ey + eyInt; - gz = gz + Kp * ez + ezInt; + gx += kp_dynamic * ex + exInt; + gy += kp_dynamic * ey + eyInt; + gz += kp_dynamic * ez + ezInt; + + + // gx = gx + Kp * ex + exInt; + // gy = gy + Kp * ey + eyInt; + // gz = gz + Kp * ez + ezInt; } // 使用校正后的角速度更新四元数 (一阶毕卡法) diff --git a/apps/earphone/xtell_Sensor/xtell_handler.c b/apps/earphone/xtell_Sensor/xtell_handler.c index a3bdb0f..4d9c709 100644 --- a/apps/earphone/xtell_Sensor/xtell_handler.c +++ b/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: