From 97e85df2f81ea55d58f88d8af9a75fca612fafa5 Mon Sep 17 00:00:00 2001 From: lmx Date: Fri, 31 Oct 2025 18:12:10 +0800 Subject: [PATCH] Ma --- Makefile | 2 +- .../sensor/{SCU722.c => SC7U22.c} | 0 .../sensor/{SCU722.h => SC7U22.h} | 0 apps/earphone/xtell_Sensor/sensor/SC7U22_Ma.c | 996 ++++++++++++++++++ apps/earphone/xtell_Sensor/sensor/SCU722_Ma.c | 0 5 files changed, 997 insertions(+), 1 deletion(-) rename apps/earphone/xtell_Sensor/sensor/{SCU722.c => SC7U22.c} (100%) rename apps/earphone/xtell_Sensor/sensor/{SCU722.h => SC7U22.h} (100%) create mode 100644 apps/earphone/xtell_Sensor/sensor/SC7U22_Ma.c delete mode 100644 apps/earphone/xtell_Sensor/sensor/SCU722_Ma.c diff --git a/Makefile b/Makefile index 05a2e18..1143626 100644 --- a/Makefile +++ b/Makefile @@ -617,7 +617,7 @@ c_SRC_FILES := \ apps/earphone/xtell_Sensor/send_data.c \ apps/earphone/xtell_Sensor/buffer/circle_buffer.c \ apps/earphone/xtell_Sensor/sensor/LIS2DH12.c \ - apps/earphone/xtell_Sensor/sensor/SCU722.c \ + apps/earphone/xtell_Sensor/sensor/SC7U22.c \ # 需要编译的 .S 文件 diff --git a/apps/earphone/xtell_Sensor/sensor/SCU722.c b/apps/earphone/xtell_Sensor/sensor/SC7U22.c similarity index 100% rename from apps/earphone/xtell_Sensor/sensor/SCU722.c rename to apps/earphone/xtell_Sensor/sensor/SC7U22.c diff --git a/apps/earphone/xtell_Sensor/sensor/SCU722.h b/apps/earphone/xtell_Sensor/sensor/SC7U22.h similarity index 100% rename from apps/earphone/xtell_Sensor/sensor/SCU722.h rename to apps/earphone/xtell_Sensor/sensor/SC7U22.h diff --git a/apps/earphone/xtell_Sensor/sensor/SC7U22_Ma.c b/apps/earphone/xtell_Sensor/sensor/SC7U22_Ma.c new file mode 100644 index 0000000..895bf47 --- /dev/null +++ b/apps/earphone/xtell_Sensor/sensor/SC7U22_Ma.c @@ -0,0 +1,996 @@ +#include "SC7U22.h" +#include "math.h" +#include "os/os_api.h" + +#if SL_Sensor_Algo_Release_Enable==0x00 +#include "printf.h" +#endif + + +//I2C SPI选择 +//#define SL_SC7U22_SPI_EN_I2C_DISABLE 0x00 //需要配合SL_SPI_IIC_INTERFACE使用 +#define SL_SPI_IIC_INTERFACE 0x01 //需要配合SL_SC7A22H_SPI_EN_I2C_DISABLE 使用 +//是否使能原始数据高通滤波 +#define SL_SC7U22_RAWDATA_HPF_ENABLE 0x00 +//中断默认电平 +#define SL_SC7U22_INT_DEFAULT_LEVEL 0x01 +//SDO 是否上拉 +#define SL_SC7U22_SDO_PullUP_ENABLE 0x01 +//AOI中断是否唤醒 +#define SL_SC7U22_AOI_Wake_Up_ENABLE 0x00 + +//FIFO_STREAM模式//FIFO_WTM模式 +//#define SL_SC7U22_FIFO_STREAM_WTM 0x01//0X00=STREAM MODE 0X01=FIFO MODE + +#define SL_SC7U22_IIC_DELAY_US 5 + +static u32 SL_MEMS_i2cRead(u8 addr, u8 reg, u8 len, u8 *buf) { + return _gravity_sensor_get_ndata(addr, reg, buf, len); +} +static u8 SL_MEMS_i2cWrite(u8 addr, u8 reg, u8 data) { + gravity_sensor_command(addr, reg, data); + return 0; +} + +unsigned char SL_SC7U22_I2c_Spi_Write(unsigned char sl_spi_iic, unsigned char reg, unsigned char dat) +{ + if (sl_spi_iic == 1) { + SL_MEMS_i2cWrite(SL_SC7U22_IIC_8BITS_WRITE_ADDR, reg, dat); + return 0; + } + // SPI not implemented + return 1; // 失败 +} + +unsigned char SL_SC7U22_I2c_Spi_Read(unsigned char sl_spi_iic, unsigned char reg, unsigned short len, unsigned char* buf) +{ + if (sl_spi_iic == 1) { + return SL_MEMS_i2cRead(SL_SC7U22_IIC_8BITS_READ_ADDR, reg, len, buf); + } + // SPI not implemented + return 0; // 失败 +} + +static void sl_delay(unsigned char sl_i) +{ + os_time_dly(sl_i); +} + + +unsigned char SL_SC7U22_Check(void) +{ + unsigned char reg_value=0; + + 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, ®_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) + return 0x01;//SC7U22 + else + return 0x00;//通信异常 +} + +unsigned char SL_SC7U22_Config(void) +{ + unsigned char Check_Flag=0; + unsigned char reg_value=0; + +#if SL_SPI_IIC_INTERFACE==0x00 //SPI + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x90 + sl_delay(1); + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x4A, 0x66); + sl_delay(1); + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x83);//goto 0x6F + sl_delay(1); + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x6F, 0x04);//I2C disable + sl_delay(1); + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x6F + sl_delay(1); + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x4A, 0x00); + sl_delay(1); +#endif + 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 + 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 + 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 + if(Check_Flag==1) + { + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00 + os_time_dly(1);//10ms + 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, 0x08);//ACC_CONF 0x08=100Hz + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x41, 0x01);//ACC_RANGE ±4G + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0x88);//GYR_CONF 0x88=100Hz + 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 + +#if SL_SC7U22_RAWDATA_HPF_ENABLE ==0x01 + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x83);//goto 0x83 + sl_delay(1); + SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x26, 1, ®_value); + reg_value=reg_value|0xA0; + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x26, reg_value);//HPF_CFG rawdata hpf +#endif + +#if SL_SC7U22_AOI_Wake_Up_ENABLE==0x01 + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x30, 0x2A);//XYZ-ENABLE + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x32, 0x01);//VTH + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x33, 0x01);//TTH + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x3F, 0x30);//HPF FOR AOI1&AOI2 +#endif + +#if SL_SC7U22_FIFO_ENABLE==0x01 + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1E,0x1D);// + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1D,0x00);// + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1D,0x20);// + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1C,0x37);// +#endif + + +#if SL_SC7U22_SDO_PullUP_ENABLE ==0x01 + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x8C);//goto 0x8C + sl_delay(1); + SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x30, 1, ®_value); + reg_value=reg_value&0xFE;//CS PullUP_enable + reg_value=reg_value&0xFD;//SDO PullUP_enable + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x30, reg_value); + + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x00);//goto 0x00 + os_time_dly(1); + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x00);//goto 0x00 + os_time_dly(1); +#else + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x8C);//goto 0x8C + sl_delay(1); + SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x30, 1, ®_value); + reg_value=reg_value&0xFE;//CS PullUP_enable + reg_value=reg_value|0x02;//SDO PullUP_disable + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x30, reg_value); + + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x00);//goto 0x00 + sl_delay(1); + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE,0x7F, 0x00);//goto 0x00 + sl_delay(1); +#endif + + return 1; + } + else + return 0; +} + + +//读取时间戳 +unsigned int SL_SC7U22_TimeStamp_Read(void) +{ + unsigned char time_data[3]; + unsigned int time_stamp; + + SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x18, 1, &time_data[0]); + SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x19, 1, &time_data[1]); + SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x20, 1, &time_data[2]); + + time_stamp=(unsigned int)(time_data[0]<<16|time_data[1]<<8|time_data[2]); + + return time_stamp; +} + +#if SL_SC7U22_FIFO_ENABLE ==0x00 +//100Hz 10ms read once +void SL_SC7U22_RawData_Read(signed short * acc_data_buf,signed short * gyr_data_buf) +{ + unsigned char raw_data[12]; + unsigned char drdy_satus=0x00; + unsigned short drdy_cnt=0; + + while((drdy_satus&0x03)!=0x03)//acc+gyro +// while((drdy_satus&0x01)!=0x01)//acc + { + drdy_satus=0x00; + sl_delay(1); + SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x0B, 1, &drdy_satus); + drdy_cnt++; + 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); +// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x40, 1, &drdy_satus); +// printf("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); +// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x07, 1, &drdy_satus); +// printf("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); +// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x31, 1, &drdy_satus); +// printf("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); +// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x03, 1, &drdy_satus); +// printf("RawData:0x03=%x\r\n",drdy_satus); +#endif + + SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x0C, 12, &raw_data[0]); + + acc_data_buf[0] =(signed short)((((unsigned char)raw_data[0])* 256) + ((unsigned char)raw_data[1]));//ACCX-16位 + acc_data_buf[1] =(signed short)((((unsigned char)raw_data[2])* 256) + ((unsigned char)raw_data[3]));//ACCY-16位 + acc_data_buf[2] =(signed short)((((unsigned char)raw_data[4])* 256) + ((unsigned char)raw_data[5]));//ACCZ-16位 + gyr_data_buf[0] =(signed short)((((unsigned char)raw_data[6])* 256) + ((unsigned char)raw_data[7]));//GYRX-16位 + 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 + +} +#else + +#if SL_Sensor_Algo_Release_Enable==0x00 +#define SL_SC7U22_WAIT_FIFO_LEN_ENABLE 0x00//0x01 +#else +#define SL_SC7U22_WAIT_FIFO_LEN_ENABLE 0x00 +#endif +unsigned char Acc_FIFO_Num; +unsigned char Gyr_FIFO_Num; + +unsigned char SL_SC7U22_FIFO_DATA[1024]; + +unsigned short SL_SC7U22_FIFO_Read(signed short *accx_buf,signed short *accy_buf,signed short *accz_buf,signed short *gyrx_buf,signed short *gyry_buf,signed short *gyrz_buf) +{ + int16_t Acc_x = 0, Acc_y = 0, Acc_z = 0; + int16_t Gyr_x = 0, Gyr_y = 0, Gyr_z = 0; + unsigned char fifo_num1=0; + unsigned char fifo_num2=0; + unsigned short fifo_num=0; + unsigned short fifo_len=0; + unsigned short temp = 0; + unsigned short i = 0 ; + unsigned char header[2]; + unsigned short j; + +#if SL_Sensor_Algo_Release_Enable==0x00 //user can set to zero +#if SL_SC7U22_WAIT_FIFO_LEN_ENABLE==0x00 + while((fifo_num1&0x20)!=0x20) + { + sl_delay(200); + SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x1F,1,&fifo_num1); + } + SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x1F,1,&fifo_num1); + SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x20,1,&fifo_num2); + if((fifo_num1&0x10)==0x10) + { + fifo_num=2048; + } + else + { + fifo_num=(fifo_num1&0x0F)*256+fifo_num2; + } +#else + while(fifo_num2<194)//32 + { + SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x1F,1,&fifo_num1); + SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x20,1,&fifo_num2); + sl_delay(20); + fifo_wait++; + if(fifo_wait>30000) break; + } + fifo_wait=0; + fifo_num=fifo_num2; +#endif + +#else + SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x1F,1,&fifo_num1); + SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x20,1,&fifo_num2); + if((fifo_num1&0x10)==0x10) + { + fifo_num=2048; + } + else + { + fifo_num=(fifo_num1&0x0F)*256+fifo_num2; + } +#endif + + 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); +#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); +#endif + fifo_len=0; + + i = 0; + Acc_FIFO_Num=0; + Gyr_FIFO_Num=0; + while(i < fifo_num*2) + { + //header process 1 + header[0] = SL_SC7U22_FIFO_DATA[i + 0]; + header[1] = SL_SC7U22_FIFO_DATA[i + 1]; + i = i + 2; + + //timestamp process 2 + if(header[1] & 0x80) + { + i = i + 4;//every frame include the timestamp, 4 bytes + } + //acc process 3 + if(header[0] & 0x04) + { + 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]); + i = i + 6; + Acc_FIFO_Num++; + } + //gyro process 3 + if(header[0] & 0x02) + { + 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]); + i = i + 6; + Gyr_FIFO_Num++; + } + + //temperature process 1 + if(header[0] & 0x01) + { + i = i + 2; + } + } + + + return fifo_len; +} +#endif + + +unsigned char SL_SC7U22_POWER_DOWN(void) +{ + unsigned char SL_Read_Reg = 0xff; + + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00 + sl_delay(20); + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, 0x00);//POWER DOWN + sl_delay(200); + SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x7D, 1,&SL_Read_Reg); + if(SL_Read_Reg==0x00) return 1; + else return 0; +} + + +unsigned char SL_SC7U22_SOFT_RESET(void) +{ + unsigned char SL_Read_Reg = 0xff; + + 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); + 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 + if(SL_Read_Reg==0x50) return 1; + else return 0; + +} + + +/****acc_enable ==0 close acc;acc_enable ==1 open acc******/ +/****gyro_enable==0 close acc;gyro_enable==1 open acc******/ +unsigned char SL_SC7U22_Open_Close_SET(unsigned char acc_enable,unsigned char gyro_enable) +{ + unsigned char SL_Read_Reg = 0xff; + unsigned char SL_Read_Check= 0xff; + + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00 + sl_delay(1); + SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x7D, 1,&SL_Read_Reg); + + if(acc_enable==0) + { + SL_Read_Reg=SL_Read_Reg&0xFB;//Bit.ACC_EN=0 + } + else if(acc_enable==1) + { + SL_Read_Reg=SL_Read_Reg|0x04;//Bit.ACC_EN=1 + } + if(gyro_enable==0) + { + SL_Read_Reg=SL_Read_Reg&0xFD;//Bit.GYR_EN=0 + } + else if(gyro_enable==1) + { + SL_Read_Reg=SL_Read_Reg|0x02;//Bit.GYR_EN=1 + } + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, SL_Read_Reg);//PWR_CTRL ENABLE ACC+GYR+TEMP + sl_delay(5);//5ms + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, SL_Read_Reg);//PWR_CTRL ENABLE ACC+GYR+TEMP + sl_delay(20);//10ms + 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 + return 0; + } + return 1; +} + + +/*******开启中断******/ +unsigned char SL_SC7U22_IN_SLEEP_SET(unsigned char acc_odr,unsigned char vth,unsigned char tth,unsigned char int_io) +{ + unsigned char SL_Read_Reg = 0xff; + unsigned char SL_Acc_Odr_Reg = 0xff; + + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00 + sl_delay(1); + if(int_io==1) + { + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x06, 0x02);//AOI1-INT1 + } + else if(int_io==2) + { + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x08, 0x02);//AOI1-INT2 + } + + SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x04, 1, &SL_Read_Reg); +#if SL_SC7U22_INT_DEFAULT_LEVEL ==0x01 + SL_Read_Reg=SL_Read_Reg|0x04; + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x04, SL_Read_Reg);//defalut high level&& push-pull +#else + reg_value=reg_value&0xDF; + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x06, SL_Read_Reg);//defalut low level&& push-pull +#endif + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x30, 0x2A);//AIO1-Enable + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x32, vth);//VTH + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x33, tth);//TTH + + if(acc_odr==12) + { + SL_Acc_Odr_Reg=0x05; + } + else if(acc_odr==25) + { + SL_Acc_Odr_Reg=0x06; + } + else if(acc_odr==50) + { + SL_Acc_Odr_Reg=0x07; + } + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, SL_Acc_Odr_Reg);//ACC_CONF + os_time_dly(1);//5ms + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, 0x04);//acc open and gyro close + os_time_dly(1);//5ms + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, 0x04);//acc open and gyro close + sl_delay(200); + SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x7D, 1,&SL_Read_Reg); + + 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 + return 0; + } + return 1; +} + +/*******ODR SET:25 50 100 200******************/ +/*******acc range:2 4 8 16*********************/ +/*******gyro range:125 250 500 1000 2000*******/ +/*******acc_hp_en: 0=disable 1=enable**********/ +/*******gyro_hp_en:0=disable 1=enable**********/ +unsigned char SL_SC7U22_WakeUp_SET(unsigned char odr_mode,unsigned char acc_range,unsigned char acc_hp_en,unsigned short gyro_range,unsigned char gyro_hp_en) +{ + unsigned char SL_Odr_Reg = 0x00; + unsigned char SL_acc_mode_Reg = 0x00; + unsigned char SL_gyro_mode_Reg = 0x00; + unsigned char SL_acc_range_Reg = 0x00; + unsigned char SL_gyro_range_Reg = 0x00; + unsigned char SL_Read_Check = 0xff; + + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00 + sl_delay(1); + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, 0x06);//PWR_CTRL ENABLE ACC+GYR + sl_delay(5); + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, 0x06);//PWR_CTRL ENABLE ACC+GYR + sl_delay(200); + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x30, 0x00);//AIO1-disable + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x32, 0xff);//vth + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x33, 0xff);//tth + + if(odr_mode==25) + { + SL_Odr_Reg=0x06; + } + else if(odr_mode==50) + { + SL_Odr_Reg=0x07; + } + else if(odr_mode==100) + { + SL_Odr_Reg=0x08; + } + else if(odr_mode==200) + { + SL_Odr_Reg=0x09; + } + if(acc_hp_en==1) + SL_acc_mode_Reg=0x80; + + SL_acc_mode_Reg=SL_acc_mode_Reg|SL_Odr_Reg; + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, SL_acc_mode_Reg);//ACC_CONF + + if(gyro_hp_en==1) + SL_gyro_mode_Reg=0x40; + else if(gyro_hp_en==2) + SL_gyro_mode_Reg=0x80; + else if(gyro_hp_en==3) + SL_gyro_mode_Reg=0xC0; + + SL_gyro_mode_Reg=SL_gyro_mode_Reg|SL_Odr_Reg; + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, SL_gyro_mode_Reg);//GYR_CONF + + if(acc_range==2) + { + SL_acc_range_Reg=0x00; + } + else if(acc_range==4) + { + SL_acc_range_Reg=0x01; + } + else if(acc_range==8) + { + SL_acc_range_Reg=0x02; + } + else if(acc_range==16) + { + SL_acc_range_Reg=0x03; + } + SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x41, SL_acc_range_Reg);//ACC_RANGE + + if(gyro_range==2000) + { + SL_gyro_range_Reg=0x00; + } + else if(gyro_range==1000) + { + SL_gyro_range_Reg=0x01; + } + else if(gyro_range==500) + { + SL_gyro_range_Reg=0x02; + } + else if(gyro_range==250) + { + SL_gyro_range_Reg=0x03; + } + else if(gyro_range==125) + { + SL_gyro_range_Reg=0x04; + } + 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); +// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x41, 1, &SL_Read_Check); +// printf("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); +// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x43, 1, &SL_Read_Check); +// printf("RawData:0x43=%x\r\n",SL_Read_Check); +#endif + + 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 + return 0; + } + return 1; +} + + +#if SL_SC7U22_FIFO_ENABLE ==0x00 +// ================================================================================================= +// Madgwick AHRS 滤波器相关变量和函数 +// ------------------------------------------------------------------------------------------------- + +// 定义常量 +#define sampleFreq 100.0f // 传感器采样频率 (Hz)，必须与实际的传感器数据更新频率一致 +#define betaDef 0.1f // 算法的比例增益 beta，影响加速度计修正陀螺仪的权重 + +// 全局变量 +static volatile float beta = betaDef; // 算法增益 beta +static volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // 表示姿态的四元数 (w, x, y, z) + +/** + * @brief 快速计算 1/sqrt(x) + * @param x 输入的浮点数 + * @return 1/sqrt(x) 的近似值 + */ +static float invSqrt(float x) { + float halfx = 0.5f * x; + float y = x; + long i = *(long*)&y; + i = 0x5f3759df - (i>>1); // 神奇的魔术数 + y = *(float*)&i; + y = y * (1.5f - (halfx * y * y)); // 牛顿迭代法，提高精度 + return y; +} + +/** + * @brief Madgwick AHRS 姿态更新函数 (IMU版本) + * @details 该函数融合了陀螺仪和加速度计的数据，计算出表示设备姿态的四元数。 + * 1. 使用陀螺仪数据积分，得到一个初步的姿态估计（预测）。 + * 2. 使用加速度计数据（当设备处于静止或低速运动时，加速度计主要测量重力）来修正这个估计（修正）。 + * 3. 通过梯度下降法找到一个最优的旋转，使得在当前姿态下，重力向量的方向与加速度计测量的方向最接近。 + * @param gx, gy, gz 陀螺仪三轴角速度 (单位: rad/s) + * @param ax, ay, az 加速度计三轴加速度 (单位: g) + */ +static void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) { + float recipNorm; + float s0, s1, s2, s3; + float qDot1, qDot2, qDot3, qDot4; + float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; + float dt = 1.0f / sampleFreq; // 采样时间间隔 + + // --- 1. 陀螺仪积分：计算四元数的变化率 --- + // 姿态运动学的基本方程，描述了姿态如何随角速度变化。 + qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); + qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); + qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); + qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); + + // --- 2. 加速度计修正 --- + // 仅当加速度计读数有效时（即模长不为0）才进行修正，防止计算NaN。 + if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + + // 将加速度计读数归一化，得到单位向量 + recipNorm = invSqrt(ax * ax + ay * ay + az * az); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + // 预先计算一些重复使用的值，提高效率 + _2q0 = 2.0f * q0; + _2q1 = 2.0f * q1; + _2q2 = 2.0f * q2; + _2q3 = 2.0f * q3; + _4q0 = 4.0f * q0; + _4q1 = 4.0f * q1; + _4q2 = 4.0f * q2; + _8q1 = 8.0f * q1; + _8q2 = 8.0f * q2; + q0q0 = q0 * q0; + q1q1 = q1 * q1; + q2q2 = q2 * q2; + q3q3 = q3 * q3; + + // --- 梯度下降法：计算修正量 --- + // 目标函数 f(q, a) = [2(q1q3 - q0q2) - ax, 2(q0q1 + q2q3) - ay, 2(0.5 - q1^2 - q2^2) - az]^T + // s0, s1, s2, s3 是目标函数 f 对四元数 q 的雅可比矩阵 J 与 f 的乘积。 + // 这个结果代表了误差函数的梯度方向，用于修正四元数的变化率。 + s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; + s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; + s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; + s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; + recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // 归一化梯度 + s0 *= recipNorm; + s1 *= recipNorm; + s2 *= recipNorm; + s3 *= recipNorm; + + // --- 应用修正量 --- + // 将计算出的修正量（梯度）从陀螺仪积分结果中减去，beta是修正的权重。 + qDot1 -= beta * s0; + qDot2 -= beta * s1; + qDot3 -= beta * s2; + qDot4 -= beta * s3; + } + + // --- 3. 积分：更新四元数 --- + // 使用一阶龙格-库塔法（即欧拉法）进行积分，得到新的四元数。 + q0 += qDot1 * dt; + q1 += qDot2 * dt; + q2 += qDot3 * dt; + q3 += qDot4 * dt; + + // --- 4. 归一化四元数 --- + // 保持四元数的模长为1，防止由于计算误差导致的累积漂移。 + recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; +} +// ================================================================================================= + +// --- 静态校准相关变量 --- +static unsigned char SL_SC7U22_Error_Flag=0; +static unsigned char SL_SC7U22_Error_cnt=0; +static unsigned char SL_SC7U22_Error_cnt2=0; +static signed short Temp_Accgyro[6] ={0}; +static signed short Error_Accgyro[6]={0}; +static signed int Sum_Avg_Accgyro[6] ={0}; +static float yaw_offset = 0.0f; + +static signed short SL_GetAbsShort(signed short v_Val_s16r) +{ + if(v_Val_s16r==(-32768)) + return 32767; + return (v_Val_s16r < 0) ? -v_Val_s16r : v_Val_s16r; +} + +/** + * @brief 姿态角解算函数 + * @details + * 该函数主要完成两项工作： + * 1. 静态校准：在初始阶段，检测传感器是否处于静止状态。如果是，则计算加速度计和陀螺仪的零点偏移（误差），用于后续的数据补偿。 + * 2. 姿态解算：使用 Madgwick 滤波器融合经过校准后的加速度计和陀螺仪数据，计算出物体的俯仰角（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 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; + + // 如果外部强制使能校准，则将标志位置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]; + } + + // 判断是否处于静止状态：加速度变化量、陀螺仪变化量、各轴加速度值都在一个很小的范围内 + // 假设1g = 8192 (对应 +/-4g 量程) + 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++; // 静止计数器累加 + } + } else { + SL_SC7U22_Error_cnt = 0; // 如果发生移动，则清空静止计数器 + } + + // 如果静止时间足够长（这里是190个采样周期，约1.9秒） + if (SL_SC7U22_Error_cnt > 190) { + // 开始累加50个采样点的数据 + 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) { + // 累加满50个点后，计算平均值 + 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; + } + + // 计算零点偏移：理想值 - 实际平均值 + // 加速度Z轴的理想值是8192，对应1g（假设量程为±4g） + 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 + } + } 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: 姿态解算 (Madgwick) + // --------------------------------------------------------------------------------- + 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]; + } + +#if 1 // 将校准后的数据写回输入数组 + for (sl_i = 0; sl_i < 6; sl_i++) { + acc_gyro_input[sl_i] = Temp_Accgyro[sl_i]; + } +#endif + // --- 2.2 转换数据单位 --- + // 将校准后的传感器原始值 (LSB) 转换为 Madgwick 算法所需的物理单位。 + // 加速度: LSB -> g (重力加速度)。转换系数 = 量程 / (2^15)。假设 +/-4g 量程, 系数 = 4 / 32768 = 1/8192。 + float ax = (float)Temp_Accgyro[0] / 8192.0f; + float ay = (float)Temp_Accgyro[1] / 8192.0f; + float az = (float)Temp_Accgyro[2] / 8192.0f; + // 角速度: LSB -> rad/s (弧度/秒)。转换系数 = (量程 * PI) / (180 * 2^15)。 + // 假设 +/-2000dps 量程, 系数 = (2000 * 3.14159) / (180 * 32768) ≈ 0.001064 + float gx = (float)Temp_Accgyro[3] * 0.001064f; + float gy = (float)Temp_Accgyro[4] * 0.001064f; + float gz = (float)Temp_Accgyro[5] * 0.001064f; + + // --- 2.3 调用 Madgwick 更新函数 --- + // 将处理好的物理单位数据传入滤波器，更新姿态四元数。 + MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az); + + // --- 2.4 将四元数转换为欧拉角 --- + // 欧拉角（Pitch, Roll, Yaw）更直观，便于使用。转换公式如下。 + // 转换结果单位为度 (乘以 180/PI ≈ 57.29578)。 + float yaw, pitch, roll; + + // Roll (横滚角，绕x轴旋转) + roll = atan2f(2.0f * (q0 * q1 + q2 * q3), 1.0f - 2.0f * (q1 * q1 + q2 * q2)) * 57.29578f; + // Pitch (俯仰角，绕y轴旋转) + float sinp = 2.0f * (q0 * q2 - q3 * q1); + if (fabsf(sinp) >= 1) + pitch = copysignf(3.14159265f / 2, sinp) * 57.29578f; // 防止万向节死锁，当sinp接近+/-1时，直接赋+/-90度 + else + pitch = asinf(sinp) * 57.29578f; + // Yaw (偏航角，绕z轴旋转) + yaw = atan2f(2.0f * (q0 * q3 + q1 * q2), 1.0f - 2.0f * (q2 * q2 + q3 * q3)) * 57.29578f; + + // --- 2.5 处理Yaw轴重置 --- + // Yaw角无法通过加速度计校正，会随时间漂移。提供一个重置机制，将当前Yaw角作为新的零点。 + if (yaw_rst) { + yaw_offset = yaw; + } + + // --- 2.6 输出最终角度 --- + // 将计算出的欧拉角存入输出数组。 + Angle_output[0] = pitch; + Angle_output[1] = roll; + Angle_output[2] = yaw - yaw_offset; // 输出减去偏移量的相对Yaw角 + + return 1; // 返回1，表示计算成功 + } + + return 2; // 校准未完成，返回错误状态 +} +#endif + + +/* +//----------------------------------------- +调用示例-by_lmx： + + +//1. +// 定义用于存放传感器数据的缓冲区 +static signed short acc_raw_data[3]; // [0]: acc_x, [1]: acc_y, [2]: acc_z +static signed short gyr_raw_data[3]; // [0]: gyr_x, [1]: gyr_y, [2]: gyr_z +static signed short combined_raw_data[6]; // 用于合并 acc 和 gyr 数据 +static float final_angle_data[3]; // [0]: Pitch, [1]: Roll, [2]: Yaw + +// 传感器数据处理任务 +void sensor_processing_task(void *priv) +{ + while (1) { + // 4. 周期性调用读取函数，获取原始数据 + SL_SC7U22_RawData_Read(acc_raw_data, gyr_raw_data); + + // 5. 合并加速度和角速度数据到一个数组中 + // SL_SC7U22_Angle_Output 函数需要一个包含6个元素的数组作为输入 + memcpy(&combined_raw_data[0], acc_raw_data, sizeof(acc_raw_data)); + memcpy(&combined_raw_data[3], gyr_raw_data, sizeof(gyr_raw_data)); + + // 6. 调用姿态解算函数 + // 参数: (校准使能, 输入的6轴数据, 输出的角度数据, Yaw轴复位标志) + // calibration_en = 1: 让函数内部自动管理校准过程 + // yaw_rst = 0: 不复位Yaw角 + unsigned char status = SL_SC7U22_Angle_Output(1, combined_raw_data, final_angle_data, 0); + + // 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]); + } else if (status == 0) { + // 传感器正在进行静态校准，此时角度数据可能不准确 + printf("Sensor is calibrating...\n"); + } else { + // status == 2, 表示校准未完成或发生错误 + printf("Angle calculation error or calibration not finished.\n"); + } + + // 延时一段时间，例如10ms (对应100Hz) + os_time_dly(1); + } +} + +// 应用程序主入口或初始化函数 +void app_main() +{ + // ... 其他初始化代码 ... + + // 2. 调用初始化函数来配置SCU722传感器 + unsigned char init_success = SL_SC7U22_Config(); + + if (init_success) { + printf("SCU722 初始化成功!\n"); + // 3. 创建一个任务来周期性地读取和处理数据 + task_create(sensor_processing_task, NULL, "sensor_task"); + } else { + printf("SCU722 初始化失败!\n"); + } + + // ... +} + +//----------------------------------------- +*/ \ No newline at end of file diff --git a/apps/earphone/xtell_Sensor/sensor/SCU722_Ma.c b/apps/earphone/xtell_Sensor/sensor/SCU722_Ma.c deleted file mode 100644 index e69de29..0000000