lmx/99_7018_lmx
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

重力分量去除后仍有偏差

Browse Source
This commit is contained in:
lmx
2025-11-13 20:30:10 +08:00
parent 046986c5c3
commit b621ef7e44
29 changed files with 168502 additions and 167884 deletions
Download Patch File Download Diff File Expand all files Collapse all files

179
apps/earphone/xtell_Sensor/sensor/SC7U22.c
View File

@ -129,8 +129,18 @@ unsigned char SL_SC7U22_Config(void)
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0xBC);//ACC_CON 高性能模式，1600Hz -- lmx
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0xBB);//ACC_CON 高性能模式，800Hz -- lmx
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x41, 0x01);//ACC_RANGE 01：±4G
#if ACC_RANGE==2
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x41, 0x00);//ACC_RANGE 00：±2G
#endif
#if ACC_RANGE==4
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x41, 0x01);//ACC_RANGE 01：±4G
#endif
#if ACC_RANGE==8
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x41, 0x02);//ACC_RANGE 02：±8G
#endif
#if ACC_RANGE==16
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x41, 0x03);//ACC_RANGE 03：±16G
#endif
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0x86);//GYR_CONF 0x87=50Hz 0x86=25Hz
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0x8C);//GYR_CONF 1600Hz -- lmx
@ -748,7 +758,15 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short
}
// 判断是否处于静止状态：加速度变化量、陀螺仪变化量、各轴加速度值都在一个很小的范围内
#if (ACC_RANGE == 2)
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] - 16384) < 3000)) { //acc<160mg gyro<40 lsb
#elif (ACC_RANGE == 4)
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
#elif (ACC_RANGE == 8)
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] - 4096) < 3000)) { //acc<160mg gyro<40 lsb
#elif (ACC_RANGE == 16)
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] - 2048) < 3000)) { //acc<160mg gyro<40 lsb
#endif
// 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++; // 静止计数器累加
@ -776,7 +794,22 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short
// 计算零点偏移：理想值 - 实际平均值
Error_Accgyro[0] = 0 - Sum_Avg_Accgyro[0];
Error_Accgyro[1] = 0 - Sum_Avg_Accgyro[1];
Error_Accgyro[2] = 8192 - Sum_Avg_Accgyro[2];
#if ACC_RANGE==2
Error_Accgyro[2] = 16384 - Sum_Avg_Accgyro[2];
#endif
#if ACC_RANGE==4
Error_Accgyro[2] = 8192 - Sum_Avg_Accgyro[2];
#endif
#if ACC_RANGE==8
Error_Accgyro[2] = 4096 - Sum_Avg_Accgyro[2];
#endif
#if ACC_RANGE==16
Error_Accgyro[2] = 2048 - Sum_Avg_Accgyro[2];
#endif
Error_Accgyro[3] = 0 - Sum_Avg_Accgyro[3];
Error_Accgyro[4] = 0 - Sum_Avg_Accgyro[4];
Error_Accgyro[5] = 0 - Sum_Avg_Accgyro[5];
@ -811,6 +844,13 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short
#endif
// --- 2.2 使用加速度计计算姿态角 ---
// 将加速度原始值转换为归一化的重力分量
#if ACC_RANGE==2
//2g：
angle_acc[0] = (float)Temp_Accgyro[0] / 16384; //ax
angle_acc[1] = (float)Temp_Accgyro[1] / 16384; //ay
angle_acc[2] = (float)Temp_Accgyro[2] / 16384; //az
#endif
#if ACC_RANGE==4
//4g：
angle_acc[0] = (float)Temp_Accgyro[0] / 8192; //ax
@ -818,6 +858,12 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short
angle_acc[2] = (float)Temp_Accgyro[2] / 8192; //az
#endif
#if ACC_RANGE==8
angle_acc[0] = (float)Temp_Accgyro[0] / 4096; //ax
angle_acc[1] = (float)Temp_Accgyro[1] / 4096; //ay
angle_acc[2] = (float)Temp_Accgyro[2] / 4096; //az
#endif
#if ACC_RANGE==16
//±16g 2048
angle_acc[0] = (float)Temp_Accgyro[0] / 2048; //ax
@ -951,63 +997,68 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short
return 2; // 校准未完成，返回错误状态
}
// =================================================================================
// 关键参数定义 - 需要根据实际情况进行调整
// ---------------------------------------------------------------------------------
// 滤波系数 α (alpha)，决定了对陀螺仪和加速度计的信任程度。
// α 越大，越相信陀螺仪，动态响应越好，但对漂移的修正越慢。
// α 越小，越相信加速度计，对漂移的修正越快，但对运动加速度越敏感。
// 典型值范围：0.95 ~ 0.99
#define FILTER_ALPHA 0.98f
// 采样时间间隔 SAMPLE_INTERVAL (单位：秒)
// 必须与此函数的频率严格对应。例如，100Hz 对应 0.01f。
#define SAMPLE_INTERVAL 0.01f
// =================================================================================
/**
* @brief 不带卡尔曼的欧若拉数据输出，以及消除零点漂移后的六轴数据
*
* @param calibration_en
* @param acc_gyro_input
* @param Angle_output
* @param yaw_rst
* @return unsigned char
* @brief 姿态角解算函数 (基于一阶互补滤波)
* @details
* 该函数主要完成两项工作：
* 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角清零。
*
* @return
* - 0: 正在进行静态校准。
* - 1: 姿态角计算成功。
* - 2: 校准未完成，无法进行计算。
*/
unsigned char Original_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst){
unsigned short acc_gyro_delta[2];
unsigned char Original_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst)
{
// --- 静态变量和宏定义 ---
const float dt = 0.01f; // 假设采样周期为 10ms (100Hz)
// 互补滤波器系数. alpha 越大, 越相信陀螺仪的积分, 动态响应越好, 但长期会漂移.
// (1-alpha) 越大, 越相信加速度计的角度, 静态越准, 但动态响应差且易受运动干扰.
// 典型值在 0.95 ~ 0.98 之间.
const float FILTER_ALPHA = 0.98f;
unsigned char sl_i = 0;
float angle_acc[3] = {0};
float gyro_val[3] = {0};
float angle_acc[3] = {0}; // [Pitch_acc, Roll_acc, unused]
float gyro_dps[3] = {0}; // [Roll_rate, Pitch_rate, Yaw_rate]
// 如果外部强制使能校准，则将标志位置1
// 如果外部强制禁用校准，则将标志位置1
if (calibration_en == 0) {
SL_SC7U22_Error_Flag = 1;
}
// =================================================================================
// 步骤 1: 静态校准 (此部分完全保留，不做任何修改)
// 步骤 1: 静态校准 (此部分逻辑完全保留)
// ---------------------------------------------------------------------------------
if (SL_SC7U22_Error_Flag == 0) {
// ... (省略与原代码完全相同的部分以节省篇幅) ...
// 计算当前数据与上一帧数据的差值，用于判断是否静止
unsigned short acc_gyro_delta[2];
acc_gyro_delta[0] = 0;
acc_gyro_delta[1] = 0;
for (sl_i = 0; sl_i < 3; sl_i++) {
acc_gyro_delta[0] += SL_GetAbsShort(acc_gyro_input[sl_i] - Temp_Accgyro[sl_i]);
acc_gyro_delta[1] += SL_GetAbsShort(acc_gyro_input[3 + sl_i] - Temp_Accgyro[3 + sl_i]);
}
// 保存当前数据，用于下一帧比较
for (sl_i = 0; sl_i < 6; sl_i++) {
Temp_Accgyro[sl_i] = acc_gyro_input[sl_i];
}
// 判断是否处于静止状态
#if (ACC_RANGE == 2)
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] - 16384) < 3000)) {
#elif (ACC_RANGE == 4)
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)) {
#elif (ACC_RANGE == 8)
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] - 4096) < 3000)) {
#elif (ACC_RANGE == 16)
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] - 2048) < 3000)) {
#endif
if (SL_SC7U22_Error_cnt < 200) SL_SC7U22_Error_cnt++;
} else {
SL_SC7U22_Error_cnt = 0;
}
// 如果静止时间足够长
if (SL_SC7U22_Error_cnt > 190) {
for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] += acc_gyro_input[sl_i];
SL_SC7U22_Error_cnt2++;
@ -1018,7 +1069,15 @@ unsigned char Original_SL_SC7U22_Angle_Output(unsigned char calibration_en, sign
for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] = Sum_Avg_Accgyro[sl_i] / 50;
Error_Accgyro[0] = 0 - Sum_Avg_Accgyro[0];
Error_Accgyro[1] = 0 - Sum_Avg_Accgyro[1];
#if ACC_RANGE==2
Error_Accgyro[2] = 16384 - Sum_Avg_Accgyro[2];
#elif ACC_RANGE==4
Error_Accgyro[2] = 8192 - Sum_Avg_Accgyro[2];
#elif ACC_RANGE==8
Error_Accgyro[2] = 4096 - Sum_Avg_Accgyro[2];
#elif ACC_RANGE==16
Error_Accgyro[2] = 2048 - Sum_Avg_Accgyro[2];
#endif
Error_Accgyro[3] = 0 - Sum_Avg_Accgyro[3];
Error_Accgyro[4] = 0 - Sum_Avg_Accgyro[4];
Error_Accgyro[5] = 0 - Sum_Avg_Accgyro[5];
@ -1031,9 +1090,9 @@ unsigned char Original_SL_SC7U22_Angle_Output(unsigned char calibration_en, sign
}
return 0; // 返回0，表示正在校准
}
// =================================================================================
// 步骤 2: 姿态解算 (使用一阶互补滤波)
// 步骤 2: 姿态解算 (一阶互补滤波)
// ---------------------------------------------------------------------------------
if (SL_SC7U22_Error_Flag == 1) { // 确认已经校准完成
// --- 2.1 数据预处理 ---
@ -1043,49 +1102,63 @@ unsigned char Original_SL_SC7U22_Angle_Output(unsigned char calibration_en, sign
}
// --- 2.2 使用加速度计计算姿态角 (Pitch 和 Roll) ---
// 注意：这里使用了atan2f来计算Roll角，它比atanf更稳定，能自动处理所有象限，避免了后续的if判断。
float acc_x = (float)Temp_Accgyro[0];
float acc_y = (float)Temp_Accgyro[1];
float acc_z = (float)Temp_Accgyro[2];
// Pitch = arcsin(ax / g)
angle_acc[0] = asinf(acc_x / sqrtf(acc_x*acc_x + acc_y*acc_y + acc_z*acc_z)) * 57.29578f;
// 使用 atan2 来计算角度，避免奇点问题和象限问题，更稳健
// Pitch = arctan(-ax / sqrt(ay^2 + az^2))
angle_acc[0] = atan2f(-acc_x, sqrtf(acc_y*acc_y + acc_z*acc_z)) * 57.29578f;
// Roll = arctan(ay / az)
angle_acc[1] = atan2f(acc_y, acc_z) * 57.29578f;
// --- 2.3 转换陀螺仪数据单位 ---
// 将陀螺仪原始值（LSB）转换为角速度（度/秒）
gyro_val[0] = Temp_Accgyro[4] * 0.061; // GYR-Y -> Pitch 速率
gyro_val[1] = Temp_Accgyro[3] * 0.061; // GYR-X -> Roll 速率
gyro_val[2] = Temp_Accgyro[5] * 0.061; // GYR-Z -> Yaw 速率
// **注意轴向对应关系**
gyro_dps[0] = Temp_Accgyro[3] * 0.061f; // GYR-X -> Roll 速率
gyro_dps[1] = Temp_Accgyro[4] * 0.061f; // GYR-Y -> Pitch 速率
gyro_dps[2] = Temp_Accgyro[5] * 0.061f; // GYR-Z -> Yaw 速率
// =================================================================================
// 步骤 2.4: 一阶互补滤波
// 公式: angle = alpha * (angle + gyro_rate * dt) + (1 - alpha) * acc_angle
// ---------------------------------------------------------------------------------
// Pitch 轴融合
Angle_output[0] = FILTER_ALPHA * (Angle_output[0] + gyro_val[0] * SAMPLE_INTERVAL) + (1.0f - FILTER_ALPHA) * angle_acc[0];
// Pitch 轴融合 (Angle_output[0] 对应 Pitch)
Angle_output[0] = FILTER_ALPHA * (Angle_output[0] + gyro_dps[1] * dt) + (1.0f - FILTER_ALPHA) * angle_acc[0];
// Roll 轴融合
Angle_output[1] = FILTER_ALPHA * (Angle_output[1] + gyro_val[1] * SAMPLE_INTERVAL) + (1.0f - FILTER_ALPHA) * angle_acc[1];
/************** Yaw 轴计算**************/
// Roll 轴融合 (Angle_output[1] 对应 Roll)
Angle_output[1] = FILTER_ALPHA * (Angle_output[1] + gyro_dps[0] * dt) + (1.0f - FILTER_ALPHA) * angle_acc[1];
/************** Yaw 轴计算 **************/
// Yaw角无法通过加速度计（重力）来校正，因此只能使用陀螺仪进行简单积分。
// 如果需要精确的Yaw角，必须引入磁力计进行修正。
if (yaw_rst == 1) {
Angle_output[2] = 0; // 如果有复位信号，则清零
}
// 增加一个简单的阈值，当角速度较小时，认为没有转动，以减少漂移
if (SL_GetAbsShort(Temp_Accgyro[5]) > 8) {
Angle_output[2] += gyro_val[2] * SAMPLE_INTERVAL;
if (fabsf(gyro_dps[2]) > 0.1f) { // 使用转换后的dps值判断更直观
Angle_output[2] += gyro_dps[2] * dt;
}
// 保持Yaw角在 -180 到 180 度之间 (可选，但推荐)
if (Angle_output[2] > 180.0f) {
Angle_output[2] -= 360.0f;
} else if (Angle_output[2] < -180.0f) {
Angle_output[2] += 360.0f;
}
// --- 将校准后的数据写回输入数组 (可选，根据你的需求保留或移除) ---
#if 1
for (sl_i = 0; sl_i < 6; sl_i++) {
acc_gyro_input[sl_i] = Temp_Accgyro[sl_i];
}
#endif
return 1; // 返回1，表示计算成功
}
return 2; // 校准未完成，返回错误状态
}
unsigned char get_calibration_state(void){
return SL_SC7U22_Error_Flag;
}