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

Go to the source code of this file.

Variables

float t2 = 1.0f/range
 
float t3 = Tbs.b.y*q0*2.0f
 
float t4 = Tbs.b.x*q3*2.0f
 
float t18 = Tbs.b.z*q1*2.0f
 
float t5 = t3+t4-t18
 
float t6 = Tbs.b.y*q1*2.0f
 
float t7 = Tbs.b.z*q0*2.0f
 
float t16 = Tbs.b.x*q2*2.0f
 
float t8 = t6+t7-t16
 
float t9 = Tbs.b.x*q0*2.0f
 
float t10 = Tbs.b.z*q2*2.0f
 
float t17 = Tbs.b.y*q3*2.0f
 
float t11 = t9+t10-t17
 
float t12 = Tbs.b.x*q1*2.0f
 
float t13 = Tbs.b.y*q2*2.0f
 
float t14 = Tbs.b.z*q3*2.0f
 
float t15 = t12+t13+t14
 
float t19 = q0*q0
 
float t20 = q1*q1
 
float t21 = q2*q2
 
float t22 = q3*q3
 
float t23 = q0*q3*2.0f
 
float t24 = q0*q2*2.0f
 
float t25 = q1*q3*2.0f
 
float t26 = q0*q1*2.0f
 
float t27 = t19+t20-t21-t22
 
float t28 = Tbs.b.x*t27
 
float t29 = q1*q2*2.0f
 
float t30 = t24+t25
 
float t31 = Tbs.b.z*t30
 
float t32 = t19-t20+t21-t22
 
float t33 = Tbs.b.y*t32
 
float t34 = t23+t29
 
float t35 = Tbs.b.x*t34
 
float t36 = q2*q3*2.0f
 
float t37 = t19-t20-t21+t22
 
float t38 = Tbs.b.z*t37
 
float t39 = t24-t25
 
float t40 = t26+t36
 
float t41 = Tbs.b.y*t40
 
float t60 = Tbs.b.x*t39
 
float t42 = t38+t41-t60
 
float t43 = t8*vd
 
float t44 = t5*ve
 
float t45 = t11*vn
 
float t46 = t43+t44+t45
 
float t47 = t5*vd
 
float t48 = t15*vn
 
float t62 = t8*ve
 
float t49 = t47+t48-t62
 
float t50 = t15*ve
 
float t51 = t8*vn
 
float t63 = t11*vd
 
float t52 = t50+t51-t63
 
float t53 = t15*vd
 
float t54 = t11*ve
 
float t64 = t5*vn
 
float t55 = t53+t54-t64
 
float t56 = t23-t29
 
float t65 = Tbs.b.y*t56
 
float t57 = t28+t31-t65
 
float t58 = t26-t36
 
float t66 = Tbs.b.z*t58
 
float t59 = t33+t35-t66
 
float t61 = P[0][0]*t2*t46
 
float t67 = P[1][1]*t2*t49
 
float t68 = P[4][0]*t2*t57
 
float t69 = P[5][0]*t2*t59
 
float t70 = P[6][0]*t2*t42
 
float t71 = P[1][0]*t2*t49
 
float t72 = P[2][0]*t2*t52
 
float t73 = P[3][0]*t2*t55
 
float t74 = t61+t68+t69+t70+t71+t72+t73
 
float t75 = t2*t46*t74
 
float t76 = P[4][1]*t2*t57
 
float t77 = P[5][1]*t2*t59
 
float t78 = P[6][1]*t2*t42
 
float t79 = P[0][1]*t2*t46
 
float t80 = P[2][1]*t2*t52
 
float t81 = P[3][1]*t2*t55
 
float t82 = t67+t76+t77+t78+t79+t80+t81
 
float t83 = t2*t49*t82
 
float t84 = P[4][2]*t2*t57
 
float t85 = P[5][2]*t2*t59
 
float t86 = P[6][2]*t2*t42
 
float t87 = P[0][2]*t2*t46
 
float t88 = P[1][2]*t2*t49
 
float t89 = P[2][2]*t2*t52
 
