dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
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]); 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)) {