基于MPU6050数据解析出角度信息,实现水准仪的Demo
前面我们实现了温度,加速度,角速度的采集,现在我们基于加速度信息可以解算出角度信息。
温度换算
int mpu6050_itf_get_temp(float* val)
{
int16_t regval;
mpu6050_get_temp(&s_mpu6050_dev,®val);
*val = 36.53 + regval/340;
}
加速度换算
根据量程配置换算,我这里是设置加速度量程 2 ± 8g 4096 LSB/g
mpu6050_itf_get_accel(accel);
accel[0] /= 4096;
accel[1] /= 4096;
accel[2] /= 4096;
角度换算,根据立体集合投影换算三轴的角度
float th[3];
th[0] = atan2(sqrt(accel[1]*accel[1]+accel[2]*accel[2]),accel[0]) * 180/PI;
th[1] = atan2(sqrt(accel[0]*accel[0]+accel[2]*accel[2]),accel[1]) * 180/PI;
th[2] = atan2(sqrt(accel[0]*accel[0]+accel[1]*accel[1]),accel[2]) * 180/PI;
printf("Θ:%f,%f,%f\r\n",th[0],th[1],th[2]);
完整的代码如下
Mpu6060.c
#include "mpu6050.h"
int mpu6050_init(mpu6050_dev_st* dev)
{
if(dev == (mpu6050_dev_st*)0)
{
return -1;
}
if(dev->init != (mpu6050_iic_init_pf)0)
{
dev->init();
}
}
int mpu6050_deinit(mpu6050_dev_st* dev)
{
if(dev == (mpu6050_dev_st*)0)
{
return -1;
}
if(dev->deinit != (mpu6050_iic_deinit_pf)0)
{
dev->deinit();
}
}
int mpu6050_cfg(mpu6050_dev_st* dev, mpu6050_cfg_st* cfg, uint32_t num)
{
if(dev == (mpu6050_dev_st*)0)
{
return -1;
}
if(cfg == (mpu6050_cfg_st*)0)
{
return -1;
}
for(uint32_t i=0; i<num; i++)
{
if(dev->write != (mpu6050_iic_write_pf)0)
{
dev->write(dev->dev_addr, cfg->reg, &(cfg->val), 1);
if(dev->delayms != (mpu6050_delayms_pf)0)
{
dev->delayms(cfg->delay_ms);
}
}
}
}
int mpu6050_get_gyro(mpu6050_dev_st* dev,uint16_t* val)
{
dev->read(dev->dev_addr,0x43,(uint8_t*)val,6);
for(int i=0; i<3; i++)
{
val[i] = ((val[i]>>8)&0x00FF) | ((val[i]<<8)&0xFF00);
}
}
int mpu6050_get_accel(mpu6050_dev_st* dev,uint16_t* val)
{
dev->read(dev->dev_addr,0x3B,(uint8_t*)val,6);
for(int i=0; i<3; i++)
{
val[i] = ((val[i]>>8)&0x00FF) | ((val[i]<<8)&0xFF00);
}
}
int mpu6050_get_temp(mpu6050_dev_st* dev,uint16_t* val)
{
dev->read(dev->dev_addr,0x41,(uint8_t*)val,2);
val[0] = ((val[0]>>8)&0x00FF) | ((val[0]<<8)&0xFF00);
}
Mpu6050.h
#ifndef MPU6050_H
#define MPU6050_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
typedef int (*mpu6050_iic_write_pf)(uint8_t dev_addr, uint8_t reg_addr, uint8_t* val, uint32_t len);
typedef int (*mpu6050_iic_read_pf)(uint8_t dev_addr, uint8_t reg_addr, uint8_t* val, uint32_t len);
typedef int (*mpu6050_iic_init_pf)(void);
typedef int (*mpu6050_iic_deinit_pf)(void);
typedef void (*mpu6050_delayms_pf)(uint32_t ms);
typedef struct{
uint8_t dev_addr;
mpu6050_iic_write_pf write;
mpu6050_iic_read_pf read;
mpu6050_iic_init_pf init;
mpu6050_iic_deinit_pf deinit;
mpu6050_delayms_pf delayms;
} mpu6050_dev_st;
typedef struct{
uint8_t reg;
uint8_t val;
uint32_t delay_ms;
} mpu6050_cfg_st;
int mpu6050_init(mpu6050_dev_st* dev);
int mpu6050_deinit(mpu6050_dev_st* dev);
int mpu6050_cfg(mpu6050_dev_st* dev, mpu6050_cfg_st* cfg, uint32_t num);
int mpu6050_get_gyro(mpu6050_dev_st* dev,uint16_t* val);
int mpu6050_get_accel(mpu6050_dev_st* dev,uint16_t* val);
int mpu6050_get_temp(mpu6050_dev_st* dev,uint16_t* val);
#ifdef __cplusplus
}
#endif
#endif
Mpu6060_itf.c
#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <string.h>
#include <stdlib.h>
//#include <linux/i2c.h>
#inlcude <math.h>
#include <linux/i2c-dev.h>
#include "mpu6050.h"
#define PI 3.141592653
/**
* \fn i2c_init
* 初始化,打开设备
* \param[in] dev_path 设备路径,比如"/dev/i2c-0"
* \return 返回打开的设备的句柄,和open返回值一样
*/
int i2c_init(char* dev_path)
{
return open(dev_path, O_RDWR);
}
/**
* \fn i2c_deinit
* 解除初始化,关闭设备
* \param[in] fd 打开的设备句柄
* \return 和close返回值一样
*/
int i2c_deinit(int fd)
{
return close(fd);
}
/**
* \fn i2c_write
* 写数据
* \param[in] fd 打开的设备句柄
* \param[in] dev_addr I2C设备地址
* \param[in] reg_addr 寄存器地址
* \param[in] data 待写入数据
* \param[in] len 待写入数据字节数
*
* \return 0成功 其他值失败
*/
int i2c_write(int fd, uint8_t dev_addr, uint8_t reg_addr, uint8_t* data, uint32_t len)
{
int ret = -1;
uint8_t* buff = malloc(len+1);
if(buff == NULL)
{
return -1;
}
buff[0] = reg_addr;
memcpy(&buff[1], data, len);
/* 构造msg */
struct i2c_msg msg = {
.addr = dev_addr,
.flags = 0,
.len = len + 1,
.buf = buff,
};
struct i2c_rdwr_ioctl_data rdwr_msg = {
.msgs = &msg,
.nmsgs = 1,
};
ret = ioctl(fd, I2C_RDWR, &rdwr_msg);
free(buff);
return ret;
}
/**
* \fn i2c_read
* 读数据
* \param[in] fd 打开的设备句柄
* \param[in] dev_addr I2C设备地址
* \param[in] reg_addr 寄存器地址
* \param[out] data 存储读出数据
* \param[in] len 待读出数据字节数
*
* \return 0成功 其他值失败
*/
int i2c_read(int fd, uint8_t dev_addr, uint8_t reg_addr, uint8_t* data, uint32_t len)
{
int ret = -1;
/* 构造msg */
struct i2c_msg msg[2] = {
{
.addr = dev_addr, /* 设备地址 */
.flags = 0, /* 标志,为0表示写数据 */
.len = 1, /* 要写的数据的长度 */
.buf = ®_addr, /* 要写的数据的地址 */
},
{
.addr = dev_addr, /* 设备地址 */
.flags = I2C_M_RD, /* 标志,I2C_M_RD表示主机向主机读数据 */
.len = len, /* 要读取的数据的长度 */
.buf = data, /* 读取的数据存放的地址 */
},
};
struct i2c_rdwr_ioctl_data rdwr_msg = {
.msgs = msg,
.nmsgs = 2,
};
ret = ioctl(fd, I2C_RDWR, &rdwr_msg);
return ret;
}
int s_fd = -1;
int mpu6050_iic_write_port(uint8_t dev_addr, uint8_t reg_addr, uint8_t* val, uint32_t len)
{
i2c_write(s_fd, dev_addr, reg_addr, val, len);
}
int mpu6050_iic_read_port(uint8_t dev_addr, uint8_t reg_addr, uint8_t* val, uint32_t len)
{
i2c_read(s_fd, dev_addr, reg_addr, val, len);
}
int mpu6050_iic_init_port(void)
{
s_fd = i2c_init("/dev/i2c-0");
}
int mpu6050_iic_deinit_port(void)
{
i2c_deinit(s_fd);
}
void mpu6050_delayms_port(uint32_t ms)
{
usleep(ms*1000);
}
mpu6050_dev_st s_mpu6050_dev =
{
.dev_addr = 0x68,
.write = mpu6050_iic_write_port,
.read = mpu6050_iic_read_port,
.init = mpu6050_iic_init_port,
.deinit = mpu6050_iic_deinit_port,
.delayms = mpu6050_delayms_port,
};
mpu6050_cfg_st s_mpu6050_cfg[] =
{
{0x6B,0x01,100}, /* 退出SLEEP,不复位,使能温度传感器,不使用cycle采样模式,使用X轴PLL */
/* Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
where Gyroscope Output Rate = 8kHz when the DLPF is disabled (DLPF_CFG = 0 or 7),
and 1kHz when the DLPF is enabled (see Register 26). */
{0x19,0x04,0}, /* 设置数据输出速率 */
{0x1A,0x02,0}, /* 设置采样率 94 98 */
{0x1B,3<<3,0}, /* 设置角速度为最大量程 3 ± 2000 °/s 16.4 LSB/°/s */
{0x1C,2<<3,0}, /* 设置加速度量程 2 ± 8g 4096 LSB/g */
{0x37,0x32,0}, /* INT Pin / Bypass 配置 */
{0x38,0x00,0}, /* 中断使能 */
{0x6A,0x00,0}, /* 不使能FIFO */
};
int mpu6050_itf_init(void)
{
mpu6050_init(&s_mpu6050_dev);
mpu6050_cfg(&s_mpu6050_dev, s_mpu6050_cfg, sizeof(s_mpu6050_cfg)/sizeof(s_mpu6050_cfg[0]));
}
int mpu6050_itf_deinit(void)
{
mpu6050_deinit(&s_mpu6050_dev);
}
int mpu6050_itf_get_gyro(int16_t* val)
{
mpu6050_get_gyro(&s_mpu6050_dev,val);
}
int mpu6050_itf_get_accel(int16_t* val)
{
mpu6050_get_accel(&s_mpu6050_dev,val);
}
int mpu6050_itf_get_temp(float* val)
{
int16_t regval;
mpu6050_get_temp(&s_mpu6050_dev,®val);
*val = 36.53 + regval/340;
}
int main(void)
{
int16_t gyro[3];
int16_t accel[3];
float temp;
mpu6050_itf_init();
int i = 60;
while(i--)
{
mpu6050_itf_get_gyro(gyro);
gyro[0] /= 16.4;
gyro[1] /= 16.4;
gyro[2] /= 16.4;
//printf("gyro:%d,%d,%d\r\n",gyro[0],gyro[1],gyro[2]);
mpu6050_itf_get_accel(accel);
accel[0] /= 4096;
accel[1] /= 4096;
accel[2] /= 4096;
//printf("accel:%d,%d,%d\r\n",accel[0],accel[1],accel[2]);
mpu6050_itf_get_temp(&temp);
printf("temp:%f℃\r\n",temp);
/* 加速度计算角度
θx=α1180/π=[arctan(Ax/squr(AyAy+AzAz))]180/π
θy=β1180/π=[arctan(Ay/squr(AxAx+AzAz))]180/π
θz=γ1180/π=[arctan(Az/squr(AxAx+AyAy))]*180/π
*/
float th[3];
th[0] = atan2(sqrt(accel[1]*accel[1]+accel[2]*accel[2]),accel[0]) * 180/PI;
th[1] = atan2(sqrt(accel[0]*accel[0]+accel[2]*accel[2]),accel[1]) * 180/PI;
th[2] = atan2(sqrt(accel[0]*accel[0]+accel[1]*accel[1]),accel[2]) * 180/PI;
printf("Θ:%f,%f,%f\r\n",th[0],th[1],th[2]);
sleep(1);
}
mpu6050_itf_deinit();
}
Mpu6050_itf.h
#ifndef MPU6050_ITF_H
#define MPU6050_ITF_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
int mpu6050_itf_init(void);
int mpu6050_itf_deinit(void);
int mpu6050_itf_get_gyro(int16_t* val);
int mpu6050_itf_get_accel(int16_t* val);
int mpu6050_itf_get_temp(float* val);
#ifdef __cplusplus
}
#endif
#endif
rz导入到开发板
编译 gcc mpu6050.c mpu6050_itf.c -o mpu6050 -lm
运行./mpu6050
测试角度如下
实时获取到角度信息,即可以现水准仪等应用,可将数据传输到云端,如果超过范围实现告警,实现地震,塌方,位移等地质灾害得预警。