/*
??????____??????????????????????_____??????????????????+---+
?????/?___\?????????????????????/?__?\?????????????????|?R?|
????/?/????????????????????????/?/_/?/?????????????????+---+
???/?/???________??____??___??/?____/___??____??__???__
??/?/??/?___/?__?‘/_??/?/?_?\/?/???/?__?\/?_??\/?/??/?/
?/?/__/?/??/?/_/?/?/?/_/??__/?/???/?/_/?/?/?/?/?/__/?/
?\___/_/???\___/?/___/\___/_/????\___?/_/?/_/____??/
?????????????????????????????????????????????????/?/
????????????????????????????????????????????____/?/
???????????????????????????????????????????/_____/
Filename:	IMUSO3.c
Author:		祥?、nieyong
說明：這是Crazepony軟件姿態解算融合文件，Crazepony已經不再使用DMP硬件解算
Part?of?this?algrithom?is?referred?from?pixhawk.
------------------------------------
*/

#include?“stm32f10x.h“
#include?“stm32f10x_it.h“
#include?
#include?“IMU.h“
#include?“IMUSO3.h“

//!?Auxiliary?variables?to?reduce?number?of?repeated?operations
static?float?q0?=?1.0f?q1?=?0.0f?q2?=?0.0f?q3?=?0.0f;	/**?quaternion?of?sensor?frame?relative?to?auxiliary?frame?*/
static?float?dq0?=?0.0f?dq1?=?0.0f?dq2?=?0.0f?dq3?=?0.0f;	/**?quaternion?of?sensor?frame?relative?to?auxiliary?frame?*/
static?float?gyro_bias[3]?=?{0.0f?0.0f?0.0f};?/**?bias?estimation?*/
static?float?q0q0?q0q1?q0q2?q0q3;
static?float?q1q1?q1q2?q1q3;
static?float?q2q2?q2q3;
static?float?q3q3;
static?uint8_t?bFilterInit?=?0;

//函數名：invSqrt（void）
//描述：求平方根的倒數
//該函數是經典的Carmack求平方根算法，效率極高，使用魔數0x5f375a86
static?float?invSqrt（float?number）?
{
????volatile?long?i;
????volatile?float?x?y;
????volatile?const?float?f?=?1.5F;

????x?=?number?*?0.5F;
????y?=?number;
????i?=?*?（（?long?*?）?&y）;
????i?=?0x5f375a86?-?（?i?>>?1?）;
????y?=?*?（（?float?*?）?&i）;
????y?=?y?*?（?f?-?（?x?*?y?*?y?）?）;
????return?y;
}

//!?Using?accelerometer?sense?the?gravity?vector.
//!?Using?magnetometer?sense?yaw.
static?void?NonlinearSO3AHRSinit（float?ax?float?ay?float?az?float?mx?float?my?float?mz）
{
????float?initialRoll?initialPitch;
????float?cosRoll?sinRoll?cosPitch?sinPitch;
????float?magX?magY;
????float?initialHdg?cosHeading?sinHeading;

????initialRoll?=?atan2（-ay?-az）;
????initialPitch?=?atan2（ax?-az）;

????cosRoll?=?cosf（initialRoll）;
????sinRoll?=?sinf（initialRoll）;
????cosPitch?=?cosf（initialPitch）;
????sinPitch?=?sinf（initialPitch）;

????magX?=?mx?*?cosPitch?+?my?*?sinRoll?*?sinPitch?+?mz?*?cosRoll?*?sinPitch;

????magY?=?my?*?cosRoll?-?mz?*?sinRoll;

????initialHdg?=?atan2f（-magY?magX）;

????cosRoll?=?cosf（initialRoll?*?0.5f）;
????sinRoll?=?sinf（initialRoll?*?0.5f）;

????cosPitch?=?cosf（initialPitch?*?0.5f）;
????sinPitch?=?sinf（initialPitch?*?0.5f）;

????cosHeading?=?cosf（initialHdg?*?0.5f）;
????sinHeading?=?sinf（initialHdg?*?0.5f）;

????q0?=?cosRoll?*?cosPitch?*?cosHeading?+?sinRoll?*?sinPitch?*?sinHeading;
????q1?=?sinRoll?*?cosPitch?*?cosHeading?-?cosRoll?*?sinPitch?*?sinHeading;
????q2?=?cosRoll?*?sinPitch?*?cosHeading?+?sinRoll?*?cosPitch?*?sin