fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-29 17:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

PositionControl: simplify horizontal thrust limiting

Browse Source
This commit is contained in:
Matthias Grob
2019-12-15 15:34:36 +01:00
committed by Kabir Mohammed
parent 83653d6581
commit 9109d22c28
1 changed files with 4 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
+4 -5
View File
@@ -317,12 +317,11 @@ void PositionControl::_velocityControl(const float dt)
thrust_max_NE = math::min(thrust_max_NE_tilt, thrust_max_NE);
// Saturate thrust in NE-direction.
Vector2f thrust_sp_xy(_thr_sp);
const Vector2f thrust_sp_xy(_thr_sp);
const float thrust_sp_xy_norm = thrust_sp_xy.norm();
if (thrust_sp_xy.norm_squared() > thrust_max_NE * thrust_max_NE) {
float mag = thrust_sp_xy.length();
_thr_sp(0) = _thr_sp(0) / mag * thrust_max_NE;
_thr_sp(1) = _thr_sp(1) / mag * thrust_max_NE;
if (thrust_sp_xy_norm > thrust_max_NE) {
_thr_sp.xy() = thrust_sp_xy / thrust_sp_xy_norm * thrust_max_NE;
}
// Use tracking Anti-Windup for NE-direction: during saturation, the integrator is used to unsaturate the output