From 13f8936cf1742ceee1cecad0b45bda580fdbbf6d Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Fri, 5 May 2017 15:11:58 +0200 Subject: [PATCH] mc_pos_control: warning for invalid thrust sp --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 12 ++++++++++++ 1 file changed, 12 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 72a8eb06cb..4fe46a5102 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1476,6 +1476,11 @@ void MulticopterPositionControl::control_auto(float dt) if (previous_in_front) { /* just use the default velocity along track */ + vel_sp_along_track = vec_prev_to_pos.length() * _params.pos_p(0); + + if (vel_sp_along_track > get_cruising_speed_xy()) { + vel_sp_along_track = get_cruising_speed_xy(); + } } else if (current_behind) { @@ -2079,6 +2084,13 @@ MulticopterPositionControl::calculate_thrust_setpoint(float dt) thrust_body_z = thr_max; } + /* if any of the thrust setpoint is bogus, send out a warning */ + for (int i = 0; i < 3; ++i) { + if (!PX4_ISFINITE(thrust_sp(i))) { + warn_rate_limited("Thrust setpoint not finite"); + } + } + _att_sp.thrust = math::max(thrust_body_z, thr_min); /* update integrals */