From 57fa9c545aefceef6adef82b454f72990c84e8a9 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 7 Jan 2022 16:31:38 +0100 Subject: [PATCH] PosControl: fix hover update equation The integrator now absorbs properly the change in hover thrust --- .../PositionControl/PositionControl.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 8008e2b338..07f8eb004e 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -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)