IMU()震动引起的角度误差很大四轴倒向一边回不来，DMP有延时

945595199

我加速度计用了滑动滤波，序列长度为5，陀螺仪没滤波
用的网上流传的IMU_Calculate(float ax, float ay, float az,float gx, float gy, float gz)算法，
我的控制频率是400hz，Kp=2.0f,Ki=0.005f,halfT=0.00125，
目前飞机总是会飘，有时倒向一边起不来了，手拨一下才起来，但感觉上是稳的，后来观察波形，发现我手抖的频率越高，角度就向0度飘的越快！表现为图片的前半部分

分析可能是因为飞机倒向一边时，震动也大，IMU计算的角度直接飘回了0度，导致P每起作用，就起不来了！
但看大家都是这个算法，没有遇到过这个问题吗？是怎么调好的？
附下用IMU()时调试的视频：
http://v.youku.com/v_show/id_XMTI4Njk2ODY4MA==.html?from=y1.7-1.2
后来，又移植了DMP，100hz读取与控制，中断未溢出，角度波形是对的（当然人眼是观察不出实际角度是不是滞后的），但飞机的响应表现的非常滞后，根本没法稳定，
表现为：只加D作用，我手猛的按下飞机比如到30度，过一会突然来一个很猛的升力，再消失。然后我去掉DMP，换成正常读取陀螺仪，也只加D作用，就表现为正常情况：非常敏感，按下瞬间就有反抗力。
DMP配置：
    if (mpu_set_gyro_fsr(2000)) return 0x10;
    if (mpu_set_accel_fsr(2)) return 0x11;   
    if (mpu_set_lpf(42)) return 0x12;
    if (mpu_set_sample_rate(100)) return 0x13;
    if (mpu_configure_fifo(0)) return 0x14;

现在这两种方法都没调好 求大神分析下原因！先谢谢！

945595199

视频在这：http://v.youku.com/v_show/id_XMTI4Njk2ODY4MA==.html?from=y1.7-1.2

zouyf12

在四轴泛滥的今天，还自己写飞控程序的越来越少了。

这个问题我是没遇到过，所以就不知道是什么原因了， 但是感觉不是振动的原因，顶多只是诱因， 因为就算振了导致加速度乱了影响最后融合的结果，
可是----- 一旦，不振时加速度解算出来的角度肯定是正确的， 就算你融合修正的速度再慢也会得到正确的角度，绝对不会像你视频里的那样，就歪在那里了。这个问题没有捷径，一行行查代码吧。看看在你歪到最大角度时， IMU里面到底计算的结果出了什么问题。

最后想说的是， 虽然你视频里四轴不稳，在平衡位置有时会来回抖，但是抖得频率很快， 说明你电机和电调真TMD好， 响应这么快， 我的新西达套装就不会这样， 响应慢， 一开始用50Hz PWM去驱动电调根本飞不了。