From 01d6241c07d0d7eedff39013bc982aeb2f8e2957 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 9 Mar 2020 09:44:30 +0100 Subject: [PATCH] mc_pos_control: prevent takeoff with thrust setpoint MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This was only necessary for stabilized mode before #10805. The unit length thrust setpoint will anyways not be available anymore soon because it gets replaced with the acceleration setpoint in m/s². --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 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 f1b8e9c5f575..b5ba19c42c06 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -578,7 +578,7 @@ MulticopterPositionControl::Run() set_vehicle_states(setpoint.vz); // limit tilt during takeoff ramupup - if (_takeoff.getTakeoffState() < TakeoffState::flight && !PX4_ISFINITE(setpoint.thrust[2])) { + if (_takeoff.getTakeoffState() < TakeoffState::flight) { constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get()); } @@ -592,7 +592,7 @@ MulticopterPositionControl::Run() constraints.speed_up, !_control_mode.flag_control_climb_rate_enabled, time_stamp_now); constraints.speed_up = _takeoff.updateRamp(_dt, constraints.speed_up); - if (_takeoff.getTakeoffState() < TakeoffState::rampup && !PX4_ISFINITE(setpoint.thrust[2])) { + if (_takeoff.getTakeoffState() < TakeoffState::rampup) { // we are not flying yet and need to avoid any corrections reset_setpoint_to_nan(setpoint); setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = 0.0f; @@ -605,8 +605,9 @@ MulticopterPositionControl::Run() // reactivate the task which will reset the setpoint to current state _flight_tasks.reActivate(); _hover_thrust_estimator.reset(); + } - } else if (_takeoff.getTakeoffState() >= TakeoffState::flight) { + if (_takeoff.getTakeoffState() >= TakeoffState::flight) { // Inform the hover thrust estimator about the measured vertical acceleration (positive acceleration is up) if (PX4_ISFINITE(_local_pos.az)) { _hover_thrust_estimator.setAccel(-_local_pos.az); @@ -660,7 +661,7 @@ MulticopterPositionControl::Run() // Part of landing logic: if ground-contact/maybe landed was detected, turn off // controller. This message does not have to be logged as part of the vehicle_local_position_setpoint topic. // Note: only adust thrust output if there was not thrust-setpoint demand in D-direction. - if (_takeoff.getTakeoffState() > TakeoffState::rampup && !PX4_ISFINITE(setpoint.thrust[2])) { + if (_takeoff.getTakeoffState() > TakeoffState::rampup) { limit_thrust_during_landing(attitude_setpoint); }