|
|
|
@ -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), |
|
|
|
|