九轴姿态解算源代码 C语言

九轴姿态解算源代码，C语言版，可用，包括加速度，磁场，陀螺仪融合

//=====================================================================================================
// MadgwickAHRS.c
//=====================================================================================================
//
// Implementation of Madgwick‘s IMU and AHRS algorithms.
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
//
// Date			Author          Notes
// 29/09/2011	SOH Madgwick    Initial release
// 02/10/2011	SOH Madgwick	Optimised for reduced CPU load
// 19/02/2012	SOH Madgwick	Magnetometer measurement is normalised
//
//=====================================================================================================

//---------------------------------------------------------------------------------------------------
// Header files

#include “MadgwickAHRS.h“
#include 

//---------------------------------------------------------------------------------------------------
// Definitions

#define sampleFreq	512.0f		// sample frequency in Hz
#define betaDef		0.1f		// 2 * proportional gain

//---------------------------------------------------------------------------------------------------
// Variable definitions

volatile float beta = betaDef;								// 2 * proportional gain （Kp）
volatile float q0 = 1.0f q1 = 0.0f q2 = 0.0f q3 = 0.0f;	// quaternion of sensor frame relative to auxiliary frame

//---------------------------------------------------------------------------------------------------
// Function declarations

float invSqrt（float x）;

//====================================================================================================
// Functions

//---------------------------------------------------------------------------------------------------
// AHRS algorithm update

void MadgwickAHRSupdate（float gx float gy float gz float ax float ay float az float mx float my float mz） {
	float recipNorm;
	float s0 s1 s2 s3;
	float qDot1 qDot2 qDot3 qDot4;
	float hx hy;
	float _2q0mx _2q0my _2q0mz _2q1mx _2bx _2bz _4bx _4bz _2q0 _2q1 _2q2 _2q3 _2q0q2 _2q2q3 q0q0 q0q1 q0q2 q0q3 q1q1 q1q2 q1q3 q2q2 q2q3 q3q3;

	// Use IMU algorithm if magnetometer measurement invalid （avoids NaN in magnetometer normalisation）
	if（（mx == 0.0f） && （my == 0.0f） && （mz == 0.0f）） {
		MadgwickAHRSupdateIMU（gx gy gz ax ay az）;
		return;
	}

	// Rate of change of quaternion from gyroscope
	qDot1 = 0.5f * （-q1 * gx - q2 * gy - q3 * gz）;
	qDot2 = 0.5f * （q0 * gx + q2 * gz - q3 * gy）;
	qDot3 = 0.5f * （q0 * gy - q1 * gz + q3 * gx）;
	qDot4 = 0.5f * （q0 * gz + q1 * gy - q2 * gx）;

	// Compute feedback only if accelerometer measurement valid （avoids NaN in accelerometer normalisation）
	if（!（（ax == 0.0f） && （ay == 0.0f） && （az == 0.0f））） {

		// Normalise accelerometer measurement
		recipNorm = invSqrt（ax * ax + ay * ay + az * az）;
		ax *= recipNorm;
		ay *= recipNorm;
		az *= recipNorm;   

		// Normalise magneto

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

     文件       9189  2012-02-19 17:23  madgwick_algorithm_c\MadgwickAHRS\MadgwickAHRS.c

     文件       1519  2011-10-02 16:21  madgwick_algorithm_c\MadgwickAHRS\MadgwickAHRS.h

     文件       8361  2011-10-02 16:21  madgwick_algorithm_c\MahonyAHRS\MahonyAHRS.c

     文件       1573  2011-10-02 16:21  madgwick_algorithm_c\MahonyAHRS\MahonyAHRS.h

     目录          0  2018-04-14 10:40  madgwick_algorithm_c\MadgwickAHRS

     目录          0  2018-04-14 10:40  madgwick_algorithm_c\MahonyAHRS

     目录          0  2018-04-14 10:40  madgwick_algorithm_c

----------- ---------  ---------- -----  ----

                20642                    7