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

PositionControl: if valid velocity and position setpoint available, always prioritize

Browse Source 
velocity setpoint generated from position error over velocity feedforward
This commit is contained in:
Dennis Mannhart
2018-02-07 13:19:34 +01:00
committed by Lorenz Meier
parent 5551021d23
commit 3c40aa0d9c
3 changed files with 61 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl.cpp
+7 -10
View File
@@ -174,18 +174,15 @@ void PositionControl::_positionController()
/* Generate desired velocity setpoint */
/* P-controller */
_vel_sp = (_pos_sp - _pos).emult(Pp) + _vel_sp;
Vector3f vel_sp_position = (_pos_sp - _pos).emult(Pp);
_vel_sp = vel_sp_position + _vel_sp;
/* Make sure velocity setpoint is constrained in all directions (xyz). */
Vector2f vel_sp_xy(&_vel_sp(0));
if (vel_sp_xy.length() >= _VelMaxXY) {
_vel_sp(0) = _vel_sp(0) * _VelMaxXY / vel_sp_xy.length();
_vel_sp(1) = _vel_sp(1) * _VelMaxXY / vel_sp_xy.length();
}
/* Saturate velocity in D-direction */
_vel_sp(2) = math::constrain(_vel_sp(2), -_VelMaxZ.up, _VelMaxZ.down);
Vector2f vel_sp_xy = ControlMath::constrainXY(Vector2f(&vel_sp_position(0)), Vector2f(&(_vel_sp - vel_sp_position)(0)),
_VelMaxXY);
_vel_sp(0) = vel_sp_xy(0);
_vel_sp(1) = vel_sp_xy(1);
_vel_sp(2) = math::constrain(_vel_sp(2), -_VelMaxZ[0], _VelMaxZ[1]);
}
void PositionControl::_velocityController(const float &dt)