MPU9250驱动代码

基于STM32F4系类单片机的MPU9250驱动代码

/*
The MIT License （MIT）

Copyright （c） 2015-? suhetao

Permission is hereby granted free of charge to any person obtaining a copy of
this software and associated documentation files （the “Software“） to deal in
the Software without restriction including without limitation the rights to
use copy modify merge publish distribute sublicense and/or sell copies of
the Software and to permit persons to whom the Software is furnished to do so
subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED “AS IS“ WITHOUT WARRANTY OF ANY KIND EXPRESS OR
IMPLIED INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM DAMAGES OR OTHER LIABILITY WHETHER
IN AN ACTION OF CONTRACT TORT OR OTHERWISE ARISING FROM OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

#include “Control.h“
#include “FastMath.h“

int Matrix_Inv3x3（float* A）
{
	// det = a11（a33a22-a32a23）-a21（a33a12-a32a13）+a31（a23a12-a22a13）
	// det = a00（a22a11-a21a12）-a10（a22a01-a21a02）+a20（a12a01-a11a02）
	float det;
	float M[9];
	// Invert the matrix
	/*
	| a11 a12 a13 |-1 | a33a22-a32a23 -（a33a12-a32a13） a23a12-a22a13 |
	| a21 a22 a23 | = 1/DET * | -（a33a21-a31a23） a33a11-a31a13 -（a23a11-a21a13） |
	| a31 a32 a33 | | a32a21-a31a22 -（a32a11-a31a12） a22a11-a21a12 |
	| a00 a01 a02 |-1 | a22a11-a21a12 -（a22a01-a21a02） a12a01-a11a02 |
	| a10 a11 a12 | = 1/DET * | -（a22a10-a20a12） a22a00-a20a02 -（a12a00-a10a02） |
	| a20 a21 a22 | | a21a10-a20a11 -（a21a00-a20a01） a11a00-a10a01 |
	*/
	det  = A[0] * （A[8] * A[4] - A[7] * A[5]） -
		A[3] * （A[8] * A[1] - A[7] * A[2]） +
		A[6] * （A[5] * A[1] - A[4] * A[2]）;
	// Row 1
	// M[0] = （a22a11-a21a12）/det;
	M[0] = （A[8] * A[4] - A[7] * A[5]） / det;
	// M[1] = -（a22a01-a21a02）/det;
	M[1] = -（A[8] * A[1] - A[7] * A[2]） / det;
	// M[2] = （a12a01-a11a02）/det;
	M[2] = （A[5] * A[1] - A[4] * A[2]） / det;
	// Row 2
	// M[3] = -（a22a10-a20a12）/det;
	M[3] = -（A[8] * A[3] - A[6] * A[5]） / det;
	// M[4] = （a22a00-a20a02）/det;
	M[4] = （A[8] * A[0] - A[6] * A[2]） / det;
	// M[5] = -（a12a00-a10a02）/det;
	M[5] = -（A[5] * A[0] - A[3] * A[2]） / det;
	// Row 3
	// M[6] = （a21a10-a20a11）/det;
	M[6] = （A[7] * A[3] - A[6] * A[4]） / det;
	// M[7] = -（a21a00-a20a01）/det;
	M[7] = -（A[7] * A[0] - A[6] * A[1]） / det;
	// M[8] = （a11a00-a10a01）/det;
	M[8] = （A[4] * A[0] - A[3] * A[1]） / det;

	A[0] = M[0]; A[1] = M[1]; A[2] = M[2];
	A[3] = M[3]; A[4] = M[4]; A[5] = M[5];
	A[6] = M[6]; A[7] = M[7]; A[8] = M[8];
	
	return 1;
}

//sweep
void Matrix_Inv（float *A int n）
{
	float d;
	int i j k;
	int kn kk;
	int ln lk;

	for （k = 0; k < n; ++k）{
		kn = k * n;
		kk = kn + k;

		d = 1.0f / A[kk];
		A[kk] = d;

		for （i = 0; i < n; ++i）{
			if （i != k）{
				A[kn + i] *= -d;
			}
		}

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2015-08-06 15:26  stm32f4_mpu9250-master\
     文件         223  2015-08-06 15:26  stm32f4_mpu9250-master\.gitignore
     目录           0  2015-08-06 15:26  stm32f4_mpu9250-master\Algorithm\
     目录           0  2015-08-06 15:26  stm32f4_mpu9250-master\Algorithm\inc\
     文件        4162  2015-08-06 15:26  stm32f4_mpu9250-master\Algorithm\inc\CKF.h
     文件        2578  2015-08-06 15:26  stm32f4_mpu9250-master\Algorithm\inc\Control.h
     文件        4998  2015-08-06 15:26  stm32f4_mpu9250-master\Algorithm\inc\Double.h
     文件        3729  2015-08-06 15:26  stm32f4_mpu9250-master\Algorithm\inc\EKF.h
     文件        3805  2015-08-06 15:26  stm32f4_mpu9250-master\Algorithm\inc\INS_EKF.h
     文件        2220  2015-08-06 15:26  stm32f4_mpu9250-master\Algorithm\inc\PID.h
     文件        2390  2015-08-06 15:26  stm32f4_mpu9250-master\Algorithm\inc\Quaternion.h
     文件        4908  2015-08-06 15:26  stm32f4_mpu9250-master\Algorithm\inc\SRCKF.h
     文件        4118  2015-08-06 15:26  stm32f4_mpu9250-master\Algorithm\inc\UKF.h
     目录           0  2015-08-06 15:26  stm32f4_mpu9250-master\Algorithm\src\
     文件       18341  2015-08-06 15:26  stm32f4_mpu9250-master\Algorithm\src\CKF.C
     文件        7504  2015-08-06 15:26  stm32f4_mpu9250-master\Algorithm\src\Control.c
     文件       13341  2015-08-06 15:26  stm32f4_mpu9250-master\Algorithm\src\EKF.c
     文件       22460  2015-08-06 15:26  stm32f4_mpu9250-master\Algorithm\src\INS_EKF.c
     文件        1101  2015-08-06 15:26  stm32f4_mpu9250-master\Algorithm\src\PID.c
     文件        7901  2015-08-06 15:26  stm32f4_mpu9250-master\Algorithm\src\Quaternion.c
     文件       14204  2015-08-06 15:26  stm32f4_mpu9250-master\Algorithm\src\SRCKF.c
     文件       16329  2015-08-06 15:26  stm32f4_mpu9250-master\Algorithm\src\UKF.c
     目录           0  2015-08-06 15:26  stm32f4_mpu9250-master\Application\
     目录           0  2015-08-06 15:26  stm32f4_mpu9250-master\Application\inc\
     文件        1167  2015-08-06 15:26  stm32f4_mpu9250-master\Application\inc\stm32f4_common.h
     文件        1219  2015-08-06 15:26  stm32f4_mpu9250-master\Application\inc\stm32f4_crc.h
     文件        1308  2015-08-06 15:26  stm32f4_mpu9250-master\Application\inc\stm32f4_delay.h
     文件        1928  2015-08-06 15:26  stm32f4_mpu9250-master\Application\inc\stm32f4_dmp.h
     文件        1859  2015-08-06 15:26  stm32f4_mpu9250-master\Application\inc\stm32f4_exti.h
     文件        1137  2015-08-06 15:26  stm32f4_mpu9250-master\Application\inc\stm32f4_gps.h
     文件       10610  2015-08-06 15:26  stm32f4_mpu9250-master\Application\inc\stm32f4_mpu9250.h
............此处省略224个文件信息