compute quaternion

This commit is contained in:
tumbili 2015-02-15 14:02:46 +01:00 committed by Lorenz Meier
parent f4f4c72f05
commit e582da9ee7
1 changed files with 3 additions and 1 deletions

View File

@ -590,9 +590,11 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
math::Matrix<3, 3> R_body = (&Rot_matrix[0]);
R = R_decl * R_body;
math::Quaternion q;
q.from_dcm(R);
/* copy rotation matrix */
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;
if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {