Parse the angle information based on MPU6050 data and implement the level meter demo
Previously, we realized the collection of temperature, acceleration, and angular velocity. Now we can calculate the angle information based on the acceleration information.
Temperature conversion
int mpu6050_itf_get_temp(float* val)
{
int16_t regval;
mpu6050_get_temp(&s_mpu6050_dev,val);
*val = 36.53 + regval/340;
}
Acceleration conversion
According to the range configuration conversion, I set the accelerometer range to 2 ± 8g 4096 LSB/g
mpu6050_itf_get_accel(accel);
accel[0] /= 4096;
accel[1] /= 4096;
accel[2] /= 4096;
Angle conversion, convert the angles of the three axes according to the stereo set projection
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]);
The complete code is as follows
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 imported into the development board
Compile gcc mpu6050.c mpu6050_itf.c -o mpu6050 -lm
Run ./mpu6050
The test angles are as follows
By obtaining angle information in real time, applications such as levels can be implemented, and data can be transmitted to the cloud. If the range is exceeded, an alarm will be issued to provide early warning of geological disasters such as earthquakes, landslides, and displacements.
提升卡
变色卡
千斤顶
京公网安备 11010802033920号
