【超小型 Linux 开发套件Quantum Tiny Linux】基于MPU6050解算角度-水准仪Demo

qinyunti 发表于 2024-7-14 16:07

<div class='showpostmsg'>基于MPU6050数据解析出角度信息，实现水准仪的Demo

 

前面我们实现了温度，加速度，角速度的采集，现在我们基于加速度信息可以解算出角度信息。

 

温度换算

int mpu6050_itf_get_temp(float* val)

{

    int16_t regval;

    mpu6050_get_temp(&s_mpu6050_dev,&regval);

    *val = 36.53 + regval/340;

}

 

 

加速度换算

根据量程配置换算，我这里是设置加速度量程 2 ± 8g 4096 LSB/g

 

        mpu6050_itf_get_accel(accel);

        accel /= 4096;

        accel /= 4096;

        accel /= 4096;

 

 

角度换算，根据立体集合投影换算三轴的角度

        float th;

        th = atan2(sqrt(accel*accel+accel*accel),accel) * 180/PI;

        th = atan2(sqrt(accel*accel+accel*accel),accel) * 180/PI;

        th = atan2(sqrt(accel*accel+accel*accel),accel) * 180/PI;

        printf("Θ:%f,%f,%f\r\n",th,th,th);

 

 

 

完整的代码如下

Mpu6060.c

<pre>
<code class="language-cpp">#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 = ((val>>8)&0x00FF) | ((val<<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 = ((val>>8)&0x00FF) | ((val<<8)&0xFF00);
}
}

int mpu6050_get_temp(mpu6050_dev_st* dev,uint16_t* val)
{
dev->read(dev->dev_addr,0x41,(uint8_t*)val,2);
val = ((val>>8)&0x00FF) | ((val<<8)&0xFF00);
}
</code></pre>

Mpu6050.h

<pre>
<code class="language-cpp">#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
</code></pre>

Mpu6060_itf.c

 

<pre>
<code class="language-cpp">#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 dev_path 设备路径,比如"/dev/i2c-0"
* \return 返回打开的设备的句柄,和open返回值一样
*/
int i2c_init(char* dev_path)
{
return open(dev_path, O_RDWR);
}

/**
* \fn i2c_deinit
* 解除初始化,关闭设备
* \param fd 打开的设备句柄
* \return 和close返回值一样
*/
int i2c_deinit(int fd)
{
return close(fd);
}

/**
* \fn i2c_write
* 写数据
* \param fd 打开的设备句柄
* \param dev_addr I2C设备地址
* \param reg_addr 寄存器地址
* \param data 待写入数据
* \param 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 = reg_addr;
memcpy(&buff, 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 fd 打开的设备句柄
* \param dev_addr I2C设备地址
* \param reg_addr 寄存器地址
* \param data 存储读出数据
* \param 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 = {
 {
 .addr = dev_addr, /* 设备地址 */
 .flags = 0, /* 标志，为0表示写数据 */
 .len = 1, /* 要写的数据的长度 */
 .buf = &reg_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));
}

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,&regval);
*val = 36.53 + regval/340;
}

int main(void)
{
int16_t gyro;
int16_t accel;
float temp;
mpu6050_itf_init();

int i = 60;
while(i--)
{
 mpu6050_itf_get_gyro(gyro);
 gyro /= 16.4;
 gyro /= 16.4;
 gyro /= 16.4;

 //printf("gyro:%d,%d,%d\r\n",gyro,gyro,gyro);
 mpu6050_itf_get_accel(accel);
 accel /= 4096;
 accel /= 4096;
 accel /= 4096;
 //printf("accel:%d,%d,%d\r\n",accel,accel,accel);
 mpu6050_itf_get_temp(&temp);
 printf("temp:%f℃\r\n",temp);

 /* 加速度计算角度
 θx=α1180/π=180/π
 θy=β1180/π=180/π
 θz=γ1180/π=*180/π
 */
 float th;
 th = atan2(sqrt(accel*accel+accel*accel),accel) * 180/PI;
 th = atan2(sqrt(accel*accel+accel*accel),accel) * 180/PI;
 th = atan2(sqrt(accel*accel+accel*accel),accel) * 180/PI;
 printf("Θ:%f,%f,%f\r\n",th,th,th);
 sleep(1);
}
mpu6050_itf_deinit();
}</code></pre>

Mpu6050_itf.h

<pre>
<code class="language-cpp">#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
</code></pre>

rz导入到开发板

编译 gcc mpu6050.c mpu6050_itf.c -o mpu6050 -lm

运行./mpu6050

 

 

测试角度如下

  

  

 

实时获取到角度信息，即可以现水准仪等应用，可将数据传输到云端，如果超过范围实现告警，实现地震，塌方，位移等地质灾害得预警。

 
</div><script> var loginstr = '<div class="locked">查看本帖全部内容，请<a href="javascript:;" style="color:#e60000" class="loginf">登录</a>或者<a href="https://bbs.eeworld.com.cn/member.php?mod=register_eeworld.php&action=wechat" style="color:#e60000" target="_blank">注册</a></div>';

if(parseInt(discuz_uid)==0){
(function($){
var postHeight = getTextHeight(400);
$(".showpostmsg").html($(".showpostmsg").html());
$(".showpostmsg").after(loginstr);
$(".showpostmsg").css({height:postHeight,overflow:"hidden"});
})(jQuery);
} </script><script type="text/javascript">(function(d,c){var a=d.createElement("script"),m=d.getElementsByTagName("script"),eewurl="//counter.eeworld.com.cn/pv/count/";a.src=eewurl+c;m.parentNode.insertBefore(a,m)})(document,523)</script>

Jacktang 发表于 2024-7-15 07:31

获取到角度信息精度应该也是可以的吧

页: [1]

电子工程世界-论坛's Archiver

【超小型 Linux 开发套件Quantum Tiny Linux】基于MPU6050解算角度-水准仪Demo