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

PositionControl: simplify horizontal thrust limiting

Browse Source
This commit is contained in:
Matthias Grob
2020-01-30 07:28:18 -05: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); thrust_max_NE = math::min(thrust_max_NE_tilt, thrust_max_NE);
// Saturate thrust in NE-direction. // 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) { if (thrust_sp_xy_norm > thrust_max_NE) {
float mag = thrust_sp_xy.length(); _thr_sp.xy() = thrust_sp_xy / thrust_sp_xy_norm * thrust_max_NE;
_thr_sp(0) = _thr_sp(0) / mag * thrust_max_NE;
_thr_sp(1) = _thr_sp(1) / mag * thrust_max_NE;
} }
// Use tracking Anti-Windup for NE-direction: during saturation, the integrator is used to unsaturate the output // Use tracking Anti-Windup for NE-direction: during saturation, the integrator is used to unsaturate the output