Skip to content
Snippets Groups Projects
Commit 5b6fda2e authored by Daniel Agar's avatar Daniel Agar
Browse files

uuv_example_app move to matrix lib

parent ac724298
No related branches found
No related tags found
No related merge requests found
......@@ -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),
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment