fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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 17:38:21 +02:00 committed by Daniel Agar
parent 748ff7f50a
commit c5deba53a2
2 changed files with 6 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/mc_pos_control/PositionControl.cpp
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
// 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()));
}

4
src/modules/mc_pos_control/PositionControl.hpp
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_UP>) _param_mpc_z_vel_max_up,
(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>)
_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_VEL_P>) _param_mpc_z_vel_p,
(ParamFloat<px4::params::MPC_Z_VEL_I>) _param_mpc_z_vel_i,