float t90 = P[3][2]*t2*t55
 
float t91 = t84+t85+t86+t87+t88+t89+t90
 
float t92 = t2*t52*t91
 
float t93 = P[4][3]*t2*t57
 
float t94 = P[5][3]*t2*t59
 
float t95 = P[6][3]*t2*t42
 
float t96 = P[0][3]*t2*t46
 
float t97 = P[1][3]*t2*t49
 
float t98 = P[2][3]*t2*t52
 
float t99 = P[3][3]*t2*t55
 
float t100 = t93+t94+t95+t96+t97+t98+t99
 
float t101 = t2*t55*t100
 
float t102 = P[4][4]*t2*t57
 
float t103 = P[5][4]*t2*t59
 
float t104 = P[6][4]*t2*t42
 
float t105 = P[0][4]*t2*t46
 
float t106 = P[1][4]*t2*t49
 
float t107 = P[2][4]*t2*t52
 
float t108 = P[3][4]*t2*t55
 
float t109 = t102+t103+t104+t105+t106+t107+t108
 
float t110 = t2*t57*t109
 
float t111 = P[4][5]*t2*t57
 
float t112 = P[5][5]*t2*t59
 
float t113 = P[6][5]*t2*t42
 
float t114 = P[0][5]*t2*t46
 
float t115 = P[1][5]*t2*t49
 
float t116 = P[2][5]*t2*t52
 
float t117 = P[3][5]*t2*t55
 
float t118 = t111+t112+t113+t114+t115+t116+t117
 
float t119 = t2*t59*t118
 
float t120 = P[4][6]*t2*t57
 
float t121 = P[5][6]*t2*t59
 
float t122 = P[6][6]*t2*t42
 
float t123 = P[0][6]*t2*t46
 
float t124 = P[1][6]*t2*t49
 
float t125 = P[2][6]*t2*t52
 
float t126 = P[3][6]*t2*t55
 
float t127 = t120+t121+t122+t123+t124+t125+t126
 
float t128 = t2*t42*t127
 
float t129 = R_LOS+t75+t83+t92+t101+t110+t119+t128
 
float t130 = 1.0f/t129
 
 H_LOS [0] = t2*t46
 
 Kfusion [0] = t130*(t61+P[0][6]*t2*t42+P[0][1]*t2*t49+P[0][2]*t2*t52+P[0][3]*t2*t55+P[0][4]*t2*t57+P[0][5]*t2*t59)
 

Variable Documentation

◆ H_LOS

H_LOS[6] = t2*t46

Definition at line 130 of file LOSX.c.

Referenced by Ekf::fuseOptFlow().

◆ Kfusion

Kfusion[23] = t130*(t61+P[0][6]*t2*t42+P[0][1]*t2*t49+P[0][2]*t2*t52+P[0][3]*t2*t55+P[0][4]*t2*t57+P[0][5]*t2*t59)

Definition at line 137 of file LOSX.c.

◆ t10

float t10 = Tbs.b.z*q2*2.0f

Definition at line 11 of file LOSX.c.

◆ t100

float t100 = t93+t94+t95+t96+t97+t98+t99

Definition at line 99 of file LOSX.c.

◆ t101

float t101 = t2*t55*t100

Definition at line 100 of file LOSX.c.

◆ t102

float t102 = P[4][4]*t2*t57

Definition at line 101 of file LOSX.c.

◆ t103

float t103 = P[5][4]*t2*t59

Definition at line 102 of file LOSX.c.

◆ t104

float t104 = P[6][4]*t2*t42

Definition at line 103 of file LOSX.c.

◆ t105

float t105 = P[0][4]*t2*t46

Definition at line 104 of file LOSX.c.

◆ t106

float t106 = P[1][4]*t2*t49

Definition at line 105 of file LOSX.c.

◆ t107

float t107 = P[2][4]*t2*t52

Definition at line 106 of file LOSX.c.

◆ t108

float t108 = P[3][4]*t2*t55

Definition at line 107 of file LOSX.c.

◆ t109

float t109 = t102+t103+t104+t105+t106+t107+t108

