mirror of https://github.com/ArduPilot/ardupilot
52 lines
1.3 KiB
C
52 lines
1.3 KiB
C
|
t2 = SF(1.0);
|
||
|
|
t3 = SF(2.0);
|
||
|
|
t4 = SF(3.0);
|
||
|
|
t5 = q3*(1.0/2.0);
|
||
|
|
t6 = q0*t2*(1.0/2.0);
|
||
|
|
t7 = q2*t4*(1.0/2.0);
|
||
|
|
t8 = q0*t3*(1.0/2.0);
|
||
|
|
t9 = q1*t2*(1.0/2.0);
|
||
|
|
t10 = q3*t4*(1.0/2.0);
|
||
|
|
t11 = q1*(1.0/2.0);
|
||
|
|
t12 = q2*t2*(1.0/2.0);
|
||
|
|
t13 = q3*t3*(1.0/2.0);
|
||
|
|
t14 = q0*(1.0/2.0);
|
||
|
|
t15 = q1*t4*(1.0/2.0);
|
||
|
|
t16 = q2*t3*(1.0/2.0);
|
||
|
|
t17 = q3*t2*(1.0/2.0);
|
||
|
|
t18 = q0*t4*(1.0/2.0);
|
||
|
|
t19 = q2*(1.0/2.0);
|
||
|
|
t20 = q1*t3*(1.0/2.0);
|
||
|
|
t21 = q0*q0;
|
||
|
|
t22 = q1*q1;
|
||
|
|
t23 = q2*q2;
|
||
|
|
t24 = q3*q3;
|
||
|
|
t25 = q0*q1*2.0;
|
||
|
|
t26 = q0*q3*2.0;
|
||
|
|
t27 = q0*q2*2.0;
|
||
|
|
A0[0][0] = daz*(1.0/2.0)-daz_b*(1.0/2.0);
|
||
|
|
A0[1][0] = day*(1.0/2.0)-day_b*(1.0/2.0);
|
||
|
|
A0[2][0] = dax*(1.0/2.0)-dax_b*(1.0/2.0);
|
||
|
|
A0[3][0] = q2*(-1.0/2.0)+t8+t9+t10;
|
||
|
|
A0[4][0] = t5+t6+t7-q1*t3*(1.0/2.0);
|
||
|
|
A0[5][0] = t14+t15+t16-q3*t2*(1.0/2.0);
|
||
|
|
A0[6][0] = t11+t12+t13-q0*t4*(1.0/2.0);
|
||
|
|
A0[7][0] = t5-t6+t7+t20;
|
||
|
|
A0[8][0] = t8+t9-t10+t19;
|
||
|
|
A0[9][0] = -t11+t12+t13+t18;
|
||
|
|
A0[10][0] = t14+t15-t16+t17;
|
||
|
|
A0[11][0] = t14-t15+t16+t17;
|
||
|
|
A0[12][0] = t11-t12+t13+t18;
|
||
|
|
A0[13][0] = -t8+t9+t10+t19;
|
||
|
|
A0[14][0] = -t5+t6+t7+t20;
|
||
|
|
A0[15][0] = -t21-t22-t23-t24;
|
||
|
|
A0[16][0] = t21-t22-t23+t24;
|
||
|
|
A0[17][0] = t21-t22+t23-t24;
|
||
|
|
A0[18][0] = t21+t22-t23-t24;
|
||
|
|
A0[19][0] = t27-q1*q3*2.0;
|
||
|
|
A0[20][0] = t25-q2*q3*2.0;
|
||
|
|
A0[21][0] = t26-q1*q2*2.0;
|
||
|
|
A0[22][0] = t25+q2*q3*2.0;
|
||
|
|
A0[23][0] = t26+q1*q2*2.0;
|
||
|
|
A0[24][0] = t27+q1*q3*2.0;
|