DMP解算-锐单电子商城

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.intmpu_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   eturn 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

资讯详情

DMP解算

亚马逊云科技宣布基于自研Amazon Graviton4的Amazon EC2 R8g实例正式可用

DMP解算

亚马逊云科技宣布基于自研Amazon Graviton4的Amazon EC2 R8g实例正式可用

最近热搜

历史搜索 清除历史记录

历史搜索清除历史记录