Definition at line 108 of file LOSX.c.

◆ t11

float t11 = t9+t10-t17

Definition at line 13 of file LOSX.c.

◆ t110

float t110 = t2*t57*t109

Definition at line 109 of file LOSX.c.

◆ t111

float t111 = P[4][5]*t2*t57

Definition at line 110 of file LOSX.c.

◆ t112

float t112 = P[5][5]*t2*t59

Definition at line 111 of file LOSX.c.

◆ t113

float t113 = P[6][5]*t2*t42

Definition at line 112 of file LOSX.c.

◆ t114

float t114 = P[0][5]*t2*t46

Definition at line 113 of file LOSX.c.

◆ t115

float t115 = P[1][5]*t2*t49

Definition at line 114 of file LOSX.c.

◆ t116

float t116 = P[2][5]*t2*t52

Definition at line 115 of file LOSX.c.

◆ t117

float t117 = P[3][5]*t2*t55

Definition at line 116 of file LOSX.c.

◆ t118

float t118 = t111+t112+t113+t114+t115+t116+t117

Definition at line 117 of file LOSX.c.

◆ t119

float t119 = t2*t59*t118

Definition at line 118 of file LOSX.c.

◆ t12

float t12 = Tbs.b.x*q1*2.0f

Definition at line 14 of file LOSX.c.

◆ t120

float t120 = P[4][6]*t2*t57

Definition at line 119 of file LOSX.c.

◆ t121

float t121 = P[5][6]*t2*t59

Definition at line 120 of file LOSX.c.

◆ t122

float t122 = P[6][6]*t2*t42

Definition at line 121 of file LOSX.c.

◆ t123

float t123 = P[0][6]*t2*t46

Definition at line 122 of file LOSX.c.

Referenced by main(), and TEST_F().

◆ t124

float t124 = P[1][6]*t2*t49

Definition at line 123 of file LOSX.c.

◆ t125

float t125 = P[2][6]*t2*t52

Definition at line 124 of file LOSX.c.

◆ t126

float t126 = P[3][6]*t2*t55

Definition at line 125 of file LOSX.c.

◆ t127

float t127 = t120+t121+t122+t123+t124+t125+t126

Definition at line 126 of file LOSX.c.

◆ t128

float t128 = t2*t42*t127

Definition at line 127 of file LOSX.c.

◆ t129

float t129 = R_LOS+t75+t83+t92+t101+t110+t119+t128

Definition at line 128 of file LOSX.c.

◆ t13

float t13 = Tbs.b.y*q2*2.0f

Definition at line 15 of file LOSX.c.

◆ t130

float t130 = 1.0f/t129

Definition at line 129 of file LOSX.c.

◆ t14

float t14 = Tbs.b.z*q3*2.0f

Definition at line 16 of file LOSX.c.

◆ t15

float t15 = t12+t13+t14

Definition at line 17 of file LOSX.c.

◆ t16

float t16 = Tbs.b.x*q2*2.0f

Definition at line 8 of file LOSX.c.

◆ t17

float t17 = Tbs.b.y*q3*2.0f

Definition at line 12 of file LOSX.c.

◆ t18

float t18 = Tbs.b.z*q1*2.0f

Definition at line 4 of file LOSX.c.

◆ t19

float t19 = q0*q0

Definition at line 18 of file LOSX.c.

◆ t2

float t2 = 1.0f/range

Definition at line 1 of file LOSX.c.

◆ t20

float t20 = q1*q1

Definition at line 19 of file LOSX.c.

◆ t21

float t21 = q2*q2

Definition at line 20 of file LOSX.c.

◆ t22

float t22 = q3*q3

Definition at line 21 of file LOSX.c.

◆ t23

float t23 = q0*q3*2.0f

Definition at line 22 of file LOSX.c.

◆ t24

float t24 = q0*q2*2.0f

Definition at line 23 of file LOSX.c.

◆ t25

float t25 = q1*q3*2.0f

Definition at line 24 of file LOSX.c.

◆ t26

float t26 = q0*q1*2.0f

