diff options
Diffstat (limited to 'src')
| -rwxr-xr-x | src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 4 |
1 files changed, 3 insertions, 1 deletions
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 86f59f0e6..c60d70b9f 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)) { |