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


#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 感谢分享
来自:航空航天 / 喷气推进
5
 
已屏蔽 原因:{{ notice.reason }}已屏蔽
{{notice.noticeContent}}
~~空空如也
warmonkey
13年3个月前 IP:未同步
319177
优化力度不够呀[s:274]
引用
评论
加载评论中,请稍候...
200字以内,仅用于支线交流,主线讨论请采用回复功能。
折叠评论
ltl
13年3个月前 IP:未同步
319197
引用第1楼warmonkey于2011-08-29 20:19发表的  :
优化力度不够呀[s:274]

要不嵌入式ASM写一遍?
引用
评论
加载评论中,请稍候...
200字以内,仅用于支线交流,主线讨论请采用回复功能。
折叠评论
vontier
11年0个月前 IP:未同步
652932
c语言白学了T^T果断看不懂。。。。。
引用
评论
加载评论中,请稍候...
200字以内,仅用于支线交流,主线讨论请采用回复功能。
折叠评论
问江南
10年11个月前 IP:未同步
656604
楼主可以简单讲解下四元数姿态的原理吗? 希望楼主不吝赐教
引用
评论
加载评论中,请稍候...
200字以内,仅用于支线交流,主线讨论请采用回复功能。
折叠评论

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

所属专业
所属分类
上级专业
同级专业
xuetianwuran
学者 笔友
文章
10
回复
21
学术分
1
2011/08/21注册,6年10个月前活动
暂无简介
主体类型:个人
所属领域:无
认证方式:邮箱
IP归属地:未同步
文件下载
加载中...
{{errorInfo}}
{{downloadWarning}}
你在 {{downloadTime}} 下载过当前文件。
文件名称:{{resource.defaultFile.name}}
下载次数:{{resource.hits}}
上传用户:{{uploader.username}}
所需积分:{{costScores}},{{holdScores}}下载当前附件免费{{description}}
积分不足,去充值
文件已丢失

当前账号的附件下载数量限制如下:
时段 个数
{{f.startingTime}}点 - {{f.endTime}}点 {{f.fileCount}}
视频暂不能访问,请登录试试
仅供内部学术交流或培训使用,请先保存到本地。本内容不代表科创观点,未经原作者同意,请勿转载。
音频暂不能访问,请登录试试
支持的图片格式:jpg, jpeg, png
插入公式
评论控制
加载中...
文号:{{pid}}
投诉或举报
加载中...
{{tip}}
请选择违规类型:
{{reason.type}}

空空如也

加载中...
详情
详情
推送到专栏从专栏移除
设为匿名取消匿名
查看作者
回复
只看作者
加入收藏取消收藏
收藏
取消收藏
折叠回复
置顶取消置顶
评学术分
鼓励
设为精选取消精选
管理提醒
编辑
通过审核
评论控制
退修或删除
历史版本
违规记录
投诉或举报
加入黑名单移除黑名单
查看IP
{{format('YYYY/MM/DD HH:mm:ss', toc)}}