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;