继上次读MPU6050的数据，作个补充，这次是读姿态角输出.内容简单易懂，标注齐全.

包子

上次上传了个MPU6050的读加速度角速度, 这次上传把加速度，角速度解算为姿态角.（德国人的方法解算，具体麻烦各位自己看数学叉积，四元数等）.

代码比较少，干净，都有注释.
依旧是串口输出，因为这里是每一轮循环延迟10ms. 而且精度比较高（用了很多float，对于飞来说我觉得是没问题的，51的我就用的float 一样fly(●'?'●)）. 如果你测试觉得不够快又允许一定误差，可以用Int整型.
（
注意：这里我并没有处理“静止”时的噪音，里面的角速度误差调整量gx，gy，gz并没有赋值. So，各位请自行添加： 常规办法比如开机，读100次x的角速度累加再除以100,得到平均误差赋值给gx即可.
）

原料： 原子哥的Stm32F4开发板（我是今年9月买的板子）.  MPU6050一个，杜邦线4条.电脑一台.hex文件一发.

用法：用原子哥教的FlyMcu 飞一会儿.（下载完）. 打开串口即可看到x，y轴的姿态角.

 

包子

迟点再上卡尔曼了.-    -. 那玩意俺还要看看....

包子

二楼还是我的，   咳咳：有什么不足的地方. 请不吝赐教.....

正点原子

看了下代码，果然很精简啊。。。不知道稳定性如何？

包子

回复【4楼】正点原子:
---------------------------------
但是，x，y计算出来是对的。所以作为一个精简版的参考分享出来吧. 你应该试了原子哥.. 你那个实验的也不错，就是太复杂了.....      当然我觉得卡尔曼会更好.       我刚刚放了半个小时，没什么问题，具体的我放到明天早上吧.  现在大疆的飞机也不会超过1个小时，所以我参考还是一个小时之内的准确性.  处理了噪音之后就可以用来弄PWM的PID处理控制电机了. 我迟点试了会上视频和代码.

包子

误差的具体检测我明天找些器材来验证再上图O    O

it_do_just

这个点还在努力，谢谢分享！！

xuyan021

我感觉这块应该有问题：

[mw_shl_code=c,true]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;[/mw_shl_code]

是不是应该改成：

[mw_shl_code=c,true]temp0 = q0 + (-q1*gx - q2*gy - q3*gz) * halfT; temp1 = q1 + ( q0*gx + q2*gz - q3*gy) * halfT; temp2 = q2 + ( q0*gy - q1*gz + q3*gx) * halfT; temp3 = q3 + ( q0*gz + q1*gy - q2*gx) * halfT; norm = sqrt(temp0*temp0 + temp1*temp1 + temp2*temp2 + temp3*temp3); q0 = temp0 / norm; q1 =temp1 / norm; q2 = temp2 / norm; q3 = temp3 / norm;[/mw_shl_code]

包子

回复【8楼】xuyan021:
---------------------------------
应该没有错哟.   我是查了下资料，这里是其中一个参考点（还有几个类似的解说我就不放出来了.都差不多） http://www.eepw.com.cn/article/247809.htm。.才放进去的.解算出来的角度也没有错，最多因为噪音存在1-2°误差.

xuyan021

回复【9楼】包子:
---------------------------------
你的确定错了，你可以再查下资料。在这里不方便上图片说明。

jermy_z

回复【10楼】xuyan021:
---------------------------------
大哥  你还是学学C语言吧

包子

回复【10楼】xuyan021:
---------------------------------
不会吧，这个哥， 我拿这个代码，在STM8的四轴上飞过得.飞了差不多10分钟都没见啥问题...。数学上确实是这么算的. 我知道你大概怎么想的但不是你想的那样   而且这个在网上德国人公开的很多人都用啊.  如果错了，小飞机咋么能飞O    O

包子

回复【10楼】xuyan021:
---------------------------------
当然了欢迎私密我，或者加我Q指出错误...小弟万分感激....

jermy_z

回复【13楼】包子:
---------------------------------
其实是它的C语言基础差  你看看他的代码，逻辑是一个逻辑。。。。。估计他不懂 a=a+2是什么意思

xuyan021

[mw_shl_code=c,true] qa = q0; qb = q1; qc = q2; q0 += (-qb * gx - qc * gy - q3 * gz); q1 += (qa * gx + qc * gz - q3 * gy); q2 += (qa * gy - qb * gz + q3 * gx); q3 += (qa * gz + qb * gy - qc * gx); // Normalise quaternion recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); q0 *= recipNorm; q1 *= recipNorm; q2 *= recipNorm; q3 *= recipNorm;[/mw_shl_code]

1. 
   

fengshu426

飘过，学习一下

wangsair

xuyan021 发表于 2015-12-14 10:13
回复【9楼】包子:
---------------------------------
你的确定错了，你可以再查下资料。在这里不方便上图 ...

你俩都没有错，只是两种表达方式而已，你改的多此一举，你C语言确实不过关啊！
举例：a*a  a没有更新
          a=a+1 a有更新，a=原来的a+1

枫之幻00

