fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 13:04:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

PositionControl: check if constraints are below global maximum

Browse Source
This commit is contained in:
Dennis Mannhart 2018-03-27 15:12:42 +02:00 committed by Lorenz Meier
parent 1cde38f82f
commit 673945f28b
2 changed files with 11 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
src/modules/mc_pos_control/PositionControl.cpp
View File

@ -108,7 +108,6 @@ void PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &se
void PositionControl::generateThrustYawSetpoint(const float &dt)
{
_updateParams();
/* Only run position/velocity controller
* if thrust needs to be generated
@ -188,7 +187,7 @@ void PositionControl::_positionController()
_VelMaxXY);
_vel_sp(0) = vel_sp_xy(0);
_vel_sp(1) = vel_sp_xy(1);
_vel_sp(2) = math::constrain(_vel_sp(2), -_constraints.vel_max_z_up, _VelMaxZ.down);
_vel_sp(2) = math::constrain(_vel_sp(2), -_VelMaxZ.up, _VelMaxZ.down);
}
void PositionControl::_velocityController(const float &dt)
@ -218,13 +217,6 @@ void PositionControl::_velocityController(const float &dt)
* NE-direction is also limited by the maximum tilt.
*/
/* Get maximum tilt */
float tilt_max = M_PI_2_F;
if (PX4_ISFINITE(_constraints.tilt_max) && _constraints.tilt_max <= tilt_max) {
tilt_max = _constraints.tilt_max;
}
Vector3f vel_err = _vel_sp - _vel;
/* Consider thrust in D-direction */
@ -252,7 +244,7 @@ void PositionControl::_velocityController(const float &dt)
* scaling is required.
*/
float thr_xy_max = fabsf(_thr_sp(2)) * tanf(tilt_max);
float thr_xy_max = fabsf(_thr_sp(2)) * tanf(_tilt_max);
_thr_sp(0) *= thr_xy_max;
_thr_sp(1) *= thr_xy_max;
@ -264,7 +256,7 @@ void PositionControl::_velocityController(const float &dt)
thrust_desired_NE(1) = Pv(1) * vel_err(1) + Dv(1) * _vel_dot(1) + _thr_int(1);
/* Get maximum allowed thrust in NE based on tilt and excess thrust */
float thrust_max_NE_tilt = fabsf(_thr_sp(2)) * tanf(tilt_max);
float thrust_max_NE_tilt = fabsf(_thr_sp(2)) * tanf(_tilt_max);
float thrust_max_NE = sqrtf(_ThrustLimit.max * _ThrustLimit.max - _thr_sp(2) * _thr_sp(2));
thrust_max_NE = math::min(thrust_max_NE_tilt, thrust_max_NE);
@ -295,7 +287,12 @@ void PositionControl::_velocityController(const float &dt)
void PositionControl::updateConstraints(const Controller::Constraints &constraints)
{
_constraints = constraints;
// update all parameters since they might have changed
_updateParams();
// maximum tilt cannot exceed 90 degrees
_tilt_max = math::min(constraints.tilt_max, M_PI_2_F);
// maximum velocity upwards cannot exceed global limit
_VelMaxZ.up = math::min(constraints.vel_max_z_up, _VelMaxZ.up);
}
void PositionControl::_updateParams()
@ -336,4 +333,5 @@ void PositionControl::_setParams()
param_get(_ThrMax_h, &_ThrustLimit.max);
param_get(_ThrMinPosition_h, &_ThrMinPosition);
param_get(_ThrMinStab_h, &_ThrMinStab);
}

3
src/modules/mc_pos_control/PositionControl.hpp
View File

@ -55,8 +55,6 @@ namespace Controller
struct Constraints {
float tilt_max;
float vel_max_z_up;
float vel_max_z_down;
float vel_max_xy;
};
}
@ -136,6 +134,7 @@ private:
float _ThrHover{0.5f};
float _ThrMinPosition{0.0f}; // minimum throttle for any position controlled mode
float _ThrMinStab{0.0f}; // minimum throttle for stabilized
float _tilt_max{1.5f}; /**< maximum tilt */
bool _skipController{false};
/* Helper methods */