圆点博士小四轴算法快速入门

HYPERLINK "http://www.eeboard.com/bbs/thread-32321-1-1.html"圆点博士小四轴算法快速入门飞控的算法代码一般包括下面三个部分：滤波，姿态，PID

圆点博士小四轴算法快速入门 飞控的算法代码一般包括下面三个部分：滤波，姿态， PID ，滤波可以用互补滤波来实现，互补滤波的资料很多，大家随便就能找到。基本公式是： 1 ，滤波完就是四元数拉。直接用老外的就可以。超级简单 2MadgwickIMU unsignedcharBS004_IMU_Update(floatax,floatay,floataz,floatgx,floatgy,floatgz) { floatnorm; floatvx,vy,vz; floatex,ey,ez; // 圆点博士四元数乘法运算 //: floatq0q0=q0*q0; floatq0q1=q0*q1; floatq0q2=q0*q2; floatq1q1=q1*q1; floatq1q3=q1*q3; floatq2q2=q2*q2; floatq2q3=q2*q3; floatq3q3=q3*q3; // 圆点博士归一化处理 //: norm=sqrt(ax*ax+ay*ay+az*az); if(norm==0)return0; ax=ax/norm; ay=ay/norm; az=az/norm; // 圆点博士建立小四轴坐标系 //: vx=2*(q1q3-q0q2);