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/PositionControlTest.cpp

@@ -326,3 +326,32 @@ TEST_F(PositionControlBasicTest, InvalidState)
 	_position_control.setState(states);
 	EXPECT_FALSE(runController());
 }
+
+
+TEST_F(PositionControlBasicTest, UpdateHoverThrust)
+{
+	// GIVEN: some hover thrust and 0 velocity change
+	const float hover_thrust = 0.6f;
+	_position_control.setHoverThrust(hover_thrust);
+
+	_input_setpoint.vx = 0.f;
+	_input_setpoint.vy = 0.f;
+	_input_setpoint.vz = -0.f;
+
+	// WHEN: we run the controller
+	EXPECT_TRUE(runController());
+
+	// THEN: the output thrust equals the hover thrust
+	EXPECT_EQ(_output_setpoint.thrust[2], -hover_thrust);
+
+	// HOWEVER WHEN: we set a new hover thrust through the update function
+	const float hover_thrust_new = 0.7f;
+	_position_control.updateHoverThrust(hover_thrust_new);
+	EXPECT_TRUE(runController());
+
+	// THEN: the integral is updated to avoid discontinuities and
+	// the output is still the same
+	EXPECT_EQ(_output_setpoint.thrust[2], -hover_thrust);
+	const Vector3f integrator_new(_position_control.getIntegral());
+	EXPECT_EQ(integrator_new(2) - hover_thrust_new, -hover_thrust);
+}