MPU9250九轴官方MPL库移植STM32F407例程

MPU9250MPL官方版本 已移植好STM32F407例程寄存器版（本人也成功移植到STM32F103上需要的请另外联系），能直接接上MPU9250使用，运行稳定后yaw角不漂移。上传只是希望更多的人开发九轴MPL库，然后可以多多交流学习。

/************************************************************************/
/*********                     AHRS.C                       *************/
/**********          Written By LYM---20170304              *************/
/**********                  Version 1.0			      ***************/
/************************************************************************/
#include “AHRS.h“

//-----------------------------------------------------------------------
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
//-----------------------------------------------------------------------
//----------------------数据变量定义-------------------------------------
//-----------------------------------------------------------------------
//=======================================================================

SENSOR_DATA Accoffset;//加速度偏移量
SENSOR_DATA Gyrooffset;//陀螺仪偏移量
SENSOR_DATA Magoffset;//磁力计偏移量


//处理后的数据
extern IMU_DATA GyroFinal;
extern IMU_DATA AccFinal;
extern IMU_DATA MagFinal;

/*
#define Kp 2.0f	//比例增益支配收敛率accellrometer/magnetometer
#define Ki 0.005f //积分增益执行速率陀螺仪的衔接gyroscopeases
#define halfT 0.002f//采样周期的一半，若周期为10ms则一般为0.005s*/

float temp_AHRS;//温度
extern float Pitch2;
extern float Roll2;
extern float Yaw2;
extern short mxmymz;

extern short gyro[3] accel[3];

#define sampleFreq	50.0f		// sample frequency in Hz  采样率 100 HZ  10ms  修改此频率可增加变化速度
#define betaDef		0.1f		// 2 * proportional gain


volatile float beta = betaDef;								// 2 * proportional gain （Kp）
volatile float qq0 = 1.0f qq1 = 0.0f qq2 = 0.0f qq3 = 0.0f;	// quaternion of sensor frame relativ


float exInt = 0 eyInt = 0ezInt = 0;//按比例缩小积分误差


//-----------------------------------------------------------------------
//----------------------具体函数实现-------------------------------------
//=======================================================================
//快速逆平方根
float invSqrt（float x）
{
	float halfx = 0.5f * x;
	float y = x;
	long i = *（long*）&y;
	i = 0x5f3759df - （i>>1）;
	y = *（float*）&i;
	y = y * （1.5f - （halfx * y * y））;
	return y;
}


//获取MPU9250数据
void Get_MPU9250_DATA（void）
{
	MPU9250_Get_Gyroscope（&gyro[0]&gyro[1]&gyro[2]）;//读取陀螺仪数据-ADC数字量
	MPU9250_Get_Accelerometer（&accel[0]&accel[1]&accel[2]）;//读取加速度计数据-ADC数字量
	MPU9250_Get_Mag（&mx&my&mz）;//读取磁力计数据-ADC数字量
	MPU9250_Get_Temperature（&temp_AHRS）;						  //读取温度-温度模拟量
}


