一，前言
学习了陀螺仪及加速度计的原理及了解了MPU6500的寄存器手册及六轴陀螺仪模块的引脚定义后，就进入了驱动框架设计及驱动具体细节设计。MPU6500的驱动代码及测试代码已上我的gitee工程16
二，测量理论及驱动框架设计
此六轴可测量3类传感器，加速度计，陀螺仪和温度。直接可以从寄存器读取。
加速度计输出值就是重力加速度g为单位，平放的话会有一个为1g，其它2个方向为0。陀螺仪就是转角速度，围绕某个轴转动就有速度值，否则为0。温度为环境温度。至于芯片中可以直接烧录黑盒DMP固件，这个需要移植，我暂时不移植了，之后可能专门做一个算法专题会自己写相关惯导算法。现在的linux驱动就是采用input子系统周期上报通过i2c读寄存器的数据，然后APP测试代码中将AD值转为带单位的值而已。AD为16位的，然后灵敏度自己通过设置测量范围后看有计算出，这个就不多说了。
三，硬件连线
就连接了5根线，I2C用了4根。ADD接地，所以设备地址为0x68，若接高电平3,3v，地址为0x69。
image.png
修改DTB
image.png
四， 遇到的问题
- 编译遇到input_register_polled_device未定义
o文件已经生成，说明link文件出错，那么就说明polled dev那个c函数没有被编译，搜索到源码后，在makefile中找到对应的配置项CONFIG_INPUT_POLLDEV设置为true，然后编译内核。然后重新编译我的外部模块，能正常编译出ko文件。 - 在板子/dev/input/目录下只有mice 没有event0目录
答案为内核需要配置CONFIG_INPUT_EVDEV=y
当然若要修改源码来固定event，方法如下
https://blog.csdn.net/savage_sdj/article/details/25278111
五，测试通过
image.png
ACC的值和实际方向是反的。先平放，可以看到加速度计z=1（就是2g-1g=1g），然后沿x轴转90度看到y等于-1，然后再转90度，看到acc的x为1，最后再平放绕z轴转，看到z轴的陀螺仪角度变化最大。数据太长，所以我用......省略了些。
# insmod apple6500_dev3.ko
[ 169.347794] input: apple6500sensor as /devices/platform/ocp/48000000.interconnect/48000000.interconnect:segment@100000/4819c000.target-module/4819c000.i2c/i2c-2/2-0068/input/input1
# ./ev2
ACC x=0.37 y=0.17 z=0.92
Gyro x=-0.01 y=0.00 z=0.01
temp=18.48
ACC x=0.37 y=0.17 z=0.92
Gyro x=-0.01 y=0.00 temp=18.49
ACC x=0.37 y=0.17 z=0.91
Gyro x=-0.01 y=-0.00 temp=18.46
ACC x=0.37 Gyro x=-0.01 y=-0.00 z=0.01
temp=18.47
ACC x=0.37 y=0.17 z=0.92
Gyro x=-0.01 y=-0.00 z=0.01
temp=18.48
ACC x=0.37 y=0.17 z=0.92
y=0.00 temp=18.47
y=0.17 z=0.92
Gyro x=-0.01 y=-0.00 z=0.01
ACC x=0.37 y=0.17 z=0.92
Gyro x=-0.01 z=0.01
.....
Gyro x=-0.42 y=1.21 z=-0.52
temp=18.48
ACC x=-0.07 y=-0.16 z=0.96
Gyro x=-2.41 y=0.20 z=-0.07
temp=18.49
ACC x=0.06 y=-0.80 z=0.57
Gyro x=-3.56 y=-0.96 z=0.44
temp=18.48
ACC x=0.05 y=-0.98 z=0.29
Gyro x=-0.53 y=0.04 z=0.03
temp=18.48
ACC x=0.02 y=-1.01 z=0.22
Gyro x=-0.43 y=0.01 z=-0.00
temp=18.49
ACC x=0.03 y=-1.01 z=0.21
Gyro x=-0.10 y=0.08 z=0.01
....
ACC x=0.87 y=0.10 z=0.32
Gyro x=-0.24 y=-0.41 z=-0.20
temp=18.50
ACC x=0.99 y=0.09 z=0.22
Gyro x=0.03 y=-0.73 z=0.36
temp=18.50
ACC x=0.97 y=0.08 z=0.04
Gyro x=0.03 y=-0.29 z=-0.06
ACC x=1.01 y=-0.13 z=0.29
Gyro x=-0.03 y=-0.05 z=0.02
temp=18.49
ACC x=1.00 y=-0.02 z=0.04
Gyro x=0.06 y=-0.18 z=-0.03
.....
Gyro x=0.53 y=0.99 z=-4.74
temp=18.51
ACC x=0.04 y=-0.05 z=1.05
Gyro x=0.49 y=-0.24 z=-4.24
temp=18.51
ACC x=0.16 y=0.41 z=1.01
Gyro x=-0.12 y=0.01 z=1.63
temp=18.51
ACC x=-0.06 y=-0.23 z=0.99
Gyro x=-0.61 y=0.08 z=7.06
temp=18.50
ACC x=0.45 y=0.41 z=0.97
Gyro x=-1.62 y=0.98 z=9.36
temp=18.51
ACC x=0.05 y=0.01 z=1.03
Gyro x=0.60 y=-2.72 z=-5.54
temp=18.49
ACC x=0.18 y=-0.04 z=1.20
Gyro x=-0.68 y=1.03 z=-0.28
temp=18.50
六，学习到的内容
1.input_polled_dev，设置一个周期后内核对按此周期定时调用poll函数。这样就省去了自定hrtimer定时器。
2.smbus兼容i2c，所以用i2c_smbus_read_i2c_block_data接口比用i2c接口简单高效。但是smbus最大的clock频率为100KHz，而i2c可以支持400KHz或2MHz。不要紧反正是最底层的函数，修改起来也容易，先用下新的函数。
- input子系统的API。input子系统中使用i2c总线驱动的API。
七，MPU6500的算法应用
这里面涉及了从旋转向量到四元数到姿态角输出的应用。并且由于是传感器，所以涉及到了卡尔曼滤波。因为之前学习slam的时候就学习过这些算法，所以复习了下基础内容。但是我评估了下那些公式推导我现在不会了，所以要明白如何推导的大概需要花费2周业余时间，若我不会公式推导，直接按推导后或网上给的写代码的思路实现算法也是可以的，但是这就失去了学习的意义，所以我现在不加，等之后安排一个算法专题的时候会预留足够时间复习这些算法的推导实现。
本轮在刻意练习linux驱动的过程中就先复习些基础理论概念
1. 内积是和投影的乘积，结果为标量，主要看小于0，则是大于90度。
2. 叉乘结果为向量，方向垂直于平面。值为角度的大小。
3. 矩阵可以理解为一种向量的变换关系。
4. 向量是原点到x,y的一点的方向及大小。是特殊矩阵只有一列。
5. 齐次坐标，再添加一维表示投影坐标系中某点的位置，因为{1,1,1}向量和{2,2,2}向量表示的点不唯一。最后一维可以理解为代表距离。
6. 为什么旋转矩阵必须是正交矩阵？
因为正交矩阵是笛卡尔坐标系的的，两两垂直的坐标。我们现在的旋转公式就是基于笛卡尔坐标的https://www.zhihu.com/question/304059390
7. 旋转向量，方向是旋转轴，长度是旋转角度（弧度）。用一个旋转向量+一个平移向量共6维的变换矩阵就可以表示一次三维变换。
8. 欧拉角就是将旋转角拆分为3个分离的转角，然后按x,y,z轴旋转顺序或其它顺序依次转动。绕z轴为偏航角yaw,绕y轴旋转为俯仰角pitch,按x轴旋转为滚转角roll。但是他有一个万向锁问题，在俯仰角为正负90度时候第一次与第三次旋转将使用同一个轴，是系统失去一个自由度，具有奇异性。所以四元数就出现了，参考了复数的思想（i代表转90度），包括一个实部和三个虚部。
image.png
9.四元数解算姿态角解析包括公式推算
Good:https://blog.csdn.net/guanjianhe/article/details/95608801
10. MPU6500计算姿态角有什么用，当然是有用的，比如飞机控制平衡使用。
11. MPU6500自带的DMP可以直接输出四元数，而四元数可以很容易的转换为姿态角。
https://blog.csdn.net/ShawnWang1994/article/details/86266126
12. MPU6500输出的角速度是欧拉角吗？不是的
https://wenku.baidu.com/view/3f3eed3a58fafab069dc0271.html
13．欧拉角与姿态角是一个概念吗？
不是的，欧拉角指的是绕某个轴旋转的角度，而姿态角指的是载体系与导航系之间的关系。只能是ZYX的旋转顺序，否则，欧拉角和姿态角之间就不相等。
https://www.zhihu.com/collection/364926826
14. 旋转向量和旋转矩阵的关系？
MPU6500初始化的角度（弧度）是旋转向量。旋转向量与旋转矩阵可以通过罗德里格斯（Rodrigues）变换进行转换
https://www.jianshu.com/p/5e130c04a602
15. 角速度怎么用欧拉角的导数表示？
https://www.zhihu.com/question/33311807/answer/129970338
16. Eigen库使用
https://www.jianshu.com/p/96d60ba797f8
八，驱动源码
之后再看看相关锁是否要添加，暂时都没有加，另外电源管理相关也没有添加。MPU6500的中断脚其实也比较有作用的，由于现在app测试代码中只有通过公式进行AD转换，没有其它功能，所以驱动也没加INT相关，之后要做某些应用的话，考虑加入。另外，对MPU6500初始化写的也比较简单，没有添加很多配置函数进行灵活设置先测试下基本功能。另外主要目的是学习linux驱动框架，而不是学习这个MPU6500芯片，哈哈~
/**********************************************************************************
Copyright (C), by AppleCai
Project : Study Kernel
Description : BSP level for MPU6500
CPU and Compiler : AM335x,ARM-LINIX-GCC
|----------------------------------------------------------------------------------
| R E V I S I O N H I S T O R Y
|----------------------------------------------------------------------------------
| Date Version Author Description
| -------- ------- ------ ---------------------------------------------------
| 2020-12-08 1.0.0 AppleCai MPU6500_001: Initial release version just add frame
| 2020-12-09 1.0.1 AppleCai MPU6500_002: add WMI reg and I2C DTB
| 2020-12-10 1.0.2 AppleCai MPU6500_003: add Initial reg and report functions
| 2020-12-11 1.0.3 AppleCai MPU6500_004: Optimize code structure
**********************************************************************************/
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/slab.h>
#include <linux/delay.h>
#include <linux/i2c.h>
#include <linux/input-polldev.h>
#include <linux/of_device.h>
#define APPLE6500_DRV_NAME "apple6500sensor"
#define POLL_INTERVAL 200
#define POLL_INTERVAL_MAX 500
/* bit op */
#define VAL_SIT_BIT(n,addr) *addr |= (1<<n)
#define VAL_CLEAR_BIT(n,addr) *addr &= ~(1<<n)
#define VAL_CHANGE_BIT(n,addr) *addr ^= (1<<n)
#define VAL_TEST_BIT(n,addr) *addr &= (1<<n)
struct apple6500_chip_reg {
/* regs */
char who_am_i;
char pwr_mgmt_1;
char pwr_mgmt_2;
char smplrt_div;
char sensor_config;
char accel_config;
char accel_config2;
char accel_xout_h;
char temp_out_h;
char gyro_xout_h;
char gyro_config;
char signal_path_reset;
};
static const struct apple6500_chip_reg chipregs={
/* reg addr */
.who_am_i= 117,
.pwr_mgmt_1= 107,
.pwr_mgmt_2= 108,
.smplrt_div= 25,
.sensor_config= 26,
.gyro_config= 27,
.accel_config= 28,
.accel_config2= 29,
.accel_xout_h= 59,
.temp_out_h= 65,
.gyro_xout_h= 67,
.signal_path_reset= 104,
};
struct apple6500_chip_config{
char chip_id;
char chip_enable_sensor;
char chip_frequency_acc;
char chip_frequency_gyro;
char chip_frequency_DIV;
char chip_reset;
char chip_sensitivity_acc;
char chip_sensitivity_gyro;
char chip_selectedclock;
};
struct apple6500;
struct apple6500_transfer_func {
int (*read) (struct apple6500 *m, unsigned off);
int (*write) (struct apple6500 *m, unsigned off, unsigned char v);
int (*readblock)(struct apple6500 *m, unsigned off,unsigned char *buf, size_t size);
};
/* apple6500 status */
struct apple6500 {
struct device *dev;
struct input_polled_dev *idev;
const struct apple6500_chip_reg *regs;
struct apple6500_chip_config cfgs;
const struct apple6500_transfer_func *tf;
};
static int apple6500_read(struct apple6500 *m, unsigned off)
{
//struct i2c_client *c = m->client;
struct i2c_client *c = to_i2c_client(m->dev);
int ret;
ret = i2c_smbus_read_byte_data(c, off);
if (ret < 0)
dev_err(&c->dev,
"failed to read register 0x%02x, error %d\n",
off, ret);
return ret;
}
static int apple6500_write(struct apple6500 *m, unsigned off, unsigned char v)
{
//struct i2c_client *c = m->client;
struct i2c_client *c = to_i2c_client(m->dev);
int error;
error = i2c_smbus_write_byte_data(c, off, v);
if (error < 0) {
dev_err(&c->dev,
"failed to write to register 0x%02x, error %d\n",
off, error);
return error;
}
return 0;
}
static int apple6500_read_block(struct apple6500 *m, unsigned off,unsigned char *buf, size_t size)
{
//struct i2c_client *c = m->client;
struct i2c_client *c = to_i2c_client(m->dev);
int err;
err = i2c_smbus_read_i2c_block_data(c, off, size, buf);
if (err < 0) {
dev_err(&c->dev,
"failed to read block data error %d\n", err);
return err;
}
return 0;
}
static const struct apple6500_transfer_func apple6500_tf_i2c = {
.write = apple6500_write,
.read = apple6500_read,
.readblock = apple6500_read_block
};
static void apple6500_poll(struct input_polled_dev *dev)
{
struct apple6500 *m = dev->private;
int ax, ay, az,gx,gy,gz,temp;
int ret;
u8 buf[14];
ret = m->tf->readblock(m, m->regs->accel_xout_h, buf, sizeof(buf));
if (ret < 0)
{
dev_err(m->dev, "polling error\n");
return;
}
ax = ( buf[0] << 8 ) | buf[1];
ay = ( buf[2] << 8 ) | buf[3];
az = ( buf[4] << 8 ) | buf[5];
temp = ( buf[6] << 8 ) | buf[7];
gx = ( buf[8] << 8 ) | buf[9];
gy = ( buf[10] << 8 ) | buf[11];
gz = ( buf[12] << 8 ) | buf[13];
input_report_abs(dev->input, ABS_X, ax);
input_report_abs(dev->input, ABS_Y, ay);
input_report_abs(dev->input, ABS_Z, az);
input_report_abs(dev->input, ABS_RX, gx);
input_report_abs(dev->input, ABS_RY, gy);
input_report_abs(dev->input, ABS_RZ, gz);
input_report_abs(dev->input, ABS_MISC, temp);//Temperature = 36.53 + regval/340
input_sync(dev->input);
}
static void apple6500_Init_reg(struct apple6500_chip_config * cfgs)
{
cfgs->chip_id = 0x70; // id for MPU6500
cfgs->chip_enable_sensor = 0; // enable all
cfgs->chip_frequency_acc = 4; // 50Hz/2=25Hz then select 20Hz
cfgs->chip_frequency_DIV = 19; // 1000/(19+1) = 50Hz clock for acc
cfgs->chip_frequency_gyro = 4; // 1K 20Hz,due to FCHOICE_B=0
cfgs->chip_reset = 1<<7; // true
cfgs->chip_sensitivity_acc = 0; // 65536/4 = 16384 LBS/s
cfgs->chip_sensitivity_gyro = 3<<3; // 65536/4000 = 16.384 LBS/s
cfgs->chip_selectedclock = 1; // PLL with axis
}
static int apple6500_CheckId(struct apple6500 *m)
{
int IdNum = 0;
IdNum = m->tf->read(m, m->regs->who_am_i);
if(m->cfgs.chip_id != IdNum)
{
dev_err(m->dev, "Not found MPU6500\n");
return -ENXIO;
}
return 0;
}
static int apple6500_ResetChipAndSelectClock(struct apple6500 *m)
{
int err = 0;
int val = 0;
val = m->cfgs.chip_reset+m->cfgs.chip_selectedclock;
err = m->tf->write(m, m->regs->pwr_mgmt_1,val);
if(err)
{
dev_err(m->dev, "reset command error\n");
}
return err;
}
static int apple6500_EnableSensor(struct apple6500 *m)
{
int err = 0;
err = m->tf->write(m, m->regs->pwr_mgmt_2,m->cfgs.chip_enable_sensor);
if(err)
{
dev_err(m->dev, "enable command error\n");
}
return err;
}
static int apple6500_SetACCSamplerate(struct apple6500 *m)
{
int err = 0;
err = m->tf->write(m, m->regs->accel_config2,m->cfgs.chip_frequency_acc);
if(err)
{
dev_err(m->dev, "set acc bandwidths error\n");
}
err = m->tf->write(m, m->regs->smplrt_div,m->cfgs.chip_frequency_DIV);
if(err)
{
dev_err(m->dev, "set acc clock division error\n");
}
return err;
}
static int apple6500_SetGyroSamplerate(struct apple6500 *m)
{
int err = 0;
err = m->tf->write(m, m->regs->sensor_config,m->cfgs.chip_frequency_gyro);
if(err)
{
dev_err(m->dev, "set gyro bandwidths error\n");
}
return err;
}
static int apple6500_SetAccSensitivity(struct apple6500 *m)
{
int err = 0;
err = m->tf->write(m, m->regs->accel_config,m->cfgs.chip_sensitivity_acc);
if(err)
{
dev_err(m->dev, "set acc sensitivity error\n");
}
return err;
}
static int apple6500_SetGyroSensitivity(struct apple6500 *m)
{
int err = 0;
err = m->tf->write(m, m->regs->gyro_config,m->cfgs.chip_sensitivity_gyro);
if(err)
{
dev_err(m->dev, "set gyro sensitivity error\n");
}
return err;
}
static int apple6500_Init(struct apple6500 *m)
{
int ret = 0;
/* load default configuration value */
apple6500_Init_reg(&m->cfgs);
/* check id */
ret = apple6500_CheckId(m);
/* set reset chip and select clock */
ret = apple6500_ResetChipAndSelectClock(m);
/* enable acc and gyro */
ret = apple6500_EnableSensor(m);
/* set sample rate and bandwith for acc*/
ret = apple6500_SetACCSamplerate(m);
/* set sample rate and bandwith for gyro*/
ret = apple6500_SetGyroSamplerate(m);
/* set sensitivity for acc*/
ret = apple6500_SetAccSensitivity(m);
/* set sensitivity for gyro*/
ret = apple6500_SetGyroSensitivity(m);
if (ret < 0)
{
dev_err(m->dev,"init error\n");
return -1;
}
return ret;
}
/* Initialize the APPLE6500 chip */
static void apple6500_open(struct input_polled_dev *dev)
{
int ret = 0;
struct apple6500 *m = dev->private;
ret = apple6500_Init(m);
if (ret < 0)
{
dev_err(m->dev, "apple6500 initialization failed\n");
return;
}
}
static void apple6500_close(struct input_polled_dev *dev)
{
//struct apple6500 *m = dev->private;
//int ret = 0;
/* sleep */
//ret = m->tf->write(m, APPLE6500_PWR_REG107,APPLE6500_PWR_SLEEP);
//if (ret < 0)
// return;
}
static const struct of_device_id apple6500_dt_ids[] = {
{ .compatible = "applecai,apple6500", .data = &chipregs},
{ /* sentinel */ }
};
/*
* I2C init/probing/exit functions
*/
static int apple6500_i2c_probe(struct i2c_client *c,
const struct i2c_device_id *id)
{
struct input_polled_dev *idev;
struct apple6500 *m;
int err;
const struct of_device_id *match;
/* Check basic functionality */
err = i2c_check_functionality(c->adapter, I2C_FUNC_SMBUS_WRITE_BYTE_DATA|
I2C_FUNC_SMBUS_READ_BYTE_DATA|
I2C_FUNC_SMBUS_READ_I2C_BLOCK);
if (!err) {
dev_err(&c->dev, "%s adapter not supported\n",
dev_driver_string(&c->adapter->dev));
return -ENODEV;
}
match = of_match_node(apple6500_dt_ids,c->dev.of_node);
if (!match)
{
dev_err(&c->dev, "No such device or address.\n");
return -ENXIO;
}
m = devm_kzalloc(&c->dev, sizeof(*m), GFP_KERNEL);
if (!m)
return -ENOMEM;
idev = devm_input_allocate_polled_device(&c->dev);
if (!idev)
return -ENOMEM;
//m->client = c;
m->dev = &c->dev;
m->idev = idev;
m->regs = match->data;
m->tf = &apple6500_tf_i2c;
i2c_set_clientdata(c, m);
idev->private = m;
idev->input->name = APPLE6500_DRV_NAME;
idev->input->id.bustype = BUS_I2C;
idev->input->id.vendor = 0xABCD;
idev->input->id.product = 0x0001;
idev->input->id.version = 0x0100;
idev->poll = apple6500_poll;
idev->poll_interval = POLL_INTERVAL;
idev->poll_interval_max = POLL_INTERVAL_MAX;
idev->open = apple6500_open;
idev->close = apple6500_close;
__set_bit(EV_ABS, idev->input->evbit);
input_set_abs_params(idev->input, ABS_X, -32768, 32767, 0, 0);
input_set_abs_params(idev->input, ABS_Y, -32768, 32767, 0, 0);
input_set_abs_params(idev->input, ABS_Z, -32768, 32767, 0, 0);
input_set_abs_params(idev->input, ABS_RX, -32768, 32767, 0, 0);
input_set_abs_params(idev->input, ABS_RY, -32768, 32767, 0, 0);
input_set_abs_params(idev->input, ABS_RZ, -32768, 32767, 0, 0);
input_set_abs_params(idev->input, ABS_MISC, -32768, 32767, 0, 0);
err = input_register_polled_device(idev);
if (err) {
input_unregister_polled_device(idev);
dev_err(&c->dev, "failed to register polled input device\n");
goto out;
}
return 0;
out:
input_unregister_polled_device(idev);
return err;
}
static int apple6500_i2c_remove(struct i2c_client *c)
{
struct apple6500 *m = i2c_get_clientdata(c);
//disable sensor
input_unregister_polled_device(m->idev);
input_free_polled_device(m->idev);
dev_info(m->dev, "%s: removed\n", APPLE6500_DRV_NAME);
kfree(m);
return 0;
}
MODULE_DEVICE_TABLE(of, apple6500_dt_ids);
static struct i2c_driver apple6500_driver = {
.driver = {
.name = APPLE6500_DRV_NAME,
.of_match_table = apple6500_dt_ids,
},
.probe = apple6500_i2c_probe,
.remove = apple6500_i2c_remove,
};
module_i2c_driver(apple6500_driver);
MODULE_AUTHOR("AppleCai");
MODULE_DESCRIPTION("APPLE6500 Acceler and Gyro Driver");
MODULE_LICENSE("GPL");
