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

mc_pos_control: fix potential thrust spike on hover thrust change

Browse Source 
Co-authored-by: Josh Henderson <hendjoshsr71@gmail.com>
Co-authored-by: Matthias Grob <maetugr@gmail.com>
This commit is contained in:
Mathieu Bresciani
2023-05-09 03:42:25 +02:00
committed by GitHub
parent da14650aa2
commit 1e4fcfc614
3 changed files with 15 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
+8 -7
View File
@@ -81,10 +81,11 @@ void PositionControl::updateHoverThrust(const float hover_thrust_new)
// T' = T => a_sp' * Th' / g - Th' = a_sp * Th / g - Th
// so a_sp' = (a_sp - g) * Th / Th' + g
// we can then add a_sp' - a_sp to the current integrator to absorb the effect of changing Th by Th'
if (hover_thrust_new > FLT_EPSILON) {
_vel_int(2) += (_acc_sp(2) - CONSTANTS_ONE_G) * _hover_thrust / hover_thrust_new + CONSTANTS_ONE_G - _acc_sp(2);
setHoverThrust(hover_thrust_new);
}
const float previous_hover_thrust = _hover_thrust;
setHoverThrust(hover_thrust_new);
_vel_int(2) += (_acc_sp(2) - CONSTANTS_ONE_G) * previous_hover_thrust / _hover_thrust
+ CONSTANTS_ONE_G - _acc_sp(2);
}
void PositionControl::setState(const PositionControlStates &states)
@@ -138,6 +139,9 @@ void PositionControl::_positionControl()
void PositionControl::_velocityControl(const float dt)
{
// Constrain vertical velocity integral
_vel_int(2) = math::constrain(_vel_int(2), -CONSTANTS_ONE_G, CONSTANTS_ONE_G);
// PID velocity control
Vector3f vel_error = _vel_sp - _vel;
Vector3f acc_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int - _vel_dot.emult(_gain_vel_d);
@@ -195,9 +199,6 @@ void PositionControl::_velocityControl(const float dt)
ControlMath::setZeroIfNanVector3f(vel_error);
// Update integral part of velocity control
_vel_int += vel_error.emult(_gain_vel_i) * dt;
// limit thrust integral
_vel_int(2) = math::min(fabsf(_vel_int(2)), CONSTANTS_ONE_G) * sign(_vel_int(2));
}
void PositionControl::_accelerationControl()