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-21 10:53:47 +08:00
parent 9ccf1acda8
commit bdadd5de1e
5 changed files with 51 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
apps/earphone/xtell_Sensor/sensor/MMC56.c
View File

@ -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;
}

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

@ -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;
}
// 使用校正后的角速度更新四元数 (一阶毕卡法)