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