From f41f60901db7c62c2a06104fd0a2480b85bb95d8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Dec 2015 15:16:29 +0100 Subject: [PATCH] Control state: Update topic correctly - only if it actually changed and is valid --- .../mc_pos_control/mc_pos_control_main.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index a49714ff52..2a68b44a5b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -374,6 +374,7 @@ MulticopterPositionControl::MulticopterPositionControl() : { memset(&_vehicle_status, 0, sizeof(_vehicle_status)); memset(&_ctrl_state, 0, sizeof(_ctrl_state)); + _ctrl_state.q[0] = 1.0f; memset(&_att_sp, 0, sizeof(_att_sp)); memset(&_manual, 0, sizeof(_manual)); memset(&_control_mode, 0, sizeof(_control_mode)); @@ -573,6 +574,13 @@ MulticopterPositionControl::poll_subscriptions() if (updated) { orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); + + /* get current rotation matrix and euler angles from control state quaternions */ + math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); + _R = q_att.to_dcm(); + math::Vector<3> euler_angles; + euler_angles = _R.to_euler(); + _yaw = euler_angles(2); } orb_check(_att_sp_sub, &updated); @@ -1120,13 +1128,6 @@ MulticopterPositionControl::task_main() poll_subscriptions(); - /* get current rotation matrix and euler angles from control state quaternions */ - math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); - _R = q_att.to_dcm(); - math::Vector<3> euler_angles; - euler_angles = _R.to_euler(); - _yaw = euler_angles(2); - parameters_update(false); hrt_abstime t = hrt_absolute_time();