这次要讲一下LIS3DH三轴加速度计的开发使用,之前是有讲过 MPU6050开发 的,但当时是在Linux下做的。额,还是个半成品,哈哈。现在手边有STM32的开发板,可以实实在在的来实现以下卡尔曼滤波了。
一、LIS3DH初识
依照惯例,先看一下手册,大致先了解一下,LIS3DH是什么模块。
下载:LIS3DH datasheet
1、描述
LIS3DH是一款超低功耗高性能三轴线性加速度计,属于“纳米”系列,**具有数字I2C / SPI串行接口标准输出。**该器件具有超低功耗工作模式,可实现高级节能和智能嵌入式功能。
LIS3DH具有动态用户可选的满量程±2g/±4g /±8g/±16g,能够测量1 Hz至5.3 kHz输出数据速率的加速度。 自检功能允许用户在最终应用中检查传感器的功能。该设备可以被配置为使用两个独立的惯性唤醒/自由落体事件以及设备本身的位置来生成中断信号。
中断发生器的阈值和时序可由最终用户即时编程。 LIS3DH具有集成的32级先进先出(FIFO)缓冲器,允许用户存储数据以限制主处理器的干预。
LIS3DH采用小型薄塑料焊盘阵列封装(LGA),保证在-40°C至+ 85°C的扩展温度范围内工作。
2、特征
电源电压范围,1.71 V至3.6 V.
独立IO供电(1.8 V)和电源电压兼容
超低功耗模式消耗低至2μA
动态可选择±2g/±4g/±8g/±16g满量程
I2C/ SPI数字输出接口
16位数据输出
2个独立的可编程中断用于自由落体和运动检测的发生器
6D/ 4D方向检测
自由落体检测
运动检测
嵌入式温度传感器
嵌入式自检
嵌入式32级16位数据输出FIFO
10000克高冲击生存能力
ECOPACK®,RoHS和“绿色”兼容
3、原理图
4、I2C/SPI通信协议
额,I2C部分我已经讲的不想再讲了。。
参看:MPU6050开发 – 进阶之I2C/SPI通信协议
这部分只看一下LIS3DH的I2C的地址
与LIS3DH关联的从属地址(SAD)为001100xb。 SDO / SA0引脚可以
用于修改设备地址的较低位。 如果SA0引脚已连接对于电源,LSb为’1’(地址0011001b),否则如果SA0引脚接地,LSb值为’0’(地址0011000b)。
它的读地址和写地址:
二、LIS3DH开发
下载:LIS3DH 资料_完整例程_手册_电路等
参看:LIS3DH 资料_完整例程_手册_电路等
相关的完整例程网上其实也是很好找的。
关键是你要搞清楚相关寄存器的配置。
1、设置
初始化相关寄存器:
void Reset_LIS3DH(void)
{
LIS3DH_WriteReg(LIS3DH_TEMP_CFG_REG,0x00);
LIS3DH_WriteReg(LIS3DH_CTRL_REG1,0x07); //XYZ轴使能
LIS3DH_WriteReg(LIS3DH_CTRL_REG2,0x00);
LIS3DH_WriteReg(LIS3DH_CTRL_REG3,0x00);
LIS3DH_WriteReg(LIS3DH_CTRL_REG4,0x00);
LIS3DH_WriteReg(LIS3DH_CTRL_REG5,0x00);
LIS3DH_WriteReg(LIS3DH_CTRL_REG6,0x00);
LIS3DH_WriteReg(LIS3DH_REFERENCE_REG,0x00);
LIS3DH_WriteReg(LIS3DH_FIFO_CTRL_REG,0x00);
LIS3DH_WriteReg(LIS3DH_INT1_CFG,0x00);
LIS3DH_WriteReg(LIS3DH_INT1_THS,0x00);
LIS3DH_WriteReg(LIS3DH_INT1_DURATION,0x00);
LIS3DH_WriteReg(LIS3DH_CLICK_CFG,0x00);
LIS3DH_WriteReg(LIS3DH_CLICK_THS,0x00);
LIS3DH_WriteReg(LIS3DH_TIME_LIMIT,0x00);
LIS3DH_WriteReg(LIS3DH_TIME_LATENCY,0x00);
LIS3DH_WriteReg(LIS3DH_TIME_WINDOW,0x00);
}
初始化LIS3DH
int gsensor_init(void)
{
uint8_t response = 0;
response |= LIS3DH_SetODR(LIS3DH_ODR_400Hz);//设置数据输出频率
response |= LIS3DH_SetMode(LIS3DH_NORMAL);//设置正常模式
response |= LIS3DH_SetFullScale(LIS3DH_FULLSCALE_16);//设置量程为±16g
response |= LIS3DH_SetAxis(LIS3DH_X_ENABLE | LIS3DH_Y_ENABLE | LIS3DH_Z_ENABLE);//使能三轴数据输出
return response;
}
设置阈值
response = gsensor_set_threshold(44,2);//44*186mg = 8184mg;
uint8_t gsensor_set_threshold(uint8_t threshold_value,uint8_t interrupt_ID)
{
/*
1 LSb = 16 mg @ FS = ±2 g
1 LSb = 32 mg @ FS = ±4 g
1 LSb = 62 mg @ FS = ±8 g
1 LSb = 186 mg @ FS = ±16 g
*/
uint8_t response = 0;
if(interrupt_ID == 1)
{
LIS3DH_HPFAOI1Enable(MEMS_DISABLE);//POSITION
response|= LIS3DH_SetIntConfiguration(LIS3DH_INT1_ZHIE_ENABLE|LIS3DH_INT1_ZLIE_ENABLE|
LIS3DH_INT1_YHIE_ENABLE|LIS3DH_INT1_YLIE_ENABLE|
LIS3DH_INT1_XHIE_ENABLE|LIS3DH_INT1_XLIE_ENABLE);
response|= LIS3DH_SetInt6D4DConfiguration(LIS3DH_INT1_6D_ENABLE);
response|= LIS3DH_SetIntMode(LIS3DH_INT_MODE_6D_POSITION);
response|= LIS3DH_SetInt1Threshold(threshold_value);//16 * 16mg == 256mg
response|= LIS3DH_SetInt1Pin(LIS3DH_I1_INT1_ON_PIN_INT1_ENABLE);
}
if(interrupt_ID == 2)
{
LIS3DH_HPFAOI2Enable(MEMS_ENABLE);//MOVEMENT
response|= LIS3DH_SetInt2Configuration(LIS3DH_INT2_ZHIE_ENABLE|LIS3DH_INT2_ZLIE_ENABLE|
LIS3DH_INT2_YHIE_ENABLE|LIS3DH_INT2_YLIE_ENABLE|
LIS3DH_INT2_XHIE_ENABLE|LIS3DH_INT2_XLIE_ENABLE);
response|= LIS3DH_SetInt26D4DConfiguration(LIS3DH_INT2_6D_ENABLE);
response|= LIS3DH_SetInt2Mode(LIS3DH_INT2_MODE_6D_MOVEMENT);
response|= LIS3DH_SetInt2Threshold(threshold_value);//
response|= LIS3DH_SetInt2Pin(LIS3DH_I2_INT2_ON_PIN_INT2_ENABLE);
}
return response;
}
2、读取数据
这里要注意,关于数据转换:
LIS3DH 内部 16bit 的 ADC,如上假设我们设置的量程是±2G,即±2000mG。那么将其转换成,加速度的公式就是:
加速度=读取到的加速度原始值*4000mG / 65536
void LIS3DH_Get_AccRaw(int16_t* pdata)
{
uint8_t i = 0;
uint8_t regValue[6] = {0, 0, 0, 0, 0, 0};
int16_t symbol[3] = {0,0,0};
for(i=0;i<6;i++)
{
LIS3DH_ReadReg(LIS3DH_OUT_X_L+i, regValue+i);
}
/* Format the data. */
for(i=0;i<3;i++)
{
if(regValue[2*i+1] & 0x80)symbol[i] = 0xF000;
else symbol[i] = 0x0000;
}
pdata[0] =symbol[0] | (( ( ( ( int16_t )regValue[1] ) << 8 ) + ( int16_t )regValue[0] ) >> 4);
pdata[1] =symbol[1] | ((( ( ( int16_t )regValue[3] ) << 8 ) + ( int16_t )regValue[2] ) >> 4);
pdata[2] =symbol[2] | ((( ( ( int16_t )regValue[5] ) << 8 ) + ( int16_t )regValue[4] ) >> 4);
}
void LIS3DH_Get_Sensitivity(uint8_t* sensitivity)
{
uint8_t fullscale = 0;
LIS3DH_ReadReg(LIS3DH_CTRL_REG4,&fullscale);
fullscale = (fullscale & 0x30) >> 4;
switch (fullscale)
{
case 0:*sensitivity = 1;break;
case 1:*sensitivity = 2;break;
case 2:*sensitivity = 4;break;
case 3:*sensitivity = 12;break;
default : break;
}
}
void LIS3DH_Get_AccValue(AxesRaw_t *pdata)
{
int16_t dataRaw[3];
uint8_t sensitivity = 0;
LIS3DH_Get_AccRaw(dataRaw);
LIS3DH_Get_Sensitivity(&sensitivity);
pdata->AXIS_X = ( int32_t )( dataRaw[0] * sensitivity );
pdata->AXIS_Y = ( int32_t )( dataRaw[1] * sensitivity );
pdata->AXIS_Z = ( int32_t )( dataRaw[2] * sensitivity );
}
3、中断
void BSP_LIS3DH_Init(void)
{
EXTI_InitTypeDef EXTI_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);//使能SYSCFG时钟
SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOC, EXTI_PinSource8); //配置GPIO与中断线的映射关系
/*中断的初始化*/
EXTI_InitStructure.EXTI_Line=EXTI_Line8;
//NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0);
EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;
EXTI_InitStructure.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTI_InitStructure); //初始化外设EXTI寄存器
NVIC_InitStructure.NVIC_IRQChannel = EXTI9_5_IRQn; //使能按键外部中断通道
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x00; //抢占优先级2,
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x07; //响应优先级2
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //使能外部中断通道
NVIC_Init(&NVIC_InitStructure); //中断优先级分组初始化
}
uint8_t gsensor_fifo_read(i16_t* pbuffer)
{
uint8_t un_read_num = 31,fifo_num_length = 0;
AxesRaw_t acc_data;//三轴原始加速度
fifo_num_length = un_read_num;
while(un_read_num--)
{
LIS3DH_Get_AccValue(&acc_data);
*pbuffer++ = acc_data.AXIS_X;
*pbuffer++ = acc_data.AXIS_Y;
*pbuffer++ = acc_data.AXIS_Z;
}
return fifo_num_length;
}
void EXTI9_5_IRQHandler (void)
{
if(EXTI_GetITStatus(EXTI_Line8)!=RESET)
{
LIS3DH_Rx_Len = LIS3DH_Fifo_Read(LIS3DHBuffer);
EXTI_ClearITPendingBit(EXTI_Line8);
}
}
OK,到此触发中断就可以获得XYZ轴的加速度。
4、获取倾斜角度
float drv_lis2dh12_get_angle(int16_t acc_x, int16_t acc_y, int16_t acc_z)
{
float angle_z, acc_g;
/* 重力加速度 */
acc_g = sqrt(pow(acc_x, 2) + pow(acc_y, 2) + pow(acc_z, 2));
if (acc_z > acc_g)
acc_z = acc_g;
/* angle_z/90 = asin(acc_z/acc_g)/π/2 */
angle_z = asin(acc_z/acc_g) * 2 / 3.14 * 90;
angle_z = 90 - angle_z;
if(angle_z < 0)
angle_z = 0;
return angle_z;
}
这里面有几个库函数:
参看:C 库函数 - pow()
参看:C 库函数 - sqrt()
参看:C 库函数 - asin()
参看:C 库函数 - acos()
描述:
C 库函数 double pow(double x, double y) 返回 x 的 y 次幂,即 xy。
C 库函数 double sqrt(double x) 返回 x 的平方根。
C 库函数 double asin(double x) 返回以弧度表示的 x 的反正弦。
C 库函数 double acos(double x) 返回以弧度表示的 x 的反余弦。
这个计算方式是如何得来的?
参看:MPU6050开发 – 数据分析
这里的Rx、Ry、Rz 就是用公式(加速度数据 = 加速度轴原始数据/加速度灵敏度)得出的各轴的加速度数据。
R 也可以通过方程1 导出,R = SQRT(Rx ^ 2 + Ry ^ 2 + Rz ^ 2)
最后就可以通过下面的公式:
Axr = arccos(Rx/R)
Ayr = arccos(Ry/R)
Azr = arccos(Rz/R)
得到X,Y,Z轴与力矢量R之间的角度(Axr,Ayr,Azr)
举个栗子:
当水平放置 MPU6050,只有 Z 轴感受到重力向量,它将输出 -1g。此时 R 就是重力向量。
Rx=0. Ry=0. Rz = -1g
满足 R ^ 2 = RX ^ 2 + RY ^ 2 + RZ ^ 2 = 1 得到重力向量与各个轴的夹角
Axr = arccos(RX / R) = 90度
Ayr = arccos(RY / R) = 90度
Azr = arccos(RZ / R) = 0 度
其中 arccos (1) = 0,arccos (0) = 90