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 d0633f9b00..5f618ff882 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -244,8 +244,6 @@ private: bool _hold_offboard_z = false; bool _limit_vel_xy = false; - bool _transition_to_non_manual = false; - math::Vector<3> _thrust_int; math::Vector<3> _pos; @@ -763,13 +761,6 @@ MulticopterPositionControl::poll_subscriptions() !PX4_ISFINITE(_pos_sp_triplet.current.alt)) { _pos_sp_triplet.current.valid = false; } - - /* to avoid time scheduling issue that occurs when the navigator has not updated the triplet - * but the mc_pos_control already received a non-manual control flag - */ - if (_transition_to_non_manual && !_control_mode.flag_control_manual_enabled) { - _transition_to_non_manual = false; - } } orb_check(_home_pos_sub, &updated); @@ -1604,6 +1595,7 @@ void MulticopterPositionControl::control_auto(float dt) } } else { + /* idle or triplet not valid, set velocity setpoint to zero */ _vel_sp.zero(); _run_pos_control = false; @@ -1678,15 +1670,15 @@ MulticopterPositionControl::do_control(float dt) control_manual(dt); _mode_auto = false; - _transition_to_non_manual = true; + /* we set tiplets to false + * this ensures that when switching to auto, the position + * controller will not use the old triplets but waits until triplets + * have been updated */ + _pos_sp_triplet.current.valid = false; _hold_offboard_xy = false; _hold_offboard_z = false; - } else if (_transition_to_non_manual) { - /* we reuse the previous setpoints */ - calculate_thrust_setpoint(dt); - } else { control_non_manual(dt); }