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

qinyunti

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

基于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[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

测试角度如下