mc_pos_control: Explicitly convert tilt variables to radians during c…

…heck and assignments
PX4 · Jun 27, 2019 · c5deba5 · c5deba5
1 parent 748ff7f
commit c5deba5
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 8 deletions.
diff --git 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
@@ -232,9 +232,9 @@ class PositionControl : public ModuleParams
 		(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,