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

SCU722驱动替换更新

Browse Source
This commit is contained in:
lmx
2025-10-31 16:58:39 +08:00
parent a96264ec36
commit 830b4637dd
27 changed files with 171037 additions and 170624 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
apps/earphone/xtell_Sensor/send_data.c
View File

@ -85,13 +85,16 @@ void collect_and_buffer_sensor_data_task(void) {
}
}
// 定义数组大小
#define ARRAY_SIZE (5)
// 在 main 函数外部声明为全局变量，它将被存储在静态数据区
unsigned char global_data_array[ARRAY_SIZE];
// 从环形缓冲区读取数据并发送
void send_sensor_data_task(void) {
//查询开关机状态
u8 temp[5]={0xBB,0xBE,0x02,0x00,0x00};
printf("xtell_ble_send\n");
send_data_to_ble_client(&temp,5);
// printf("xtell_ble_send\n");
send_data_to_ble_client(&global_data_array,ARRAY_SIZE);
}
extern void create_process(u16* pid, const char* name, void *priv, void (*func)(void *priv), u32 msec);
@ -101,26 +104,25 @@ void rcsp_adv_fill_mac_addr(u8 *mac_addr_buf) //by xtell
swapX(bt_get_mac_addr(), mac_addr_buf, 6);
}
void xtell_task_create(void){
xlog("xtell_task_create\n");
//开经典蓝牙
// user_send_cmd_prepare(USER_CTRL_WRITE_SCAN_ENABLE, 0, NULL); //打开蓝牙可发现,已连接时不能操作
// delay_2ms(50);
// user_send_cmd_prepare(USER_CTRL_WRITE_CONN_ENABLE, 0, NULL); //打开蓝牙可连接
// delay_2ms(50);
// connect_last_device_from_vm(); //自动回连上一个设备
//写入测试数据
for (int i = 0; i < ARRAY_SIZE; i++) { //ARRAY_SIZE字节的数组
global_data_array[i] = i % 256;
}
// 初始化环形缓冲区
circle_buffer_init(&sensor_cb, sensor_data_buffer, SENSOR_DATA_BUFFER_SIZE);
// 创建一个定时器，每200ms调用一次核心计算任务
create_process(&xtell_i2c_test_id, "xtell_i2c_test", NULL, xtell_i2c_test, (u32)(SAMPLING_PERIOD_S * 1000));
// create_process(&xtell_i2c_test_id, "xtell_i2c_test", NULL, xtell_i2c_test, (u32)(SAMPLING_PERIOD_S * 1000));
// 创建一个定时器，每1000ms采集一次数据
create_process(&collect_data_id, "collect_data", NULL, collect_and_buffer_sensor_data_task, 1000);
// create_process(&collect_data_id, "collect_data", NULL, collect_and_buffer_sensor_data_task, 1000);
// 创建一个定时器，每200ms尝试发送一次数据
create_process(&send_data_id, "send_data", NULL, send_sensor_data_task, 200);
create_process(&send_data_id, "send_data", NULL, send_sensor_data_task, 1);
}

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

