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

PosControl: fix hover update equation

Browse Source 
The integrator now absorbs properly the change in hover thrust
This commit is contained in:
bresch 2022-01-07 16:31:38 +01:00 committed by Matthias Grob
parent 6c97700eaa
commit 57fa9c545a
1 changed files with 11 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
View File

@ -72,8 +72,17 @@ void PositionControl::setHorizontalThrustMargin(const float margin)
void PositionControl::updateHoverThrust(const float hover_thrust_new)
{
_vel_int(2) += (hover_thrust_new - _hover_thrust) * (CONSTANTS_ONE_G / hover_thrust_new);
setHoverThrust(hover_thrust_new);
// Given that the equation for thrust is T = a_sp * Th / g - Th
// with a_sp = desired acceleration, Th = hover thrust and g = gravity constant,
// we want to find the acceleration that needs to be added to the integrator in order obtain
// the same thrust after replacing the current hover thrust by the new one.
// 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);
}
}
void PositionControl::setState(const PositionControlStates &states)