四元数互补滤波后姿态恢复慢

BBA · 发表于 2019-3-21 21:29:19

模仿原子哥的四元数互补滤波，在运动的时候反应速度正常，但静止水平时姿态很缓慢地下降回准确值，大概都要4秒，而且是幅度大的那种，Pitch从7下降到2，不知道什么原因，2ms进行一次四元数更新，MPU6050是用中断+DMA读取的，测过有1000HZ，传感器原始看起来没问题
extern uint32 INT_Times;
while (1)
{
if(bool_mainLoop) //1ms进入一次
{
bool_mainLoop=0;
if(++times1>=2)
{
//SensorThread(); //传感器数据读取与处理
imuUpdate(0.002); //2ms执行一次 500HZ
times1=0;
INT_Times++;
}