forked from Archive/PX4-Autopilot
15 lines
349 B
C
15 lines
349 B
C
|
t2 = 1.0/range;
|
||
|
|
t3 = q0*q0;
|
||
|
|
t4 = q1*q1;
|
||
|
|
t5 = q2*q2;
|
||
|
|
t6 = q3*q3;
|
||
|
|
t7 = q0*q1*2.0;
|
||
|
|
t8 = q0*q3*2.0;
|
||
|
|
t9 = q0*q2*2.0;
|
||
|
|
t10 = q1*q3*2.0;
|
||
|
|
A0[0][1] = t2*(vn*(t9+t10)+vd*(t3-t4-t5+t6)-ve*(t7-q2*q3*2.0));
|
||
|
|
A0[0][2] = -t2*(ve*(t3-t4+t5-t6)+vd*(t7+q2*q3*2.0)-vn*(t8-q1*q2*2.0));
|
||
|
|
A0[0][3] = -t2*(t3+t4-t5-t6);
|
||
|
|
A0[0][4] = -t2*(t8+q1*q2*2.0);
|
||
|
|
A0[0][5] = t2*(t9-t10);
|