forked from Archive/PX4-Autopilot
90 lines
2.2 KiB
C
90 lines
2.2 KiB
C
float t3 = ve-vwe;
|
|
float t4 = q0*q0;
|
|
float t5 = q1*q1;
|
|
float t6 = q2*q2;
|
|
float t7 = q3*q3;
|
|
float t8 = vd-vwd;
|
|
float t9 = q0*q1*2.0f;
|
|
float t10 = q2*q3*2.0f;
|
|
float t11 = vn-vwn;
|
|
float t13 = q0*q2*2.0f;
|
|
float t14 = q1*q3*2.0f;
|
|
float t18 = t4-t5-t6+t7;
|
|
float t19 = t8*t18;
|
|
float t20 = t9-t10;
|
|
float t21 = t3*t20;
|
|
float t22 = t13+t14;
|
|
float t23 = t11*t22;
|
|
float t2 = t19-t21+t23;
|
|
float t15 = q0*q3*2.0f;
|
|
float t16 = q1*q2*2.0f;
|
|
float t24 = t9+t10;
|
|
float t25 = t4-t5+t6-t7;
|
|
float t26 = t3*t25;
|
|
float t27 = t8*t24;
|
|
float t28 = t15-t16;
|
|
float t29 = t11*t28;
|
|
float t12 = t26+t27-t29;
|
|
float t30 = t13-t14;
|
|
float t31 = t4+t5-t6-t7;
|
|
float t32 = t11*t31;
|
|
float t33 = t8*t30;
|
|
float t34 = t15+t16;
|
|
float t35 = t3*t34;
|
|
float t17 = t32-t33+t35;
|
|
float t44 = t2*t18*2.0f;
|
|
float t45 = t12*t24*2.0f;
|
|
float t46 = t17*t30*2.0f;
|
|
float t36 = t44+t45-t46;
|
|
float t37 = t2*t2;
|
|
float t38 = t12*t12;
|
|
float t39 = t17*t17;
|
|
float t40 = t37+t38+t39;
|
|
float t41 = 1.0f/t40;
|
|
float t48 = t12*t25*2.0f;
|
|
float t49 = t2*t20*2.0f;
|
|
float t50 = t17*t34*2.0f;
|
|
float t42 = t48-t49+t50;
|
|
float t52 = t17*t31*2.0f;
|
|
float t53 = t2*t22*2.0f;
|
|
float t54 = t12*t28*2.0f;
|
|
float t43 = t52+t53-t54;
|
|
float t47 = t36*t36;
|
|
float t51 = t42*t42;
|
|
float t55 = t43*t43;
|
|
float t57 = 1.0f/(t17*t17);
|
|
float t58 = 1.0f/t17;
|
|
float t63 = t18*t58;
|
|
float t64 = t2*t30*t57;
|
|
float t56 = t63+t64;
|
|
float t66 = t22*t58;
|
|
float t67 = t2*t31*t57;
|
|
float t59 = t66-t67;
|
|
float t60 = t37*t57;
|
|
float t61 = t60+1.0f;
|
|
float t62 = 1.0f/(t61*t61);
|
|
float t65 = t56*t56;
|
|
float t68 = t59*t59;
|
|
float t70 = t20*t58;
|
|
float t71 = t2*t34*t57;
|
|
float t69 = t70+t71;
|
|
float t72 = t69*t69;
|
|
float t78 = t25*t58;
|
|
float t79 = t12*t34*t57;
|
|
float t73 = t78-t79;
|
|
float t81 = t28*t58;
|
|
float t82 = t12*t31*t57;
|
|
float t74 = t81+t82;
|
|
float t75 = t38*t57;
|
|
float t76 = t75+1.0f;
|
|
float t77 = 1.0f/(t76*t76);
|
|
float t80 = t73*t73;
|
|
float t83 = t74*t74;
|
|
float t85 = t24*t58;
|
|
float t86 = t12*t30*t57;
|
|
float t84 = t85+t86;
|
|
float t87 = t84*t84;
|
|
float tas_var = t41*t47*vd_var*0.25f+t41*t51*ve_var*0.25f+t41*t55*vn_var*0.25f+t41*t47*vwd_var*0.25f+t41*t51*vwe_var*0.25f+t41*t55*vwn_var*0.25f;
|
|
float aoa_var = t62*t65*vd_var+t62*t72*ve_var+t62*t68*vn_var+t62*t65*vwd_var+t62*t72*vwe_var+t62*t68*vwn_var;
|
|
float aos_var = t77*t87*vd_var+t77*t80*ve_var+t77*t83*vn_var+t77*t87*vwd_var+t77*t80*vwe_var+t77*t83*vwn_var;
|