求三轴加速度求倾角公式,用的是ADXL345 已经得到X Y Z值了(附程序)

2020-01-18 18:46发布

求三轴加速度求倾角公式,用的是ADXL345 已经得到X Y Z值了(附程序)  51模拟IIC
1 2 轴还可以想想通  3轴的就想不同了(立体感太差)
#include  <REG51.H>       
#include  <math.h>    //Keil library  
#include  <stdio.h>   //Keil library       
#include  <INTRINS.H>
#define   uchar unsigned char
#define   uint unsigned int       
#define   DataPort P0    //LCD1602数据端口
sbit          SCL=P1^0;      //IIC时钟引脚定义
sbit           SDA=P1^1;      //IIC数据引脚定义
sbit      LCM_RS=P2^0;   //LCD1602命令端口               
sbit      LCM_RW=P2^1;   //LCD1602命令端口               
sbit      LCM_EN=P2^2;   //LCD1602命令端口

#define        SlaveAddress   0xA6          //定义器件在IIC总线中的从地址,根据ALT  ADDRESS地址引脚不同修改
                              //ALT  ADDRESS引脚接地时地址为0xA6,接电源时地址为0x3A
typedef unsigned char  BYTE;
typedef unsigned short WORD;

BYTE BUF[8];                         //接收数据缓存区             
uchar ge,shi,bai,qian,wan;           //显示变量
int  dis_data;                       //变量

void delay(unsigned int k);
void InitLcd();                      //初始化lcd1602
void Init_ADXL345(void);             //初始化ADXL345

void WriteDataLCM(uchar dataW);
void WriteCommandLCM(uchar CMD,uchar Attribc);
void DisplayOneChar(uchar X,uchar Y,uchar DData);
void conversion(uint temp_data);

void  Single_Write_ADXL345(uchar REG_Address,uchar REG_data);   //单个写入数据
uchar Single_Read_ADXL345(uchar REG_Address);                   //单个读取内部寄存器数据
void  Multiple_Read_ADXL345();                                  //连续的读取内部寄存器数据
//------------------------------------
void Delay5us();
void Delay5ms();
void ADXL345_Start();
void ADXL345_Stop();
void ADXL345_SendACK(bit ack);
bit  ADXL345_RecvACK();
void ADXL345_SendByte(BYTE dat);
BYTE ADXL345_RecvByte();
void ADXL345_ReadPage();
void ADXL345_WritePage();
//-----------------------------------

//*********************************************************
void conversion(uint temp_data)  
{  
    wan=temp_data/10000+0x30 ;
    temp_data=temp_data%10000;   //取余运算
        qian=temp_data/1000+0x30 ;
    temp_data=temp_data%1000;    //取余运算
    bai=temp_data/100+0x30   ;
    temp_data=temp_data%100;     //取余运算
    shi=temp_data/10+0x30    ;
    temp_data=temp_data%10;      //取余运算
    ge=temp_data+0x30;        
}

/*******************************/
void delay(unsigned int k)       
{                                               
unsigned int i,j;                               
for(i=0;i<k;i++)
{                       
for(j=0;j<121;j++)                       
{;}}                                               
}
/*******************************/
void WaitForEnable(void)       
{                                       
DataPort=0xff;               
LCM_RS=0;LCM_RW=1;_nop_();
LCM_EN=1;_nop_();_nop_();
while(DataPort&0x80);       
LCM_EN=0;                               
}                                       
/*******************************/
void WriteCommandLCM(uchar CMD,uchar Attribc)
{                                       
if(Attribc)WaitForEnable();       
LCM_RS=0;LCM_RW=0;_nop_();
DataPort=CMD;_nop_();       
LCM_EN=1;_nop_();_nop_();LCM_EN=0;
}                                       
/*******************************/
void WriteDataLCM(uchar dataW)
{                                       
WaitForEnable();               
LCM_RS=1;LCM_RW=0;_nop_();
DataPort=dataW;_nop_();       
LCM_EN=1;_nop_();_nop_();LCM_EN=0;
}               
/***********************************/
void InitLcd()                               
{                       
WriteCommandLCM(0x38,1);       
WriteCommandLCM(0x08,1);       
WriteCommandLCM(0x01,1);       
WriteCommandLCM(0x06,1);       
WriteCommandLCM(0x0c,1);
}                       
/***********************************/
void DisplayOneChar(uchar X,uchar Y,uchar DData)
{                                               
Y&=1;                                               
X&=15;                                               
if(Y)X|=0x40;                                       
X|=0x80;                       
WriteCommandLCM(X,0);               
WriteDataLCM(DData);               
}                                               

/**************************************
延时5微秒(STC90C52RC@12M)
不同的工作环境,需要调整此函数,注意时钟过快时需要修改
当改用1T的MCU时,请调整此延时函数
**************************************/
void Delay5us()
{
    _nop_();_nop_();_nop_();_nop_();
    _nop_();_nop_();_nop_();_nop_();
        _nop_();_nop_();_nop_();_nop_();
}

