From 35df66ce5d50bc35266944c3497f2928a3508c88 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Dec 2015 16:33:25 +0000 Subject: [PATCH] MC pos control: Fix attitude and thrust SP generation --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 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 2596e8d89e..97d0e097f6 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1652,8 +1652,6 @@ MulticopterPositionControl::task_main() _vel_sp_prev = _vel; } - math::Matrix<3, 3> R_sp; - /* generate attitude setpoint from manual controls */ if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) { @@ -1694,10 +1692,20 @@ MulticopterPositionControl::task_main() } } + math::Matrix<3, 3> R_sp; + /* construct attitude setpoint rotation matrix */ R_sp.from_euler(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body); memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body)); + /* reset the acceleration set point for all non-attitude flight modes */ + if (!(_control_mode.flag_control_offboard_enabled && + !(_control_mode.flag_control_position_enabled || + _control_mode.flag_control_velocity_enabled))) { + + _thrust_sp_prev = R_sp * math::Vector<3>(0, 0, -_att_sp.thrust); + } + /* copy quaternion setpoint to attitude setpoint topic */ math::Quaternion q_sp; q_sp.from_dcm(R_sp); @@ -1721,8 +1729,6 @@ MulticopterPositionControl::task_main() !(_control_mode.flag_control_position_enabled || _control_mode.flag_control_velocity_enabled))) { - _thrust_sp_prev = R_sp * math::Vector<3>(0, 0, -_att_sp.thrust); - if (_att_sp_pub != nullptr) { orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);