LSM303DLH六轴角度传感器驱动
最近使用了意法半导体的LSM303DLH六轴角度传感器，使用软件IIC通讯,我主要使用Z轴加速度计算垂直方向倾角，使用磁场传感器计算水平旋转角。
//LSM303DLH.c
/*************************************************************************************************************
* 文件名: LSM303DLH.c
* 功能: LSM303DLH驱动
* 作者: [email protected]
* 创建时间: 2019-01-10
* 最后修改时间: 2019-01-10
* 详细: LSM303DLH六轴角度传感器
依赖SoftwareIIC
*************************************************************************************************************/
#include "system.h"
#include "LSM303DLH.h"
#include "math.h"
#include
#include "SoftwareIIC.h"
//调试宏开关
#define LSM303DLH_DBUG 1
#if LSM303DLH_DBUG
#include "system.h"
#define LSM303DLH_Debug(format,...) uart_printf(format,##__VA_ARGS__)
#else
#define LSM303DLH_Debug(format,...) /\
/
#endif //LSM303DLH_DBUG
#define PI 3.1415926535898
u8 LSM303DLH_ReadOneReg(LSM303DLH_HANDLE *pHandle,LSM303DLH_REG_TYPE RegAddr); //LSM303DLH读取一个寄存器
void LSM303DLH_ReadMultReg(LSM303DLH_HANDLE *pHandle,LSM303DLH_REG_TYPE RegAddr, u8 RegNum, u8 DataBuff[]); //LSM303DLH读取多个寄存器
void LSM303DLH_WriteOneReg(LSM303DLH_HANDLE *pHandle,LSM303DLH_REG_TYPE RegAddr,u8 data); //LSM303DLH写一个寄存器
/*************************************************************************************************************************
*函数 : LSM303DLH_Init(LSM303DLH_HANDLE *pHandle, u8 SlaveAddr)
*功能 : LSM303DLH初始化
*参数 : pHandle:句柄；SlaveAddr_A：加速度传感器通讯地址；SlaveAddr_M：磁场传感器通讯地址；
*返回 : TRUE:初始化成功；FALSE:初始化失败
*依赖 : 底层宏定义
*作者 : [email protected]
*时间 : 2019-01-30
*最后修改时间 : 2019-01-30
*说明 :
*************************************************************************************************************************/
bool LSM303DLH_Init(LSM303DLH_HANDLE *pHandle, u8 SlaveAddr_A, u8 SlaveAddr_M)
{
u8 temp;
u8 retry = 0;
if(pHandle == NULL) return FALSE;
pHandle->SlaveAddr_A = SlaveAddr_A; //加速度传感器通讯地址
pHandle->SlaveAddr_M = SlaveAddr_M; //磁场传感器通讯地址
for(retry = 0;retry < 3;retry ++)
{
LSM303DLH_WriteOneReg(pHandle, LSM303_CTRL_REG1_A_0x20, 0x27); //正常模式，50Hz速度
SYS_DelayMS(1); //延时3ms
temp = LSM303DLH_ReadOneReg(pHandle, LSM303_CTRL_REG1_A_0x20); //读取0x20寄存器，默认值为0x07
if(temp != 0x27) //值不对
{
uart_printf("初始化失败，LSM303_CTRL_REG1_A_0x20默认值错误：0x%02X\r\n", temp);
SYS_DelayMS(10);
}
else break;
}
if(temp != 0x27) //值不对
{
return FALSE;
}
SYS_DelayMS(1); //延时1ms
for(retry = 0;retry < 3;retry ++)
{
LSM303DLH_WriteOneReg(pHandle, LSM303_CRA_REG_M_0x00, 0x10); //磁场传感器15Hz,正常测量
SYS_DelayMS(1); //延时1ms
LSM303DLH_WriteOneReg(pHandle, LSM303_MR_REG_M_0x02, 0x00); //磁场传感器连续转换模式
SYS_DelayMS(1); //延时1ms
temp = LSM303DLH_ReadOneReg(pHandle, LSM303_MR_REG_M_0x02);
if(temp != 0) //值不对
{
uart_printf("初始化失败，LSM303_MR_REG_M_0x02值错误：0x%02X\r\n", temp);
SYS_DelayMS(10);
}
else break;
}
return TRUE;
}
/*************************************************************************************************************************
*函数 : u8 LSM303DLH_ReadOneReg(LSM303DLH_HANDLE *pHandle,LSM303DLH_REG_TYPE RegAddr)
*功能 : LSM303DLH读取一个寄存器
*参数 : pHandle:句柄；RegAddr：寄存器地址
*返回 : 读取的寄存器值
*依赖 : 底层宏定义
*作者 : [email protected]
*时间 : 2018-04-09
*最后修改时间 : 2018-04-09
*说明 :
*************************************************************************************************************************/
u8 LSM303DLH_ReadOneReg(LSM303DLH_HANDLE *pHandle,LSM303DLH_REG_TYPE RegAddr)
{
u8 data;
u8 SlaveAddr = (RegAddr>0x19)?pHandle->SlaveAddr_A:pHandle->SlaveAddr_M; //大于0x19的寄存器是加速度传感器
SIIC_Start(&pHandle->IIC_Handle); //产生IIC起始信号
if(SIIC_SendByte(&pHandle->IIC_Handle, SlaveAddr) == FALSE) //发送设备地址+写信号
{
DEBUG("没有收到ACK\r\n");
}
SIIC_SendByte(&pHandle->IIC_Handle, RegAddr); //发送寄存器地址
SIIC_Start(&pHandle->IIC_Handle); //产生IIC起始信号
SIIC_SendByte(&pHandle->IIC_Handle, SlaveAddr|BIT0); //发送设备地址+读信号
data = SIIC_ReadByte(&pHandle->IIC_Handle, TRUE); //SIIC读取一个字节
SIIC_Stop(&pHandle->IIC_Handle); //产生IIC停止信号
return data;
}
/*************************************************************************************************************************
*函数 : void LSM303DLH_ReadMultReg(LSM303DLH_HANDLE *pHandle,LSM303DLH_REG_TYPE RegAddr, u8 RegNum, u8 DataBuff[])
*功能 : LSM303DLH读取多个寄存器
*参数 : pHandle:句柄；RegAddr：寄存器地址；RegNum：寄存器数量；DataBuff：返回结果缓冲区
*返回 : 无
*依赖 : 底层宏定义
*作者 : [email protected]
*时间 : 2018-04-09
*最后修改时间 : 2018-04-09
*说明 :
*************************************************************************************************************************/
void LSM303DLH_ReadMultReg(LSM303DLH_HANDLE *pHandle,LSM303DLH_REG_TYPE RegAddr, u8 RegNum, u8 DataBuff[])
{
u8 i;
u8 SlaveAddr = (RegAddr>0x19)?pHandle->SlaveAddr_A:pHandle->SlaveAddr_M; //大于0x19的寄存器是加速度传感器
SIIC_Start(&pHandle->IIC_Handle); //产生IIC起始信号
SIIC_SendByte(&pHandle->IIC_Handle, SlaveAddr); //发送设备地址+写信号
SIIC_SendByte(&pHandle->IIC_Handle, RegAddr|BIT7); //发送寄存器地址,地址最高位为1意味着连续读取
SIIC_Start(&pHandle->IIC_Handle); //产生IIC起始信号
SIIC_SendByte(&pHandle->IIC_Handle, SlaveAddr|BIT0); //发送设备地址+读信号
for(i = 0;i < RegNum;i ++)
{
if(i == (RegNum-1)) //最后一字节不响应ACK
{
DataBuff[i] = SIIC_ReadByte(&pHandle->IIC_Handle, FALSE); //SIIC读取一个字节-NAK
}
else
{
DataBuff[i] = SIIC_ReadByte(&pHandle->IIC_Handle, TRUE); //SIIC读取一个字节-ACK
}
}
SIIC_Stop(&pHandle->IIC_Handle); //产生IIC停止信号
}
/*************************************************************************************************************************
*函数 : void LSM303DLH_WriteOneReg(LSM303DLH_HANDLE *pHandle,LSM303DLH_REG_TYPE RegAddr,u8 data)
*功能 : LSM303DLH写一个寄存器
*参数 : pHandle:句柄；RegAddr：寄存器地址；data：要写入的值
*返回 : 无
*依赖 : 底层宏定义
*作者 : [email protected]
*时间 : 2018-04-09
*最后修改时间 : 2018-04-09
*说明 :
*************************************************************************************************************************/
void LSM303DLH_WriteOneReg(LSM303DLH_HANDLE *pHandle,LSM303DLH_REG_TYPE RegAddr,u8 data)
{
u8 SlaveAddr = (RegAddr>0x19)?pHandle->SlaveAddr_A:pHandle->SlaveAddr_M; //大于0x19的寄存器是加速度传感器
SIIC_Start(&pHandle->IIC_Handle); //产生IIC起始信号
SIIC_SendByte(&pHandle->IIC_Handle, SlaveAddr); //发送设备地址+写信号
SIIC_SendByte(&pHandle->IIC_Handle, RegAddr); //发送寄存器地址
SIIC_SendByte(&pHandle->IIC_Handle, data); //发送要写入的数据
SIIC_Stop(&pHandle->IIC_Handle); //产生IIC停止信号
}
/*************************************************************************************************************************
*函数 : bool LSM303DLH_ReadAcceleration(LSM303DLH_HANDLE *pHandle, s16 *pXa,s16 *pYa, s16 *pZa)
*功能 : LSM303DLH 读取三轴加速度
*参数 : pHandle:句柄；pXa：返回X轴加速度；pYa：返回Y轴加速度；pZa：返回Z轴加速度
*返回 : TRUE:成功；FALSE:失败
*依赖 : 底层宏定义
*作者 : [email protected]
*时间 : 2019-01-30
*最后修改时间 : 2019-01-30
*说明 : 返回的数据直接就是有符号数，无需处理
*************************************************************************************************************************/
bool LSM303DLH_ReadAcceleration(LSM303DLH_HANDLE *pHandle, s16 *pXa,s16 *pYa, s16 *pZa)
{
u8 buff[6];
s16 temp;
LSM303DLH_ReadMultReg(pHandle, LSM303_OUT_X_L_A_0x28, 6, buff); //读取数据
temp = buff[1];
temp<<=8;
temp|= buff[0];
*pXa = temp; //X轴
temp = buff[3];
temp<<=8;
temp|= buff[2];
*pYa = temp; //Y轴
temp = buff[5];
temp<<=8;
temp|= buff[4];
*pZa = temp; //Z轴
return TRUE;
}
/*************************************************************************************************************************
*函数 : bool LSM303DLH_ReadMagnetic(LSM303DLH_HANDLE *pHandle, s16 *pXm,s16 *pYm, s16 *pZm)
*功能 : LSM303DLH 读取磁场强度值
*参数 : pHandle:句柄；pXm：返回X轴磁场强度；pYm：返回Y轴磁场强度；pZm：返回Z轴磁场强度
*返回 : TRUE:成功；FALSE:失败
*依赖 : 底层宏定义
*作者 : [email protected]
*时间 : 2019-01-30
*最后修改时间 : 2019-01-30
*说明 : 返回的数据直接就是有符号数，无需处理
*************************************************************************************************************************/
bool LSM303DLH_ReadMagnetic(LSM303DLH_HANDLE *pHandle, s16 *pXm,s16 *pYm, s16 *pZm)
{
u8 buff[6];
s16 temp;
LSM303DLH_ReadMultReg(pHandle, LSM303_OUT_X_H_M_0x03, 6, buff); //读取数据
temp = buff[0];
temp<<=8;
temp|= buff[1];
*pXm = temp; //X轴
temp = buff[2];
temp<<=8;
temp|= buff[3];
*pYm = temp; //Y轴
temp = buff[4];
temp<<=8;
temp|= buff[5];
*pZm = temp; //Z轴
return TRUE;
}
/*************************************************************************************************************************
*函数 : int LSM303DLH_CalculationZAxisAngle(s16 Ax, s16 Ay, s16 Az)
*功能 : LSM303DLH 计算Z轴倾角(扩大100倍)
*参数 : Ax，Ay，Az:3个轴的加速度；
*返回 : Z轴倾角
*依赖 : 底层宏定义
*作者 : [email protected]
*时间 : 2019-01-30
*最后修改时间 : 2019-01-30
*说明 :
*************************************************************************************************************************/
int LSM303DLH_CalculationZAxisAngle(s16 Ax, s16 Ay, s16 Az)
{
double A;
float fx,fy,fz;
A = sqrt((int)Ax*Ax + (int)Ay*Ay + (int)Az*Az); //计算角加速度的矢量模长 |A|=根号下(X*X+Y*Y+Z*Z)
fx = Ax/A;
fy = Ay/A;
fz = Az/A;
//Z方向
A = fx*fx+fy*fy;
A = sqrt(A);
A = (double)A/fz;
A = atan(A);
A = A*180/PI;
if(A < 0)
{
A += 90;
A = 0-A;
}
else
{
A = 90-A;
}
/*uart_printf("temp=%d Az=%d\r\n",(int)A, Az);
A = atan(Az/A);
A = 90-A*360/PI;*/
return A*100;
}
/*************************************************************************************************************************
*函数 : int LSM303DLH_CalculationXAxisAngle(s16 Ax, s16 Ay, s16 Az)
*功能 : LSM303DLH 计算X轴倾角(扩大100倍)
*参数 : Ax，Ay，Az:3个轴的加速度；
*返回 : Z轴倾角
*依赖 : 底层宏定义
*作者 : [email protected]
*时间 : 2019-01-30
*最后修改时间 : 2019-01-30
*说明 :
*************************************************************************************************************************/
int LSM303DLH_CalculationXAxisAngle(s16 Ax, s16 Ay, s16 Az)
{
double A;
float fx,fy,fz;
A = sqrt((int)Ax*Ax + (int)Ay*Ay + (int)Az*Az); //计算角加速度的矢量模长 |A|=根号下(X*X+Y*Y+Z*Z)
fx = Ax/A;
fy = Ay/A;
fz = Az/A;
//X方向
A = fz*fz+fy*fy;
A = sqrt(A);
A = (double)A/fx;
A = atan(A);
A = A*180/PI;
if(A < 0)
{
A += 90; //向上为正
}
else
{
A = 90-A;
A = 0-A; //向下为负
}
/*uart_printf("temp=%d Az=%d\r\n",(int)A, Az);
A = atan(Az/A);
A = 90-A*360/PI;*/
return A*100;
}
//LSM303DLH.h
/*************************************************************************************************************
* 文件名: LSM303DLH.h
* 功能: LSM303DLH驱动
* 作者: [email protected]
* 创建时间: 2019-01-10
* 最后修改时间: 2019-01-10
* 详细: LSM303DLH六轴角度传感器
依赖SoftwareIIC
*************************************************************************************************************/
#ifndef _LSM303DLH_H_
#define _LSM303DLH_H_
#include "system.h"
#include "SoftwareIIC.h"
//LSM303DLH 寄存器定义
typedef enum
{
//罗盘
LSM303_CRA_REG_M_0x00 = 0x00, //RW
LSM303_CRB_REG_M_0x01 = 0x01, //RW
LSM303_MR_REG_M_0x02 = 0x02, //RW
LSM303_OUT_X_H_M_0x03 = 0x03, //R
LSM303_OUT_X_L_M_0x04 = 0x04, //R
LSM303_OUT_Y_H_M_0x05 = 0x05, //R
LSM303_OUT_Y_L_M_0x06 = 0x06, //R
LSM303_OUT_Z_H_M_0x07 = 0x07, //R
LSM303_OUT_Z_L_M_0x08 = 0x08, //R
LSM303_SR_REG_Mg_0x09 = 0x09, //R
LSM303_IRA_REG_M_0x0A = 0x0A, //R
LSM303_IRB_REG_M_0x0B = 0x0B, //R
LSM303_IRC_REG_M_0x0C = 0x0C, //R
//加速度
LSM303_CTRL_REG1_A_0x20 = 0x20, //RW
LSM303_CTRL_REG2_A_0x21 = 0x21, //RW
LSM303_CTRL_REG3_A_0x22 = 0x22, //RW
LSM303_CTRL_REG4_A_0x23 = 0x23, //RW
LSM303_CTRL_REG5_A_0x24 = 0x24, //RW
LSM303_HP_FILTER_RESET_A_0x25 = 0x25, //R
LSM303_REFERENCE_A_0x26 = 0x26, //RW
LSM303_STATUS_REG_A_0x27 = 0x27, //R
LSM303_OUT_X_L_A_0x28 = 0x28, //R
LSM303_OUT_X_H_A_0x29 = 0x29, //R
LSM303_OUT_Y_L_A_0x2A = 0x2A, //R
LSM303_OUT_Y_H_A_0x2B = 0x2B, //R
LSM303_OUT_Z_L_A_0x2C = 0x2C, //R
LSM303_OUT_Z_H_A_0x2D = 0x2D, //R
LSM303_INT1_CFG_A_0x30 = 0x30, //RW
LSM303_INT1_SOURCE_A_0x31 = 0x31, //R
LSM303_INT1_THS_A_0x32 = 0x32, //RW
LSM303_INT1_DURATION_A_0x33 = 0x33, //RW
LSM303_INT2_CFG_A_0x34 = 0x34, //RW
LSM303_INT2_SOURCE_A_0x35 = 0x35, //R
LSM303_INT2_THS_A_0x36 = 0x36, //RW
LSM303_INT2_DURATION_A_0x37 = 0x37, //RW
}LSM303DLH_REG_TYPE;
//LSM303DLH 句柄
typedef struct
{
SIIC_HANDLE IIC_Handle; //IIC接口
u8 SlaveAddr_A; //加速度传感器通讯地址
u8 SlaveAddr_M; //磁场传感器通讯地址
}LSM303DLH_HANDLE;
bool LSM303DLH_Init(LSM303DLH_HANDLE *pHandle, u8 SlaveAddr_A, u8 SlaveAddr_M); //LSM303DLH初始化
bool LSM303DLH_ReadAcceleration(LSM303DLH_HANDLE *pHandle, s16 *pXa,s16 *pYa, s16 *pZa); //LSM303DLH 读取三轴加速度
bool LSM303DLH_ReadMagnetic(LSM303DLH_HANDLE *pHandle, s16 *pXm,s16 *pYm, s16 *pZm); //LSM303DLH 读取磁场强度值
int LSM303DLH_CalculationZAxisAngle(s16 Ax, s16 Ay, s16 Az); //LSM303DLH 计算Z轴倾角(扩大100倍)
int LSM303DLH_CalculationXAxisAngle(s16 Ax, s16 Ay, s16 Az); //LSM303DLH 计算X轴倾角(扩大100倍)
#endif /*_LSM303DLH_H_*/
//测试
DS18B20_HANDLE DS18B20Handle; //DS18B20句柄
s16 DeviceTemp;
s16 Xa, Ya, Za;
int zDipAngle;
LSM303DLH_DeInit(); //LSM303DLH硬件初始化
//软件IIC初始化
if(SIIC_Init(&g_SysFlag.LSM303DLH_Handle.IIC_Handle, LSM303DLH_SDA_GPIOX, LSM303DLH_SCL_GPIOX, LSM303DLH_SDA_GPIO_BIT, LSM303DLH_SCL_GPIO_BIT, 10) == FALSE)
{
DEBUG("软件IIC初始化失败！\r\n");
}
LSM303DLH_Init(&g_SysFlag.LSM303DLH_Handle, 0x30, 0x3C); //LSM303DLH初始化
LSM303DLH_ReadAcceleration(&g_SysFlag.LSM303DLH_Handle, &Xa, &Ya, &Za); //LSM303DLH 读取三轴加速度
zDipAngle = LSM303DLH_CalculationZAxisAngle( Xa, Ya, Za); //LSM303DLH 计算Z轴倾角(扩大100倍)
//uart_printf("X:%d;Y:%d;Z:%d; \t",Xa,Ya,Za);
uart_printf("Z倾角：%d.%02d; \t", zDipAngle/100, abs(zDipAngle)%100);
g_SysFlag.Angle = zDipAngle = LSM303DLH_CalculationXAxisAngle(Xa, Ya, Za); //LSM303DLH 计算X轴倾角(扩大100倍)
uart_printf("X倾角：%d.%02d; \t", zDipAngle/100, abs(zDipAngle)%100);
LSM303DLH_ReadMagnetic(&g_SysFlag.LSM303DLH_Handle, &Xa, &Ya, &Za); //LSM303DLH 读取磁场强度值
uart_printf("X:%d;Y:%d;Z:%d;\r\n",Xa,Ya,Za);
//硬件接口宏定义
/////////////////////////////////////////////////////////////////////////////////////////////
//LSM303DLH支持
//IO支持
#define LSM303DLH_SCL_GPIOX GPIOB //SCL GPIO
#define LSM303DLH_SCL_GPIO_BIT 5 //SCL 引脚位
#define LSM303DLH_SDA_GPIOX GPIOB //SDA GPIO
#define LSM303DLH_SDA_GPIO_BIT 6 //SDA 引脚位
#define LSM303DLH_SA0_A PBout(7) //地址选择
//IO初始化函数
__inline void LSM303DLH_DeInit(void)
{
SYS_DeviceClockEnable(DEV_GPIOB,TRUE); //GPIO B 时钟使能
SYS_GPIOx_OneInit(GPIOB ,7 , OUT_PP, SPEED_25M);
LSM303DLH_SA0_A = 0; //地址输出0
}附上软件IIC接口：https://blog.csdn.net/cp1300/article/details/75644988