SCU722驱动替换更新
This commit is contained in:
3
.vscode/settings.json
vendored
3
.vscode/settings.json
vendored
@ -17,6 +17,7 @@
|
||||
"key_event_deal.h": "c",
|
||||
"app_umidigi_chargestore.h": "c",
|
||||
"hci_lmp.h": "c",
|
||||
"bluetooth.h": "c"
|
||||
"bluetooth.h": "c",
|
||||
"SCU722.C": "cpp"
|
||||
}
|
||||
}
|
||||
@ -1664,11 +1664,11 @@ void send_data_to_ble_client(const u8* data, u16 length)
|
||||
// 发送数据
|
||||
int ret = app_send_user_data(ATT_CHARACTERISTIC_ae02_01_VALUE_HANDLE, data, length, ATT_OP_NOTIFY);
|
||||
if (ret == 0) { // 假设 0 表示成功
|
||||
printf("Data sent successfully: Length %d\n", length);
|
||||
// printf("Data sent successfully: Length %d\n", length);
|
||||
} else {
|
||||
printf("Failed to send data: Length %d, Error code: %d\n", length, ret);
|
||||
// printf("Failed to send data: Length %d, Error code: %d\n", length, ret);
|
||||
}
|
||||
} else {
|
||||
printf("Insufficient buffer space to send data: Length %d\n", length);
|
||||
// printf("Insufficient buffer space to send data: Length %d\n", length);
|
||||
}
|
||||
}
|
||||
@ -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);
|
||||
}
|
||||
|
||||
@ -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
947
apps/earphone/xtell_Sensor/sensor/SCU722.c
Normal 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, ®_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, ®_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
|
||||
// =================================================================================================
|
||||
// 卡尔曼滤波器（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
138
apps/earphone/xtell_Sensor/sensor/SCU722.h
Normal 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__
|
||||
@ -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;
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -6310,7 +6310,6 @@ objs/apps/earphone/xtell_Sensor/xtell_handler.c.o
|
||||
-r=objs/apps/earphone/xtell_Sensor/xtell_handler.c.o,rcsp_sys_event_handler_specific,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/xtell_handler.c.o,default_event_handler,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/xtell_handler.c.o,get_tone_config,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/xtell_handler.c.o,log_print,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/xtell_handler.c.o,__set_sbc_cap_bitpool,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/xtell_handler.c.o,ble_bqb_test_thread_init,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/xtell_handler.c.o,bt_ble_init,l
|
||||
@ -6355,8 +6354,7 @@ objs/apps/earphone/xtell_Sensor/send_data.c.o
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,xtell_task_create,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,circle_buffer_init,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,create_process,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,xtell_i2c_test,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,puts,l
|
||||
-r=objs/apps/earphone/xtell_Sensor/send_data.c.o,global_data_array,pl
|
||||
objs/apps/earphone/xtell_Sensor/buffer/circle_buffer.c.o
|
||||
-r=objs/apps/earphone/xtell_Sensor/buffer/circle_buffer.c.o,circle_buffer_init,pl
|
||||
-r=objs/apps/earphone/xtell_Sensor/buffer/circle_buffer.c.o,circle_buffer_write,pl
|
||||
|
||||
339148
cpu/br28/tools/sdk.lst
339148
cpu/br28/tools/sdk.lst
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
Binary file not shown.
Binary file not shown.
@ -1,5 +1,5 @@
|
||||
objs/apps/earphone//xtell_Sensor/xtell_app_main.c.o: \
|
||||
apps/earphone//xtell_Sensor/xtell_app_main.c \
|
||||
objs/apps/earphone/xtell_Sensor/xtell_app_main.c.o: \
|
||||
apps/earphone/xtell_Sensor/xtell_app_main.c \
|
||||
include_lib\system/includes.h include_lib/system/init.h \
|
||||
include_lib/system/event.h include_lib/system/generic/typedef.h \
|
||||
include_lib/driver/cpu/br28\asm/cpu.h \
|
||||
|
||||
Binary file not shown.
@ -1,5 +1,5 @@
|
||||
objs/apps/earphone//xtell_Sensor/xtell_handler.c.o: \
|
||||
apps/earphone//xtell_Sensor/xtell_handler.c \
|
||||
objs/apps/earphone/xtell_Sensor/xtell_handler.c.o: \
|
||||
apps/earphone/xtell_Sensor/xtell_handler.c \
|
||||
include_lib\system/includes.h include_lib/system/init.h \
|
||||
include_lib/system/event.h include_lib/system/generic/typedef.h \
|
||||
include_lib/driver/cpu/br28\asm/cpu.h \
|
||||
|
||||
Binary file not shown.
Reference in New Issue
Block a user