linux 下调试 mpu6050 三轴加速度
供自己备忘；
1. 参考资料：
b 站视频
https://www.bilibili.com/video/BV1cL4y1x7FA/?spm_id_from=333.337.search-card.all.click&vd_source=d7a07b8689c9e646f0214227c06f304ccsdn 其它博客
https://blog.csdn.net/qq_65198598/article/details/1388050342. 简单调试
需求是通过 mpu6050 获取三轴加速度的值；每秒读取 100 次；
通过官方自带的驱动，读取 iio 很慢，而且还使用了中断脚；看大家都自己写 mpu6050 的数据读取驱动，我也写一个。
2.1 dts 配置如下
0x68 是 7 位 i2c 地址，对应芯片 AD0 脚是接地的；
2.2 Makefile 文件如下
内核路径修改为自己的，交叉编译器也是；前提是内核编译过，配置好交叉编译器。
KERN_DIR = /opt/liangtao/sigmastar/IKAYAKI_DLM00V017_SSD222D/kernel
all:
make -C $(KERN_DIR) M=`pwd` modules
arm-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 clean
obj-m += i2c_mpu6050.o2.3 驱动代码修改如下
直接拷贝正点原子的代码改的；
修改的点为：
- 移除了陀螺仪数据的读取，因为项目不需要
- 使用定时器加工作队列来执行三轴加速度的数据读取，想减少应用读取消耗的时间（修改了内核的 HZ 为 1000，因为 100 的情况下，定时精度低）
- 使用 i2c 的连续数据读取功能
- 参考网上的初始 mpu6050 的方法，重新初始化 mpu6050
- 复位 mpu6050，并延时 100ms
- 关闭休眠，使用 X 轴陀螺作为时钟参考
- 将加速度采样配置为 200Hz
- 设置数字低通滤波器，带宽为采样率的一半
- 设置陀螺仪量程
- 设置加速度量程
- 关闭所有中断
- 关闭 i2c 主模式
- 关闭 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 = 4
err += i2c_write_mpu6050(mpu6050_client, SMPLRT_DIV, 0X04);
// 设置数字低通滤波器，带宽为采样频率的一半，就是 100，查表值为 94Hz; 配置 DLPF_CFG = 2
err += i2c_write_mpu6050(mpu6050_client, CONFIG, 0X02);
// 设置陀螺仪量程为 +- 2000
err += i2c_write_mpu6050(mpu6050_client, GYRO_CONFIG, 0X18);
// 设置加速度量程为 +- 2G
err += 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);
// 关闭 FIFO
err += 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_operations
mpu6050_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 ,GZ
static 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;
}原文地址：https://blog.csdn.net/liangtao_1996/article/details/143590467
免责声明：本站文章内容转载自网络资源，如本站内容侵犯了原著者的合法权益，可联系本站删除。更多内容请关注自学内容网（zxcms.com）！