今天和大家分享下基于小红板（GD32F207）送餐机器人的陀螺仪（MPU6050）的DMP移植部分。该部分中我将用到原来51单片机中的按位操作及位绑定，使用到了软件I2C。
一、MPU6050
MPU6050 它是全球首例整合性6 轴运动处理组件，俗称的六轴陀螺仪（x y z 三轴的倾斜角度和三轴方向的加速度）。它集成了陀螺仪和加速度计于一体的芯片，它极大程度上免除了独立使用的陀螺仪和加速度计在时间上的误差，而且减少了占用PCB 板的空间.
MPU-60X0 对陀螺仪和加速度计分别用了三个16 位的ADC，将其测量的模拟量转化为可输出的数字量。为了精确跟踪快速和慢速的运动，传感器的测量范围都是用户可控的，陀螺仪可测范围为±250，±500，±1000，±2000°/秒（dps），加速度计可测范围为±2，±4，±8，±16g。一个片上1024 字节的FIFO，有助于降低系统功耗。和所有设备寄存器之间的通信采用400kHz 的I2C 接口或1MHz 的SPI接口（SPI 仅MPU-6000 可用）。对于需要高速传输的应用，对寄存器的读取和中断可20MHz 的SPI。另外，片上还内嵌了一个温度传感器和在工作环境下仅有±1%变动的振荡器。芯片尺寸4×4×0.9mm，采用QFN 封装（无引线方形封装），可承受最大10000g 的冲击，并有可编程的低通滤波器。关于电源，MPU-60X0 可支持VDD范围2.5V±5%，3.0V±5%，或3.3V±5%。另外MPU-6050 还有一个VLOGIC 引脚，用来为I2C 输出提供逻辑电平。VLOGIC 电压可取1.8±5%或者VDD。
二、软件I2C时序模拟
内部寄存器可通过400kHz 的I2C 接口或1MHz 的SPI 接口来操作，二线接口，包括串行数据线（SDA）和串行时钟（SCL）。连接到I2C 接口的设备可做主设备或从设备。主设备将Slave 地址传到总线上，从设备用与其匹配的地址来识别主设备。当连接到系统芯片时，MPU-60X0 总是作为从设备。SDA 和SCL 信号线通常需要接上拉电阻到VDD。最大总线速率为400kHz。MPU-60X0 的Slave 地址为b110100X，7 位字长，最低有效位X由AD0 管脚上的逻辑电平决定。这样就可以允许两个MPU-60X0连接到同一条I2C 总线，此时，一个设备的地址为b1101000（AD0为逻辑低电平），另一个为b1101001（AD0 为逻辑高）。
1、开始（S）和结束（P）标志：开始信号：处理器让SCL时钟保持高电平，然后让SDA数据信号由高变低就表示一个开始信号。同时IIC总线上的设备检测到这个开始信号它就知道处理器要发送数据了。停止信号：处理器让SCL时钟保持高电平，然后让SDA数据信号由低变高就表示一个停止信号。同时IIC总线上的设备检测到这个停止信号它就知道处理器已经结束了数据传输，我们就可以各忙各个的了，如休眠等。
2、数据格式/应答：响应信号（ACK）：处理器把数据发给外接IIC设备，如何知道IIC设备数据已经收到呢？就需要外接IIC设备回应一个信号给处理器。处理器发完8bit数据后就不再驱动总线了（SDA引脚变输入），而SDA和SDL硬件设计时都有上拉电阻，所以这时候SDA变成高电平。那么在第8个数据位，如果外接IIC设备能收到信号的话接着在第9个周期把SDA拉低，那么处理器检测到SDA拉低就能知道外接IIC设备数据已经收到。
I2C 的数据字节定义为8-bits 长度，对每次传送的总字节数量没有限制。对每一次传输必须伴有一个应答（ACK）信号，其时钟由主设备提供，而真正的应答信号由从设备发出，在时钟为高时，通过拉低并保持SDA 的值来实现。IIC数据从最高位开始传输。再看数据怎么传：SDA上传输的数据必须在SCL为高电平期间保持稳定：因为外接IIC设备在SCL为高电平的期间采集数据方知SDA是高或低电平。SDA上的数据只能在SCL为低电平期间翻转变化。如果从设备忙，它可以使SCL 保持在低电平，这会强制使主设备进入等待状态。当从设备空闲后，并且释放时钟线，原来的数据传输才会继续。
3、通信：开始标志（S）发出后，主设备会传送一个7 位的Slave 地址，并且后面跟着一个第8位，称为Read/Write 位。R/W位表示主设备是在接受从设备的数据还是在向其写数据。然后，主设备释放SDA 线，等待从设备的应答信号（ACK）。每个字节的传输都要跟随有一个应答位。应答产生时(既把SDA拉低)，从设备将SDA 线拉低并且在SCL 为高电平时保持低。数据传输总是以停止标志（P）结束，然后释放通信线路。然而，主设备也可以产生重复的开始信号去操作另一台从设备，而不发出结束标志。综上可知，所有的SDA 信号变化都要在SCL 时钟为低电平时进行，除了开始和结束标志.
三、代码部分
1、主函数部分：函数主要就是对初始化系统时钟，串口，IIC，MPU6050，然后在循环中打印MPU6050六轴的信息。
int main(void)
{
int t=0;
SysTick_Configuration();
NVIC_Configuration(); //设置NVIC中断分组2:2位抢占优先级，2位响应优先级
USART1_GIPIO_CONFIG(); //串口初始化为115200
IIC_Init(); //=====模拟IIC初始化
MPU6050_initialize(); //=====MPU6050初始化
DMP_Init(); //=====初始化DMP
while(1){
Read_DMP();//读取加速度、角速度、倾角
for(t=0;t<700;t++)
{
delay_us(200);
}
}
}
2、DMP移植修改
DMP移植其实非常的简单，我们只有主意以下节点
#define MPU6050
#define MOTION_DRIVER_TARGET_MSP430
#if defined MOTION_DRIVER_TARGET_MSP430
/*#include "msp430.h"
#include "msp430_i2c.h"
#include "msp430_clock.h"
#include "msp430_interrupt.h" */ 这里需要注释掉，因为官方是基于msp430的所以这里需要注释掉
下面四个函数要和你自己写的函数的名称一致，用于DMP中调用
#define i2c_write i2cWrite
#define i2c_read i2cRead
#define delay_ms delay_ms
#define get_ms myget_ms
下面的部分也需要修改过来，大家仔细看下面的代码修改，其实会发现很简单，就是改下格式，如.who_am_i =0x75改成0x75, //who_am_i，也就是把前面部分给删掉就行了。
/*
const struct gyro_reg_s reg = {
.who_am_i = 0x75,
.rate_div = 0x19,
.lpf = 0x1A,
.prod_id = 0x0C,
.user_ctrl = 0x6A,
.fifo_en = 0x23,
.gyro_cfg = 0x1B,
.accel_cfg = 0x1C,
.motion_thr = 0x1F,
.motion_dur = 0x20,
.fifo_count_h = 0x72,
.fifo_r_w = 0x74,
.raw_gyro = 0x43,
.raw_accel = 0x3B,
.temp = 0x41,
.int_enable = 0x38,
.dmp_int_status = 0x39,
.int_status = 0x3A,
.pwr_mgmt_1 = 0x6B,
.pwr_mgmt_2 = 0x6C,
.int_pin_cfg = 0x37,
.mem_r_w = 0x6F,
.accel_offs = 0x06,
.i2c_mst = 0x24,
.bank_sel = 0x6D,
.mem_start_addr = 0x6E,
.prgm_start_h = 0x70
#ifdef AK89xx_SECONDARY
,.raw_compass = 0x49,
.yg_offs_tc = 0x01,
.s0_addr = 0x25,
.s0_reg = 0x26,
.s0_ctrl = 0x27,
.s1_addr = 0x28,
.s1_reg = 0x29,
.s1_ctrl = 0x2A,
.s4_ctrl = 0x34,
.s0_do = 0x63,
.s1_do = 0x64,
.i2c_delay_ctrl = 0x67
#endif
};
const struct hw_s hw = {
.addr = 0x68,
.max_fifo = 1024,
.num_reg = 118,
.temp_sens = 340,
.temp_offset = -521,
.bank_size = 256
#if defined AK89xx_SECONDARY
,.compass_fsr = AK89xx_FSR
#endif
};
*/
const struct hw_s hw={
0x68, //addr
1024, //max_fifo
118, //num_reg
340, //temp_sens
-521, //temp_offset
256 //bank_size
};
const struct gyro_reg_s reg = {
0x75, //who_am_i
0x19, //rate_div
0x1A, //lpf
0x0C, //prod_id
0x6A, //user_ctrl
0x23, //fifo_en
0x1B, //gyro_cfg
0x1C, //accel_cfg
0x1F, // motion_thr
0x20, // motion_dur
0x72, // fifo_count_h
0x74, // fifo_r_w
0x43, // raw_gyro
0x3B, // raw_accel
0x41, // temp
0x38, // int_enable
0x39, // dmp_int_status
0x3A, // int_status
0x6B, // pwr_mgmt_1
0x6C, // pwr_mgmt_2
0x37, // int_pin_cfg
0x6F, // mem_r_w
0x06, // accel_offs
0x24, // i2c_mst
0x6D, // bank_sel
0x6E, // mem_start_addr
0x70 // prgm_start_h
};
//const struct test_s test = {
// .gyro_sens = 32768/250,
// .accel_sens = 32768/16,
// .reg_rate_div = 0, /* 1kHz. */
// .reg_lpf = 1, /* 188Hz. */
// .reg_gyro_fsr = 0, /* 250dps. */
// .reg_accel_fsr = 0x18, /* 16g. */
// .wait_ms = 50,
// .packet_thresh = 5, /* 5% */
// .min_dps = 10.f,
// .max_dps = 105.f,
// .max_gyro_var = 0.14f,
// .min_g = 0.3f,
// .max_g = 0.95f,
// .max_accel_var = 0.14f
//};
const struct test_s test={
32768/250, //gyro_sens
32768/16, // accel_sens
0, // reg_rate_div
1, // reg_lpf
0, // reg_gyro_fsr
0x18, // reg_accel_fsr
50, // wait_ms
5, // packet_thresh
10.0f, // min_dps
105.0f, // max_dps
0.14f, // max_gyro_var
0.3f, // min_g
0.95f, // max_g
0.14f // max_accel_var
};
/*
static struct gyro_state_s st = {
.reg = ®,
.hw = &hw,
.test = &test
}; */
static struct gyro_state_s st={
®,
&hw,
{0},
&test
};
//st.chip_cfg.dmp_on = 1;
//st.dhip_cfg.fifo_enabel = 1;
/*
#elif defined MPU6500
const struct gyro_reg_s reg = {
.who_am_i = 0x75,
.rate_div = 0x19,
.lpf = 0x1A,
.prod_id = 0x0C,
.user_ctrl = 0x6A,
.fifo_en = 0x23,
.gyro_cfg = 0x1B,
.accel_cfg = 0x1C,
.accel_cfg2 = 0x1D,
.lp_accel_odr = 0x1E,
.motion_thr = 0x1F,
.motion_dur = 0x20,
.fifo_count_h = 0x72,
.fifo_r_w = 0x74,
.raw_gyro = 0x43,
.raw_accel = 0x3B,
.temp = 0x41,
.int_enable = 0x38,
.dmp_int_status = 0x39,
.int_status = 0x3A,
.accel_intel = 0x69,
.pwr_mgmt_1 = 0x6B,
.pwr_mgmt_2 = 0x6C,
.int_pin_cfg = 0x37,
.mem_r_w = 0x6F,
.accel_offs = 0x77,
.i2c_mst = 0x24,
.bank_sel = 0x6D,
.mem_start_addr = 0x6E,
.prgm_start_h = 0x70
#ifdef AK89xx_SECONDARY
,.raw_compass = 0x49,
.s0_addr = 0x25,
.s0_reg = 0x26,
.s0_ctrl = 0x27,
.s1_addr = 0x28,
.s1_reg = 0x29,
.s1_ctrl = 0x2A,
.s4_ctrl = 0x34,
.s0_do = 0x63,
.s1_do = 0x64,
.i2c_delay_ctrl = 0x67
#endif
};
const struct hw_s hw = {
.addr = 0x68,
.max_fifo = 1024,
.num_reg = 128,
.temp_sens = 321,
.temp_offset = 0,
.bank_size = 256
#if defined AK89xx_SECONDARY
,.compass_fsr = AK89xx_FSR
#endif
};
*/
//const struct test_s test = {
// .gyro_sens = 32768/250,
// .accel_sens = 32768/16,
// .reg_rate_div = 0, /* 1kHz. */
// .reg_lpf = 1, /* 188Hz. */
// .reg_gyro_fsr = 0, /* 250dps. */
// .reg_accel_fsr = 0x18, /* 16g. */
// .wait_ms = 50,
// .packet_thresh = 5, /* 5% */
// .min_dps = 10.f,
// .max_dps = 105.f,
// .max_gyro_var = 0.14f,
// .min_g = 0.3f,
// .max_g = 0.95f,
// .max_accel_var = 0.14f
//};
//
//static struct gyro_state_s st = {
// .reg = ®,
// .hw = &hw,
// .test = &test
//};
代码中我们还需要注释下面部分代码
struct gyro_reg_s {
unsigned char who_am_i;
unsigned char rate_div;
unsigned char lpf;
unsigned char prod_id;
unsigned char user_ctrl;
unsigned char fifo_en;
unsigned char gyro_cfg;
unsigned char accel_cfg;
//unsigned char accel_cfg2;
//unsigned char lp_accel_odr;
unsigned char motion_thr;
unsigned char motion_dur;
unsigned char fifo_count_h;
unsigned char fifo_r_w;
unsigned char raw_gyro;
unsigned char raw_accel;
unsigned char temp;
unsigned char int_enable;
unsigned char dmp_int_status;
unsigned char int_status;
//unsigned char accel_intel;
unsigned char pwr_mgmt_1;
unsigned char pwr_mgmt_2;
通过上面部分修改，DMP库基本修改完了，接下来我们就可以直接调用DMP库了。
3、MPU6050初始化
3.1/**************************实现函数*********************************************函数原型: void MPU6050_initialize(void)
*功 能: 初始化 MPU6050 以进入可用状态。
*******************************************************************************/
void MPU6050_initialize(void) {
MPU6050_setClockSource(MPU6050_CLOCK_PLL_YGYRO); //设置时钟
MPU6050_setFullScaleGyroRange(MPU6050_GYRO_FS_2000);//陀螺仪最大量程 +-1000度每秒
MPU6050_setFullScaleAccelRange(MPU6050_ACCEL_FS_2); //加速度度最大量程 +-2G
MPU6050_setSleepEnabled(0); //进入工作状态
MPU6050_setI2CMasterModeEnabled(0); //不让MPU6050 控制AUXI2C
MPU6050_setI2CBypassEnabled(0); //主控制器的I2C与 MPU6050的AUXI2C 直通。控制器可以直接访问HMC5883L
}
3.2/**************************************************************************函数功能：MPU6050内置DMP的初始化
入口参数：无
返回 值：无
作 者：shamozhihuong
**************************************************************************/
void DMP_Init(void)
{
u8 temp[1]={0};
i2cRead(0x68,0x75,1,temp);
printf("mpu_set_sensor complete ......\r\n");
if(temp[0]!=0x68)NVIC_SystemReset();
if(!mpu_init())
{
if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL))//设置传感器为陀螺仪+加速度传感器
printf("mpu_set_sensor complete ......\r\n");
if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL))//设置数据进入FIFO的传感器
printf("mpu_configure_fifo complete ......\r\n");
if(!mpu_set_sample_rate(DEFAULT_MPU_HZ))//设置采样频率为200HK，这里对于数字滤波可以考虑下
printf("mpu_set_sample_rate complete ......\r\n");
if(!dmp_load_motion_driver_firmware())//加载dmp固件
printf("dmp_load_motion_driver_firmware complete ......\r\n");
if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)))//设置陀螺仪方向，还没有搞懂为什么这样设置参数
printf("dmp_set_orientation complete ......\r\n");
if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | //设置dmp功能
DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
DMP_FEATURE_GYRO_CAL))
printf("dmp_enable_feature complete ......\r\n");
if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ)) //设置DMP输出速率(最大不超过200Hz)
printf("dmp_set_fifo_rate complete ......\r\n");
run_self_test(); //自检
if(!mpu_set_dmp_state(1)) //使能DMP
printf("mpu_set_dmp_state complete ......\r\n");
}
}
4、姿态换算函数
/**************************************************************************
函数功能：读取MPU6050内置DMP的姿态信息
入口参数：无
返回 值：无
作 者：shamozhihuong
**************************************************************************/
void Read_DMP(void)
{
unsigned long sensor_timestamp;
unsigned char more;
long quat[4];
dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
if (sensors & INV_WXYZ_QUAT )
{
q0=quat[0] / q30;
q1=quat[1] / q30;
q2=quat[2] / q30;
q3=quat[3] / q30;
Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;
Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw
}
printf("俯仰角:%f\n",Pitch);
printf("横滚角:%f\n",Roll);
printf("航向角:%f\n",Yaw);
gx=gyro[0];
gy=gyro[1];
gz=gyro[2];
ax=accel[0];
ay=accel[1];
az=accel[2];
printf("gx:%f\n",gx);
printf("gy:%f\n",gy);
printf("gz:%f\n",gz);
printf("ax:%f\n",ax);
printf("ay:%f\n",ay);
printf("az:%f\n",az);
}
四、试验验证
五、代码
|
GD32
4442 人阅读
|
1 人回复
|
2016-02-22
/3 
