diff --git a/ROMFS/px4fmu_common/init.d-posix/6101_hexa_x_tilt b/ROMFS/px4fmu_common/init.d-posix/6101_hexa_x_tilt index c12bc83f46..036617c567 100644 --- a/ROMFS/px4fmu_common/init.d-posix/6101_hexa_x_tilt +++ b/ROMFS/px4fmu_common/init.d-posix/6101_hexa_x_tilt @@ -25,7 +25,7 @@ then param set MPC_Z_VEL_P 0.3 param set MC_PITCHRATE_MAX 20.0 param set MC_ROLLRATE_MAX 20.0 - # param set MC_YAWRATE_MAX 60.0 + param set MC_YAWRATE_MAX 60.0 param set MPC_ACC_DOWN_MAX 2 param set MPC_ACC_HOR 0.5 param set MPC_ACC_HOR_MAX 2 @@ -39,7 +39,7 @@ then param set SDLOG_PROFILE 179 param set OMNI_ATT_MODE 2 - param set OMNI_DFC_MAX_THR 0.15 + param set OMNI_DFC_MAX_THR 0.20 fi set MAV_TYPE 13 diff --git a/ROMFS/px4fmu_common/init.d/airframes/6100_hexa_tilt_x b/ROMFS/px4fmu_common/init.d/airframes/6100_hexa_tilt_x index 7ba73c0e26..c2c8d4ec05 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/6100_hexa_tilt_x +++ b/ROMFS/px4fmu_common/init.d/airframes/6100_hexa_tilt_x @@ -39,6 +39,7 @@ then param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 param set MC_YAW_FF 0.5 + param set MC_YAWRATE_MAX 60.0 param set MPC_HOLD_MAX_XY 0.25 param set MPC_THR_MIN 0.15 @@ -47,7 +48,7 @@ then param set BAT_N_CELLS 4 param set OMNI_ATT_MODE 2 - param set OMNI_DFC_MAX_THR 0.15 + param set OMNI_DFC_MAX_THR 0.20 fi # Set mixer diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index a99d567086..b5527629b7 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -89,7 +89,7 @@ bool PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s & return mapping_succeeded; } -void PositionControl::setConstraints(const vehicle_constraints_s &constraints) +void PositionControl::setConstraints(const vehicle_constraints_s &constraints, const float lim_thr_hor_max) { _constraints = constraints; @@ -107,6 +107,8 @@ void PositionControl::setConstraints(const vehicle_constraints_s &constraints) _constraints.speed_down = _lim_vel_down; } + _lim_thr_hor_max = lim_thr_hor_max; + // ignore _constraints.speed_xy TODO: remove it completely as soon as no task uses it anymore to avoid confusion } @@ -321,6 +323,7 @@ void PositionControl::_velocityControl(const float dt) float thrust_max_NE_tilt = fabsf(_thr_sp(2)) * tanf(_constraints.tilt); float thrust_max_NE = sqrtf(_lim_thr_max * _lim_thr_max - _thr_sp(2) * _thr_sp(2)); thrust_max_NE = math::min(thrust_max_NE_tilt, thrust_max_NE); + thrust_max_NE = math::min(_lim_thr_hor_max, thrust_max_NE); // Saturate thrust in NE-direction. _thr_sp(0) = thrust_desired_NE(0); diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp index 680f0e19de..6fe7c69e3e 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp @@ -137,8 +137,9 @@ public: * Pass constraints that are stricter than the global limits * Note: NAN value means no constraint, take maximum limit of controller. * @param constraints a PositionControl structure with supported constraints + * @param lim_thr_hor_max maximum horizontal thrust (read from parameter for OMNI_ATT_MODE = 2, otherwise = 1.0F) */ - void setConstraints(const vehicle_constraints_s &constraints); + void setConstraints(const vehicle_constraints_s &constraints, const float lim_thr_hor_max); /** * Apply P-position and PID-velocity controller that updates the member @@ -207,6 +208,7 @@ private: float _lim_thr_min{}; ///< Minimum collective thrust allowed as output [-1,0] e.g. -0.9 float _lim_thr_max{}; ///< Maximum collective thrust allowed as output [-1,0] e.g. -0.1 float _lim_tilt{}; ///< Maximum tilt from level the output attitude is allowed to have + float _lim_thr_hor_max; ///< Maximum direct-force horizontal thrust allowed as output [0, 1] float _hover_thrust{}; ///< Thrust [0,1] with which the vehicle hovers not aacelerating down or up with level orientation diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 05ba55f2bc..dc31b52411 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -647,7 +647,9 @@ MulticopterPositionControl::Run() // Run position control _control.setState(_states); - _control.setConstraints(constraints); + + float max_hor_thrust = (_param_omni_att_mode.get() == 2) ? _param_omni_dfc_max_thr.get() : 1.0F; + _control.setConstraints(constraints, max_hor_thrust); if (!_control.setInputSetpoint(setpoint)) { warn_rate_limited("PositionControl: invalid setpoints");