@ -81,10 +81,10 @@ axis_info_xtell gsensor_xtell; // 存储is_sensor_stable计算出的平均值
//////////////////////////////////////////////////////////////////////////////////////////////////
//实现
// --- I2C底层函数封装 ---
u32 SL_MEMS_i2cRead(u8 addr, u8 reg, u8 len, u8 *buf) {
static u32 SL_MEMS_i2cRead(u8 addr, u8 reg, u8 len, u8 *buf) {
return _gravity_sensor_get_ndata(addr, reg, buf, len);
}
u8 SL_MEMS_i2cWrite(u8 addr, u8 reg, u8 data) {
static u8 SL_MEMS_i2cWrite(u8 addr, u8 reg, u8 data) {
gravity_sensor_command(addr, reg, data);
return 0;
}

947
apps/earphone/xtell_Sensor/sensor/SCU722.c Normal file
View File

@ -0,0 +1,947 @@
#include "SCU722.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, &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)
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, 0x06);//ACC_CONF 0x07=50Hz
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, 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
#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, &reg_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, &reg_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, &reg_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
// =================================================================================================
// 卡尔曼滤波器（Kalman Filter）相关变量定义
// 卡尔曼滤波器是一种高效的递归滤波器，它能够从一系列不完全及包含噪声的测量中，估计动态系统的状态。
// 在这里，它被用来融合加速度计和陀螺仪的数据，以获得更精确、更稳定的姿态角（Pitch 和 Roll）。
// -------------------------------------------------------------------------------------------------
// --- 状态变量 ---
float angle[3] = {0, 0, 0}, angle_dot[3] = {0, 0, 0}; // 姿态角（Pitch, Roll, Yaw）和角速度
float angle0[3] = {0, 0, 0}, angle_dot0[3] = {0, 0, 0}; // 姿态角的估计值
// --- 卡尔曼滤波器参数 ---
// Q_angle: 过程噪声协方差，表示角度预测模型的不确定性。值越小，表示越相信陀螺仪的积分结果。
// Q_gyro: 过程噪声协方差，表示陀螺仪偏置（bias）的不确定性。
// R_angle: 测量噪声协方差，表示通过加速度计计算出的角度测量值的不确定性。值越小，表示越相信加速度计的测量结果。
// dt: 采样时间间隔（单位：秒），这里是10ms (0.01s)，对应100Hz的采样率。
float Q_angle = 0.0003, Q_gyro = 0.001, R_angle = 0.005, dt = 0.01;
// --- 协方差矩阵 P ---
// P矩阵表示系统状态估计的不确定性程度。它是一个2x2的矩阵：
// P[0][0]: 角度估计的方差
// P[0][1], P[1][0]: 角度和陀螺仪偏置的协方差
// P[1][1]: 陀螺仪偏置估计的方差
// P0, P1, P2 分别用于 Pitch, Roll, Yaw 的计算（尽管Yaw未使用卡尔曼滤波）。
float P[2][2] = {{ 1, 0 }, { 0, 1 }};
float P0[2][2] = {{ 1, 0 }, { 0, 1 }}; // Pitch 轴的协方差矩阵
float P1[2][2] = {{ 1, 0 }, { 0, 1 }}; // Roll 轴的协方差矩阵
float P2[2][2] = {{ 1, 0 }, { 0, 1 }}; // Yaw 轴的协方差矩阵（未使用）
// --- 中间计算变量 ---
float Pdot0[4] = {0, 0, 0, 0}; // P0矩阵的微分，用于预测步骤
float Pdot1[4] = {0, 0, 0, 0}; // P1矩阵的微分，用于预测步骤
float Pdot2[4] = {0, 0, 0, 0}; // P2矩阵的微分，用于预测步骤
const float C_0 = 1.0; // 测量矩阵H的元素，因为直接测量角度，所以为1
const float C_1 = 1.0;
const float C_2 = 1.0;
float q_bias0[3] = {0, 0, 0}; // 陀螺仪零点偏置（bias）的估计值
float angle_err0[3] = {0, 0, 0}; // 测量值与预测值之间的误差
float PCt0_0[3] = {0, 0, 0}, PCt0_1[3] = {0, 0, 0}; // 中间变量，用于计算卡尔曼增益
float E0[3] = {0, 0, 0}; // 误差的协方差
float K0_0[3] = {0, 0, 0}, K0_1[3] = {0, 0, 0}; // 卡尔曼增益K
float t0_0[3] = {0, 0, 0}, t0_1[3] = {0, 0, 0}; // 中间变量，用于更新P矩阵
// =================================================================================================
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;
}
unsigned char SL_SC7U22_Error_Flag=0;
unsigned char SL_SC7U22_Error_cnt=0;
unsigned char SL_SC7U22_Error_cnt2=0;
signed short Temp_Accgyro[6] ={0};
signed short Error_Accgyro[6]={0};
signed int Sum_Avg_Accgyro[6] ={0};
/**
* @brief 姿态角解算函数
* @details
* 该函数主要完成两项工作：
* 1. 静态校准：在初始阶段，检测传感器是否处于静止状态。如果是，则计算加速度计和陀螺仪的零点偏移（误差），用于后续的数据补偿。
* 2. 姿态解算：使用卡尔曼滤波器融合经过校准后的加速度计和陀螺仪数据，计算出物体的俯仰角（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;
float angle_acc[3] = {0};
float gyro_val[3] = {0};
// 如果外部强制使能校准，则将标志位置1
if (calibration_en == 0) {
SL_SC7U22_Error_Flag = 1;
}
// =================================================================================
// 步骤 1: 静态校准
// ---------------------------------------------------------------------------------
// SL_SC7U22_Error_Flag 为0时，表示需要进行校准。
// 校准的原理是：当传感器长时间处于静止状态时，认为其三轴加速度输出应为(0, 0, g)，三轴角速度输出应为(0, 0, 0)。
// 通过采集一段时间的静止数据并求平均，可以得到传感器的零点偏移量。
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 < 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（假设量程为±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
}
} 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];
}
#if 1 // 将校准后的数据写回输入数组
for (sl_i = 0; sl_i < 6; sl_i++) {
acc_gyro_input[sl_i] = Temp_Accgyro[sl_i];
}
#endif
// --- 2.2 使用加速度计计算姿态角 ---
// 将加速度原始值转换为归一化的重力分量
angle_acc[0] = (float)Temp_Accgyro[0] / 8192; //ax
angle_acc[1] = (float)Temp_Accgyro[1] / 8192; //ay
angle_acc[2] = (float)Temp_Accgyro[2] / 8192; //az
// 限制范围，防止asinf/atanf计算错误
if (angle_acc[0] > 1.0) angle_acc[0] = 1.0;
if (angle_acc[0] < -1.0) angle_acc[0] = -1.0;
if (angle_acc[1] > 1.0) angle_acc[1] = 1.0;
if (angle_acc[1] < -1.0) angle_acc[1] = -1.0;
if (angle_acc[2] > 1.0) angle_acc[2] = 1.0;
if (angle_acc[2] < -1.0) angle_acc[2] = -1.0;
// 根据重力分量计算Pitch和Roll角（单位：度）
// Pitch = arcsin(ax / g)
angle_acc[0] = asinf(angle_acc[0]) * 57.32484; //Pitch:-90~+90
// Roll = -arctan(ay / az)
angle_acc[1] = -atanf(angle_acc[1] / angle_acc[2]) * 57.32484; //Roll: -180~+180
// 对Roll角进行象限补偿
if (angle_acc[2] < 0) {
if (angle_acc[1] >= 0) {
angle_acc[1] = -180 + angle_acc[1];
} else {
angle_acc[1] = 180 + angle_acc[1];
}
}
// --- 2.3 转换陀螺仪数据单位 ---
// 将陀螺仪原始值（LSB）转换为角速度（度/秒）
// 转换系数0.061 ≈ 2000dps / 32768 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和Roll分别进行滤波
// ---------------------------------------------------------------------------------
/************** Pitch 轴滤波 **************/
// --- 预测步骤 ---
// 1. 预测状态：根据上一时刻的角度和当前角速度，预测当前角度
angle0[0] += (gyro_val[0] - q_bias0[0]) * dt;
// 2. 预测协方差：更新P矩阵，表示预测状态的不确定性
Pdot0[0] = Q_angle - P0[0][1] - P0[1][0] + P0[1][1] * dt;
Pdot0[1] = -P0[1][1];
Pdot0[2] = -P0[1][1];
Pdot0[3] = Q_gyro;
P0[0][0] += Pdot0[0] * dt;
P0[0][1] += Pdot0[1] * dt;
P0[1][0] += Pdot0[2] * dt;
P0[1][1] += Pdot0[3] * dt;
// --- 更新步骤 ---
// 1. 计算卡尔曼增益 K
PCt0_0[0] = C_0 * P0[0][0];
PCt0_1[0] = C_0 * P0[1][0];
E0[0] = R_angle + C_0 * PCt0_0[0];
if (E0[0] == 0) { E0[0] = 0.0001; } // 防止除零
K0_0[0] = PCt0_0[0] / E0[0];
K0_1[0] = PCt0_1[0] / E0[0];
// 2. 计算测量余差（innovation）
angle_err0[0] = angle_acc[0] - angle0[0];
// 3. 更新状态估计：结合预测值和测量值，得到最优估计
angle0[0] += K0_0[0] * angle_err0[0];
// 4. 更新陀螺仪偏置估计
q_bias0[0] += K0_1[0] * angle_err0[0];
angle_dot0[0] = gyro_val[0] - q_bias0[0];
// 5. 更新协方差矩阵 P
t0_0[0] = PCt0_0[0];
t0_1[0] = C_0 * P0[0][1];
P0[0][0] -= K0_0[0] * t0_0[0];
P0[0][1] -= K0_0[0] * t0_1[0];
P0[1][0] -= K0_1[0] * t0_0[0];
P0[1][1] -= K0_1[0] * t0_1[0];
// 输出最终的Pitch角
Angle_output[0] = angle0[0];
/************** Roll 轴滤波 (过程同Pitch) **************/
// --- 预测步骤 ---
angle0[1] += (gyro_val[1] - q_bias0[1]) * dt;
Pdot1[0] = Q_angle - P1[0][1] - P1[1][0] + P1[1][1] * dt;
Pdot1[1] = -P1[1][1];
Pdot1[2] = -P1[1][1];
Pdot1[3] = Q_gyro;
P1[0][0] += Pdot1[0] * dt;
P1[0][1] += Pdot1[1] * dt;
P1[1][0] += Pdot1[2] * dt;
P1[1][1] += Pdot1[3] * dt;
// --- 更新步骤 ---
PCt0_0[1] = C_1 * P1[0][0];
PCt0_1[1] = C_1 * P1[1][0];
E0[1] = R_angle + C_1 * PCt0_0[1];
if (E0[1] == 0) { E0[1] = 0.0001; }
K0_0[1] = PCt0_0[1] / E0[1];
K0_1[1] = PCt0_1[1] / E0[1];
angle_err0[1] = angle_acc[1] - angle0[1];
angle0[1] += K0_0[1] * angle_err0[1];
q_bias0[1] += K0_1[1] * angle_err0[1];
angle_dot0[1] = gyro_val[1] - q_bias0[1];
t0_0[1] = PCt0_0[1];
t0_1[1] = C_1 * P1[0][1];
P1[0][0] -= K0_0[1] * t0_0[1];
P1[0][1] -= K0_0[1] * t0_1[1];
P1[1][0] -= K0_1[1] * t0_0[1];
P1[1][1] -= K0_1[1] * t0_1[1];
// 输出最终的Roll角
Angle_output[1] = angle0[1];
/************** Yaw 轴计算 **************/
// Yaw角无法通过加速度计（重力）来校正，因此这里只使用陀螺仪进行简单积分。
// 这种方法会因为陀螺仪的漂移而导致误差随时间累积。
if (yaw_rst == 1) {
Angle_output[2] = 0; // 如果有复位信号，则清零
}
// 增加一个简单的阈值，当角速度较小时，认为没有转动，以减少漂移
if (SL_GetAbsShort(Temp_Accgyro[5]) > 8) {
Angle_output[2] += gyro_val[2] * dt;
}
return 1; // 返回1，表示计算成功
}
return 2; // 校准未完成，返回错误状态
}
#endif

