当前位置：首页 > news >正文

linux 下调试 mpu6050 三轴加速度

news 2024/11/13 21:59:49

供自己备忘；

1. 参考资料：

b 站视频

https://www.bilibili.com/video/BV1cL4y1x7FA/?spm_id_from=333.337.search-card.all.click&vd_source=d7a07b8689c9e646f0214227c06f304c

csdn 其它博客

https://blog.csdn.net/qq_65198598/article/details/138805034

mpu6050 校准算法

2. 简单调试

需求是通过 mpu6050 获取三轴加速度的值；每秒读取 100 次；

通过官方自带的驱动，读取 iio 很慢，而且还使用了中断脚；看大家都自己写 mpu6050 的数据读取驱动，我也写一个。

2.1 dts 配置如下

0x68 是 7 位 i2c 地址，对应芯片 AD0 脚是接地的；

2.2 Makefile 文件如下

内核路径修改为自己的，交叉编译器也是；前提是内核编译过，配置好交叉编译器。

KERN_DIR = /opt/liangtao/sigmastar/IKAYAKI_DLM00V017_SSD222D/kernelall:make -C $(KERN_DIR) M=`pwd` modulesarm-linux-gnueabihf-gcc mpu6050_read.c -o mpu6050_read -lm#cp i2c_mpu6050.ko mpu6050_read /opt/liangtao/output/clean:make -C $(KERN_DIR) M=`pwd` modules cleanobj-m += i2c_mpu6050.o

2.3 驱动代码修改如下

直接拷贝正点原子的代码改的；

正点原子教程

修改的点为：

移除了陀螺仪数据的读取，因为项目不需要
使用定时器加工作队列来执行三轴加速度的数据读取，想减少应用读取消耗的时间（修改了内核的 HZ 为 1000，因为 100 的情况下，定时精度低）
使用 i2c 的连续数据读取功能
参考网上的初始 mpu6050 的方法，重新初始化 mpu6050
1. 复位 mpu6050，并延时 100ms
2. 关闭休眠，使用 X 轴陀螺作为时钟参考
3. 将加速度采样配置为 200Hz
4. 设置数字低通滤波器，带宽为采样率的一半
5. 设置陀螺仪量程
6. 设置加速度量程
7. 关闭所有中断
8. 关闭 i2c 主模式
9. 关闭 FIFO
因为 mpu6050 的数据格式为大端格式，「低地址放高字节」，所以在驱动调了顺序。

