From e582da9ee79315c8fe5939ec869bf6a092ed9d4d Mon Sep 17 00:00:00 2001 From: tumbili Date: Sun, 15 Feb 2015 14:02:46 +0100 Subject: [PATCH] compute quaternion --- .../attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 86f59f0e6a..c60d70b9f8 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -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)) {