我把你的程序改成AVR的，用的AVR128,晶振8MHZ,只是多加了LCD和串口，但是速度特别慢，我从0°翻转到90°，上位机的飞机模型以每秒2°左右的速度缓慢上升，但是串口接收数据还行，每秒约十多个数据，改了SMPLRT_DIV和CONFIG也没有效果，不知道什么原因，大神求帮忙

popo139700

大神，这个有用吗？

包子

枫之幻00 发表于 2016-3-2 10:30
我把你的程序改成AVR的，用的AVR128,晶振8MHZ,只是多加了LCD和串口，但是速度特别慢，我从0°翻转到90°， ...

额，要改一些IMU中的参数.

包子

popo139700 发表于 2016-3-2 10:59
大神，这个有用吗？

我都用这个做了个小四轴. 灰起来了你说有用么-  -？

包子

枫之幻00 发表于 2016-3-2 10:30
我把你的程序改成AVR的，用的AVR128,晶振8MHZ,只是多加了LCD和串口，但是速度特别慢，我从0°翻转到90°， ...

你试试把harfT 改成0.04  .   应该会觉得数据跑的快很多，然后你要去理解kp ki hartT去调出理想的参数

包子

枫之幻00 发表于 2016-3-2 10:30
我把你的程序改成AVR的，用的AVR128,晶振8MHZ,只是多加了LCD和串口，但是速度特别慢，我从0°翻转到90°， ...

当然啊，不是让你真的改到0.04，只是让你感受一下而已，，，，具体的要你自己去调啊

包子

枫之幻00 发表于 2016-3-2 10:30
我把你的程序改成AVR的，用的AVR128,晶振8MHZ,只是多加了LCD和串口，但是速度特别慢，我从0°翻转到90°， ...

先定义Kp，Ki，以及halfT 。
Kp，Ki，控制加速度计修正陀螺仪积分姿态的速度

这里面有讲，我随机百度搜的，人家终归详细一点

http://www.360doc.cn/article/17770789_404315374.html

popo139700

包子 发表于 2016-3-3 20:46
我都用这个做了个小四轴. 灰起来了你说有用么-  -？

我按照你的方法，用mpu6050搞了一下，发现没用，

包子

搞了一下。。233  这个是德国人的，具体你也可以去下载原版的IMU，也一样 要根据你的单片机运算速度改一些参数.

包子

popo139700 发表于 2016-3-4 16:33
我按照你的方法，用mpu6050搞了一下，发现没用，

要不你就用原子的方法吧。那个我觉得很麻烦  0  0...

popo139700

包子 发表于 2016-3-4 18:36
要不你就用原子的方法吧。那个我觉得很麻烦  0  0...

我买的不是原子的配件

包子

popo139700 发表于 2016-3-4 18:59
我买的不是原子的配件

算了，我上传个视频完事-   -

包子

http://v.youku.com/v_show/id_XMTQ4OTkzODIzMg==.html?from=y1.7-1.2
请看左上角的X  Y 测试，都是翻转到90°的.   基本翻过去就到了. 先是X 然后是Y

popo139700

大神，能把你的工程发给我嘛？753422272@qq.com

liarlaker

楼主，我用你的程序，板子放在桌子上不动，为啥输出数据一直跑呢？

x:0.085758
y:0.090364

x:0.139571
y:0.064811

x:0.159081
y:0.029631

x:0.188537
y:0.048975

x:0.209407
y:0.037967

x:0.234662
y:0.026954

x:0.242533
y:0.037424

x:0.245208
y:0.036805

x:0.257427
y:0.019395

x:0.243104
y:0.017567

x:0.071368
y:0.071734

x:-0.080335
y:0.148147

x:-0.140336
y:0.219431

x:-0.203921
y:0.251299

x:-0.283517
y:0.288399

x:-0.330854
y:0.321877

x:-0.388592
y:0.350371

x:-0.445088
y:0.374403

x:-0.544700
y:0.420669

x:-0.675148
y:0.500147

x:-0.790288
y:0.568097

x:-0.894897
y:0.643107

x:-1.019272
y:0.741019

x:-1.151975
y:0.783386

x:-1.232210
y:0.829101

x:-1.344500
y:0.866801

x:-1.449728
y:0.913022

x:-1.529626
y:0.961818

x:-1.635537
y:1.007029

x:-1.720760
y:1.042221

x:-1.749209
y:1.073298

x:-1.790861
y:1.109567

x:-1.857938
y:1.149705

x:-1.875960
y:1.142617

x:-1.745819
y:1.080400

x:-1.716789
y:1.075150

x:-1.674641
y:1.060866

x:-1.624220
y:1.042702

x:-1.554975
y:1.017662

x:-1.498643
y:0.974835

x:-1.445902
y:0.960710

x:-1.378494
y:0.948640

x:-1.314704
y:0.938660

x:-1.247857
y:0.921730

x:-1.187532
y:0.920757

x:0.040103
y:0.010941

x:0.076256
y:0.008705

x:0.128525
y:0.000379

x:0.156842
y:-0.004452