#include <linux/init.h>
#include <linux/module.h>
#include <linux/fs.h>
#include <linux/cdev.h>
#include <linux/uaccess.h>
#include <linux/i2c.h>
#include <linux/types.h>
#include <linux/kernel.h>
#include <linux/delay.h>
#include <linux/ide.h>
#include <linux/errno.h>
#include <linux/gpio.h>
#include <asm/mach/map.h>
#include <linux/of.h>
#include <linux/of_address.h>
#include <linux/of_gpio.h>
#include <asm/io.h>
#include <linux/device.h>
#include <linux/platform_device.h>
#include <linux/workqueue.h>// 宏定义
#define SMPLRT_DIV      0x19
#define CONFIG          0x1A
#define GYRO_CONFIG     0x1B
#define ACCEL_CONFIG    0x1C
#define FIFO_EN         0x23
#define INT_ENABLE      0x38
#define ACCEL_XOUT_H    0x3B
#define ACCEL_XOUT_L    0x3C
#define ACCEL_YOUT_H    0x3D
#define ACCEL_YOUT_L    0x3E
#define ACCEL_ZOUT_H    0x3F
#define ACCEL_ZOUT_L    0x40
#define TEMP_OUT_H      0x41
#define TEMP_OUT_L      0x42
#define GYRO_XOUT_H     0x43
#define GYRO_XOUT_L     0x44
#define GYRO_YOUT_H     0x45
#define GYRO_YOUT_L     0x46
#define GYRO_ZOUT_H     0x47
#define GYRO_ZOUT_L     0x48
#define USER_CTRL       0x6A
#define PWR_MGMT_1      0x6B
#define WHO_AM_I        0x75
#define SlaveAddress    0xD0
#define Address         0x68         // MPU6050 地址
#define I2C_RETRIES     0x0701
#define I2C_TIMEOUT     0x0702
#define I2C_SLAVE       0x0703       // IIC 从器件的地址设置
#define I2C_BUS_MODE    0x0780#define DEV_NAME "i2c_mpu6050"
#define DEV_CNT (1)static dev_t mpu6050_devno;              // 定义字符设备的设备号
static struct cdev mpu6050_chr_dev;      // 定义字符设备结构体 chr_dev
struct class *class_mpu6050;             // 保存创建的类
struct device *device_mpu6050;           // 保存创建的设备
struct device_node *mpu6050_device_node; // mpu6050 的设备树节点结构体struct i2c_client *mpu6050_client = NULL; // 保存 mpu6050 设备对应的 i2c_client 结构体，匹配成功后由 .prob 函数带回。// 定时器，定时读取 mpu6050 的数据
static struct timer_list t;
static char mpu6050_result[6];// 工作队列，用于执行耗时的 i2c 操作
static struct workqueue_struct *mpu6050_wq;
static struct work_struct mpu6050_work;// 一个互斥锁，对读取数据进行保护
static DEFINE_MUTEX(mpu6050_mutex);/* 通过 i2c 向 mpu6050 写入数据* mpu6050_client：mpu6050 的 i2c_client 结构体。* address, 数据要写入的地址，* data, 要写入的数据* 返回值，错误，-1。成功，0  */
static int i2c_write_mpu6050(struct i2c_client *mpu6050_client, u8 address, u8 data)
{int error = 0;u8 write_data[2];struct i2c_msg send_msg; // 要发送的数据结构体/* 设置要发送的数据 */write_data[0] = address;write_data[1] = data;/* 发送 iic 要写入的地址 reg */send_msg.addr = mpu6050_client->addr; // mpu6050 在 iic 总线上的地址send_msg.flags = 0;                   // 标记为发送数据send_msg.buf = write_data;            // 写入的首地址send_msg.len = 2;                     // reg 长度/* 执行发送 */error = i2c_transfer(mpu6050_client->adapter, &send_msg, 1);if (error != 1) {printk("i2c_write_mpu6050 error %d\n", error);return -1;}return 0;
}/* 通过 i2c 向 mpu6050 写入数据* mpu6050_client：mpu6050 的 i2c_client 结构体。* address, 要读取的地址，* data，保存读取得到的数据* length，读长度* 返回值，错误，-1。成功，0*/
static int i2c_read_mpu6050(struct i2c_client *mpu6050_client, u8 address, void *data, u32 length)
{int error = 0;u8 address_data = address;struct i2c_msg mpu6050_msg[2];/* 设置读取位置 msg */mpu6050_msg[0].addr = mpu6050_client->addr; // mpu6050 在 iic 总线上的地址mpu6050_msg[0].flags = 0;                   // 标记为发送数据mpu6050_msg[0].buf = &address_data;         // 写入的首地址mpu6050_msg[0].len = 1;                     // 写入长度/* 设置读取位置 msg */mpu6050_msg[1].addr = mpu6050_client->addr; // mpu6050 在 iic 总线上的地址mpu6050_msg[1].flags = I2C_M_RD;            // 标记为读取数据mpu6050_msg[1].buf = data;                  // 读取得到的数据保存位置mpu6050_msg[1].len = length;                // 读取长度error = i2c_transfer(mpu6050_client->adapter, mpu6050_msg, 2);if (error != 2) {printk("i2c_read_mpu6050 error %d\n", error);return -1;}return 0;
}static void mpu6050_work_func(struct work_struct *work)
{char buf[6];i2c_read_mpu6050(mpu6050_client, ACCEL_XOUT_H, &buf[0], 6);mutex_lock(&mpu6050_mutex);mpu6050_result[1] = buf[0];mpu6050_result[0] = buf[1];mpu6050_result[3] = buf[2];mpu6050_result[2] = buf[3];mpu6050_result[5] = buf[4];mpu6050_result[4] = buf[5];mutex_unlock(&mpu6050_mutex);
}static void mpu6050_timer_func(unsigned long data)
{mod_timer(&t, jiffies + msecs_to_jiffies(9));// 只在任务未在队列中时才添加新任务if (!work_pending(&mpu6050_work)) {queue_work(mpu6050_wq, &mpu6050_work);}
}/* 初始化 mpu6050* 返回值，成功，返回 0。失败，返回 -1*/
static int mpu6050_init(void)
{int err = 0;/* 配置 mpu6050 */// 设置 PWR_MGMT_1 寄存器的第 7 位，触发复位err += i2c_write_mpu6050(mpu6050_client, PWR_MGMT_1, 0x80);// 等待复位完成msleep(100);// 设置 关闭休眠，不使用循环模式，使能温度传感器，使用 X 轴陀螺作为时钟参考（手册推荐）err += i2c_write_mpu6050(mpu6050_client, PWR_MGMT_1, 0X01);// 计划每秒读取 100 次加速度的数据，将加速度配置为 200HZ// 采样频率 = 陀螺仪输出频率 / (1+SMPLRT_DIV) => SMPLRT_DIV = 4err += i2c_write_mpu6050(mpu6050_client, SMPLRT_DIV, 0X04);// 设置数字低通滤波器，带宽为采样频率的一半，就是 100，查表值为 94Hz; 配置 DLPF_CFG = 2err += i2c_write_mpu6050(mpu6050_client, CONFIG, 0X02);// 设置陀螺仪量程为 +- 2000err += i2c_write_mpu6050(mpu6050_client, GYRO_CONFIG, 0X18);// 设置加速度量程为 +- 2Gerr += i2c_write_mpu6050(mpu6050_client, ACCEL_CONFIG, 0X00);// 关闭所有中断err += i2c_write_mpu6050(mpu6050_client, INT_ENABLE, 0X00);// 关闭 i2c 主模式err += i2c_write_mpu6050(mpu6050_client, USER_CTRL, 0X00);// 关闭 FIFOerr += i2c_write_mpu6050(mpu6050_client, FIFO_EN, 0X00);if (err < 0) {printk("mpu6050_init error\n");return -1;}return 0;
}static int mpu6050_open(struct inode *inode, struct file *filp)
{mod_timer(&t, jiffies + msecs_to_jiffies(1));return 0;
}static ssize_t mpu6050_read(struct file *filp, char __user *buf, size_t cnt, loff_t *off)
{int error;char buffer[6]; // 保存mpu6050转换得到的原始数据mutex_lock(&mpu6050_mutex);memcpy(buffer, mpu6050_result, 6);mutex_unlock(&mpu6050_mutex);error = copy_to_user(buf, buffer, sizeof(buffer));if (error != 0) {printk("copy_to_user error!");return -1;}return sizeof(buffer);
}static int mpu6050_release(struct inode *inode, struct file *filp)
{return 0;
}static struct file_operations mpu6050_chr_dev_fops =
{.owner = THIS_MODULE,.open = mpu6050_open,.read = mpu6050_read,.release = mpu6050_release,
};static int mpu6050_probe(struct i2c_client *client, const struct i2c_device_id *id)
{int ret;printk("mpu6050_probe, HZ = %d\n", HZ);mpu6050_client = client;/* 初始化 mpu6050 */ret = mpu6050_init();if (ret < 0) {printk("fail to init mpu6050\n");goto alloc_err;}// 采用动态分配的方式，获取设备编号，次设备号为 0，// 设备名称为 i2c_mpu6050，可通过命令 cat /proc/devices 查看// DEV_CNT 为 1，当前只申请一个设备编号ret = alloc_chrdev_region(&mpu6050_devno, 0, DEV_CNT, DEV_NAME);if (ret < 0) {printk("fail to alloc mpu6050_devno\n");goto alloc_err;}// 关联字符设备结构体 cdev 与文件操作结构体 file_operationsmpu6050_chr_dev.owner = THIS_MODULE;cdev_init(&mpu6050_chr_dev, &mpu6050_chr_dev_fops);// 添加设备至 cdev_map 散列表中ret = cdev_add(&mpu6050_chr_dev, mpu6050_devno, DEV_CNT);if (ret < 0) {printk("fail to add cdev\n");goto add_err;}/* 创建类 */class_mpu6050 = class_create(THIS_MODULE, DEV_NAME);/* 创建设备 DEV_NAME 指定设备名 */device_mpu6050 = device_create(class_mpu6050, NULL, mpu6050_devno, NULL, DEV_NAME);/* 初始化工作队列 */mpu6050_wq = create_singlethread_workqueue("mpu6050_wq");INIT_WORK(&mpu6050_work, mpu6050_work_func);/* 创建一个定时器，开始以 100hz 的频率来采样 */setup_timer(&t, mpu6050_timer_func, 0);return 0;add_err:// 添加设备失败时，需要注销设备号unregister_chrdev_region(mpu6050_devno, DEV_CNT);printk("mpu6050_probe error! \n");
alloc_err:return -1;
}static int mpu6050_remove(struct i2c_client *client)
{device_destroy(class_mpu6050, mpu6050_devno);       // 清除设备class_destroy(class_mpu6050);                       // 清除类cdev_del(&mpu6050_chr_dev);                         // 清除设备号unregister_chrdev_region(mpu6050_devno, DEV_CNT);   // 取消注册字符设备del_timer_sync(&t);                                 // 删除定时器flush_workqueue(mpu6050_wq);                        // 确保所有任务完成destroy_workqueue(mpu6050_wq);                      // 销毁工作队列return 0;
}static const struct i2c_device_id mpu6050_device_id[] = {{"invensense,mpu6050", 0},{/* sentinel */}
};static const struct of_device_id mpu6050_of_match_table[] = {{.compatible = "invensense,mpu6050"},{/* sentinel */}
};struct i2c_driver mpu6050_driver = {.probe = mpu6050_probe,.remove = mpu6050_remove,.id_table = mpu6050_device_id,.driver = {.name = "invensense,mpu6050",.owner = THIS_MODULE,.of_match_table = mpu6050_of_match_table,},
};static int __init mpu6050_driver_init(void)
{int ret;printk("mpu6050_driver_init\n");ret = i2c_add_driver(&mpu6050_driver);return ret;
}static void __exit mpu6050_driver_exit(void)
{printk("mpu6050_driver_exit\n");i2c_del_driver(&mpu6050_driver);
}module_init(mpu6050_driver_init);
module_exit(mpu6050_driver_exit);MODULE_LICENSE("GPL");

