PX4 Firmware
PX4 Autopilot Software http://px4.io
Variables
K_VELY.c File Reference

Go to the source code of this file.

Variables

float t2 = q0*q3*2.0f
 
float t9 = q1*q2*2.0f
 
float t3 = t2-t9
 
float t4 = q0*q0
 
float t5 = q1*q1
 
float t6 = q2*q2
 
float t7 = q3*q3
 
float t8 = t4-t5+t6-t7
 
float t10 = q0*q1*2.0f
 
float t11 = q2*q3*2.0f
 
float t12 = t10+t11
 
float t13 = q1*vd*2.0f
 
float t14 = q0*ve*2.0f
 
float t26 = q3*vn*2.0f
 
float t15 = t13+t14-t26
 
float t16 = q0*vd*2.0f
 
float t17 = q2*vn*2.0f
 
float t27 = q1*ve*2.0f
 
float t18 = t16+t17-t27
 
float t19 = q3*vd*2.0f
 
float t20 = q2*ve*2.0f
 
float t21 = q1*vn*2.0f
 
float t22 = t19+t20+t21
 
float t23 = q3*ve*2.0f
 
float t24 = q0*vn*2.0f
 
float t28 = q2*vd*2.0f
 
float t25 = t23+t24-t28
 
float t29 = P[0][0]*t15
 
float t30 = P[1][1]*t18
 
float t31 = P[5][4]*t8
 
float t32 = P[6][4]*t12
 
float t33 = P[0][4]*t15
 
float t34 = P[1][4]*t18
 
float t35 = P[2][4]*t22
 
float t78 = P[4][4]*t3
 
float t79 = P[3][4]*t25
 
float t36 = t31+t32+t33+t34+t35-t78-t79
 
float t37 = P[5][6]*t8
 
float t38 = P[6][6]*t12
 
float t39 = P[0][6]*t15
 
float t40 = P[1][6]*t18
 
float t41 = P[2][6]*t22
 
float t81 = P[4][6]*t3
 
float t82 = P[3][6]*t25
 
float t42 = t37+t38+t39+t40+t41-t81-t82
 
float t43 = t12*t42
 
float t44 = P[5][0]*t8
 
float t45 = P[6][0]*t12
 
float t46 = P[1][0]*t18
 
float t47 = P[2][0]*t22
 
float t83 = P[4][0]*t3
 
float t84 = P[3][0]*t25
 
float t48 = t29+t44+t45+t46+t47-t83-t84
 
float t49 = t15*t48
 
float t50 = P[5][1]*t8
 
float t51 = P[6][1]*t12
 
float t52 = P[0][1]*t15
 
float t53 = P[2][1]*t22
 
float t85 = P[4][1]*t3
 
float t86 = P[3][1]*t25
 
float t54 = t30+t50+t51+t52+t53-t85-t86
 
float t55 = t18*t54
 
float t56 = P[5][2]*t8
 
float t57 = P[6][2]*t12
 
float t58 = P[0][2]*t15
 
float t59 = P[1][2]*t18
 
float t60 = P[2][2]*t22
 
float t87 = P[4][2]*t3
 
float t88 = P[3][2]*t25
 
float t61 = t56+t57+t58+t59+t60-t87-t88
 
float t62 = t22*t61
 
float t63 = P[5][3]*t8
 
float t64 = P[6][3]*t12
 
float t65 = P[0][3]*t15
 
float t66 = P[1][3]*t18
 
float t67 = P[2][3]*t22
 
float t89 = P[4][3]*t3
 
float t90 = P[3][3]*t25
 
float t68 = t63+t64+t65+t66+t67-t89-t90
 
float t69 = P[5][5]*t8
 
float t70 = P[6][5]*t12
 
float t71 = P[0][5]*t15
 
float t72 = P[1][5]*t18
 
float t73 = P[2][5]*t22
 
float t92 = P[4][5]*t3
 
float t93 = P[3][5]*t25
 
float t74 = t69+t70+t71+t72+t73-t92-t93
 
float t75 = t8*t74
 
float t80 = t3*t36
 
float t91 = t25*t68
 
float t76 = R_VEL+t43+t49+t55+t62+t75-t80-t91
 
float t77 = 1.0f/t76
 
 Kfusion [0] = t77*(t29-P[0][4]*t3+P[0][5]*t8+P[0][6]*t12+P[0][1]*t18+P[0][2]*t22-P[0][3]*t25)
 

Variable Documentation

◆ Kfusion

Kfusion[23] = t77*(t29-P[0][4]*t3+P[0][5]*t8+P[0][6]*t12+P[0][1]*t18+P[0][2]*t22-P[0][3]*t25)

Definition at line 93 of file K_VELY.c.

