From 1f98ebdb47a7bcef76fa5d4debdb8d8809511ca0 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 9 Mar 2020 09:16:34 +0100 Subject: [PATCH] mc_pos_control: switch order of setpoint amendment to make sure the takeoff limitation is always done last. --- .../mc_pos_control/mc_pos_control_main.cpp | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 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 98a93bdada..36dd72fab6 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -593,6 +593,16 @@ MulticopterPositionControl::Run() constraints.speed_up = _param_mpc_z_vel_max_up.get(); } + // limit tilt during takeoff ramupup + if (_takeoff.getTakeoffState() < TakeoffState::flight && !PX4_ISFINITE(setpoint.thrust[2])) { + constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get()); + } + + // limit altitude only if local position is valid + if (PX4_ISFINITE(_states.position(2))) { + limit_altitude(setpoint); + } + // handle smooth takeoff _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, constraints.want_takeoff, constraints.speed_up, !_control_mode.flag_control_climb_rate_enabled, time_stamp_now); @@ -612,15 +622,6 @@ MulticopterPositionControl::Run() _flight_tasks.reActivate(); } - if (_takeoff.getTakeoffState() < TakeoffState::flight && !PX4_ISFINITE(setpoint.thrust[2])) { - constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get()); - } - - // limit altitude only if local position is valid - if (PX4_ISFINITE(_states.position(2))) { - limit_altitude(setpoint); - } - // Run position control _control.setState(_states); _control.setConstraints(constraints);