最后求角度的时候好像有点错误，四元数更行没有问题。 呵呵，这是个最简单，如果想找出错误，请查看 《惯性导航》这本书


#include<c8051f330>
#include<math.h>
#include<stdio.h>
float pitch,roll,yaw;//俯仰、滚转、方位；
float pi=3.1415926;
float T
void  dealy(void)                        //延时
{
int i;
for(i=0;i<20000;i++);
}

void Oscillator_Init()                   //启用内部16M晶振
{
OSCICN    = 0x07;
}
void init_sys_wdt()                      //禁用看门狗
{
EA=0;
WDTCN=0xde;
WDTCN=0xad;
EA=1;                                   //开放中断,每个中断由它对应的中断屏蔽设置决定
                                         //EA=1，开放中断，若EA=0，禁止所有中断
}
void Port_IO_Init()                      //端口引脚初始化
{

P0MDOUT=0xFF;                           //P0引脚的输出方式配置为推挽方式
P2MDOUT=0xFF;                           //P2引脚的输出方式配置为推挽方式
P3MDOUT=0xFF;                           //P3引脚输出方式配置为推挽方式
XBR0=0x04;
XBR1=0x00;
XBR2=0x40;                
}
void Timer_Init()
{
TCON=0x40;
TMOD=0x20;
CKCON=0x10;
TH1=0xB0;
}
void uart_init()
{
SCON0=0x90;
TH1=0xA0;
CKCON=0x00;
}

void Init_Device(void)
{
Oscillator_Init();
uart_init();
AD_INIT();
  Timer_Init();
Port_IO_Init();
}
void AD_INIT()
{
EA=0;
AMX0N=0x11;
AD0LJST=0;
ADC0CN=0x01;
REF0CN=0x04;
}
void angle()  //姿态矩阵
{
//DX1，DX2，DX3分别为三个时刻段内角度增量，即：DX1=W1*T,DX2=W2*T,DX3=W3*T,其中W1，W2，W3为三个时刻段内的角速度，T为采样时间间隔，DX为三子样优化后角度增量，这是X轴，其他也表示一样的方法。
float DX,DX1,DX2,DX3,DY,DY1,DY2,DY3,DZ,DZ1,DZ2,DZ3;
float model,q0,q1,q2,q3;
float d0,d1,d2,d3;
float model_q;
float T11,T12,T13,T21,T22,T23,T31,T32,T33;
DX=DX1+DX2+DX3+(9/20)*(DY1*DZ3+DZ1*DY3)+(27/40)*(DY2*(DZ3-DZ1)+DX2*(DX3-DX1));
DY=DY1+DY2+DY3+(9/20)*(DY1*DY3+DX1*DZ3)+(27/40)*(DZ2*(DX3-DX1)+DY2*(DY3-DY1));
DZ=DZ1+DZ2+DZ3+(9/20)*(DZ1*DZ3+DY1*DX3)+(27/40)*(DX2*(DY3-DY1)+DY2*(DZ3-DZ1));
model=sqrt(DX*DX+DY*DY+DZ*DZ);//三轴角增量的模
if(model==0)
{
q0=1;
q1=0;
q2=0;
q3=0;
}
else
{
q0=cos(model/2);
q1=(DX/model)*sin(model/2);
q2=(DY/model)*sin(model/2);
q3=(DZ/model)*sin(model/2);
}
d0=q0*q0-q1*q1-q2*q2-q3*q3;
d1=q0*q1+q1*q0+q3*q2-q2*q3;
d2=q2*q0-q3*q2+q0*q3+q1*q3;
d3=q3*q0+q2*q1-q1*q2+q0*q3;
q0=d0;
q1=d1;
q2=d2;
q3=d3;
//四元数归一
model_q=sqrt(q0*q0+q1*q1+q2*q2+q3*q3);//四元数模
d0=q0/model_q;
d1=q1/model_q;
d2=q2/model_q;
d3=q3/model_q;
q0=d0;
q1=d1;
q2=d2;
q3=d3;
//
//解算姿态
T11=q0*q0+q1*q1-q2*q2-q3*q3;
T12=2*(q1*q2-q0*q3);
T13=2*(q1*q3+q0*q2);
T21=2*(q1*q2+q0*q3);
T22=q0*q0-q1*q1+q2*q2-q3*q3;
T23=2*(q2*q3-q0*q1);
T31=2*(q1*q3-q0*q2);
T32=2*(q2*q3+q0*q1);
T33=q0*q0-q1*q1-q2*q2+q3*q3;//pitch为俯仰角，roll为滚转，yaw为航向
pitch=asin(T32);
if(T22==0&&T12>0)
yaw=pi/2;
if(T22==0&&T12<0)
yaw=-pi/2;
if(T22>0&&T12>0)
yaw=atan(-T31/T33);
if(T22>0&&T12<0)
yaw=atan(-T31/T33);
if(T22<0&&T12>0)
yaw=atan(-T31/T33)+2*pi;
if(T22<0&&T12<0)
yaw=atan(-T31/T33)-2*pi;

roll=-atan(T12/T11);
if(T12>0)
{
if(T11<0)
roll=pi;
}
else
{
if((T11<0)&&(T12>0))
roll=roll-pi;
if((T11<0)&&(T12<0))
roll=pi+roll;
}
}
}
void calibrate_w()
{
}
void uart0()
{
}
void BD()
{
}
viod main()
{
Init_Device;
BD();
}