断断续续进行姿态解算已经大概有两个月了,这两个月中遇到了不少的问题.但结果还算是圆满吧. 我手里有两块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系统里面,到时候定期更新我的博客,大家敬请关注.