From 054ea8644a4e4578ea313f59af86a31bd3764c3a Mon Sep 17 00:00:00 2001 From: lmx Date: Tue, 18 Nov 2025 18:47:05 +0800 Subject: [PATCH] =?UTF-8?q?=E5=9B=9B=E5=85=83=E6=95=B0=E6=B1=82=E8=A7=92?= =?UTF-8?q?=E5=BA=A6=E5=92=8C=E5=8E=BB=E9=87=8D=E5=8A=9B=E5=88=86=E9=87=8F?= =?UTF-8?q?=EF=BC=8C=E8=AF=AF=E5=B7=AE=E5=87=8F=E5=B0=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- apps/earphone/xtell_Sensor/README.md | 30 +++++++++++ .../xtell_Sensor/calculate/skiing_tracker.c | 54 +++++++++++++++++-- apps/earphone/xtell_Sensor/send_data.c | 14 +++-- 3 files changed, 90 insertions(+), 8 deletions(-) diff --git a/apps/earphone/xtell_Sensor/README.md b/apps/earphone/xtell_Sensor/README.md index 85bbea1..659566d 100644 --- a/apps/earphone/xtell_Sensor/README.md +++ b/apps/earphone/xtell_Sensor/README.md @@ -32,6 +32,7 @@ sensor_processing_task当中就进行了计算，包括卡尔曼等，在timer # 11.13 + 代码主要文件夹：apps\earphone\xtell_Sensor - apps\earphone\xtell_Sensor\send_data.c，‘ xtell_task_create ’ 函数，传感器计算程序逻辑开始位置，包括传感器读取数据的任务、蓝牙发送任务、速度距离计算任务 @@ -63,3 +64,32 @@ sensor_processing_task当中就进行了计算，包括卡尔曼等，在timer - 重复定位的数据？ - 静态时的角度误差？ + + +定时器1的回调函数（10ms调用一次）**A**：读取传感器数据，放进buff + +定时器2的回调函数（5ms调用一次）**B**：读取buff的传感器数据，去除重力分离的计算 + +- **数据没有对齐**，A 的回调调用计数 > B 的回调调用计数 +- **丢数据了**，A 读取传感器数据的回调函数中，打印了buff已满的log +- **重复定位**：移动后回到原先的位置，前后的计算得到的三轴角度相同 +- **静态时的角度误差**：1°左右 +- 定时器2不进行重力分离计算，只进行计数，也仍然有数据没有对齐和丢数据的情况 + + + +将读取传感器数据、去除重力分量计算放到同一个任务下，同步进行 + +- 数据没有丢失，数据也对齐了 + - 在小倾斜的坡面下，去除重力分量后的总的加速度，**小于0.1m/s^2** + - 在大倾斜的坡面下（如旋转超过70°），去除重力分量后的总的加速度，在**0.4m/s^2上下** +- 貌似是角度越大，越接近方向锁，导致角度更容易漂移造错数据错误 + +采用四元数的方式做去除重力分量的计算： + +- 将读取传感器数据、去除重力分量计算放到同一个任务下 + + - 在小倾斜的坡面下，去除重力分量后的总的加速度，低于**0.04m/s^2** + - 在大倾斜的坡面下（如旋转超过70°），去除重力分量后的总的加速度，在**0.1m/s^2上下** + + - 大倾斜角度的误差要靠磁力计来消除（yaw无法通过加速度计来消除偏差） diff --git a/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c b/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c index 1e84dec..922558c 100644 --- a/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c +++ b/apps/earphone/xtell_Sensor/calculate/skiing_tracker.c @@ -9,6 +9,16 @@ #define G_ACCELERATION 9.81f #define DEG_TO_RAD (3.14159265f / 180.0f) +#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 + // --- 静止检测 --- //两个判断是否静止的必要条件：动态零速更新(ZUPT)阈值 // 加速方差阈值，提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新 @@ -168,7 +178,7 @@ void q_remove_gravity_with_quaternion(const float *acc_device, const float *q, f } /** - * @brief 使用四元数将设备坐标系的线性加速度转换到世界坐标系,并且移除重力分量 + * @brief 使用四元数将设备坐标系的线性加速度转换到世界坐标系 * @details 同样，此方法比使用欧拉角更优。 * @param acc_linear_device 输入：设备坐标系下的线性加速度 [x, y, z] * @param q 输入：表示姿态的四元数 [w, x, y, z] @@ -188,7 +198,7 @@ void q_transform_to_world_with_quaternion(const float *acc_linear_device, const 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; + // acc_linear_world[2] -= G_ACCELERATION; } @@ -451,8 +461,15 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d break; case WHEEL: case NO_CONSTANT_SPEED: - q_transform_to_world_with_quaternion(acc_device_ms2, quaternion_data, tracker->acc_no_g); + float linear_acc_device[3]; + float linear_acc_world[3]; + // 在设备坐标系下，移除重力，得到线性加速度 + q_remove_gravity_with_quaternion(acc_device_ms2, quaternion_data, linear_acc_device); + // 将设备坐标系下的线性加速度，旋转到世界坐标系 + q_transform_to_world_with_quaternion(linear_acc_device, quaternion_data, linear_acc_world); + // 将最终用于积分的加速度存入 tracker 结构体 + memcpy(tracker->acc_no_g, linear_acc_world, sizeof(linear_acc_world)); float acc_world_temp[3]; // 临时变量存储当前周期的加速度 for (int i = 0; i < 2; i++) { // 只处理水平方向的 x 和 y 轴 @@ -512,6 +529,37 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d break; } + + + + #if 1 + float linear_acc_device[3]; + float linear_acc_world[3]; + float tmp_world_acc[3]; + // 在设备坐标系下，移除重力，得到线性加速度 + q_remove_gravity_with_quaternion(acc_device_ms2, quaternion_data, linear_acc_device); + + // 将设备坐标系下的线性加速度，旋转到世界坐标系 + q_transform_to_world_with_quaternion(linear_acc_device, quaternion_data, tmp_world_acc); + + + 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("===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 + + } diff --git a/apps/earphone/xtell_Sensor/send_data.c b/apps/earphone/xtell_Sensor/send_data.c index 570b877..b17cf47 100644 --- a/apps/earphone/xtell_Sensor/send_data.c +++ b/apps/earphone/xtell_Sensor/send_data.c @@ -179,7 +179,7 @@ typedef struct { signed short acc_data[3]; signed short gyr_data[3]; float angle[3]; //pitch roll yaw - float quaternion_output[4]; + float quaternion_output[4]; //四元数数据 } sensor_data_t; static sensor_data_t sensor_read_buffer[SENSOR_DATA_BUFFER_SIZE]; // 存放sensor读到的数据 @@ -213,10 +213,12 @@ void sensor_read_data(){ 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){ count = 0; @@ -233,9 +235,11 @@ void sensor_read_data(){ } } 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)); if(mutex1 == 0){ @@ -329,7 +333,7 @@ void BLE_send_data(){ // send_data_to_ble_client(&log_buffer,strlen(log_buffer)); memset(&log_buffer, 0, 100); - #if 0 + #if 1 // 使用 snprintf 进行格式化 num_chars_written = snprintf( log_buffer, // 目标缓冲区 @@ -557,7 +561,7 @@ void xtell_task_create(void){ // create_process(&sensor_read_data_id, "read",NULL, sensor_read_data, 10); - +// // create_process(&calculate_data_id, "calculate",NULL, calculate_data, 4); // create_process(&ble_send_data_id, "send",NULL, BLE_send_data, 1);