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

MulticopterPositionControl: add horizontal margin for saturation cases

Browse Source
This commit is contained in:
Matthias Grob
2021-09-07 14:24:25 +02:00
parent 06a91ec752
commit 58ea97a699
7 changed files with 54 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
+17 -9
View File
@@ -65,6 +65,11 @@ void PositionControl::setThrustLimits(const float min, const float max)
_lim_thr_max = max;
}
void PositionControl::setHorizontalThrustMargin(const float margin)
{
_lim_thr_xy_margin = margin;
}
void PositionControl::updateHoverThrust(const float hover_thrust_new)
{
_vel_int(2) += (hover_thrust_new - _hover_thrust) * (CONSTANTS_ONE_G / hover_thrust_new);
@@ -137,13 +142,19 @@ void PositionControl::_velocityControl(const float dt)
vel_error(2) = 0.f;
}
// Saturate maximal vertical thrust
_thr_sp(2) = math::max(_thr_sp(2), -_lim_thr_max);
// Prioritize vertical control while keeping a horizontal margin
const Vector2f thrust_sp_xy(_thr_sp);
const float thrust_sp_xy_norm = thrust_sp_xy.norm();
// Get allowed horizontal thrust after prioritizing vertical control
const float thrust_max_squared = _lim_thr_max * _lim_thr_max;
const float thrust_z_squared = _thr_sp(2) * _thr_sp(2);
const float thrust_max_xy_squared = thrust_max_squared - thrust_z_squared;
// Determine how much vertical thrust is left keeping horizontal margin
const float allocated_horizontal_thrust = math::min(thrust_sp_xy_norm, _lim_thr_xy_margin);
const float thrust_z_max_squared = math::sq(_lim_thr_max) - math::sq(allocated_horizontal_thrust);
// Saturate maximal vertical thrust
_thr_sp(2) = math::max(_thr_sp(2), -sqrtf(thrust_z_max_squared));
// Determine how much horizontal thrust is left after prioritizing vertical control
const float thrust_max_xy_squared = thrust_z_max_squared - math::sq(_thr_sp(2));
float thrust_max_xy = 0;
if (thrust_max_xy_squared > 0) {
@@ -151,9 +162,6 @@ void PositionControl::_velocityControl(const float dt)
}
// Saturate thrust in horizontal direction
const Vector2f thrust_sp_xy(_thr_sp);
const float thrust_sp_xy_norm = thrust_sp_xy.norm();
if (thrust_sp_xy_norm > thrust_max_xy) {
_thr_sp.xy() = thrust_sp_xy / thrust_sp_xy_norm * thrust_max_xy;
}