fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 03:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_pos_control: Explicitly convert tilt variables to radians during check and assignments

Browse Source
This commit is contained in:
Mathieu Bresciani
2019-06-27 11:38:21 -04:00
committed by Daniel Agar
parent 748ff7f50a
commit c5deba53a2
2 changed files with 6 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl.cpp
+4 -6
View File
@@ -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 // 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. // 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) if (!PX4_ISFINITE(constraints.tilt)
|| !(constraints.tilt < math::max(_param_mpc_tiltmax_air.get(), _param_mpc_man_tilt_max.get()))) { || !(constraints.tilt < tilt_max_radians)) {
_constraints.tilt = math::max(_param_mpc_tiltmax_air.get(), _param_mpc_man_tilt_max.get()); _constraints.tilt = tilt_max_radians;
} }
if (!PX4_ISFINITE(constraints.speed_up) || !(constraints.speed_up < _param_mpc_z_vel_max_up.get())) { 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() void PositionControl::updateParams()
{ {
ModuleParams::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()));
} }
src/modules/mc_pos_control/PositionControl.hpp
+2 -2
View File
@@ -232,9 +232,9 @@ private:
(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn, (ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn,
(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up, (ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up,
(ParamFloat<px4::params::MPC_TILTMAX_AIR>) (ParamFloat<px4::params::MPC_TILTMAX_AIR>)
_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<px4::params::MPC_MAN_TILT_MAX>) (ParamFloat<px4::params::MPC_MAN_TILT_MAX>)
_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<px4::params::MPC_Z_P>) _param_mpc_z_p, (ParamFloat<px4::params::MPC_Z_P>) _param_mpc_z_p,
(ParamFloat<px4::params::MPC_Z_VEL_P>) _param_mpc_z_vel_p, (ParamFloat<px4::params::MPC_Z_VEL_P>) _param_mpc_z_vel_p,
(ParamFloat<px4::params::MPC_Z_VEL_I>) _param_mpc_z_vel_i, (ParamFloat<px4::params::MPC_Z_VEL_I>) _param_mpc_z_vel_i,