/**************************************
延时5毫秒(STC90C52RC@12M)
不同的工作环境,需要调整此函数
当改用1T的MCU时,请调整此延时函数
**************************************/
void Delay5ms()
{
    WORD n = 560;

    while (n--);
}

/**************************************
起始信号
**************************************/
void ADXL345_Start()
{
    SDA = 1;                    //拉高数据线
    SCL = 1;                    //拉高时钟线
    Delay5us();                 //延时
    SDA = 0;                    //产生下降沿
    Delay5us();                 //延时
    SCL = 0;                    //拉低时钟线
}

/**************************************
停止信号
**************************************/
void ADXL345_Stop()
{
    SDA = 0;                    //拉低数据线
    SCL = 1;                    //拉高时钟线
    Delay5us();                 //延时
    SDA = 1;                    //产生上升沿
    Delay5us();                 //延时
}

/**************************************
发送应答信号
入口参数:ack (0:ACK 1:NAK)
**************************************/
void ADXL345_SendACK(bit ack)
{
    SDA = ack;                  //写应答信号
    SCL = 1;                    //拉高时钟线
    Delay5us();                 //延时
    SCL = 0;                    //拉低时钟线
    Delay5us();                 //延时
}

/**************************************
接收应答信号
**************************************/
bit ADXL345_RecvACK()
{
    SCL = 1;                    //拉高时钟线
    Delay5us();                 //延时
    CY = SDA;                   //读应答信号
    SCL = 0;                    //拉低时钟线
    Delay5us();                 //延时

    return CY;
}

/**************************************
向IIC总线发送一个字节数据
**************************************/
void ADXL345_SendByte(BYTE dat)
{
    BYTE i;

    for (i=0; i<8; i++)         //8位计数器
    {
        dat <<= 1;              //移出数据的最高位
        SDA = CY;               //送数据口
        SCL = 1;                //拉高时钟线
        Delay5us();             //延时
        SCL = 0;                //拉低时钟线
        Delay5us();             //延时
    }
    ADXL345_RecvACK();
}

/**************************************
从IIC总线接收一个字节数据
**************************************/
BYTE ADXL345_RecvByte()
{
    BYTE i;
    BYTE dat = 0;

    SDA = 1;                    //使能内部上拉,准备读取数据,
    for (i=0; i<8; i++)         //8位计数器
    {
        dat <<= 1;
        SCL = 1;                //拉高时钟线
        Delay5us();             //延时
        dat |= SDA;             //读数据               
        SCL = 0;                //拉低时钟线
        Delay5us();             //延时
    }
    return dat;
}

//******单字节写入*******************************************

void Single_Write_ADXL345(uchar REG_Address,uchar REG_data)
{
    ADXL345_Start();                  //起始信号
    ADXL345_SendByte(SlaveAddress);   //发送设备地址+写信号
    ADXL345_SendByte(REG_Address);    //内部寄存器地址,请参考中文pdf22页
    ADXL345_SendByte(REG_data);       //内部寄存器数据,请参考中文pdf22页
    ADXL345_Stop();                   //发送停止信号
}

//********单字节读取*****************************************
uchar Single_Read_ADXL345(uchar REG_Address)
{  uchar REG_data;
    ADXL345_Start();                          //起始信号
    ADXL345_SendByte(SlaveAddress);           //发送设备地址+写信号
    ADXL345_SendByte(REG_Address);                   //发送存储单元地址,从0开始       
    ADXL345_Start();                          //起始信号
    ADXL345_SendByte(SlaveAddress+1);         //发送设备地址+读信号
    REG_data=ADXL345_RecvByte();              //读出寄存器数据
        ADXL345_SendACK(1);   
        ADXL345_Stop();                           //停止信号
    return REG_data;
}
//*********************************************************
//
//连续读出ADXL345内部加速度数据,地址范围0x32~0x37
//
//*********************************************************
void Multiple_read_ADXL345(void)
{   uchar i;
    ADXL345_Start();                          //起始信号
    ADXL345_SendByte(SlaveAddress);           //发送设备地址+写信号
    ADXL345_SendByte(0x32);                   //发送存储单元地址,从0x32开始       
    ADXL345_Start();                          //起始信号
    ADXL345_SendByte(SlaveAddress+1);         //发送设备地址+读信号
         for (i=0; i<6; i++)                      //连续读取6个地址数据,存储中BUF
    {
        BUF = ADXL345_RecvByte();          //BUF[0]存储0x32地址中的数据
        if (i == 5)
        {
           ADXL345_SendACK(1);                //最后一个数据需要回NOACK
        }
        else
        {
          ADXL345_SendACK(0);                //回应ACK
       }
   }
    ADXL345_Stop();                          //停止信号
    Delay5ms();
}


