From 65b8bcae129e56d1e0e673a1b9530551904c9650 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 2 Dec 2016 15:59:13 +0100 Subject: [PATCH] mc_pos_control: always require takeoff setpoint Problem: The drone started taking off in LOITER (HOLD) mode even if on ground and did not do a proper takeoff jump. The effect was mostly a hovering close above ground or tilting/flipping because of integrator wind-up. Solution: Stay at minimum throttle until a takeoff setpoint is issued. --- .../mc_pos_control/mc_pos_control_main.cpp | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 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 e6722032ed..ce47f13bad 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1825,9 +1825,20 @@ MulticopterPositionControl::control_position(float dt) float vel_z_change = (_vel(2) - _vel_prev(2)) / dt; _acc_z_lp = _acc_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * vel_z_change; - /* adjust limits for landing mode */ - if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && - _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + // We can only run the control if we're already in-air or have a takeoff setpoint. + // Otherwise, we should just bail out + const bool got_takeoff_setpoint = (_pos_sp_triplet.current.valid && + _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF); + + if (_vehicle_land_detected.landed && !got_takeoff_setpoint) { + PX4_INFO("still on ground"); + // Keep throttle low while still on ground. + thr_max = 0.0f; + + } else if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && + _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + + /* adjust limits for landing mode */ /* limit max tilt and min lift when landing */ tilt_max = _params.tilt_max_land;