Browse Source

uuv_example_app move to matrix lib

sbg
Daniel Agar 7 years ago
parent
commit
5b6fda2e4b
  1. 12
      src/examples/uuv_example_app/uuv_example_app.cpp

12
src/examples/uuv_example_app/uuv_example_app.cpp

@ -133,15 +133,13 @@ int uuv_example_app_main(int argc, char *argv[]) @@ -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),

Loading…
Cancel
Save