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-03 18:48:15 +08:00
parent 97e85df2f8
commit 671730a351
96 changed files with 170886 additions and 186935 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
apps/earphone/xtell_Sensor/sensor/LIS2DH12.c
View File

@ -33,7 +33,7 @@
#define LIS2DH12_R_ADDR 0x33
// --- IIC 寄存器地址宏定义 ---
#define LIS2DH12_WHO_AM_I 0x0F
#define LIS2DH12_WHO_AM_I 0x01 //0F
#define LIS2DH12_CTRL_REG1 0x20
#define LIS2DH12_CTRL_REG4 0x23
#define LIS2DH12_CTRL_REG5 0x24
@ -93,7 +93,7 @@ static u8 SL_MEMS_i2cWrite(u8 addr, u8 reg, u8 data) {
char LIS2DH12_Check() {
u8 reg_value = 0;
SL_MEMS_i2cRead(LIS2DH12_R_ADDR, LIS2DH12_WHO_AM_I, 1, &reg_value);
if (reg_value == 0x33) {
if (reg_value == 0x6A) { //0x33
return 0x01;
}
return 0x00;
@ -150,7 +150,7 @@ void LIS2DH12_calibrate() {
// 初始化并配置LIS2DH12传感器
u8 LIS2DH12_Config(void) {
if (LIS2DH12_Check() != 1) {
xlog("LIS2DH12 I2C检查失败\n");
xlog("LIS2DH12 I2C error\n");
return -1;
}
@ -163,7 +163,7 @@ u8 LIS2DH12_Config(void) {
// 执行开机校准
LIS2DH12_calibrate();
xlog("LIS2DH12 I2C检查成功\n");
xlog("LIS2DH12 I2C success\n");
return 0;
}

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

@ -1,12 +1,18 @@
#include "SCU722.h"
#include "SC7U22.h"
#include "math.h"
#include "os/os_api.h"
#if SL_Sensor_Algo_Release_Enable==0x00
#include "printf.h"
#endif
#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
//I2C SPI选择
//#define SL_SC7U22_SPI_EN_I2C_DISABLE 0x00 //需要配合SL_SPI_IIC_INTERFACE使用
@ -61,13 +67,11 @@ static void sl_delay(unsigned char sl_i)
unsigned char SL_SC7U22_Check(void)
{
unsigned char reg_value=0;
xlog("SL_SC7U22_Check\n");
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, SC7U22_WHO_AM_I, 1, &reg_value);
#if SL_Sensor_Algo_Release_Enable==0x00
printf("0x%x=0x%x\r\n",SC7U22_WHO_AM_I,reg_value);
#endif
if(reg_value==0x6A)
xlog("0x%x=0x%x\r\n",SC7U22_WHO_AM_I,reg_value);
if(reg_value==0x6A) //设备的id
return 0x01;//SC7U22
else
return 0x00;//通信异常
@ -75,6 +79,7 @@ unsigned char SL_SC7U22_Check(void)
unsigned char SL_SC7U22_Config(void)
{
xlog("SL_SC7U22_Config\n");
unsigned char Check_Flag=0;
unsigned char reg_value=0;
@ -95,23 +100,18 @@ unsigned char SL_SC7U22_Config(void)
Check_Flag=SL_SC7U22_Check();
// Check_Flag= SL_SC7U22_SOFT_RESET();
// Check_Flag=1;//强制初始化
#if SL_Sensor_Algo_Release_Enable==0x00
printf("SL_SC7U22_Check=0x%x\r\n",Check_Flag);
#endif
xlog("SL_SC7U22_Check=0x%x\r\n",Check_Flag);
if(Check_Flag==1)
{
Check_Flag= SL_SC7U22_POWER_DOWN();
}
#if SL_Sensor_Algo_Release_Enable==0x00
printf("SL_SC7U22_POWER_DOWN=0x%x\r\n",Check_Flag);
#endif
xlog("SL_SC7U22_POWER_DOWN=0x%x\r\n",Check_Flag);
if(Check_Flag==1)
{
Check_Flag= SL_SC7U22_SOFT_RESET();
}
#if SL_Sensor_Algo_Release_Enable==0x00
printf("SL_SC7U22_SOFT_RESET=0x%x\r\n",Check_Flag);
#endif
xlog("SL_SC7U22_SOFT_RESET=0x%x\r\n",Check_Flag);
if(Check_Flag==1)
{
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00
@ -119,9 +119,9 @@ 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
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, 0x41, 0x01);//ACC_RANGE ±8G
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0x86);//GYR_CONF 0x87=50Hz
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, 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
@ -218,24 +218,23 @@ void SL_SC7U22_RawData_Read(signed short * acc_data_buf,signed short * gyr_data_
if(drdy_cnt>30000) break;
}
#if SL_Sensor_Algo_Release_Enable==0x00
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x30, 1, &drdy_satus);
// printf("RawData:0x40=%x\r\n",drdy_satus);
// xlog("RawData:0x40=%x\r\n",drdy_satus);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x40, 1, &drdy_satus);
// printf("RawData:0x40=%x\r\n",drdy_satus);
// xlog("RawData:0x40=%x\r\n",drdy_satus);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x06, 1, &drdy_satus);
// printf("RawData:0x06=%x\r\n",drdy_satus);
// xlog("RawData:0x06=%x\r\n",drdy_satus);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x07, 1, &drdy_satus);
// printf("RawData:0x07=%x\r\n",drdy_satus);
// xlog("RawData:0x07=%x\r\n",drdy_satus);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x7D, 1, &drdy_satus);
// printf("RawData:0x7D=%x\r\n",drdy_satus);
// xlog("RawData:0x7D=%x\r\n",drdy_satus);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x31, 1, &drdy_satus);
// printf("RawData:0x31=%x\r\n",drdy_satus);
// xlog("RawData:0x31=%x\r\n",drdy_satus);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x02, 1, &drdy_satus);
// printf("RawData:0x02=%x\r\n",drdy_satus);
// xlog("RawData:0x02=%x\r\n",drdy_satus);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x03, 1, &drdy_satus);
// printf("RawData:0x03=%x\r\n",drdy_satus);
#endif
// xlog("RawData:0x03=%x\r\n",drdy_satus);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x0C, 12, &raw_data[0]);
@ -246,9 +245,7 @@ void SL_SC7U22_RawData_Read(signed short * acc_data_buf,signed short * gyr_data_
gyr_data_buf[1] =(signed short)((((unsigned char)raw_data[8])* 256) + ((unsigned char)raw_data[9]));//GYRY-16位
gyr_data_buf[2] =(signed short)((((unsigned char)raw_data[10])* 256) + ((unsigned char)raw_data[11]));//GYRZ-16位
#if SL_Sensor_Algo_Release_Enable==0x00
printf("RawData:AX=%d,AY=%d,AZ=%d,GX=%d,GY=%d,GZ=%d\r\n",acc_data_buf[0],acc_data_buf[1],acc_data_buf[2],gyr_data_buf[0],gyr_data_buf[1],gyr_data_buf[2]);
#endif
xlog("RawData:AX=%d,AY=%d,AZ=%d,GX=%d,GY=%d,GZ=%d\r\n",acc_data_buf[0],acc_data_buf[1],acc_data_buf[2],gyr_data_buf[0],gyr_data_buf[1],gyr_data_buf[2]);
}
#else
@ -322,10 +319,10 @@ unsigned short SL_SC7U22_FIFO_Read(signed short *accx_buf,signed short *accy_buf
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x21, fifo_num*2, SL_SC7U22_FIFO_DATA);//读取FIFO数据 BYTE NUM
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1D, 0x00);//BY PASS MODE
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1D, 0x20);//Stream MODE
printf("SC7U22_FIFO_NUM1:%d\n",fifo_num);
xlog("SC7U22_FIFO_NUM1:%d\n",fifo_num);
#if SL_Sensor_Algo_Release_Enable==0x00
// printf("0x1F:0x%x 0x20:0x%x\n",fifo_num1,fifo_num2);
// printf("SC7U22_FIFO_NUM1:%d\n",fifo_num);
// xlog("0x1F:0x%x 0x20:0x%x\n",fifo_num1,fifo_num2);
// xlog("SC7U22_FIFO_NUM1:%d\n",fifo_num);
#endif
fifo_len=0;
@ -350,7 +347,7 @@ unsigned short SL_SC7U22_FIFO_Read(signed short *accx_buf,signed short *accy_buf
accx_buf[Acc_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 0] * 256 + SL_SC7U22_FIFO_DATA[i + 1])) ;
accy_buf[Acc_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 2] * 256 + SL_SC7U22_FIFO_DATA[i + 3])) ;
accz_buf[Acc_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 4] * 256 + SL_SC7U22_FIFO_DATA[i + 5])) ;
printf("AccNum : %d ,Acc_x : %4d, Acc_y : %4d, Acc_z : %4d,\r\n",Acc_FIFO_Num, accx_buf[Acc_FIFO_Num], accy_buf[Acc_FIFO_Num], accz_buf[Acc_FIFO_Num]);
xlog("AccNum : %d ,Acc_x : %4d, Acc_y : %4d, Acc_z : %4d,\r\n",Acc_FIFO_Num, accx_buf[Acc_FIFO_Num], accy_buf[Acc_FIFO_Num], accz_buf[Acc_FIFO_Num]);
i = i + 6;
Acc_FIFO_Num++;
}
@ -360,7 +357,7 @@ unsigned short SL_SC7U22_FIFO_Read(signed short *accx_buf,signed short *accy_buf
gyrx_buf[Gyr_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 0] * 256 + SL_SC7U22_FIFO_DATA[i + 1])) ;
gyry_buf[Gyr_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 2] * 256 + SL_SC7U22_FIFO_DATA[i + 3])) ;
gyrz_buf[Gyr_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 4] * 256 + SL_SC7U22_FIFO_DATA[i + 5])) ;
printf("GyrNum : %d, Gyr_x : %4d, Gyr_y : %4d, Gyr_z : %4d,\r\n",Gyr_FIFO_Num, gyrx_buf[Gyr_FIFO_Num], gyry_buf[Gyr_FIFO_Num], gyrz_buf[Gyr_FIFO_Num]);
xlog("GyrNum : %d, Gyr_x : %4d, Gyr_y : %4d, Gyr_z : %4d,\r\n",Gyr_FIFO_Num, gyrx_buf[Gyr_FIFO_Num], gyry_buf[Gyr_FIFO_Num], gyrz_buf[Gyr_FIFO_Num]);
i = i + 6;
Gyr_FIFO_Num++;
}
@ -398,21 +395,16 @@ unsigned char SL_SC7U22_SOFT_RESET(void)
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00
os_time_dly(1);
#if SL_Sensor_Algo_Release_Enable==0x00
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x04, 1,&SL_Read_Reg);
printf("SL_SC7U22_SOFT_RESET1 0x04=0x%x\r\n",SL_Read_Reg);
xlog("SL_SC7U22_SOFT_RESET1 0x04=0x%x\r\n",SL_Read_Reg);
SL_Read_Reg = 0xff;
#endif
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x04, 0x10);//BOOT
#if SL_Sensor_Algo_Release_Enable==0x00
#endif
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x4A, 0xA5);//SOFT_RESET
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x4A, 0xA5);//SOFT_RESET
os_time_dly(20);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x04, 1,&SL_Read_Reg);
#if SL_Sensor_Algo_Release_Enable==0x00
printf("SL_SC7U22_SOFT_RESET2 0x08=0x%x\r\n",SL_Read_Reg);
#endif
xlog("SL_SC7U22_SOFT_RESET2 0x08=0x%x\r\n",SL_Read_Reg);
if(SL_Read_Reg==0x50) return 1;
else return 0;
@ -453,9 +445,7 @@ unsigned char SL_SC7U22_Open_Close_SET(unsigned char acc_enable,unsigned char gy
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x7D, 1,&SL_Read_Check);
if(SL_Read_Reg!=SL_Read_Check)
{
#if SL_Sensor_Algo_Release_Enable==0x00
printf("SL_Read_Reg=0x%x SL_Read_Check=0x%x\r\n",SL_Read_Reg,SL_Read_Check);
#endif
xlog("SL_Read_Reg=0x%x SL_Read_Check=0x%x\r\n",SL_Read_Reg,SL_Read_Check);
return 0;
}
return 1;
@ -513,9 +503,7 @@ unsigned char SL_SC7U22_IN_SLEEP_SET(unsigned char acc_odr,unsigned char vth,uns
if(SL_Read_Reg!=0x04)
{
#if SL_Sensor_Algo_Release_Enable==0x00
printf("SL_Read_Reg=0x%x 0x04\r\n",SL_Read_Reg);
#endif
xlog("SL_Read_Reg=0x%x 0x04\r\n",SL_Read_Reg);
return 0;
}
return 1;
@ -617,23 +605,20 @@ unsigned char SL_SC7U22_WakeUp_SET(unsigned char odr_mode,unsigned char acc_rang
}
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, SL_gyro_range_Reg);//GYR_RANGE 2000dps
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, SL_gyro_range_Reg);//GYR_RANGE 2000dps
#if SL_Sensor_Algo_Release_Enable==0x00
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x40, 1, &SL_Read_Check);
// printf("RawData:0x40=%x\r\n",SL_Read_Check);
// xlog("RawData:0x40=%x\r\n",SL_Read_Check);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x41, 1, &SL_Read_Check);
// printf("RawData:0x41=%x\r\n",SL_Read_Check);
// xlog("RawData:0x41=%x\r\n",SL_Read_Check);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x42, 1, &SL_Read_Check);
// printf("RawData:0x42=%x\r\n",SL_Read_Check);
// xlog("RawData:0x42=%x\r\n",SL_Read_Check);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x43, 1, &SL_Read_Check);
// printf("RawData:0x43=%x\r\n",SL_Read_Check);
#endif
// xlog("RawData:0x43=%x\r\n",SL_Read_Check);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x43, 1,&SL_Read_Check);
if(SL_Read_Check!=SL_gyro_range_Reg)
{
#if SL_Sensor_Algo_Release_Enable==0x00
printf("SL_Read_Check=0x%x SL_gyro_range_Reg=0x%x\r\n",SL_Read_Check,SL_gyro_range_Reg);
#endif
xlog("SL_Read_Check=0x%x SL_gyro_range_Reg=0x%x\r\n",SL_Read_Check,SL_gyro_range_Reg);
return 0;
}
return 1;
@ -773,17 +758,14 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short
}
// 计算零点偏移：理想值 - 实际平均值
// 加速度Z轴的理想值是8192，对应1g（假设量程为±8g）
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];
#if SL_Sensor_Algo_Release_Enable == 0x00
printf("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]);
printf("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]);
#endif
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 {
// 如果在累加过程中发生移动，则重新开始
@ -982,13 +964,13 @@ void sensor_processing_task(void *priv)
// 7. 检查函数返回的状态
if (status == 1) {
// 计算成功，final_angle_data 中的数据有效
printf("Pitch: %.2f, Roll: %.2f, Yaw: %.2f\n", final_angle_data[0], final_angle_data[1], final_angle_data[2]);
xlog("Pitch: %.2f, Roll: %.2f, Yaw: %.2f\n", final_angle_data[0], final_angle_data[1], final_angle_data[2]);
} else if (status == 0) {
// 传感器正在进行静态校准，此时角度数据可能不准确
printf("Sensor is calibrating...\n");
xlog("Sensor is calibrating...\n");
} else {
// status == 2, 表示校准未完成或发生错误
printf("Angle calculation error or calibration not finished.\n");
xlog("Angle calculation error or calibration not finished.\n");
}
// 延时一段时间，例如10ms (对应100Hz)
@ -1005,11 +987,11 @@ void app_main()
unsigned char init_success = SL_SC7U22_Config();
if (init_success) {
printf("SCU722 初始化成功!\n");
xlog("SCU722 初始化成功!\n");
// 3. 创建一个任务来周期性地读取和处理数据
task_create(sensor_processing_task, NULL, "sensor_task");
} else {
printf("SCU722 初始化失败!\n");
xlog("SCU722 初始化失败!\n");
}
// ...

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

@ -11,7 +11,7 @@ Copyright (c) 2022 Silan MEMS. All Rights Reserved.
#include "printf.h"
//是否使能串口打印调试
#define SL_Sensor_Algo_Release_Enable 0x01
#define SL_Sensor_Algo_Release_Enable 0x00
//是否开启FIFO模式，默认STREAM模式
#define SL_SC7U22_FIFO_ENABLE 0x00

2
apps/earphone/xtell_Sensor/sensor/SC7U22_Ma.c
View File

@ -657,7 +657,7 @@ static float invSqrt(float x) {
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1); // 神奇的魔术数
i = 0x5f3759df - (i>>1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y)); // 牛顿迭代法，提高精度
return y;