void AHRS_Dataprepare（void）
{
	Get_MPU9250_DATA（）;//先获取数据
	
	//16.4 = 2^16/4000 lsb °/s     1/16.4=0.061     0.0174 = 3.14/180
	//陀螺仪数据从ADC转化为弧度每秒（这里需要减去偏移值）
	GyroFinal.X=（gyro[0]-Gyrooffset.X）*0.061*0.0174;
	GyroFinal.Y=（gyro[1]-Gyrooffset.Y）*0.061*0.0174;
	GyroFinal.Z=（gyro[2]-Gyrooffset.Z）*0.061*0.0174;		//读出值减去基准值乘以单位，计算陀螺仪角速度
	/*
	AccFinal.X=（float）（Accbuf.X-Accoffset.X）;	
	AccFinal.Y=（float）（Accbuf.Y-Accoffset.Y）;		
	AccFinal.Z=（float）（Accbuf.Z-Accoffset.Z）; 
	*/
	//+-8g2^16/16=4096lsb/g--0.244mg/lsb
	//此处0.0098是：（9.8m/s^2）/1000乘以mg得m

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件      12570  2017-03-05 23:45  STM32F407 MPU9250-MPL版本 成功CSDN\HARDWARE\AHRS\AHRS.c

     文件       3103  2017-03-05 00:33  STM32F407 MPU9250-MPL版本 成功CSDN\HARDWARE\AHRS\AHRS.h

     文件       2560  2016-09-13 14:50  STM32F407 MPU9250-MPL版本 成功CSDN\HARDWARE\KEY\key.c

     文件       1240  2016-09-13 14:00  STM32F407 MPU9250-MPL版本 成功CSDN\HARDWARE\LED\LED.c

     文件       6548  2016-10-14 22:50  STM32F407 MPU9250-MPL版本 成功CSDN\HARDWARE\MPU6050\MPU6050.c

     文件      19342  2016-04-18 16:59  STM32F407 MPU9250-MPL版本 成功CSDN\HARDWARE\MPU9250\MPL\driver\eMPL\dmpKey.h

     文件       6765  2016-04-18 16:59  STM32F407 MPU9250-MPL版本 成功CSDN\HARDWARE\MPU9250\MPL\driver\eMPL\dmpmap.h

     文件     107417  2017-03-06 13:21  STM32F407 MPU9250-MPL版本 成功CSDN\HARDWARE\MPU9250\MPL\driver\eMPL\inv_mpu.c

     文件       5278  2016-04-18 16:59  STM32F407 MPU9250-MPL版本 成功CSDN\HARDWARE\MPU9250\MPL\driver\eMPL\inv_mpu.h

     文件      58394  2017-03-06 13:15  STM32F407 MPU9250-MPL版本 成功CSDN\HARDWARE\MPU9250\MPL\driver\eMPL\inv_mpu_dmp_motion_driver.c

     文件       3538  2016-04-18 16:59  STM32F407 MPU9250-MPL版本 成功CSDN\HARDWARE\MPU9250\MPL\driver\eMPL\inv_mpu_dmp_motion_driver.h

     文件      10454  2016-04-18 16:59  STM32F407 MPU9250-MPL版本 成功CSDN\HARDWARE\MPU9250\MPL\driver\include\log.h

     文件       1018  2016-04-18 16:59  STM32F407 MPU9250-MPL版本 成功CSDN\HARDWARE\MPU9250\MPL\driver\include\mlinclude.h

     文件       2268  2016-04-18 16:59  STM32F407 MPU9250-MPL版本 成功CSDN\HARDWARE\MPU9250\MPL\driver\include\mlmath.h

     文件       2134  2016-04-18 16:59  STM32F407 MPU9250-MPL版本 成功CSDN\HARDWARE\MPU9250\MPL\driver\include\mlos.h

     文件       9518  2016-04-18 16:59  STM32F407 MPU9250-MPL版本 成功CSDN\HARDWARE\MPU9250\MPL\driver\include\mltypes.h

     文件      11757  2016-04-18 16:59  STM32F407 MPU9250-MPL版本 成功CSDN\HARDWARE\MPU9250\MPL\driver\include\mpu.h

     文件        860  2016-04-18 16:59  STM32F407 MPU9250-MPL版本 成功CSDN\HARDWARE\MPU9250\MPL\driver\include\stdint_invensense.h

     文件       6118  2016-04-18 16:59  STM32F407 MPU9250-MPL版本 成功CSDN\HARDWARE\MPU9250\MPL\driver\stm32L\log_stm32.c

     文件       1546  2016-04-18 16:59  STM32F407 MPU9250-MPL版本 成功CSDN\HARDWARE\MPU9250\MPL\driver\stm32L\packet.h

     文件      11066  2017-03-06 13:22  STM32F407 MPU9250-MPL版本 成功CSDN\HARDWARE\MPU9250\MPL\eMPL-hal\eMPL_outputs.c

     文件       1316  2016-04-18 16:59  STM32F407 MPU9250-MPL版本 成功CSDN\HARDWARE\MPU9250\MPL\eMPL-hal\eMPL_outputs.h

     文件      45891  2016-04-18 16:59  STM32F407 MPU9250-MPL版本 成功CSDN\HARDWARE\MPU9250\MPL\mllite\data_builder.c

     文件       9509  2016-04-18 16:59  STM32F407 MPU9250-MPL版本 成功CSDN\HARDWARE\MPU9250\MPL\mllite\data_builder.h

     文件      18333  2016-04-18 16:59  STM32F407 MPU9250-MPL版本 成功CSDN\HARDWARE\MPU9250\MPL\mllite\hal_outputs.c

     文件       1703  2016-04-18 16:59  STM32F407 MPU9250-MPL版本 成功CSDN\HARDWARE\MPU9250\MPL\mllite\hal_outputs.h

     文件        660  2016-04-18 16:59  STM32F407 MPU9250-MPL版本 成功CSDN\HARDWARE\MPU9250\MPL\mllite\invensense.h

     文件       1591  2016-04-18 16:59  STM32F407 MPU9250-MPL版本 成功CSDN\HARDWARE\MPU9250\MPL\mllite\message_layer.c

     文件        934  2016-04-18 16:59  STM32F407 MPU9250-MPL版本 成功CSDN\HARDWARE\MPU9250\MPL\mllite\message_layer.h

     文件       1029  2016-04-18 16:59  STM32F407 MPU9250-MPL版本 成功CSDN\HARDWARE\MPU9250\MPL\mllite\mlmath.c

............此处省略191个文件信息