From c5deba53a25d809d0c5d27bb232a13ab97cb8fc2 Mon Sep 17 00:00:00 2001 From: Mathieu Bresciani Date: Thu, 27 Jun 2019 17:38:21 +0200 Subject: [PATCH] mc_pos_control: Explicitly convert tilt variables to radians during check and assignments --- src/modules/mc_pos_control/PositionControl.cpp | 10 ++++------ src/modules/mc_pos_control/PositionControl.hpp | 4 ++-- 2 files changed, 6 insertions(+), 8 deletions(-) diff --git a/src/modules/mc_pos_control/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl.cpp index 7de97f461d..4808efabe9 100644 --- a/src/modules/mc_pos_control/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl.cpp @@ -331,9 +331,11 @@ void PositionControl::updateConstraints(const vehicle_constraints_s &constraints // For safety check if adjustable constraints are below global constraints. If they are not stricter than global // constraints, then just use global constraints for the limits. + const float tilt_max_radians = math::radians(math::max(_param_mpc_tiltmax_air.get(), _param_mpc_man_tilt_max.get())); + if (!PX4_ISFINITE(constraints.tilt) - || !(constraints.tilt < math::max(_param_mpc_tiltmax_air.get(), _param_mpc_man_tilt_max.get()))) { - _constraints.tilt = math::max(_param_mpc_tiltmax_air.get(), _param_mpc_man_tilt_max.get()); + || !(constraints.tilt < tilt_max_radians)) { + _constraints.tilt = tilt_max_radians; } if (!PX4_ISFINITE(constraints.speed_up) || !(constraints.speed_up < _param_mpc_z_vel_max_up.get())) { @@ -352,8 +354,4 @@ void PositionControl::updateConstraints(const vehicle_constraints_s &constraints void PositionControl::updateParams() { ModuleParams::updateParams(); - - // Tilt needs to be in radians - _param_mpc_tiltmax_air.set(math::radians(_param_mpc_tiltmax_air.get())); - _param_mpc_man_tilt_max.set(math::radians(_param_mpc_man_tilt_max.get())); } diff --git a/src/modules/mc_pos_control/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl.hpp index 89d0b21b59..2b0eb74a3c 100644 --- a/src/modules/mc_pos_control/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl.hpp @@ -232,9 +232,9 @@ private: (ParamFloat) _param_mpc_z_vel_max_dn, (ParamFloat) _param_mpc_z_vel_max_up, (ParamFloat) - _param_mpc_tiltmax_air, // maximum tilt for any position controlled mode in radians + _param_mpc_tiltmax_air, // maximum tilt for any position controlled mode in degrees (ParamFloat) - _param_mpc_man_tilt_max, // maximum til for stabilized/altitude mode in radians + _param_mpc_man_tilt_max, // maximum til for stabilized/altitude mode in degrees (ParamFloat) _param_mpc_z_p, (ParamFloat) _param_mpc_z_vel_p, (ParamFloat) _param_mpc_z_vel_i,