mirror of https://github.com/ArduPilot/ardupilot
76 lines
2.1 KiB
C++
76 lines
2.1 KiB
C++
float t2 = cos(gPhi);
|
|
float t3 = cos(gTheta);
|
|
float t4 = sin(gPhi);
|
|
float t5 = sin(gTheta);
|
|
float t6 = q0*q0;
|
|
float t7 = q1*q1;
|
|
float t8 = q2*q2;
|
|
float t9 = q3*q3;
|
|
float t10 = t6+t7-t8-t9;
|
|
float t11 = sin(gPsi);
|
|
float t12 = cos(gPsi);
|
|
float t13 = q0*q2*2.0f;
|
|
float t14 = q1*q3*2.0f;
|
|
float t15 = t13+t14;
|
|
float t16 = q0*q3*2.0f;
|
|
float t18 = q1*q2*2.0f;
|
|
float t17 = t16-t18;
|
|
float t19 = t3*t11;
|
|
float t20 = t4*t5*t12;
|
|
float t21 = t19+t20;
|
|
float t22 = t16+t18;
|
|
float t23 = t5*t11;
|
|
float t41 = t3*t4*t12;
|
|
float t24 = t23-t41;
|
|
float t25 = q0*q1*2.0f;
|
|
float t31 = q2*q3*2.0f;
|
|
float t26 = t25-t31;
|
|
float t27 = t6-t7+t8-t9;
|
|
float t28 = t5*t12;
|
|
float t29 = t3*t4*t11;
|
|
float t30 = t28+t29;
|
|
float t32 = t3*t12;
|
|
float t46 = t4*t5*t11;
|
|
float t33 = t32-t46;
|
|
float t35 = t4*t17;
|
|
float t36 = t2*t5*t10;
|
|
float t37 = t2*t3*t15;
|
|
float t38 = t35+t36-t37;
|
|
float t39 = magZ*t38;
|
|
float t40 = t10*t21;
|
|
float t42 = t15*t24;
|
|
float t43 = t2*t12*t17;
|
|
float t44 = t40+t42-t43;
|
|
float t45 = magY*t44;
|
|
float t47 = t10*t33;
|
|
float t48 = t15*t30;
|
|
float t49 = t2*t11*t17;
|
|
float t50 = t47+t48+t49;
|
|
float t51 = magX*t50;
|
|
float t52 = -t39+t45+t51;
|
|
float t53 = 1.0/t52;
|
|
float t54 = t4*t27;
|
|
float t55 = t2*t3*t26;
|
|
float t56 = t2*t5*t22;
|
|
float t57 = -t54+t55+t56;
|
|
float t58 = magZ*t57;
|
|
float t59 = t21*t22;
|
|
float t60 = t24*t26;
|
|
float t61 = t2*t12*t27;
|
|
float t62 = t59-t60+t61;
|
|
float t63 = magY*t62;
|
|
float t64 = t26*t30;
|
|
float t65 = t22*t33;
|
|
float t66 = t2*t11*t27;
|
|
float t67 = t64-t65+t66;
|
|
float t68 = magX*t67;
|
|
float t69 = t58-t63+t68;
|
|
float t70 = t53*t69;
|
|
float t34 = tan(t70);
|
|
float t71 = t34*t34;
|
|
float t72 = t71+1.0;
|
|
float t73 = 1.0/(t52*t52);
|
|
A0[0][0] = -t72*(t53*(magZ*(t4*t26+t2*t3*t27)+magY*(t24*t27+t2*t12*t26)+magX*(t27*t30-t2*t11*t26))-t69*t73*(magZ*(t4*t15+t2*t3*t17)+magY*(t17*t24+t2*t12*t15)+magX*(t17*t30-t2*t11*t15)));
|
|
A0[0][1] = t72*(t53*(magZ*(t2*t3*t22-t2*t5*t26)+magY*(t22*t24+t21*t26)+magX*(t22*t30+t26*t33))+t69*t73*(magZ*(t2*t3*t10+t2*t5*t15)+magY*(t10*t24-t15*t21)+magX*(t10*t30-t15*t33)));
|
|
A0[0][2] = t72*(t53*(-magZ*(t4*t22+t2*t5*t27)+magY*(t21*t27-t2*t12*t22)+magX*(t27*t33+t2*t11*t22))-t69*t73*(magZ*(t4*t10-t2*t5*t17)+magY*(t17*t21+t2*t10*t12)+magX*(t17*t33-t2*t10*t11)));
|