Ma
This commit is contained in:
2
Makefile
2
Makefile
@ -617,7 +617,7 @@ c_SRC_FILES := \
|
|||||||
apps/earphone/xtell_Sensor/send_data.c \
|
apps/earphone/xtell_Sensor/send_data.c \
|
||||||
apps/earphone/xtell_Sensor/buffer/circle_buffer.c \
|
apps/earphone/xtell_Sensor/buffer/circle_buffer.c \
|
||||||
apps/earphone/xtell_Sensor/sensor/LIS2DH12.c \
|
apps/earphone/xtell_Sensor/sensor/LIS2DH12.c \
|
||||||
apps/earphone/xtell_Sensor/sensor/SCU722.c \
|
apps/earphone/xtell_Sensor/sensor/SC7U22.c \
|
||||||
|
|
||||||
|
|
||||||
# 需要编译的 .S 文件
|
# 需要编译的 .S 文件
|
||||||
|
|||||||
996
apps/earphone/xtell_Sensor/sensor/SC7U22_Ma.c
Normal file
996
apps/earphone/xtell_Sensor/sensor/SC7U22_Ma.c
Normal file
@ -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");
|
||||||
|
}
|
||||||
|
|
||||||
|
// ...
|
||||||
|
}
|
||||||
|
|
||||||
|
//-----------------------------------------
|
||||||
|
*/
|
||||||
Reference in New Issue
Block a user