|
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) | |