体感9D姿态角计算

 最近有做一款体感的可穿戴设备，所以呢，有涉及到一些9D姿态角度计算，当然此时就涉及到一些算法，之前没有接触过四轴什么的，只是知道江湖传闻：“很难”，但是实际从应用角度---还好，现在就说说关于姿态角度思路：

     主要就三个部分：采集原始数据，欧拉角计算，磁力计补偿。。。

 一、采集数据

     用的MPU9250这款传感器，采集的原始数据包括：加速度、角速度、磁力计数据。这个可以用I2C和SPI两种方式通讯采集，具体的配置与采集详见我的博文：http://blog.csdn.net/black_yu/article/details/51815049

二、欧拉角计算

     很多大侠老是卡在这里，其实我觉得没啥必要去研究四元数算法有多么精妙，直接用老外madgwick的函数就行了：

unsigned char BS004_IMU_Update(float ax,float ay,float az,float gx,float gy,float gz) 
{
        float norm;
        float vx, vy, vz;
        float ex, ey, ez;  
  //        
        //四元数乘法运算
        float q0q0 = q0 * q0;                                                        
        float q0q1 = q0 * q1;
        float q0q2 = q0 * q2;