From e416a8cb8399f6548ae762a28b044c639bed4af3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 2 Dec 2016 10:54:25 +0100 Subject: [PATCH] mc_pos_control: fix takeoff bug This resolves a bug where a takeoff would go sideways instead of straight up. What happened was that the position setpoint got shifted around even though there was actually no real setpoint set but only a setpoint of type IDLE. This then lead to a position setpoint far away from the takeoff point and therefore scary takeoffs. This fix prevents the part of the position controller which moves/integrates the position setpoint from running in the case of an idle setpoint. This bug could be reproduced by switching the vehicle to mission mode without a valid mission, then switch to hold mode, and then send the arm and takeoff command. --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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 b2daed32ab..647f96bb68 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1144,7 +1144,9 @@ void MulticopterPositionControl::control_auto(float dt) } } - if (current_setpoint_valid) { + if (current_setpoint_valid && + (_pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_IDLE)) { + /* scaled space: 1 == position error resulting max allowed speed */ math::Vector<3> cruising_speed = _params.vel_cruise;