完善资料让更多小伙伴认识你，还能领取20积分哦， 立即完善>
3天内不再提示
电子发烧友论坛|
#include"imu.h"
#include #define pi 3.14159265f #define Kp 0.8f #define Ki 0.001f #define halfT 0.004f float idata q0=1,q1=0,q2=0,q3=0; float idata exInt=0,eyInt=0,ezInt=0; void dmp(float gx, float gy, float gz, float ax, float ay, float az,double *rx,double *ry,double *rz) { float idata norm; float idata vx, vy, vz; float idata ex, ey, ez; float idata q0q0 = q0*q0; float idata q0q1 = q0*q1; float idata q0q2 = q0*q2; float idata q0q3 = q0*q3; float idata q1q1 = q1*q1; float idata q1q2 = q1*q2; float idata q1q3 = q1*q3; float idata q2q2 = q2*q2; float idata q2q3 = q2*q3; float idata q3q3 = q3*q3; norm = sqrt(ax*ax + ay*ay + az*az); ax = ax /norm; ay = ay / norm; az = az / norm; vx = 2*(q1q3 - q0q2); vy = 2*(q0q1 + q2q3); vz = q0q0 - q1q1 - q2q2 + q3q3 ; ex = (ay*vz - az*vy) ; ey = (az*vx - ax*vz) ; ez = (ax*vy - ay*vx) ; exInt = exInt + ex * Ki; eyInt = eyInt + ey * Ki; ezInt = ezInt + ez * Ki; gx = gx + Kp*ex + exInt; gy = gy + Kp*ey + eyInt; gz = gz + Kp*ez + ezInt; q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT; q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT; q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT; q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT; norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); q0 = q0 / norm; q1 = q1 / norm; q2 = q2 / norm; q3 = q3 / norm; *rx=asin(2*(q0*q2-q1*q3 ))* 57.2957795f; // 俯仰 *ry=asin(2*(q0*q1+q2*q3 ))* 57.2957795f; // 横滚 *rz=atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1 -q2*q2-q3*q3) * 57.3; } imu(Angle_gx*0.0174533,Angle_gy*0.0174533,Angle_gz*0.0174533,Angle_ax,Angle_ay,Angle_az); 其中数据都是经过原始数据/LSB的。 然后发现了得到的数据不正常，因为得到的数据都是随着时间缓慢变化，不是跟随着芯片运动而变化的 
0
|
|
相关推荐
7个回答
|
|
|
12T的单片机，你跑浮点数没有死机就算你幸运了。用这个已经淘汰的、慢得比蜗牛还慢的不行的。你必须换速度快，特别是运算浮点数能力强的。STC15W4K48S4这个便宜，7块钱一块。最好是STM32F4。我最开始不懂，用89做，直接跪了。后来换了STC15W4K48S4系列，但是遇到了很多奇葩问题。有时候某寄存器某位写1了就直接不干活了。最后用的STM32F405RGT6.感觉非常不错，就是死贵。22块钱涨到36块钱。
|
|
|
|
|
|
楼主的问题挺常见的,甚至我自己也遇到过.我本身使用的是DMP,所以对这段程序也一直不够理解.不过可以确定的是,数据缓慢的问题可以提高Kp的值,有人和我说这个值应该在2.0左右.你不妨试试.
|
|
|
|
|
|
是不是因为我用的是51单片机的原因 速度太慢
|
|
|
|
|
|
有可能哦
|
|
|
|
|
|
不知道你的51是哪个厂家的51。STC15W4K48S4系列有个开源的四轴，你可以去参考一下
|
|
|
|
|
|
有可能...事实上用51单片机做四轴是比较麻烦的...
|
|
|
|
|
|
这芯片太古老了
|
|
|
|
|
只有小组成员才能发言，加入小组>>
请问下图大疆lightbridge2遥控器主板电源芯片型号是什么？
4444 浏览 1 评论
使用常见的二极管、三极管和mos做MCU和模组的电平转换电路，但是模组和MCU无法正常通信，为什么？
316浏览 2评论
为了提高USIM卡电路的可靠性和稳定性，在电路设计中须注意的点有哪些？
305浏览 2评论
316浏览 2评论
324浏览 2评论
357浏览 2评论
/6 
关注我们的微信
下载发烧友APP
电子发烧友观察
版权所有 © 湖南华秋数字科技有限公司
电子发烧友 （电路图） 湘公网安备 43011202000918 号 电信与信息服务业务经营许可证：合字B2-20210191
工商网监
湘ICP备2023018690号
1224
评论
淘帖
4607