◆ t10

float t10 = q0*q1*2.0f

Definition at line 9 of file K_VELY.c.

◆ t11

float t11 = q2*q3*2.0f

Definition at line 10 of file K_VELY.c.

◆ t12

float t12 = t10+t11

Definition at line 11 of file K_VELY.c.

◆ t13

float t13 = q1*vd*2.0f

Definition at line 12 of file K_VELY.c.

◆ t14

float t14 = q0*ve*2.0f

Definition at line 13 of file K_VELY.c.

◆ t15

float t15 = t13+t14-t26

Definition at line 15 of file K_VELY.c.

◆ t16

float t16 = q0*vd*2.0f

Definition at line 16 of file K_VELY.c.

◆ t17

float t17 = q2*vn*2.0f

Definition at line 17 of file K_VELY.c.

◆ t18

float t18 = t16+t17-t27

Definition at line 19 of file K_VELY.c.

◆ t19

float t19 = q3*vd*2.0f

Definition at line 20 of file K_VELY.c.

◆ t2

float t2 = q0*q3*2.0f

Definition at line 1 of file K_VELY.c.

◆ t20

float t20 = q2*ve*2.0f

Definition at line 21 of file K_VELY.c.

◆ t21

float t21 = q1*vn*2.0f

Definition at line 22 of file K_VELY.c.

◆ t22

float t22 = t19+t20+t21

Definition at line 23 of file K_VELY.c.

◆ t23

float t23 = q3*ve*2.0f

Definition at line 24 of file K_VELY.c.

◆ t24

float t24 = q0*vn*2.0f

Definition at line 25 of file K_VELY.c.

◆ t25

float t25 = t23+t24-t28

Definition at line 27 of file K_VELY.c.

◆ t26

float t26 = q3*vn*2.0f

Definition at line 14 of file K_VELY.c.

◆ t27

float t27 = q1*ve*2.0f

Definition at line 18 of file K_VELY.c.

◆ t28

float t28 = q2*vd*2.0f

Definition at line 26 of file K_VELY.c.

◆ t29

float t29 = P[0][0]*t15

Definition at line 28 of file K_VELY.c.

◆ t3

float t3 = t2-t9

Definition at line 3 of file K_VELY.c.

◆ t30

float t30 = P[1][1]*t18

Definition at line 29 of file K_VELY.c.

◆ t31

float t31 = P[5][4]*t8

Definition at line 30 of file K_VELY.c.

◆ t32

float t32 = P[6][4]*t12

Definition at line 31 of file K_VELY.c.

◆ t33

float t33 = P[0][4]*t15

Definition at line 32 of file K_VELY.c.

◆ t34

float t34 = P[1][4]*t18

Definition at line 33 of file K_VELY.c.

◆ t35

float t35 = P[2][4]*t22

Definition at line 34 of file K_VELY.c.

◆ t36

float t36 = t31+t32+t33+t34+t35-t78-t79

Definition at line 37 of file K_VELY.c.

◆ t37

float t37 = P[5][6]*t8

Definition at line 38 of file K_VELY.c.

◆ t38

float t38 = P[6][6]*t12

Definition at line 39 of file K_VELY.c.

◆ t39

float t39 = P[0][6]*t15

Definition at line 40 of file K_VELY.c.

◆ t4

float t4 = q0*q0

Definition at line 4 of file K_VELY.c.

◆ t40

float t40 = P[1][6]*t18

Definition at line 41 of file K_VELY.c.

◆ t41

float t41 = P[2][6]*t22

Definition at line 42 of file K_VELY.c.

◆ t42

float t42 = t37+t38+t39+t40+t41-t81-t82

Definition at line 45 of file K_VELY.c.

◆ t43

float t43 = t12*t42

Definition at line 46 of file K_VELY.c.

◆ t44

float t44 = P[5][0]*t8

Definition at line 47 of file K_VELY.c.

◆ t45

float t45 = P[6][0]*t12

Definition at line 48 of file K_VELY.c.

◆ t46

float t46 = P[1][0]*t18

Definition at line 49 of file K_VELY.c.

◆ t47

float t47 = P[2][0]*t22

Definition at line 50 of file K_VELY.c.

◆ t48

float t48 = t29+t44+t45+t46+t47-t83-t84

Definition at line 53 of file K_VELY.c.

◆ t49

float t49 = t15*t48

Definition at line 54 of file K_VELY.c.

◆ t5

float t5 = q1*q1

Definition at line 5 of file K_VELY.c.

◆ t50

float t50 = P[5][1]*t8

Definition at line 55 of file K_VELY.c.

◆ t51

float t51 = P[6][1]*t12

Definition at line 56 of file K_VELY.c.

◆ t52