Definition at line 25 of file LOSX.c.

◆ t27

float t27 = t19+t20-t21-t22

Definition at line 26 of file LOSX.c.

◆ t28

float t28 = Tbs.b.x*t27

Definition at line 27 of file LOSX.c.

◆ t29

float t29 = q1*q2*2.0f

Definition at line 28 of file LOSX.c.

◆ t3

float t3 = Tbs.b.y*q0*2.0f

Definition at line 2 of file LOSX.c.

◆ t30

float t30 = t24+t25

Definition at line 29 of file LOSX.c.

◆ t31

float t31 = Tbs.b.z*t30

Definition at line 30 of file LOSX.c.

◆ t32

float t32 = t19-t20+t21-t22

Definition at line 31 of file LOSX.c.

◆ t33

float t33 = Tbs.b.y*t32

Definition at line 32 of file LOSX.c.

◆ t34

float t34 = t23+t29

Definition at line 33 of file LOSX.c.

◆ t35

float t35 = Tbs.b.x*t34

Definition at line 34 of file LOSX.c.

◆ t36

float t36 = q2*q3*2.0f

Definition at line 35 of file LOSX.c.

◆ t37

float t37 = t19-t20-t21+t22

Definition at line 36 of file LOSX.c.

◆ t38

float t38 = Tbs.b.z*t37

Definition at line 37 of file LOSX.c.

◆ t39

float t39 = t24-t25

Definition at line 38 of file LOSX.c.

◆ t4

float t4 = Tbs.b.x*q3*2.0f

Definition at line 3 of file LOSX.c.

◆ t40

float t40 = t26+t36

Definition at line 39 of file LOSX.c.

◆ t41

float t41 = Tbs.b.y*t40

Definition at line 40 of file LOSX.c.

◆ t42

float t42 = t38+t41-t60

Definition at line 42 of file LOSX.c.

◆ t43

float t43 = t8*vd

Definition at line 43 of file LOSX.c.

◆ t44

float t44 = t5*ve

Definition at line 44 of file LOSX.c.

◆ t45

float t45 = t11*vn

Definition at line 45 of file LOSX.c.

◆ t46

float t46 = t43+t44+t45

Definition at line 46 of file LOSX.c.

◆ t47

float t47 = t5*vd

Definition at line 47 of file LOSX.c.

◆ t48

float t48 = t15*vn

Definition at line 48 of file LOSX.c.

◆ t49

float t49 = t47+t48-t62

Definition at line 50 of file LOSX.c.

◆ t5

float t5 = t3+t4-t18

Definition at line 5 of file LOSX.c.

◆ t50

float t50 = t15*ve

Definition at line 51 of file LOSX.c.

◆ t51

float t51 = t8*vn

Definition at line 52 of file LOSX.c.

◆ t52

float t52 = t50+t51-t63

Definition at line 54 of file LOSX.c.

◆ t53

float t53 = t15*vd

Definition at line 55 of file LOSX.c.

◆ t54

float t54 = t11*ve

Definition at line 56 of file LOSX.c.

◆ t55

float t55 = t53+t54-t64

Definition at line 58 of file LOSX.c.

◆ t56

float t56 = t23-t29

Definition at line 59 of file LOSX.c.

◆ t57

float t57 = t28+t31-t65

Definition at line 61 of file LOSX.c.

◆ t58

float t58 = t26-t36

Definition at line 62 of file LOSX.c.

◆ t59

float t59 = t33+t35-t66

Definition at line 64 of file LOSX.c.

◆ t6

float t6 = Tbs.b.y*q1*2.0f

Definition at line 6 of file LOSX.c.

◆ t60

float t60 = Tbs.b.x*t39

Definition at line 41 of file LOSX.c.

◆ t61

float t61 = P[0][0]*t2*t46

Definition at line 65 of file LOSX.c.

◆ t62

float t62 = t8*ve

Definition at line 49 of file LOSX.c.

◆ t63

float t63 = t11*vd

Definition at line 53 of file LOSX.c.

◆ t64

float t64 = t5*vn

Definition at line 57 of file LOSX.c.

