From ff25c7f48abba7f0575b75069e27baf340132534 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 13 Mar 2018 10:23:17 +0100 Subject: [PATCH] ensure attitude setpoint initialization before arming - On initialization _v_att_sp got filled with zeros leaving invalid quaternions - While not armed mc_pos_control did not publish any attitude setpoint which makes no sense - The attitude control just uses the data in _v_att_sp if it was (ever) updated or not --- src/modules/mc_att_control/mc_att_control_main.cpp | 4 ++++ src/modules/mc_pos_control/mc_pos_control_main.cpp | 10 ++++------ 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 546a844a14..ed1999caf3 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -389,6 +389,10 @@ _loop_update_rate_hz(initial_update_rate_hz) _vehicle_status.is_rotary_wing = true; + /* initialize quaternions in messages to be valid */ + _v_att.q[0] = 1.f; + _v_att_sp.q_d[0] = 1.f; + _params.rate_p.zero(); _params.rate_i.zero(); _params.rate_int_lim.zero(); 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 1738e0d8cb..19721cb778 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -3234,13 +3234,11 @@ MulticopterPositionControl::task_main() * in this case the attitude setpoint is published by the mavlink app. * - if the vehicle is a VTOL and it's just doing a transition (the VTOL attitude control module will generate * attitude setpoints for the transition). - * - if not armed */ - if (_control_mode.flag_armed && - (!(_control_mode.flag_control_offboard_enabled && - !(_control_mode.flag_control_position_enabled || - _control_mode.flag_control_velocity_enabled || - _control_mode.flag_control_acceleration_enabled)))) { + if (!(_control_mode.flag_control_offboard_enabled && + !(_control_mode.flag_control_position_enabled || + _control_mode.flag_control_velocity_enabled || + _control_mode.flag_control_acceleration_enabled))) { if (_att_sp_pub != nullptr) { orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);