mirror of https://github.com/ArduPilot/ardupilot
41 lines
1.2 KiB
C
41 lines
1.2 KiB
C
|
t2 = SF(1.5E1);
|
||
|
|
t3 = SF(1.4E1);
|
||
|
|
t4 = SF(1.3E1);
|
||
|
|
t5 = SF(1.2E1);
|
||
|
|
t6 = SF(1.1E1);
|
||
|
|
t7 = SF(1.0E1);
|
||
|
|
t8 = SF(9.0);
|
||
|
|
t9 = SF(8.0);
|
||
|
|
t10 = SF(7.0);
|
||
|
|
t11 = SF(6.0);
|
||
|
|
t12 = SF(5.0);
|
||
|
|
t13 = SF(4.0);
|
||
|
|
t14 = SF(1.7E1);
|
||
|
|
t15 = SF(2.3E1);
|
||
|
|
t16 = SF(2.0E1);
|
||
|
|
t17 = SF(1.8E1);
|
||
|
|
t18 = SF(2.1E1);
|
||
|
|
t19 = SF(2.4E1);
|
||
|
|
t20 = SF(2.5E1);
|
||
|
|
t21 = SF(2.2E1);
|
||
|
|
t22 = SF(1.9E1);
|
||
|
|
A0[0][0] = q0*t3*-2.0+q1*t2*2.0+q2*t5*2.0+q3*t4*2.0;
|
||
|
|
A0[1][0] = q0*t7*2.0+q1*t6*2.0+q2*t9*2.0-q3*t8*2.0;
|
||
|
|
A0[2][0] = q0*t2*2.0+q1*t3*2.0-q2*t4*2.0+q3*t5*2.0;
|
||
|
|
A0[3][0] = q0*t6*2.0-q1*t7*2.0+q2*t8*2.0+q3*t9*2.0;
|
||
|
|
A0[4][0] = q0*t5*2.0+q1*t4*2.0+q2*t3*2.0-q3*t2*2.0;
|
||
|
|
A0[5][0] = q0*t9*-2.0+q1*t8*2.0+q2*t7*2.0+q3*t6*2.0;
|
||
|
|
A0[6][0] = q0*t11*2.0+q1*t10*2.0-q2*t13*2.0+q3*t12*2.0;
|
||
|
|
A0[7][0] = q0*t10*-2.0+q1*t11*2.0+q2*t12*2.0+q3*t13*2.0;
|
||
|
|
A0[8][0] = q0*t13*2.0-q1*t12*2.0+q2*t11*2.0+q3*t10*2.0;
|
||
|
|
A0[9][0] = dvy*t14-dvz*t15;
|
||
|
|
A0[10][0] = dvx*t17-dvy*t19;
|
||
|
|
A0[11][0] = dvx*t20-dvz*t22;
|
||
|
|
A0[12][0] = dvx*t14+dvz*t16;
|
||
|
|
A0[13][0] = dvx*t15+dvy*t16;
|
||
|
|
A0[14][0] = dvy*t18+dvz*t17;
|
||
|
|
A0[15][0] = dvx*t18+dvz*t19;
|
||
|
|
A0[16][0] = dvy*t20+dvz*t21;
|
||
|
|
A0[17][0] = dvx*t21+dvy*t22;
|
||
|
|
A0[18][0] = SF(1.6E1);
|