138
apps/earphone/xtell_Sensor/sensor/SCU722.h Normal file
View File

@ -0,0 +1,138 @@
/**************************************************
Copyright (c) 2022 Silan MEMS. All Rights Reserved.
@Silan MEMS Sensor Product Line
@Code Author:Zhou Min
**************************************************/
#ifndef __SCU722_H__
#define __SCU722_H__
#include "gSensor/gSensor_manage.h"
#include "printf.h"
//是否使能串口打印调试
#define SL_Sensor_Algo_Release_Enable 0x01
//是否开启FIFO模式，默认STREAM模式
#define SL_SC7U22_FIFO_ENABLE 0x00
/***使用前请根据实际情况配置以下参数******/
/**SC7U22的SDO 接地: 0****************/
/**SC7U22的SDO 接电源:1****************/
#define SL_SC7U22_SDO_VDD_GND 1
/*****************************************/
/***使用前请根据实际IIC地址配置参数***/
/**SC7U22的IIC 接口地址为 7bits: 0****/
/**SC7U22的IIC 接口地址为 8bits: 1****/
#define SL_SC7U22_IIC_7BITS_8BITS 0
/*****************************************/
#if SL_SC7U22_SDO_VDD_GND==0
#define SL_SC7U22_IIC_7BITS_ADDR 0x18
#define SL_SC7U22_IIC_8BITS_WRITE_ADDR 0x30
#define SL_SC7U22_IIC_8BITS_READ_ADDR 0x31
#else
#define SL_SC7U22_IIC_7BITS_ADDR 0x19
#define SL_SC7U22_IIC_8BITS_WRITE_ADDR 0x32
#define SL_SC7U22_IIC_8BITS_READ_ADDR 0x33
#endif
#if SL_SC7U22_IIC_7BITS_8BITS==0
#define SL_SC7U22_IIC_ADDRESS SL_SC7U22_IIC_7BITS_ADDR
#else
#define SL_SC7U22_IIC_WRITE_ADDRESS SL_SC7U22_IIC_8BITS_WRITE_ADDR
#define SL_SC7U22_IIC_READ_ADDRESS SL_SC7U22_IIC_8BITS_READ_ADDR
#endif
unsigned char SL_SC7U22_I2c_Spi_Write(unsigned char sl_spi_iic, unsigned char reg, unsigned char dat);
unsigned char SL_SC7U22_I2c_Spi_Read(unsigned char sl_spi_iic, unsigned char reg, unsigned short len, unsigned char* buf);
/*************I2C通信检测函数******************/
unsigned char SL_SC7U22_Check(void);
/*************函数返回值*****************/
/**return : 1 IIC通信正常,IC正常**************/
/**return : 0 IIC通信异常,IC异常**********/
/*************传感器初始化函数*******************/
unsigned char SL_SC7U22_Config(void);
/*************函数返回值*****************/
/**return : 1 IIC通信正常,IC正常*************/
/**return : 0; IIC通信异常,IC异常*********/
/*************SC7U22 Sensor Time**************/
unsigned int SL_SC7U22_TimeStamp_Read(void);
/*************函数返回值*****************/
/**return : 内部传感器时间***************/
#if SL_SC7U22_FIFO_ENABLE ==0x00
/******实时读取数据寄存器数据，相当于从400Hz的FIFO中取出数据******/
void SL_SC7U22_RawData_Read(signed short* acc_data_buf, signed short* gyr_data_buf);
/************* 输入XYZ三轴数据存放的地址*****************/
/************* *acc_data_buf: ACC数据***********************/
/************* *gyr_data_buf: GYR数据***********************/
#else
/******实时读取数据寄存器FIFO数据******/
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);
/*************输入XYZ三轴数据首地址**************************/
/*************accx_buf[0]: ACC_X的第一个数据**************/
/*************accy_buf[0]: ACC_Y的第一个数据**************/
/*************accz_buf[0]: ACC_Z的第一个数据**************/
/*************gyrx_buf[0]: GYR_X的第一个数据**************/
/*************gyry_buf[0]: GYR_Y的第一个数据**************/
/*************gyrz_buf[0]: GYR_Z的第一个数据**************/
/****************函数返回值****************************/
/**return : len 表示数组长度*************************/
#endif
/*********进入传感器关闭模式*************/
unsigned char SL_SC7U22_POWER_DOWN(void);
/**0: 关闭模式失败***********************/
/**1: 关闭模式成功***********************/
/*********SC7U22 RESET***************/
unsigned char SL_SC7U22_SOFT_RESET(void);
/**0: 成功*****************************/
/**1: 失败**************************/
/*************GSensor and GyroSensor开启和关闭函数*********/
unsigned char SL_SC7U22_Open_Close_SET(unsigned char acc_enable,unsigned char gyro_enable);
/**acc_enable: 0=关闭ACC Sensor; 1=开启ACC Sensor*********/
/**gyro_enable: 0=关闭GYRO Sensor; 1=开启GYRO Sensor*******/
/**return: 0=设置失败，1=设置成功**************************/
/*********进入睡眠模式并开启中断函数*************/
unsigned char SL_SC7U22_IN_SLEEP_SET(unsigned char acc_odr,unsigned char vth,unsigned char tth,unsigned char int_io);
/**acc_odr: 12/25/50**************************************/
/**vth: 运动检测，阈值参数****************************/
/**tth: 运动检测，持续时间阈值，小于该时间则过滤**********/
/**int_io: 1=INT1, 2=INT2*********************************/
/**return: 0=设置失败，1=设置成功**************************/
/*********进入唤醒模式，设置参数并关闭中断函数***********/
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);
/**odr_mode: 25HZ/50Hz/100Hz/200Hz ACC+GYRO***************/
/**acc_range: ±2G/±4G/±8G/±16G*****************************/
/**acc_hp_en: 0=关闭高性能模式;1=开启*****/
/**gyro_range: ±125dps/±250dps/±500dps/±1000dps/±2000dps***/
/**gyro_hp_en: 0=关闭高性能模式;1=开启高性能模式; ********/
/**return: 0=设置失败，1=设置成功**************************/
/*********SC7U22 Angle Cauculate***************/
unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en,signed short *acc_gyro_input,float *Angle_output, unsigned char yaw_rst);
/**in calibration_en: 1=enable 0=disable***********************/
/**in/out acc_gyro_input[0]: ACC-X*****************************/
/**in/out acc_gyro_input[1]: ACC-Y*****************************/
/**in/out acc_gyro_input[2]: ACC-Z*****************************/
/**in/out acc_gyro_input[3]: GYR-X*****************************/
/**in/out acc_gyro_input[4]: GYR-Y*****************************/
/**in/out acc_gyro_input[5]: GYR-Z*****************************/
/**output Angle_output[0]: Pitch*****************************/
/**output Angle_output[1]: Roll******************************/
/**output Angle_output[2]: Yaw*******************************/
/**input yaw_rst: reset yaw value***************************/
/**寄存器宏定义*******************************/
#define SC7U22_WHO_AM_I 0x01
#endif // __SCU722_H__

