From 14b0511c6c8ffa6ddbed7b493350705501497813 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 19 Oct 2016 09:40:00 +0200 Subject: [PATCH] MC pos control multiplatform style fix. --- src/examples/mc_pos_control_multiplatform/mc_pos_control.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/examples/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/examples/mc_pos_control_multiplatform/mc_pos_control.cpp index ab661b92e6..bc131d4953 100644 --- a/src/examples/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/examples/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -932,14 +932,13 @@ void MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4 _R(i, 2) = body_z(i); } - /* calculate euler angles, for logging only, must not be used for control */ - math::Vector<3> eul = _R.to_euler(); - math::Quaternion q; q.from_dcm(_R); memcpy(_att_sp_msg.data().q_d, q.data, sizeof(_att_sp_msg.data().q_d)); _att_sp_msg.data().q_d_valid = true; + /* calculate euler angles, for logging only, must not be used for control */ + math::Vector<3> eul = _R.to_euler(); _att_sp_msg.data().roll_body = eul(0); _att_sp_msg.data().pitch_body = eul(1); /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */