From 4c9dba869211d9129318f0524f0df024319cccec Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 7 Feb 2020 10:27:54 +0100 Subject: [PATCH] VTOL type and tiltrotor: use math::constrain() for constraining Signed-off-by: Silvan Fuhrer --- src/modules/vtol_att_control/tiltrotor.cpp | 4 +--- src/modules/vtol_att_control/vtol_type.cpp | 3 +-- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index aa3c2d2f08..183f2090ef 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -397,9 +397,7 @@ float Tiltrotor::thrust_compensation_for_tilt() { // only compensate for tilt angle up to 0.5 * max tilt - float compensated_tilt = _tilt_control; - compensated_tilt = compensated_tilt < 0.0f ? 0.0f : compensated_tilt; - compensated_tilt = compensated_tilt > 0.5f ? 0.5f : compensated_tilt; + float compensated_tilt = math::constrain(_tilt_control, 0.0f, 0.5f); // increase vertical thrust by 1/cos(tilt), limmit to [0,1] return math::constrain(_v_att_sp->thrust_body[2] / cosf(compensated_tilt * M_PI_2_F), 0.0f, 1.0f); diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 62211d1648..b1b0b84b4b 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -434,8 +434,7 @@ float VtolType::pusher_assist() forward_thrust = (sinf(-pitch_down) - sinf(_params->down_pitch_max)) * _params->forward_thrust_scale; // limit forward actuation to [0, 0.9] - forward_thrust = forward_thrust < 0.0f ? 0.0f : forward_thrust; - forward_thrust = forward_thrust > 0.9f ? 0.9f : forward_thrust; + forward_thrust = math::constrain(forward_thrust, 0.0f, 0.9f); // return the vehicle to level position float pitch_new = 0.0f;