17
apps/earphone/xtell_Sensor/xtell_handler.c
View File

@ -51,7 +51,7 @@
#define LOG_TAG "[EARPHONE]"
#define LOG_ERROR_ENABLE
#define LOG_DEBUG_ENABLE
#define LOG_INFO_ENABLE
#define xlog_ENABLE
#if(USE_DMA_UART_TEST) //使用dm串口测试时不能同时打开
@ -212,7 +212,7 @@ static int bt_connction_status_event_handler(struct bt_event *bt)
/*
* 蓝牙初始化完成
*/
log_info("BT_STATUS_INIT_OK\n");
xlog("BT_STATUS_INIT_OK\n");
init_ok = 1;
__set_sbc_cap_bitpool(38);
@ -248,7 +248,7 @@ static int bt_connction_status_event_handler(struct bt_event *bt)
case BT_STATUS_SECOND_CONNECTED:
clear_current_poweron_memory_search_index(0);
case BT_STATUS_FIRST_CONNECTED:
log_info("BT_STATUS_CONNECTED\n");
xlog("BT_STATUS_CONNECTED\n");
xtell_bl_state = 1; //蓝牙连接成功 置1
if(strcmp(xt_ble_new_name,"CM-1111") != 0){
//蓝牙连接成功
@ -264,7 +264,7 @@ static int bt_connction_status_event_handler(struct bt_event *bt)
/* tone_play(TONE_CONN); */
/*os_time_dly(40); // for test*/
log_info("tone status:%d\n", tone_get_status());
xlog("tone status:%d\n", tone_get_status());
if (get_call_status() == BT_CALL_HANGUP) {
if (app_var.phone_dly_discon_time) {
sys_timeout_del(app_var.phone_dly_discon_time);
@ -275,12 +275,13 @@ static int bt_connction_status_event_handler(struct bt_event *bt)
}
}
/*int timeout = 5000 + rand32() % 10000;
sys_timeout_add(NULL, connect_phone_test, timeout);*/
break;
case BT_STATUS_FIRST_DISCONNECT:
case BT_STATUS_SECOND_DISCONNECT:
log_info("BT_STATUS_DISCONNECT\n");
xlog("BT_STATUS_DISCONNECT\n");
xtell_bl_state = 0; //断开蓝牙 清0
//蓝牙断开连接
if(bt_newname){ //已经改成新蓝牙名字，断开才播报
@ -315,7 +316,7 @@ static int bt_connction_status_event_handler(struct bt_event *bt)
break;
case BT_STATUS_SNIFF_STATE_UPDATE:
log_info(" BT_STATUS_SNIFF_STATE_UPDATE %d\n", bt->value); //0退出SNIFF
xlog(" BT_STATUS_SNIFF_STATE_UPDATE %d\n", bt->value); //0退出SNIFF
if (bt->value == 0) {
sniff_out = 1;
sys_auto_sniff_controle(MY_SNIFF_EN, bt->args);
@ -344,10 +345,10 @@ static int bt_connction_status_event_handler(struct bt_event *bt)
break;
case BT_STATUS_AVRCP_INCOME_OPID:
log_info("BT_STATUS_AVRCP_INCOME_OPID:%d\n", bt->value);
xlog("BT_STATUS_AVRCP_INCOME_OPID:%d\n", bt->value);
break;
default:
log_info(" BT STATUS DEFAULT\n");
xlog(" BT STATUS DEFAULT\n");
break;
}
return 0;