float t52 = P[0][1]*t15

Definition at line 57 of file K_VELY.c.

◆ t53

float t53 = P[2][1]*t22

Definition at line 58 of file K_VELY.c.

◆ t54

float t54 = t30+t50+t51+t52+t53-t85-t86

Definition at line 61 of file K_VELY.c.

◆ t55

float t55 = t18*t54

Definition at line 62 of file K_VELY.c.

◆ t56

float t56 = P[5][2]*t8

Definition at line 63 of file K_VELY.c.

◆ t57

float t57 = P[6][2]*t12

Definition at line 64 of file K_VELY.c.

◆ t58

float t58 = P[0][2]*t15

Definition at line 65 of file K_VELY.c.

◆ t59

float t59 = P[1][2]*t18

Definition at line 66 of file K_VELY.c.

◆ t6

float t6 = q2*q2

Definition at line 6 of file K_VELY.c.

◆ t60

float t60 = P[2][2]*t22

Definition at line 67 of file K_VELY.c.

◆ t61

float t61 = t56+t57+t58+t59+t60-t87-t88

Definition at line 70 of file K_VELY.c.

◆ t62

float t62 = t22*t61

Definition at line 71 of file K_VELY.c.

◆ t63

float t63 = P[5][3]*t8

Definition at line 72 of file K_VELY.c.

◆ t64

float t64 = P[6][3]*t12

Definition at line 73 of file K_VELY.c.

◆ t65

float t65 = P[0][3]*t15

Definition at line 74 of file K_VELY.c.

◆ t66

float t66 = P[1][3]*t18

Definition at line 75 of file K_VELY.c.

◆ t67

float t67 = P[2][3]*t22

Definition at line 76 of file K_VELY.c.

◆ t68

float t68 = t63+t64+t65+t66+t67-t89-t90

Definition at line 79 of file K_VELY.c.

◆ t69

float t69 = P[5][5]*t8

Definition at line 80 of file K_VELY.c.

◆ t7

float t7 = q3*q3

Definition at line 7 of file K_VELY.c.

◆ t70

float t70 = P[6][5]*t12

Definition at line 81 of file K_VELY.c.

◆ t71

float t71 = P[0][5]*t15

Definition at line 82 of file K_VELY.c.

◆ t72

float t72 = P[1][5]*t18

Definition at line 83 of file K_VELY.c.

◆ t73

float t73 = P[2][5]*t22

Definition at line 84 of file K_VELY.c.

◆ t74

float t74 = t69+t70+t71+t72+t73-t92-t93

Definition at line 87 of file K_VELY.c.

◆ t75

float t75 = t8*t74

Definition at line 88 of file K_VELY.c.

◆ t76

float t76 = R_VEL+t43+t49+t55+t62+t75-t80-t91

Definition at line 91 of file K_VELY.c.

◆ t77

float t77 = 1.0f/t76

Definition at line 92 of file K_VELY.c.

◆ t78

float t78 = P[4][4]*t3

Definition at line 35 of file K_VELY.c.

◆ t79

float t79 = P[3][4]*t25

Definition at line 36 of file K_VELY.c.

◆ t8

float t8 = t4-t5+t6-t7

Definition at line 8 of file K_VELY.c.

◆ t80

float t80 = t3*t36

Definition at line 89 of file K_VELY.c.

◆ t81

float t81 = P[4][6]*t3

Definition at line 43 of file K_VELY.c.

◆ t82

float t82 = P[3][6]*t25

Definition at line 44 of file K_VELY.c.

◆ t83

float t83 = P[4][0]*t3

Definition at line 51 of file K_VELY.c.

◆ t84

float t84 = P[3][0]*t25

Definition at line 52 of file K_VELY.c.

◆ t85

float t85 = P[4][1]*t3

Definition at line 59 of file K_VELY.c.

◆ t86

float t86 = P[3][1]*t25

Definition at line 60 of file K_VELY.c.

◆ t87

float t87 = P[4][2]*t3

Definition at line 68 of file K_VELY.c.

◆ t88

float t88 = P[3][2]*t25

Definition at line 69 of file K_VELY.c.

◆ t89

float t89 = P[4][3]*t3

Definition at line 77 of file K_VELY.c.

◆ t9

float t9 = q1*q2*2.0f

Definition at line 2 of file K_VELY.c.

◆ t90

float t90 = P[3][3]*t25

Definition at line 78 of file K_VELY.c.

◆ t91

float t91 = t25*t68

Definition at line 90 of file K_VELY.c.

◆ t92

float t92 = P[4][5]*t3

Definition at line 85 of file K_VELY.c.

◆ t93

float t93 = P[3][5]*t25

Definition at line 86 of file K_VELY.c.