x:0.082322
y:0.009472

x:0.043312
y:0.019553

x:0.045023
y:0.048342

x:0.027423
y:0.038607

x:0.033107
y:0.059322

x:0.030687
y:0.082588

x:0.048264
y:0.095905

x:0.035921
y:0.109234

x:0.021153
y:0.110347

x:0.016212
y:0.166812

x:0.006073
y:0.190681

x:0.009014
y:0.205771

x:-0.027798
y:0.246305

x:-0.041643
y:0.285219

x:-0.064433
y:0.307302

x:-0.079585
y:0.323933

x:-0.086869
y:0.342283

x:-0.092739
y:0.358962

x:-0.098672
y:0.387396

x:-0.140038
y:0.410263

x:-0.176960
y:0.469851

x:-0.209230
y:0.492365

x:-0.189486
y:0.530461

x:-0.204046
y:0.553115

x:-0.244747
y:0.577970

x:-0.264186
y:0.594402

x:-0.309801
y:0.608376

x:-0.336562
y:0.612150

x:-0.366481
y:0.622377

x:-0.378966
y:0.632446

x:-0.372106
y:0.625186

x:-0.361795
y:0.632184

x:-0.338282
y:0.645914

x:-0.328282
y:0.658546

x:-0.320654
y:0.669178

x:-0.279344
y:0.678109

x:-0.246038
y:0.677574

x:-0.179152
y:0.644095

x:-0.127712
y:0.630460

x:-0.059399
y:0.603693

x:-0.001792
y:0.586755

x:0.039055
y:0.578164

x:0.091995
y:0.569509

x:0.136203
y:0.568086

x:0.180120
y:0.552602

x:0.234458
y:0.537894

x:0.288139
y:0.528255

x:0.330026
y:0.514982

x:0.383561
y:0.493683

x:0.457020
y:0.486194

x:0.540459
y:0.479419

x:0.578807
y:0.470937

x:0.619354
y:0.460751

x:0.648943
y:0.446994

x:0.695708
y:0.434998

x:0.727852
y:0.423767

x:0.778266
y:0.413201

x:0.822345
y:0.401299

x:0.850542
y:0.393309

x:0.861562
y:0.386228

x:0.885136
y:0.378066

x:0.922423
y:0.374520

x:0.956192
y:0.365703

x:0.999676
y:0.369665

x:0.974562
y:0.396328

x:0.795611
y:0.565942

x:0.183742
y:0.803476

x:-0.609927
y:1.112040

x:-1.100897
y:1.303605

x:-1.444892
y:1.398877

x:-1.790910
y:1.531655

x:-2.061426
y:1.657934

x:-2.357977
y:1.760585

x:-2.621505
y:1.857611

x:-0.811700
y:1.224484

x:-0.710628
y:1.190486

x:-0.674651
y:1.197378

x:-0.625782
y:1.198030

x:-0.593523
y:1.186557

x:-2.085795
y:1.560309

x:-2.044940
y:1.520995

x:-2.437728
y:1.554718

x:-2.629806
y:1.581620

x:-2.551419
y:1.580219

x:-2.477421
y:1.586064

x:-2.404214
y:1.603984

x:-2.331426
y:1.616686

x:-2.258587
y:1.630461

x:-2.187645
y:1.642955

x:-2.116072
y:1.655914

x:-2.044993
y:1.668346

x:-1.974622
y:1.679827

x:-1.904409
y:1.692847

x:-1.834985
y:1.704364

x:-1.765232
y:1.717658

x:-1.696913
y:1.729478

x:-1.628728
y:1.740880

x:-1.560014
y:1.752716

x:-1.492592
y:1.764420

x:-1.424953
y:1.776294

x:-1.357795
y:1.787642

x:-1.291081
y:1.799496

x:-1.223560
y:1.811449

x:-1.157914
y:1.823147

x:-1.091506
y:1.835784

x:-1.026214
y:1.846995

x:-0.961171
y:1.858184

x:-0.896964
y:1.868801

x:-0.833311
y:1.878595

x:-0.768934
y:1.889265

x:-0.705284
y:1.901338

x:-0.641845
y:1.911731

x:-0.578891
y:1.923006

x:-0.516791
y:1.933588

x:-0.454495
y:1.944516

x:-0.392809
y:1.955305

x:-0.331219
y:1.965908

x:-0.270304
y:1.976902

x:-0.209340
y:1.986335

x:-0.149224
y:1.997236

x:-0.090514
y:2.008207

x:-0.037214
y:2.009542

x:0.018655
y:2.014787

x:0.075749
y:2.023648

x:0.133968

xp_peng2016

liarlaker 发表于 2016-3-8 15:46
楼主，我用你的程序，板子放在桌子上不动，为啥输出数据一直跑呢？

x:0.085758

你这刷屏的   过份了

liarlaker

xp_peng2016 发表于 2016-3-8 16:17
你这刷屏的   过份了

我已经删了好多了，为了保证数据完整性，还是有点长，见谅

tongjinlv

www.trtos.com

tongjinlv

www.trtos.com   操作系统，稳定，简单容易上手