PosControl: fix hover update equation

The integrator now absorbs properly the change in hover thrust

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)