四旋翼飞行器的飞控实现

15

16 // Rate of change of quaternion from gyroscope

17 qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);

18 qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);

19 qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);

20 qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);

21

22 // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)

23 if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

24

25 // Normalise accelerometer measurement

26 recipNorm = invSqrt(ax * ax + ay * ay + az * az);

27 ax *= recipNorm;

28 ay *= recipNorm;

29 az *= recipNorm;

30

31 // Auxiliary variables to avoid repeated arithmetic

32 _2q0 = 2.0f * q0;

33 _2q1 = 2.0f * q1;

34 _2q2 = 2.0f * q2;

35 _2q3 = 2.0f * q3;

36 _4q0 = 4.0f * q0;

37 _4q1 = 4.0f * q1;

38 _4q2 = 4.0f * q2;

39 _8q1 = 8.0f * q1;

40 _8q2 = 8.0f * q2;

41 q0q0 = q0 * q0;

42 q1q1 = q1 * q1;

43 q2q2 = q2 * q2;

44 q3q3 = q3 * q3;

45

46 // Gradient decent algorithm corrective step

47 s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;

48 s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;

49 s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;

50 s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;

51 recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude

52 s0 *= recipNorm;

53 s1 *= recipNorm;

54 s2 *= recipNorm;

55 s3 *= recipNorm;

56

57 // Apply feedback step

58 qDot1 -= beta * s0;

59 qDot2 -= beta * s1;

60 qDot3 -= beta * s2;

61 qDot4 -= beta * s3;

62 }

63

64 // Integrate rate of change of quaternion to yield quaternion

65 q0 += qDot1 * (1.0f / sampleFreq);

66 q1 += qDot2 * (1.0f / sampleFreq);