MPC: add updateHoverThrust function

This function updates the vertical velocity integrator with the change
in hover thrust to avoid propagating discontinuities through the
controller when changing the hover thrust.
This is also important when using the hover thrust estimator as its
estimate has unconstrained dynamics and can cause drops or kicks when
the estimate updates faster than the velocity integrator.

src/modules/mc_pos_control/PositionControl/PositionControl.cpp

@@ -63,6 +63,12 @@ void PositionControl::setThrustLimits(const float min, const float max)
 	_lim_thr_max = max;
 }
 
+void PositionControl::updateHoverThrust(const float hover_thrust_new)
+{
+	_vel_int(2) += hover_thrust_new - _hover_thrust;
+	setHoverThrust(hover_thrust_new);
+}
+
 void PositionControl::setState(const PositionControlStates &states)
 {
 	_pos = states.position;