1.硬件：mpu9250， stm32f103zet6。
2.inv_mpu.h文件，一种基于i2c的直觉陀螺仪的驱动器，所含函数有(移植官方MSP430的DMP驱动过来)：
1.static int set_int_enable(unsigned char enable) 模块中断使能函数
2.int mpu_reg_dump(void) 测试打印函数
3.int mpu_read_reg(unsigned char reg, unsigned char *data) 3.向芯片读寄存器值，除了.MEMERY和FIFO
4.int mpu_init(void) MPU9250的初始化
5.int mpu_lp_accel_mode(unsigned char rate) 进入低功耗模式
6.int mpu_get_gyro_reg(short *data, unsigned long *timestamp) 获取新的原始陀螺仪数据
7.int mpu_get_accel_reg(short *data, unsigned long *timestamp获取新的原始加速度数据
8.int mpu_get_temperature(long *data, unsigned long *timestamp) 获取新的温度数据
9.int mpu_set_accel_bias(const long *accel_bias) 偏差配置函数
10.int mpu_reset_fifo(void) 重置FIFO函数
11.int mpu_get_gyro_fsr(unsigned short *fsr) 获得陀螺仪全尺寸范围函数
12.int mpu_set_gyro_fsr(unsigned short fsr) 设置陀螺仪全尺寸范围函数
13.int mpu_get_accel_fsr(unsigned char *fsr) 获得加速度全尺寸范围函数
14.int mpu_set_accel_fsr(unsigned char fsr) 配置加速度全尺寸范围函数
15.int mpu_get_lpf(unsigned short *lpf) .获得DLPF范围函数
16.int mpu_set_lpf(unsigned short lpf) 配置DLPF范围函数
17.int mpu_get_sample_rate(unsigned short *rate) 获得采样频率范围函数
18.int mpu_set_sample_rate(unsigned short rate) 配置采样频率范围函数
19.int mpu_get_compass_sample_rate(unsigned short *rate) 获得罗盘采样频率范围函数
20.int mpu_set_compass_sample_rate(unsigned short rate) 配置罗盘采样频率范围函数
21.int mpu_get_gyro_sens(float *sens) 获得陀螺仪灵敏度比例因子函数
22.int mpu_get_accel_sens(unsigned short *sens) 获得加速计灵敏度比例因子函数
23.int mpu_get_fifo_config(unsigned char *sensors) 获得开启的FIFO通道函数
24.int mpu_configure_fifo(unsigned char sensors) 配置开启FIFO通道函数
25.int mpu_get_power_state(unsigned char *power_on) 获得芯片工作状态
26.int mpu_set_sensors(unsigned char sensors) 配置传感器的时钟和工作状态函数
27.int mpu_get_int_status(short *status).获得中断状态函数
28.int mpu_read_fifo(short *gyro, short *accel, unsigned long *timestamp,unsigned char *sensors, unsigned char *more) 获得FIFO数据函数
29.int mpu_read_fifo_stream(unsigned short length, unsigned char *data,unsigned char *more) 获得FIFO数据长度函数
30.int mpu_set_bypass(unsigned char bypass_on) 设置旁路模式函数
31.int mpu_set_int_level(unsigned char active_low) 设置中断优先级函数
32.int mpu_set_int_latched(unsigned char enable) 设置中断锁存函数-
设置自检函数
33.static int get_st_biases(long *gyro, long *accel, unsigned char hw_test) 获取所有的偏差值函数
34.int mpu_run_self_test(long *gyro, long *accel) 行自检值函数
35.int mpu_write_mem(unsigned short mem_addr, unsigned short length,unsigned char *data) 向DMP写记忆函数
36.int mpu_read_mem(unsigned short mem_addr, unsigned short length,unsigned char *data) 向DMP读记忆函数
37.int mpu_load_firmware(unsigned short length, const unsigned char*firmware,unsigned short start_addr, unsigned short sample_rate) 加载并验证DMP映像函数
38.int mpu_set_dmp_state(unsigned char enable) DMP状态控制函数
39.int mpu_get_dmp_state(unsigned char *enabled) DMP状态读取函数
2.创建IIC协议。可实现包括 IIC 的初始化（IO 口）、IIC 开始、IIC 结束、ACK、IIC读写等功能即可，在其他函数里面，只需要调用相关的 IIC 函数就可以和外部 IIC 器件通信。部分代码如下：
//IIC 初始化
void IIC_Init(void)
{
GPIO_InitTypeDef GPIO_Initure;
__HAL_RCC_GPIOB_CLK_ENABLE(); //使能 GPIOB 时钟
//PH4,5 初始化设置
GPIO_Initure.Pin=GPIO_PIN_10|GPIO_PIN_11;
GPIO_Initure.Mode=GPIO_MODE_OUTPUT_PP; //推挽输出
GPIO_Initure.Pull=GPIO_PULLUP; //上拉
GPIO_Initure.Speed=GPIO_SPEED_FREQ_HIGH;//高速
HAL_GPIO_Init(GPIOB,&GPIO_Initure);
IIC_SDA=1;
IIC_SCL=1;
}
//产生 IIC 起始信号
void IIC_Start(void)
{
SDA_OUT(); //sda 线输出
IIC_SDA=1;
IIC_SCL=1;
delay_us(4);
IIC_SDA=0;//START:when CLK is high,DATA change form high to low
delay_us(4);
IIC_SCL=0;//钳住 I2C 总线，准备发送或接收数据
}
//产生 IIC 停止信号
void IIC_Stop(void)
{
SDA_OUT();//sda 线输出
IIC_SCL=0;
IIC_SDA=0;//STOP:when CLK is high DATA change form low to high
delay_us(4);
IIC_SCL=1;
IIC_SDA=1;//发送 I2C 总线结束信号
delay_us(4);
}
//等待应答信号到来
//返回值：1，接收应答失败
// 0，接收应答成功
u8 IIC_Wait_Ack(void)
{
u8 ucErrTime=0;
SDA_IN(); //SDA 设置为输入
IIC_SDA=1;delay_us(1);
IIC_SCL=1;delay_us(1);
while(READ_SDA)
{
ucErrTime++;
if(ucErrTime>250)
{
IIC_Stop();
return 1;
}
}
IIC_SCL=0;//时钟输出 0
return 0;
}
//产生 ACK 应答
void IIC_Ack(void)
{
IIC_SCL=0;
SDA_OUT();
IIC_SDA=0;
delay_us(2);
IIC_SCL=1;
delay_us(2);
IIC_SCL=0;
}
//不产生 ACK 应答
void IIC_NAck(void)
{
IIC_SCL=0;
SDA_OUT();
IIC_SDA=1;
delay_us(2);
IIC_SCL=1;
delay_us(2);
IIC_SCL=0;
}
//IIC 发送一个字节
//返回从机有无应答
//1，有应答
//0，无应答
void IIC_Send_Byte(u8 txd)
{
u8 t;
SDA_OUT();
IIC_SCL=0;//拉低时钟开始数据传输
for(t=0;t<8;t++)
{
IIC_SDA=(txd&0x80)>>7;
txd<<=1;
delay_us(2);
IIC_SCL=1;
delay_us(2);
IIC_SCL=0;
delay_us(2);
}
}
//读 1 个字节，ack=1 时，发送 ACK，ack=0，发送 nACK
u8 IIC_Read_Byte(unsigned char ack)
{
unsigned char i,receive=0;
SDA_IN();//SDA 设置为输入
for(i=0;i<8;i++ )
{
IIC_SCL=0;
delay_us(2);
IIC_SCL=1;
receive<<=1;
if(READ_SDA)receive++;
delay_us(1);
}
if (!ack)
IIC_NAck();//发送 nACK
else
IIC_Ack(); //发送 ACK
return receive;
}可以运用stm32cubemx建立iic，这里可以借鉴别人的，不多赘述。
4.四元函数解算
short gryox,gyroy,gyroz;
short accx,accy,accz;
void IMU_GetData(void)
{
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);
MPU_Get_Accelerometer(&accx,&accy,&accz);}
//定义各变量
#define RtA 57.295779f //弧度->角度
#define AtR 0.0174533f //角度->弧度
#define Acc_G 0.0011963f
#define Gyro_G 0.0610351f
#define Gyro_Gr 0.0010653f#define Kp 18.0f
#define Ki 0.008f
#define halfT 0.008f
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
float exInt = 0, eyInt = 0, ezInt = 0;
float Angle[3]={0};//最终角度
void IMU_Update(short gyrox,short gyroy,short gyroz,short accx,short accy,short accz)
{
float ax=accx,ay=accy,az=accz;
float gx=gyrox,gy=gyroy,gz=gyroz;
float norm;
float vx, vy, vz;
float ex, ey, ez;
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
float q1q1 = q1*q1;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
if(ax*ay*az==0)//此时任意方向角加速度为0
return;
gx *= Gyro_Gr;
gy *= Gyro_Gr;
gz *= Gyro_Gr;
//对速度值归一化norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax /norm;
ay = ay / norm;
az = az / norm;// 提取姿态矩阵的重力分量
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3 ;// 得到误差向量，并进行积分而消除
ex = (ay*vz - az*vy) ;
ey = (az*vx - ax*vz) ;
ez = (ax*vy - ay*vx) ;exInt = exInt + ex * Ki;
eyInt = eyInt + ey * Ki;
ezInt = ezInt + ez * Ki;// 互补滤波，将误差输入Pid控制器与本次姿态更新中陀螺仪测得的角速度相加，得到一个修正的角速度值，获得的修正的角速度值去更新四元素，从而获得准确的姿态角信息。
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;// 解四元数
q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;// 四元数归一化
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
//求解姿态角（欧拉角）
ANgle[0] = asin(-2 * q1 * q3 + 2 * q0* q2)* RtA ; //pitch
ANgle[1] = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* RtA; //roll ANgle[2] =atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* RtA; // yaw
}
4.“inv_mpu.c”和“inv_mpu_dmp_motion_driver.c”的文件的调用
现移植mpu9250的官方DMP库中eMpl。
关于inv_mpu.c：
1.因为所要移植的DMP库原先是基于STM32F4 的，可将关于msp430的文件注释掉。
2.加上#include mpu9250.h，#include usart.h（用于串口输出）。
3.定义所用传感器以及驱动（可用MSP430驱动）
4.添加DMP_读取角度等
short gyro[3], accel[3], sensors; unsigned char more; long quat[4]; float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f; float pitch,roll,yaw; u8 DMP_DATA_UPDATA(void) { unsigned long sensor_timestamp; gyro_data_ready_cb(); //数据采集结束标志位 if (hal.new_gyro && hal.dmp_on) //用于计算四元数函数 { if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more)) {return 1;} if (!more) hal.new_gyro = 0; 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; yaw=atan2(2*(q1*q2+q0*q3),q0*q0+q1*q1-q2*q2-q3*q3)*57.3; } }
关于inv_mpu_dmp_motion_driver.c：
1.添加dmpKey.h和dmpmap.h文件以及串口通信文件。
2.将相关msp430的文件注释掉。
3.定义目标版采用MSP430，对f103无影响。
5.添加MPU9250的初始化程序
可根据正点原子MPU6050驱动程序进行修改：
5.1.初始化MPU9250（时钟、端口、引脚、iic总线、传感器）
5.2.设置各传感器量程
5.3.获取数字滤波器、温度以及陀螺仪原始值等功能
5.4.实现IIC连续写读，添加函数MPU_Write/Read_Len, MPU_IIC_Start/Stop等，即跟上面所诉一样进行调用iic即可
6.main.c主函数进行补充
主要完成系统初始化以及主循环功能（可对正点原子MPU6050工程main.c文件的基础上进行修改）：
while(1)
{
error = mpu_dmp_init(); //初始化mpu_dmp库
printf("ERROR:%d
",error); //串口显示初始化错误值
switch (error) //对不同错误值类型判断
{
case 0:printf("DMP库初始化正常
");break;
case 1:printf("设置传感器失败
");break;
case 2:printf("设置FIFO失败
");break;
case 3:printf("设置采样率失败
");break;
case 4:printf("加载dmp固件失败
");break;
case 5:printf("设置陀螺仪方向失败
");break;
case 6:printf("设置dmp功能失败
");break;
case 7:printf("设置DMP输出速率失败
");break;
case 8:printf("自检失败
");break;
case 9:printf("使能DMP失败
");break;
case 10:printf("初始化MPU6050失败
");break;
default :printf("未知错误
");break;
}
if(error == 0)break; //如果没有错误直接退出循环
void MPU_Read(void)
{
if(mpu_dmp_get_data(&yaw,&pitch,&roll)==0) //dmp处理得到数据，对返回值进行判断
{
printf("Pitch:%f Roll:%f Yaw:%f
",pitch,roll,yaw);//串口输出三轴角度
mpu9250.speed++; //显示速度自加
if(mpu9250.speed == 1) //显示速度阈值设置
{
mpu9250.flag = 1; //采集成功标志位设置为有效
mpu9250.speed = 0; //显示速度归零
}
}
else //采集不成功
{
mpu9250.flag = 0; //采集成功标志位设置为无效
}
}
/**
* @brief MPU6050数据上报
* @param 无
* @retval 无
*/
void DATA_Report(void)
{
if(mpu9250.flag == 1) //采集成功时
{
temp=pitch*100; //赋temp为pitch
if(temp<0) //对数据正负判断，判断为负时
{
temp=-temp; //对负数据取反
sprintf((char *)tmp_buf," Pitch:-%.3d.%.2d",temp/100,temp%100);//字符串格式化命令
}
else //判断为正时
{
sprintf((char *)tmp_buf," Pitch: %.3d.%.2d",temp/100,temp%100);//字符串格式化命令
}
temp=roll*100; //赋temp为pitch
if(temp<0) //对数据正负判断，判断为负时
{
temp=-temp; //对负数据取反
sprintf((char *)tmp_buf," Roll :-%.3d.%.2d",temp/100,temp%100);//字符串格式化命令
}
else //判断为正时
{
sprintf((char *)tmp_buf," Roll : %.3d.%.2d",temp/100,temp%100);//字符串格式化命令
}
temp=yaw*100; //赋temp为pitch
if(temp<0) //对数据正负判断，判断为负时
{
temp=-temp; //对负数据取反
sprintf((char *)tmp_buf," Yaw :-%.3d.%.2d",temp/100,temp%100);//字符串格式化命令
}
else //判断为正时
{
sprintf((char *)tmp_buf," Yaw : %.3d.%.2d",temp/100,temp%100);//字符串格式化命令
}
mpu9250.flag = 0; //采集成功标志位设置为无效
}
else ; //防卡死
}DMP相关文件工程在下面链接：
链接：https://pan.baidu.com/s/1BJWJsD-4n0pl7FQ_z2Du0Q?pwd=zhr6 提取码：zhr6
2628
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
