Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 11:23:06 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

uuv_example_app move to matrix lib

Browse Source
This commit is contained in:
Daniel Agar
2018-03-21 14:16:02 -04:00
parent ac7242987c
commit 5b6fda2e4b
1 changed files with 5 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/examples/uuv_example_app/uuv_example_app.cpp
+5 -7
View File
@@ -133,15 +133,13 @@ int uuv_example_app_main(int argc, char *argv[])
// get current rotation matrix from control state quaternions, the quaternions are generated by the
// attitude_estimator_q application using the sensor data
math::Quaternion q_att(raw_ctrl_state.q[0], raw_ctrl_state.q[1], raw_ctrl_state.q[2],
raw_ctrl_state.q[3]); // control_state is frequently updated
math::Matrix<3, 3> R =
q_att.to_dcm(); // create rotation matrix for the quaternion when post multiplying with a column vector
matrix::Quatf q_att(raw_ctrl_state.q); // control_state is frequently updated
matrix::Dcmf R = q_att; // create rotation matrix for the quaternion when post multiplying with a column vector
// orientation vectors
math::Vector<3> x_B(R(0, 0), R(1, 0), R(2, 0)); // orientation body x-axis (in world coordinates)
math::Vector<3> y_B(R(0, 1), R(1, 1), R(2, 1)); // orientation body y-axis (in world coordinates)
math::Vector<3> z_B(R(0, 2), R(1, 2), R(2, 2)); // orientation body z-axis (in world coordinates)
matrix::Vector3f x_B(R(0, 0), R(1, 0), R(2, 0)); // orientation body x-axis (in world coordinates)
matrix::Vector3f y_B(R(0, 1), R(1, 1), R(2, 1)); // orientation body y-axis (in world coordinates)
matrix::Vector3f z_B(R(0, 2), R(1, 2), R(2, 2)); // orientation body z-axis (in world coordinates)
PX4_INFO("x_B:\t%8.4f\t%8.4f\t%8.4f",
(double)x_B(0),