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

		//?Normali

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2018-12-10?17:35??Madgwick??Mahony\
?????目錄???????????0??2018-10-11?17:59??Madgwick??Mahony\madgwick_algorithm_c\
?????目錄???????????0??2018-10-11?20:06??Madgwick??Mahony\madgwick_algorithm_c\MadgwickAHRS\
?????文件????????9210??2018-12-10?17:23??Madgwick??Mahony\madgwick_algorithm_c\MadgwickAHRS\MadgwickAHRS.c
?????文件????????1519??2011-10-02?16:21??Madgwick??Mahony\madgwick_algorithm_c\MadgwickAHRS\MadgwickAHRS.h
?????目錄???????????0??2018-10-11?19:29??Madgwick??Mahony\madgwick_algorithm_c\MahonyAHRS\
?????文件????????8411??2018-12-10?17:23??Madgwick??Mahony\madgwick_algorithm_c\MahonyAHRS\MahonyAHRS.c
?????文件????????1573??2011-10-02?16:21??Madgwick??Mahony\madgwick_algorithm_c\MahonyAHRS\MahonyAHRS.h
?????目錄???????????0??2018-10-11?17:59??Madgwick??Mahony\madgwick_algorithm_matlab\
?????目錄???????????0??2018-10-11?17:59??Madgwick??Mahony\madgwick_algorithm_matlab\@MadgwickAHRS\
?????文件????????4678??2011-09-28?23:40??Madgwick??Mahony\madgwick_algorithm_matlab\@MadgwickAHRS\MadgwickAHRS.m
?????目錄???????????0??2018-10-11?17:59??Madgwick??Mahony\madgwick_algorithm_matlab\@MahonyAHRS\
?????文件????????4718??2011-09-28?23:40??Madgwick??Mahony\madgwick_algorithm_matlab\@MahonyAHRS\MahonyAHRS.m
?????文件??????101819??2011-09-24?13:39??Madgwick??Mahony\madgwick_algorithm_matlab\ExampleData.mat
?????文件????????3263??2012-11-06?19:00??Madgwick??Mahony\madgwick_algorithm_matlab\Examplescript.m
?????目錄???????????0??2018-10-11?17:59??Madgwick??Mahony\madgwick_algorithm_matlab\quaternion_library\
?????文件????????2465??2011-09-27?16:51??Madgwick??Mahony\madgwick_algorithm_matlab\quaternion_library\Testscript.m
?????文件?????????650??2011-09-27?16:52??Madgwick??Mahony\madgwick_algorithm_matlab\quaternion_library\axisAngle2quatern.m
?????文件?????????965??2011-09-27?16:52??Madgwick??Mahony\madgwick_algorithm_matlab\quaternion_library\axisAngle2rotMat.m
?????文件?????????938??2011-09-27?16:52??Madgwick??Mahony\madgwick_algorithm_matlab\quaternion_library\euler2rotMat.m
?????文件?????????878??2011-09-27?16:52??Madgwick??Mahony\madgwick_algorithm_matlab\quaternion_library\quatern2euler.m
?????文件?????????825??2011-09-27?16:52??Madgwick??Mahony\madgwick_algorithm_matlab\quaternion_library\quatern2rotMat.m
?????文件?????????378??2011-09-27?16:52??Madgwick??Mahony\madgwick_algorithm_matlab\quaternion_library\quaternConj.m
?????文件?????????661??2011-09-27?16:51??Madgwick??Mahony\madgwick_algorithm_matlab\quaternion_library\quaternProd.m
?????文件?????????645??2011-09-27?16:51??Madgwick??Mahony\madgwick_algorithm_matlab\quaternion_library\rotMat2euler.m
?????文件????????1434??2011-09-27?16:51??Madgwick??Mahony\madgwick_algorithm_matlab\quaternion_library\rotMat2quatern.m