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-11 19:31:34 +08:00
parent 23a71377a2
commit 58ad14691e
37 changed files with 172314 additions and 171919 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -119,9 +119,18 @@ unsigned char SL_SC7U22_Config(void)
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, 0x0E);//PWR_CTRL ENABLE ACC+GYR+TEMP
os_time_dly(1);//10ms
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0x06);//ACC_CONF 0x07=50Hz 0x06=25Hz
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0x06);//ACC_CONF 0x07=50Hz 0x06=25Hz
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0xA8);//高性能模式，连续4个数据平均1次，100Hz -- lmx
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0xAC);//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 10：±8G 01：±4G
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, 0x86);//GYR_CONF 0x87=50Hz 0x86=25Hz
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0x8C);//GYR_CONF 1600Hz -- lmx
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0xAC);//GYR_CONF 1600Hz -- lmx
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0xAB);//GYR_CONF 800Hz -- lmx
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, 0x00);//GYR_RANGE 2000dps
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, 0x00);//GYR_RANGE 2000dps
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x04, 0x50);//COM_CFG
@ -927,6 +936,140 @@ 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
*/
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 sl_i = 0;
float angle_acc[3] = {0};
float gyro_val[3] = {0};
// 如果外部强制使能校准，则将标志位置1
if (calibration_en == 0) {
SL_SC7U22_Error_Flag = 1;
}
// =================================================================================
// 步骤 1: 静态校准 (此部分完全保留，不做任何修改)
// ---------------------------------------------------------------------------------
if (SL_SC7U22_Error_Flag == 0) {
// ... (省略与原代码完全相同的部分以节省篇幅) ...
// 计算当前数据与上一帧数据的差值，用于判断是否静止
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_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)) {
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++;
if (SL_SC7U22_Error_cnt2 > 49) {
SL_SC7U22_Error_Flag = 1;
SL_SC7U22_Error_cnt2 = 0;
SL_SC7U22_Error_cnt = 0;
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];
Error_Accgyro[2] = 8192 - Sum_Avg_Accgyro[2];
Error_Accgyro[3] = 0 - Sum_Avg_Accgyro[3];
Error_Accgyro[4] = 0 - Sum_Avg_Accgyro[4];
Error_Accgyro[5] = 0 - Sum_Avg_Accgyro[5];
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;
for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] = 0;
}
return 0; // 返回0，表示正在校准
}
// =================================================================================
// 步骤 2: 姿态解算 (使用一阶互补滤波)
// ---------------------------------------------------------------------------------
if (SL_SC7U22_Error_Flag == 1) { // 确认已经校准完成
// --- 2.1 数据预处理 ---
// 应用零点偏移补偿
for (sl_i = 0; sl_i < 6; sl_i++) {
Temp_Accgyro[sl_i] = acc_gyro_input[sl_i] + Error_Accgyro[sl_i];
}
// --- 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;
// 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 速率
// =================================================================================
// 步骤 2.4: 一阶互补滤波
// ---------------------------------------------------------------------------------
// Pitch 轴融合
Angle_output[0] = FILTER_ALPHA * (Angle_output[0] + gyro_val[0] * SAMPLE_INTERVAL) + (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 轴计算**************/
// 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;
}
return 1; // 返回1，表示计算成功
}
return 2; // 校准未完成，返回错误状态
}
unsigned char get_calibration_state(void){
return SL_SC7U22_Error_Flag;
}

2
apps/earphone/xtell_Sensor/sensor/SC7U22.h
View File

@ -130,6 +130,8 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en,signed short *
/**output Angle_output[2]: Yaw*******************************/
/**input yaw_rst: reset yaw value***************************/
unsigned char Original_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst);
unsigned char get_calibration_state(void);
/**寄存器宏定义*******************************/
#define SC7U22_WHO_AM_I 0x01