//=====================================================================================================
//?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