博客之路一MPU6050的dmp代码移植的过程

**

STM32之路一MPU6050的dmp代码移植的过程

**

文件下载链接
mpu6050移植
需要移植的文件

  “anbt_mpu_dmp_driver.h”
  “anbt_mpu_dmp_driver.c”
  “anbt_dmp_mpu6050.c”
  “anbt_dmp_mpu6050.h”
  "anbt_i2c.c"
  "anbt_i2c.h"

一、主函数初始化、定义
main函数返回值可以根据自己需求设置
1、四元数输出
2、欧拉角输出
如果使用VOFA+软件调试，查看姿态等信息可以在主函数添加以下代码：
VOFA+测试版软件

 printf("d0: %f,%f,%f\n",Pitch,Roll,Yaw);   //实现输出  使用VOF+软件读出

main.c

#include "system.h"
#include "SysTick.h"
#include "led.h"
#include "usart.h"


#include "anbt_dmp_fun.h"
#include "anbt_i2c.h"
#include "anbt_dmp_mpu6050.h"
#include "anbt_dmp_driver.h"


#define BYTE0(dwTemp)       (*(char *)(&dwTemp))
#define BYTE1(dwTemp)       (*((char *)(&dwTemp) + 1))
#define BYTE2(dwTemp)       (*((char *)(&dwTemp) + 2))
#define BYTE3(dwTemp)       (*((char *)(&dwTemp) + 3))

#define q30  1073741824.0f

void Data_Send_Status(float Pitch,float Roll,float Yaw,int16_t *gyro,int16_t *accel);
void Send_Data(int16_t *Gyro,int16_t *Accel);
/**
  * @brief  主函数
  * @param  无  
  * @retval 无
  */

int main()
{
	unsigned long sensor_timestamp;
	short gyro[3], accel[3], sensors;//陀螺仪存放数组，加速度存放数组，返回状态量
	unsigned char more;
	long quat[4];//四元数存放数组
	float Yaw=0.00,Roll,Pitch;//欧拉角
	float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;//计算姿态过程用到的变量
	 
	SysTick_Init(72);
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);  //中断优先级分组 分2组
	LED_Init();
	USART1_Init(115200);
	delay_ms(500);
	
	
	ANBT_I2C_Configuration();		//IIC初始化
	//delay_ms(30); 
	AnBT_DMP_MPU6050_Init();			//6050DMP初始化
	while(1)
	{
    LED1=0;
		 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; // pitch
			 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;		//感觉没有价值，注掉
//	  	 printf("pitch: %.2f    roll:%.2f		yaw:%.2f\r\n",Pitch,Roll,Yaw);		//普通串口输出
//			 printf("X_A:  %d   Y_A:  %d  Z_A:  %d\r\n",accel[0],accel[1],accel[2]);
//			 printf("                                      \r\n");
//			 printf("X_G:  %d   Y_G:  %d  Z_G:  %d\r\n",gyro[0],gyro[1],gyro[2]);
//			 printf("                                      \r\n");
			Data_Send_Status(Pitch,Roll,Yaw,gyro,accel);
//			Send_Data(gyro,accel);
             printf("d0: %f,%f,%f\n",Pitch,Roll,Yaw);   //实现输出  使用VOF+软件读出
			 delay_ms(200);

	}
		 
  }
}
	

void Data_Send_Status(float Pitch,float Roll,float Yaw,int16_t *gyro,int16_t *accel)
{
	unsigned char i=0;
	unsigned char _cnt=0,sum = 0;
	unsigned int _temp;
	u8 data_to_send[50];

	data_to_send[_cnt++]=0xAA;
	data_to_send[_cnt++]=0xAA;
	data_to_send[_cnt++]=0x01;
	data_to_send[_cnt++]=0;
	
	_temp = (int)(Roll*100);
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	_temp = 0-(int)(Pitch*100);
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	_temp = (int)(Yaw*100);
	data_to_send[_cnt++]=BYTE1(_temp);
	data_to_send[_cnt++]=BYTE0(_temp);
	
	data_to_send[3] = _cnt-4;
	//和校验
	for(i=0;i<_cnt;i++)
		sum+= data_to_send[i];
	data_to_send[_cnt++]=sum;
	
	//串口发送数据
	for(i=0;i<_cnt;i++)
		AnBT_Uart1_Send_Char(data_to_send[i]);
}

void Send_Data(int16_t *Gyro,int16_t *Accel)
{
	unsigned char i=0;
	unsigned char _cnt=0,sum = 0;
//	unsigned int _temp;
	u8 data_to_send[50];

	data_to_send[_cnt++]=0xAA;
	data_to_send[_cnt++]=0xAA;
	data_to_send[_cnt++]=0x02;
	data_to_send[_cnt++]=0;
	

	data_to_send[_cnt++]=BYTE1(Accel[0]);
	data_to_send[_cnt++]=BYTE0(Accel[0]);
	data_to_send[_cnt++]=BYTE1(Accel[1]);
	data_to_send[_cnt++]=BYTE0(Accel[1]);
	data_to_send[_cnt++]=BYTE1(Accel[2]);
	data_to_send[_cnt++]=BYTE0(Accel[2]);
	
	data_to_send[_cnt++]=BYTE1(Gyro[0]);
	data_to_send[_cnt++]=BYTE0(Gyro[0]);
	data_to_send[_cnt++]=BYTE1(Gyro[1]);
	data_to_send[_cnt++]=BYTE0(Gyro[1]);
	data_to_send[_cnt++]=BYTE1(Gyro[2]);
	data_to_send[_cnt++]=BYTE0(Gyro[2]);
	data_to_send[_cnt++]=0;
	data_to_send[_cnt++]=0;
	data_to_send[_cnt++]=0;
	
	data_to_send[3] = _cnt-4;
	//和校验
	for(i=0;i<_cnt;i++)
		sum+= data_to_send[i];
	data_to_send[_cnt++]=sum;
	
	//串口发送数据
	for(i=0;i<_cnt;i++)
		AnBT_Uart1_Send_Char(data_to_send[i]);
}