2.4 应用计算加速度值

网上的资料都说 mpu6050 的 1g 加速度为 9.81m/s²

因为配置的范围是 +- 2G，寄存器值为 2 字节；所以单 g 范围为 65536 / 4 = 16384

采样值 / 16384 = 实际每个轴的加速度，单位为 g

mpu6050 在静止时，一般满足以下公式

AccX² + AccY² + AccZ² = G²

初步调试发现加速度值偏的比较多。网上找了方法来校准。

2.5 校准三轴加速度值

校准公式如下：

使用如下 MATLAB 代码来计算校准值：

clear;clc;
axm=[0.040283   -0.000732   0.027100  -0.971558   1.030396    0.039429];
aym=[-0.006592  0.010254    0.997559  -0.008301   0.003540   -1.013672];
azm=[0.932739   -1.081665  -0.065552  -0.059082  -0.105591   -0.118530];
am=[axm',aym',azm']; %axm, aym, azm分别是采集的三轴加速度计数据，最好是6个面进行采集
G=[1 1 1 1 1 1]';
f=@(a,am)(a(1)*am(:,1)+a(2)).^2+(a(3)*am(:,2)+a(4)).^2+(a(5)*am(:,3)+a(6)).^2;
a0=[1 0 1 0 1 0];
a=lsqcurvefit(f,a0,am,G)

点击下图中的运行，得出的结果就是公式中的变量值，

对应：Kx，Bx；Ky，By；Kz，Bz

应用校准后，使用示例如下：

#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>
#include <string.h>
#include <stdlib.h>
#include <time.h>
#include <sys/time.h>
#include <math.h>void print_timestamp() 
{struct timeval tv;gettimeofday(&tv, NULL);printf("[%ld.%06ld] ", tv.tv_sec, tv.tv_usec);
}int main(int argc, char *argv[])
{int error;short resive_data[6];  //保存收到的 mpu6050转换结果数据，依次为 AX(x轴角度), AY, AZ 。GX(x轴加速度), GY ,GZstatic int count = 0;float x, y, z, total_accel;const float acc_param_k[3] = {0.9987, 0.9939, 0.9925};const float acc_param_a[3] = {-0.0296, 0.0085, 0.0742};/*打开文件*/int fd = open("/dev/i2c_mpu6050", O_RDWR);if (fd < 0) {printf("open file : %s failed !\n", argv[0]);return -1;}while (1) {/* 读取数据 */print_timestamp();error = read(fd, resive_data, 6);if (error != 6) {printf("read file error! \n");close(fd);break;}print_timestamp();/* 打印数据 */printf("AX = %d, AY = %d, AZ = %d\n", (int)resive_data[0], (int)resive_data[1], (int)resive_data[2]);x = resive_data[0] / 16384.0;y = resive_data[1] / 16384.0;z = resive_data[2] / 16384.0;total_accel = sqrtf(x * x + y * y + z * z);count++;printf("x: %f, y: %f, z: %f, total_accel: %f, count: %d\n", x, y, z, total_accel, count);x = acc_param_k[0] * x + acc_param_a[0];y = acc_param_k[1] * y + acc_param_a[1];z = acc_param_k[2] * z + acc_param_a[2];total_accel = sqrtf(x * x + y * y + z * z);printf("calc x: %f, y: %f, z: %f, total_accel: %f, count: %d\n\n", x, y, z, total_accel, count);//sleep(1);usleep(10000);}/*关闭文件*/error = close(fd);if (error < 0) {printf("close file error! \n");}return 0;
}

2.6 简单调试

经过简单读取数据调试，发现将 mpu6050 加速度采样配置为 200HZ 时，抖动较大，于是参考别人的代码，配置成了 50HZ

    // 计划每秒读取 100 次加速度的数据，将加速度配置为 50HZ// 采样频率 = 陀螺仪输出频率 / (1+SMPLRT_DIV) => SMPLRT_DIV = 19err += i2c_write_mpu6050(mpu6050_client, SMPLRT_DIV, 0X13);// 设置数字低通滤波器，带宽为采样频率的一半，就是 25，查表值为 21Hz; 配置 DLPF_CFG = 4err += i2c_write_mpu6050(mpu6050_client, CONFIG, 0X04);

查看全文

http://www.mrgr.cn/news/70422.html