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


#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();
}
+50  科创币    warmonkey   2011-08-29   有用资料
+50  科创币    ehco   2011-08-30   感谢分享
来自 喷气推进
 
2011-8-29 20:19:20
1楼
优化力度不够呀[s:274]
折叠评论
加载评论中,请稍候...
折叠评论
2楼
引用第1楼warmonkey于2011-08-29 20:19发表的  :
优化力度不够呀[s:274]

要不嵌入式ASM写一遍?
折叠评论
加载评论中,请稍候...
折叠评论
2013-11-09 18:38:40
2013-11-9 18:38:40
3楼
c语言白学了T^T果断看不懂。。。。。
折叠评论
加载评论中,请稍候...
折叠评论
2013-12-06 10:25:04
2013-12-6 10:25:04
4楼
楼主可以简单讲解下四元数姿态的原理吗? 希望楼主不吝赐教
折叠评论
加载评论中,请稍候...
折叠评论

想参与大家的讨论?现在就 登录 或者 注册

插入资源
全部
图片
视频
音频
附件
全部
未使用
已使用
正在上传
空空如也~
上传中..{{f.progress}}%
处理中..
上传失败,点击重试
{{f.name}}
空空如也~
(视频){{r.oname}}
{{selectedResourcesId.indexOf(r.rid) + 1}}
ID:{{user.uid}}
{{user.username}}
{{user.info.certsName}}
{{user.description}}
{{format("YYYY/MM/DD", user.toc)}}注册,{{fromNow(user.tlv)}}活动
{{submitted?"":"投诉"}}
请选择违规类型:
{{reason.description}}
支持的图片格式:jpg, jpeg, png