stm32飞控

很不错的飞控 float q0 = 1, q1 = 0, q2 = 0, q3 = 0;    // quaternion elements representing the estimated orientation float exInt = 0, eyInt = 0, ezInt = 0;    // scaled integral error void IMUupdate(T_int16_xyz *gyr, T_int16_xyz *acc, T_float_angle *angle) { float ax = acc->X,ay = acc->Y,az = acc->Z; float gx = gyr->X,gy = gyr->Y,gz = gyr->Z;   float norm; //  float hx, hy, hz, bx, bz;   float vx, vy, vz;// wx, wy, wz;   float ex, ey, ez;   // ???????????   float q0q0 = q0*q0;   float q0q1 = q0*q1;   float q0q2 = q0*q2; //  float q0q3 = q0*q3;   float q1q1 = q1*q1; //  float q1q2 = q1*q2;   float q1q3 = q1*q3;   float q2q2 = q2*q2;   float q2q3 = q2*q3;   float q3q3 = q3*q3; if(ax*ay*az==0)