姿态解算c代码

/*
      ____                      _____                  +---+
     / ___\                     / __ \                 | 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