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

cun

Browse Source
This commit is contained in:
lmx
2025-11-18 17:27:06 +08:00
parent d0d9c0a630
commit ebca849be3
8 changed files with 1123 additions and 165 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -129,6 +129,8 @@ 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, 0x40, 0xA8);//ACC_CON 高性能模式，100Hz，平均数4 -- lmx
#if ACC_RANGE==2
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x41, 0x00);//ACC_RANGE 00：±2G
#endif
@ -144,8 +146,9 @@ unsigned char SL_SC7U22_Config(void)
// 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, 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, 0x42, 0xE8);//GYR_CONF 100Hz, 噪声优化开启,4个平均一次 -- 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
@ -997,6 +1000,10 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short
return 2; // 校准未完成，返回错误状态
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//
/**
* @brief 姿态角解算函数 (基于一阶互补滤波)
* @details
@ -1164,6 +1171,227 @@ unsigned char get_calibration_state(void){
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// 如果没有定义 PI，请取消下面的注释
// #define PI 3.14159265358979323846f
// =================================================================================================
// Mahony AHRS (Attitude and Heading Reference System) 相关变量定义
// Mahony滤波器是一种高效的互补滤波器，它使用四元数来表示姿态，从而避免了万向节死锁问题。
// 它通过一个PI控制器来校正陀ar螺仪的积分漂移。
// -------------------------------------------------------------------------------------------------
// --- 滤波器参数 ---
// Kp: 比例增益，决定了加速度计数据校正陀螺仪的权重。值越大，对加速度计的响应越快，但对运动加速度更敏感。
// Ki: 积分增益，决定了用于校正陀螺仪静态漂移的权重。
// Q_dt: 采样时间间隔（单位：秒），这里是10ms (0.01s)，对应100Hz的采样率。
const float Kp = 2.0f;
const float Ki = 0.005f;
const float Q_dt = 0.01f;
// --- 状态变量 ---
// 四元数 (Quaternion)，表示当前的姿态。初始化为 (1, 0, 0, 0)，代表初始姿态为水平。
static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;
// 陀螺仪积分误差项，用于补偿静态漂移
static float exInt = 0.0f, eyInt = 0.0f, ezInt = 0.0f;
// =================================================================================================
/**
* @brief 姿态角解算函数 (基于四元数和Mahony滤波器)
* @details
* 该函数主要完成两项工作：
* 1. 静态校准：在初始阶段，检测传感器是否处于静止状态。如果是，则计算加速度计和陀螺仪的零点偏移（误差），用于后续的数据补偿。
* 2. 姿态解算：使用基于四元数的Mahony互补滤波器融合经过校准后的加速度计和陀螺仪数据，计算出物体的俯仰角（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，则将整个姿态滤波器状态重置。
*
* @return
* - 0: 正在进行静态校准。
* - 1: 姿态角计算成功。
* - 2: 校准未完成，无法进行计算。
*/
unsigned char Q_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst, float *quaternion_output)
{
unsigned char sl_i = 0;
// 如果外部强制禁用校准，则将标志位置1
if (calibration_en == 0) {
SL_SC7U22_Error_Flag = 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++;
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];
#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];
// 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: 姿态解算 (Mahony AHRS)
// ---------------------------------------------------------------------------------
if (SL_SC7U22_Error_Flag == 1) { // 确认已经校准完成
// --- 2.1 Yaw轴/姿态重置 ---
// 注意：重置yaw会重置整个姿态滤波器，使设备回到初始水平姿态
if (yaw_rst == 1) {
q0 = 1.0f; q1 = 0.0f; q2 = 0.0f; q3 = 0.0f;
exInt = 0.0f; eyInt = 0.0f; ezInt = 0.0f;
}
// --- 2.2 数据预处理 ---
// 应用零点偏移补偿
for (sl_i = 0; sl_i < 6; sl_i++) {
Temp_Accgyro[sl_i] = acc_gyro_input[sl_i] + Error_Accgyro[sl_i];
}
// 将校准后的数据写回输入数组 (可选)
#if 1
for (sl_i = 0; sl_i < 6; sl_i++) {
acc_gyro_input[sl_i] = Temp_Accgyro[sl_i];
}
#endif
// 获取校准后的数据
float ax = (float)Temp_Accgyro[0];
float ay = (float)Temp_Accgyro[1];
float az = (float)Temp_Accgyro[2];
// 将陀螺仪数据从 LSB 转换为弧度/秒 (rad/s)
// 转换系数 0.061 ≈ 2000dps / 32768 LSB; PI/180 ≈ 0.01745
float gx = (float)Temp_Accgyro[3] * 0.061f * 0.0174533f; // Roll rate
float gy = (float)Temp_Accgyro[4] * 0.061f * 0.0174533f; // Pitch rate
float gz = (float)Temp_Accgyro[5] * 0.061f * 0.0174533f; // Yaw rate
// --- 2.3 Mahony算法核心 ---
float norm;
float vx, vy, vz; // 估计的重力向量
float ex, ey, ez; // 加速度计测量和估计的重力向量之间的误差
// 归一化加速度计测量值，得到单位重力向量
norm = sqrtf(ax * ax + ay * ay + az * az);
if (norm > 0.0f) { // 防止除以零
ax = ax / norm;
ay = ay / norm;
az = az / norm;
// 根据当前姿态(四元数)估计重力向量的方向
vx = 2.0f * (q1 * q3 - q0 * q2);
vy = 2.0f * (q0 * q1 + q2 * q3);
vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
// 计算测量值与估计值之间的误差（叉积）
ex = (ay * vz - az * vy);
ey = (az * vx - ax * vz);
ez = (ax * vy - ay * vx);
// 积分误差项，用于消除陀螺仪的静态漂移
exInt = exInt + ex * Ki * Q_dt;
eyInt = eyInt + ey * Ki * Q_dt;
ezInt = ezInt + ez * Ki * Q_dt;
// 使用PI控制器校正陀螺仪的测量值
gx = gx + Kp * ex + exInt;
gy = gy + Kp * ey + eyInt;
gz = gz + Kp * ez + ezInt;
}
// 使用校正后的角速度更新四元数 (一阶毕卡法)
q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * 0.5f * Q_dt;
q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * 0.5f * Q_dt;
q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * 0.5f * Q_dt;
q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * 0.5f * Q_dt;
// 归一化四元数，保持其单位长度
norm = sqrtf(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
// --- 2.4 将四元数转换为欧拉角 (Pitch, Roll, Yaw) ---
// 转换公式将四元数转换为 ZYX 顺序的欧拉角
// 结果单位为弧度，乘以 57.29578f 转换为度
// Pitch (绕Y轴旋转)
Angle_output[0] = asinf(-2.0f * (q1 * q3 - q0 * q2)) * 57.29578f;
// Roll (绕X轴旋转)
Angle_output[1] = atan2f(2.0f * (q0 * q1 + q2 * q3), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * 57.29578f;
// Yaw (绕Z轴旋转) - 注意：无磁力计的6轴IMU，Yaw角会随时间漂移
Angle_output[2] = atan2f(2.0f * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.29578f;
if (quaternion_output != NULL) {
quaternion_output[0] = q0; // w
quaternion_output[1] = q1; // x
quaternion_output[2] = q2; // y
quaternion_output[3] = q3; // z
}
return 1; // 返回1，表示计算成功
}
return 2; // 校准未完成，返回错误状态
}
#endif
///////////////////////////////////////////////////////////////////////////////////////////////////

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

@ -132,6 +132,7 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en,signed short *
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 SIX_SL_SC7U22_Angle_Output(unsigned char auto_calib_start, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst);
unsigned char Q_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst, float *quaternion_output);
unsigned char get_calibration_state(void);
/**寄存器宏定义*******************************/
#define SC7U22_WHO_AM_I 0x01