forked from Archive/PX4-Autopilot
compute quaternion
This commit is contained in:
parent
f4f4c72f05
commit
e582da9ee7
|
@ -590,9 +590,11 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
|
||||||
|
|
||||||
math::Matrix<3, 3> R_body = (&Rot_matrix[0]);
|
math::Matrix<3, 3> R_body = (&Rot_matrix[0]);
|
||||||
R = R_decl * R_body;
|
R = R_decl * R_body;
|
||||||
|
math::Quaternion q;
|
||||||
|
q.from_dcm(R);
|
||||||
/* copy rotation matrix */
|
/* copy rotation matrix */
|
||||||
memcpy(&att.R[0], &R.data[0][0], sizeof(att.R));
|
memcpy(&att.R[0], &R.data[0][0], sizeof(att.R));
|
||||||
|
memcpy(&att.q[0],&q.data[0],sizeof(att.q));
|
||||||
att.R_valid = true;
|
att.R_valid = true;
|
||||||
|
|
||||||
if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
|
if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
|
||||||
|
|
Loading…
Reference in New Issue