mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-31 17:41:35 +08:00
39 lines
753 B
C
39 lines
753 B
C
t2 = sin(ant_yaw);
|
|
t3 = cos(ant_yaw);
|
|
t4 = q0*q3*2.0;
|
|
t5 = q0*q0;
|
|
t6 = q1*q1;
|
|
t7 = q2*q2;
|
|
t8 = q3*q3;
|
|
t9 = q1*q2*2.0;
|
|
t10 = t5+t6-t7-t8;
|
|
t11 = t3*t10;
|
|
t12 = t4+t9;
|
|
t13 = t3*t12;
|
|
t14 = t5-t6+t7-t8;
|
|
t15 = t2*t14;
|
|
t16 = t13+t15;
|
|
t17 = t4-t9;
|
|
t19 = t2*t17;
|
|
t20 = t11-t19;
|
|
t18 = 1.0/(t20*t20);
|
|
t21 = t16*t16;
|
|
t22 = 1.0/pow(t11-t19][2.0);
|
|
t23 = q1*t3*2.0;
|
|
t24 = q2*t2*2.0;
|
|
t25 = t23+t24;
|
|
t26 = 1.0/t20;
|
|
t27 = q1*t2*2.0;
|
|
t28 = t21*t22;
|
|
t29 = t28+1.0;
|
|
t30 = 1.0/t29;
|
|
t31 = q0*t3*2.0;
|
|
t32 = t31-q3*t2*2.0;
|
|
t33 = q3*t3*2.0;
|
|
t34 = q0*t2*2.0;
|
|
t35 = t33+t34;
|
|
A0[0][0] = (t35/(t11-t2*(t4-q1*q2*2.0))-t16*t18*t32)/(t18*t21+1.0);
|
|
A0[0][1] = -t30*(t26*(t27-q2*t3*2.0)+t16*t22*t25);
|
|
A0[0][2] = t30*(t25*t26-t16*t22*(t27-q2*t3*2.0));
|
|
A0[0][3] = t30*(t26*t32+t16*t22*t35);
|