From fb1b6a0ab91cac70624c80fdf47be200d641982e Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 10 Mar 2020 11:53:19 +0100 Subject: [PATCH] mc_pos_control: hotfix for takeoff ramp stuck with NAN Reported by @dusan19 in #13973. --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 ++++++ 1 file changed, 6 insertions(+) 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 ce658bcb30..98a93bdada 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -587,6 +587,12 @@ MulticopterPositionControl::Run() // check if all local states are valid and map accordingly set_vehicle_states(setpoint.vz); + // fix to prevent the takeoff ramp to ramp to a too high value or get stuck because of NAN + // TODO: this should get obsolete once the takeoff limiting moves into the flight tasks + if (!PX4_ISFINITE(constraints.speed_up) || (constraints.speed_up > _param_mpc_z_vel_max_up.get())) { + constraints.speed_up = _param_mpc_z_vel_max_up.get(); + } + // 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);