发新帖我要提问
返回列表
打印
[应用相关]

LSM303DLH六轴角度传感器驱动

[复制链接]
807|9
手机看帖
扫描二维码
随时随地手机跟帖
跳转到指定楼层
楼主
观海|  楼主 | 2021-8-2 12:40 | 只看该作者 回帖奖励 |倒序浏览 |阅读模式
轴角, 角度传感器, lh, AN, AD, TE
最近使用了意法半导体的LSM303DLH六轴角度传感器，使用软件IIC通讯,我主要使用Z轴加速度计算垂直方向倾角，使用磁场传感器计算水平旋转角。

//LSM303DLH.c

/*************************************************************************************************************
* 文件名:                        LSM303DLH.c
* 功能:                        LSM303DLH驱动
* 作者:                        cp1300@139.com
* 创建时间:                2019-01-10
* 最后修改时间:        2019-01-10
* 详细:                        LSM303DLH六轴角度传感器
                                        依赖SoftwareIIC
*************************************************************************************************************/
#include "system.h"
#include "LSM303DLH.h"
#include "math.h"
#include <stdio.h>
#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:初始化失败
*依赖                        :         底层宏定义
*作者               :        cp1300@139.com
*时间                     :        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;
}



使用特权

评论回复 赏
沙发
观海|  楼主 | 2021-8-2 12:43 | 只看该作者
/*************************************************************************************************************************
*函数                :        void LSM303DLH_ReadMultReg(LSM303DLH_HANDLE *pHandle,LSM303DLH_REG_TYPE RegAddr, u8 RegNum, u8 DataBuff[])
*功能                :        LSM303DLH读取多个寄存器
*参数                :        pHandle:句柄；RegAddr：寄存器地址；RegNum：寄存器数量；DataBuff：返回结果缓冲区
*返回                :        无
*依赖                        :         底层宏定义
*作者               :        cp1300@139.com
*时间                     :        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 = SIIC_ReadByte(&pHandle->IIC_Handle, FALSE);        //SIIC读取一个字节-NAK
                }
                else
                {
                        DataBuff = SIIC_ReadByte(&pHandle->IIC_Handle, TRUE);        //SIIC读取一个字节-ACK
                }       
        }
        SIIC_Stop(&pHandle->IIC_Handle);                                                                        //产生IIC停止信号
}


使用特权

评论回复 赏
板凳
观海|  楼主 | 2021-8-2 12:45 | 只看该作者
/*************************************************************************************************************************
*函数                :        void LSM303DLH_WriteOneReg(LSM303DLH_HANDLE *pHandle,LSM303DLH_REG_TYPE RegAddr,u8 data)
*功能                :        LSM303DLH写一个寄存器
*参数                :        pHandle:句柄；RegAddr：寄存器地址；data：要写入的值
*返回                :        无
*依赖                        :         底层宏定义
*作者               :        cp1300@139.com
*时间                     :        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停止信号
}




使用特权

评论回复 赏
地板
观海|  楼主 | 2021-8-2 12:46 | 只看该作者

/*************************************************************************************************************************
*函数                :        bool LSM303DLH_ReadAcceleration(LSM303DLH_HANDLE *pHandle, s16 *pXa,s16 *pYa, s16 *pZa)
*功能                :        LSM303DLH 读取三轴加速度
*参数                :        pHandle:句柄；pXa：返回X轴加速度；pYa：返回Y轴加速度；pZa：返回Z轴加速度
*返回                :        TRUE:成功；FALSE:失败
*依赖                        :         底层宏定义
*作者               :        cp1300@139.com
*时间                     :        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;
}


使用特权

评论回复 赏
5楼
观海|  楼主 | 2021-8-2 12:47 | 只看该作者
/*************************************************************************************************************************
*函数                :        bool LSM303DLH_ReadMagnetic(LSM303DLH_HANDLE *pHandle, s16 *pXm,s16 *pYm, s16 *pZm)
*功能                :        LSM303DLH 读取磁场强度值
*参数                :        pHandle:句柄；pXm：返回X轴磁场强度；pYm：返回Y轴磁场强度；pZm：返回Z轴磁场强度
*返回                :        TRUE:成功；FALSE:失败
*依赖                        :         底层宏定义
*作者               :        cp1300@139.com
*时间                     :        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;
}



使用特权

评论回复 赏
6楼
观海|  楼主 | 2021-8-2 12:48 | 只看该作者
/*************************************************************************************************************************
*函数                :        int LSM303DLH_CalculationZAxisAngle(s16 Ax, s16 Ay, s16 Az)
*功能                :        LSM303DLH 计算Z轴倾角(扩大100倍)
*参数                :        Ax，Ay，Az:3个轴的加速度；
*返回                :        Z轴倾角
*依赖                        :         底层宏定义
*作者               :        cp1300@139.com
*时间                     :        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;
}


使用特权

评论回复 赏
7楼
观海|  楼主 | 2021-8-2 12:50 | 只看该作者
/*************************************************************************************************************************
*函数                :        int LSM303DLH_CalculationXAxisAngle(s16 Ax, s16 Ay, s16 Az)
*功能                :        LSM303DLH 计算X轴倾角(扩大100倍)
*参数                :        Ax，Ay，Az:3个轴的加速度；
*返回                :        Z轴倾角
*依赖                        :         底层宏定义
*作者               :        cp1300@139.com
*时间                     :        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;
}


使用特权

评论回复 赏
8楼
观海|  楼主 | 2021-8-2 12:51 | 只看该作者
//LSM303DLH.h

/*************************************************************************************************************
* 文件名:                        LSM303DLH.h
* 功能:                        LSM303DLH驱动
* 作者:                        cp1300@139.com
* 创建时间:                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_*/


使用特权

评论回复 赏
9楼
观海|  楼主 | 2021-8-2 12:52 | 只看该作者
//测试

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);




使用特权

评论回复 赏
10楼
观海|  楼主 | 2021-8-2 12:54 | 只看该作者
//硬件接口宏定义


/
//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
}


使用特权

评论回复 赏
返回列表
发新帖 我要提问
高级模式
B Color Image Link Quote Code 收费 Smilies
您需要登录后才可以回帖 登录 | 注册

本版积分规则

观海

99

主题

4103

帖子

1

粉丝