From bc3f5daeec8697d1aaf7c9d8461948eeab881168 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Dec 2015 21:06:01 +0000 Subject: [PATCH] Fix attitude yaw generation, store last acceleration setpoint for all non-velocity / position flight modes. --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 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 9f650a9d66..2596e8d89e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1652,9 +1652,10 @@ 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 - && !_control_mode.flag_control_velocity_enabled) { + if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) { /* reset yaw setpoint to current position if needed */ if (reset_yaw_sp) { @@ -1694,7 +1695,6 @@ MulticopterPositionControl::task_main() } /* construct attitude setpoint rotation matrix */ - math::Matrix<3, 3> R_sp; 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)); @@ -1704,8 +1704,6 @@ MulticopterPositionControl::task_main() memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d)); _att_sp.timestamp = hrt_absolute_time(); - _thrust_sp_prev = R_sp * math::Vector<3>(0, 0, -_att_sp.thrust); - } else { reset_yaw_sp = true; } @@ -1723,6 +1721,8 @@ 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);