◆ t65

float t65 = Tbs.b.y*t56

Definition at line 60 of file LOSX.c.

◆ t66

float t66 = Tbs.b.z*t58

Definition at line 63 of file LOSX.c.

◆ t67

float t67 = P[1][1]*t2*t49

Definition at line 66 of file LOSX.c.

◆ t68

float t68 = P[4][0]*t2*t57

Definition at line 67 of file LOSX.c.

◆ t69

float t69 = P[5][0]*t2*t59

Definition at line 68 of file LOSX.c.

◆ t7

float t7 = Tbs.b.z*q0*2.0f

Definition at line 7 of file LOSX.c.

◆ t70

float t70 = P[6][0]*t2*t42

Definition at line 69 of file LOSX.c.

◆ t71

float t71 = P[1][0]*t2*t49

Definition at line 70 of file LOSX.c.

◆ t72

float t72 = P[2][0]*t2*t52

Definition at line 71 of file LOSX.c.

◆ t73

float t73 = P[3][0]*t2*t55

Definition at line 72 of file LOSX.c.

◆ t74

float t74 = t61+t68+t69+t70+t71+t72+t73

Definition at line 73 of file LOSX.c.

◆ t75

float t75 = t2*t46*t74

Definition at line 74 of file LOSX.c.

◆ t76

float t76 = P[4][1]*t2*t57

Definition at line 75 of file LOSX.c.

◆ t77

float t77 = P[5][1]*t2*t59

Definition at line 76 of file LOSX.c.

◆ t78

float t78 = P[6][1]*t2*t42

Definition at line 77 of file LOSX.c.

◆ t79

float t79 = P[0][1]*t2*t46

Definition at line 78 of file LOSX.c.

◆ t8

float t8 = t6+t7-t16

Definition at line 9 of file LOSX.c.

◆ t80

float t80 = P[2][1]*t2*t52

Definition at line 79 of file LOSX.c.

◆ t81

float t81 = P[3][1]*t2*t55

Definition at line 80 of file LOSX.c.

◆ t82

float t82 = t67+t76+t77+t78+t79+t80+t81

Definition at line 81 of file LOSX.c.

◆ t83

float t83 = t2*t49*t82

Definition at line 82 of file LOSX.c.

◆ t84

float t84 = P[4][2]*t2*t57

Definition at line 83 of file LOSX.c.

◆ t85

float t85 = P[5][2]*t2*t59

Definition at line 84 of file LOSX.c.

◆ t86

float t86 = P[6][2]*t2*t42

Definition at line 85 of file LOSX.c.

◆ t87

float t87 = P[0][2]*t2*t46

Definition at line 86 of file LOSX.c.

◆ t88

float t88 = P[1][2]*t2*t49

Definition at line 87 of file LOSX.c.

◆ t89

float t89 = P[2][2]*t2*t52

Definition at line 88 of file LOSX.c.

◆ t9

float t9 = Tbs.b.x*q0*2.0f

Definition at line 10 of file LOSX.c.

◆ t90

float t90 = P[3][2]*t2*t55

Definition at line 89 of file LOSX.c.

◆ t91

float t91 = t84+t85+t86+t87+t88+t89+t90

Definition at line 90 of file LOSX.c.

◆ t92

float t92 = t2*t52*t91

Definition at line 91 of file LOSX.c.

◆ t93

float t93 = P[4][3]*t2*t57

Definition at line 92 of file LOSX.c.

◆ t94

float t94 = P[5][3]*t2*t59

Definition at line 93 of file LOSX.c.

◆ t95

float t95 = P[6][3]*t2*t42

Definition at line 94 of file LOSX.c.

◆ t96

float t96 = P[0][3]*t2*t46

Definition at line 95 of file LOSX.c.

◆ t97

float t97 = P[1][3]*t2*t49

Definition at line 96 of file LOSX.c.

◆ t98

float t98 = P[2][3]*t2*t52

Definition at line 97 of file LOSX.c.

◆ t99

float t99 = P[3][3]*t2*t55

Definition at line 98 of file LOSX.c.