//*****************************************************************

//初始化ADXL345,根据需要请参考pdf进行修改************************
void Init_ADXL345()
{
   Single_Write_ADXL345(0x31,0x0B);   //测量范围,正负16g,13位模式
   Single_Write_ADXL345(0x2C,0x08);   //速率设定为12.5 参考pdf13页
   Single_Write_ADXL345(0x2D,0x08);   //选择电源模式   参考pdf24页
   Single_Write_ADXL345(0x2E,0x80);   //使能 DATA_READY 中断
   Single_Write_ADXL345(0x1E,0x00);   //X 偏移量 根据测试传感器的状态写入pdf29页
   Single_Write_ADXL345(0x1F,0x00);   //Y 偏移量 根据测试传感器的状态写入pdf29页
   Single_Write_ADXL345(0x20,0x05);   //Z 偏移量 根据测试传感器的状态写入pdf29页
}
//***********************************************************************
//显示x轴
void display_x()
{   float temp;
    dis_data=(BUF[1]<<8)+BUF[0];  //合成数据   
        if(dis_data<0){
        dis_data=-dis_data;
    DisplayOneChar(2,0,'-');      //显示正负符号位
        }
        else DisplayOneChar(2,0,' '); //显示空格

    temp=(float)dis_data*3.9;  //计算数据和显示,查考ADXL345快速入门第4页
    conversion(temp);          //转换出显示需要的数据
        DisplayOneChar(0,0,'X');   //第0行,第0列 显示X
    DisplayOneChar(1,0,':');
    DisplayOneChar(3,0,qian);
        DisplayOneChar(4,0,'.');
    DisplayOneChar(5,0,bai);
    DisplayOneChar(6,0,shi);
        DisplayOneChar(7,0,'g');
}

//***********************************************************************
//显示y轴
void display_y()
{     float temp;
    dis_data=(BUF[3]<<8)+BUF[2];  //合成数据   
        if(dis_data<0){
        dis_data=-dis_data;
    DisplayOneChar(2,1,'-');      //显示正负符号位
        }
        else DisplayOneChar(2,1,' '); //显示空格

    temp=(float)dis_data*3.9;  //计算数据和显示,查考ADXL345快速入门第4页
    conversion(temp);          //转换出显示需要的数据
        DisplayOneChar(0,1,'Y');   //第1行,第0列 显示y
    DisplayOneChar(1,1,':');
    DisplayOneChar(3,1,qian);
        DisplayOneChar(4,1,'.');
    DisplayOneChar(5,1,bai);
    DisplayOneChar(6,1,shi);  
        DisplayOneChar(7,1,'g');  
}

//***********************************************************************
//显示z轴
void display_z()
{      float temp;
    dis_data=(BUF[5]<<8)+BUF[4];    //合成数据   
        if(dis_data<0){
        dis_data=-dis_data;
    DisplayOneChar(10,1,'-');       //显示负符号位
        }
        else DisplayOneChar(10,1,' ');  //显示空格

    temp=(float)dis_data*3.9;  //计算数据和显示,查考ADXL345快速入门第4页
    conversion(temp);          //转换出显示需要的数据
        DisplayOneChar(10,0,'Z');  //第0行,第10列 显示Z
    DisplayOneChar(11,0,':');
    DisplayOneChar(11,1,qian);
        DisplayOneChar(12,1,'.');
    DisplayOneChar(13,1,bai);
    DisplayOneChar(14,1,shi);
        DisplayOneChar(15,1,'g');  
}


//*********************************************************
//******主程序********
//*********************************************************
void main()
{
  uchar devid;
  delay(500);                           //上电延时               
  InitLcd();                      //液晶初始化ADXL345
  Init_ADXL345();                 //初始化ADXL345
  devid=Single_Read_ADXL345(0X00);//读出的数据为0XE5,表示正确
  while(1)                         //循环
  {
    Multiple_Read_ADXL345();       //连续读出数据,存储在BUF中
    display_x();                   //---------显示X轴
    display_y();                   //---------显示Y轴
    display_z();                   //---------显示Z轴
    delay(200);                    //延时            
  }
}
友情提示: 此问题已得到解决,问题已经关闭,关闭后问题禁止继续编辑,回答。
73条回答
蕾蕾
2020-01-25 15:19
41538006 发表于 2012-2-15 21:28
我用三维立体设计软件测试过公式,全部ok!
但是过程有点深,说起来太麻烦了! ...

您好,我现在正在做电子罗盘,使用磁阻传感器HMC5883L和加计ADXL345,现在我要结合他们两个的数据,也就是我的结合公式需要用到俯仰角和翻滚角的cos和sin值,我的加计的X和Y轴都不会说和水平面平行,那我应该怎么算pitch和roll?

一周热门 更多>