姿态解算基本完成,程序编写笔记

断断续续进行姿态解算已经大概有两个月了,这两个月中遇到了不少的问题.但结果还算是圆满吧. 我手里有两块2440 的板子,不知道是什么原因其中一块板子不能正常读取

ADXL345,只好花了几天的时间写了 2440下的模拟 IIC 程序. 网上有另外一位博友写的模拟 程序,我测试其没通过(读取的数据是错误).下面是两个月来编写程序的笔记,

已经可以成功初始化,下面进行数据读取实验

Multiple_read_ADXL345应该是读取正常
Multiple_read_L3G4200D 读取正常
数据基本测试输出正常

我估计,可能是由于bin 文件要比axf文件小的多的多,实际运行要比MDK在线调试的速度要快的多.从而导致AHRS器件不能正确响应.

下面进行数据的滤波,计划用滑变平均进行滤波   2013.3.10

2013.3.19 日滤波完成   下面进行定时器的编程

2013.3.27   已经完成 定时器的编程,下面进行角度的积分运算

2013.3.28  进行按键编程,程序AHRS传感器的切换   已经完成

下面进行真实值的计算,角度的积分

2013.3.29日已经完成 L3G4200D的积分 ,和校准.  数据存在一定的漂移

/*---------------------------------------------------------------------------------------------------------*/
发送数据  低 8位在前,高 8位在后
ADXL345 ,16g 13位模式下  3.9mg/LSB
L3G4200D  （默认 00）（00：250dps)   为8.75 mdps/digit 目前采用的   8.75*2
这些用于真实值的计算

生成的 bin可以在内存中直接运行,但是不能下载到 nand flash中运行

2013.4.12  确定使用卡尔曼滤波

2013.4.18  进行 四元数程序的编写,和与AHRS.c 算法的比较

2013.4 .27  确定  转序  yaw pitch roll   意外发现卡尔曼出现一个小错误

2013.4 .27  突然遇到一个问题, 四元数与卡尔曼的关系 ,  四元数是根据角增量 ,求出欧拉角 ,
卡尔曼可以  对加速度角度和陀螺仪角速度互补滤波  得 到  角度  和角速率  .但是如何把两者结合起来呢

方案1, 可以直接用卡尔曼 得到的 角速率 与时间乘机 得角增量   ,方案一晃动时存在严重的漂移
方案2, 既然卡尔曼可以得到角度  ,则再次 角度的差及为角增量   ,需要测试

213.4.28   如果计算欧拉角 ,则三个 角增量务必准确 ,三个角相互影响   无法只计算水平的角度

2013.4.29   验证四元数算法的正确 性 ,无滤波积分 可三维旋转
2013.4.30   更改  pitch 的反函数为 asin()
目前的 旋转  角度 只能控制在  pitch ∈{-pi/2,pi/2}  roll∈{-pi,pi}   yaw ∈{-pi,pi}

2013.5.1   增加串口查询复位功能
近期任务:
1,看完英文文档
2,把HMC5883L 航向角加入到 程序中(简单计算航向角,抑制其漂移)
3,测试 imu.c
4,测试 quaternion_imp.c

2013.5.2   日  利用下面这个算法 ,直接积分, 效果反而比加上加计的效果要好,看样子没有正确利用加速度计
//进行限幅处理 ,如果过大则是机体转动引起的 ,而不是噪声数据.
if((temp<=(bias_yaw+50))&&(temp>=(bias_yaw-50)) )
{
bias_yaw*=0.999;
bias_yaw +=(float)temp*0.001;
}
temp=temp-bias_yaw;
Yaw_inc=(float)temp*0.0003054*0.005;            //角增量  时间0.005s 200HZ
//只进行角度积分的角增量
temp=filter_0(temp_gyro_y);
if((temp<=(bias_pitch+50))&&(temp>=(bias_pitch-50)) )
{
bias_pitch*=0.999;
bias_pitch +=(float)temp*0.001;
}
temp=temp-bias_pitch;
Pitch_inc=(float)temp*0.0003054*0.005;            //角增量  时间0.005s 200HZ

temp=filter_0(temp_gyro_x);
if((temp<=(bias_roll+50))&&(temp>=(bias_roll-50)) )
{
bias_roll*=0.999;
bias_roll +=(float)temp*0.001;
}
temp=temp-bias_roll;
Roll_inc=(float)temp*0.0003054*0.005;            //角增量  时间0.005s 200HZ

目前我能想到比较好的折中的方法 是:

1,是否进行加计求 欧拉角的优化 ,不过首先要求得准确的 yaw
2,再利用矩阵旋转 ,利用加速度计计算出准确的欧拉角  .

说是 ,如果 在加计的基础上更新四元数  似乎 在角度的范围内有更好的表现 ,但是不能全姿态 .考虑是否放弃  卡尔曼

2013.5.3  日

对比 互补滤波与卡尔曼 ,互补滤波 抑制振动较好,但是相对略有滞后, 可能与 滑动平均长度有关
暂时放弃卡尔曼  ,下面进行 imu.c进行测试

2013.5.6   尝试利用模拟iic 通信    ,接口  CRO ( GPF0  GPF1 )( PA18 PA17  ) (EINT0  EINT1)    SDA  SCL
NI ( GPF0  GPF1 )( 9   10     ) (EINT0  ENIT1)

2013.5.13  日 已经基本完成 模拟IIC 程序 的编写,   但是目前存在的问题 是 ,用模拟 IIC读取的数据 略比硬件IIC读取的数据噪声大.

硬件IIC 的时间大概是   1.5ms  模拟 iic  时间 定义在 c  时间大概为  1.6ms  目前 模拟IIC 程序基本编写完成  ,不进行 硬件IIC模拟iic 读取效果的对比

目前来看,虽然模拟 IIC 读取的数据噪声略比硬件IIC 大,但是是乎不怎么明显,我也懒的去对比效果了.姿态解算算是告于段落,下面进行嵌入式linux的学习,过两个月把算法移植到

liunx系统里面,到时候定期更新我的博客,大家敬请关注.