四轴姿态解算完后要做的事。。。双闭环PID！！！

super_mice · 发表于 2014-8-14 10:59:50

小四轴一般分这么几个步骤：
1、把mpu6050的dmp配置好，数据输出频率大概是100h，可以读出四元数，加速度计，陀螺仪（已经修正），FIFO剩余数据量。
2、再把dmp的四元数值转换为欧拉角。废话不多说，上代码
q0=四元数w;q1=四元数x;q2=四元数y;q3=四元数z;//转换到欧拉角
Roll = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // 绕y轴
Pitch = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // 绕x轴
Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //绕z轴 180/3.14=57.3

3、计算目标欧拉角和实际欧拉角的差，输入PID控制器
调整PID这个东西最头疼了，找到一篇关于双闭环PID的不错的博客，据说效果不错，供大家参考。

http://blog.csdn.net/super_mice/article/details/38436723