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;

src/modules/mc_pos_control/PositionControl/PositionControl.hpp

@@ -114,10 +114,17 @@ public:
 	void setTiltLimit(const float tilt) { _lim_tilt = tilt; }
 
 	/**
-	 * Set the maximum tilt angle in radians the output attitude is allowed to have
-	 * @param thrust [0,1] with which the vehicle hovers not aacelerating down or up with level orientation
+	 * Set the normalized hover thrust
+	 * @param thrust [0,1] with which the vehicle hovers not acelerating down or up with level orientation
+	 */
+	void setHoverThrust(const float hover_thrust) { _hover_thrust = hover_thrust; }
+
+	/**
+	 * Update the hover thrust without immediately affecting the output
+	 * by adjusting the integrator. This prevents propagating the dynamics
+	 * of the hover thrust signal directly to the output of the controller.
 	 */
-	void setHoverThrust(const float thrust) { _hover_thrust = thrust; }
+	void updateHoverThrust(const float hover_thrust_new);
 
 	/**
 	 * Pass the current vehicle state to the controller
@@ -156,6 +163,11 @@ public:
 	 */
 	void resetIntegral() { _vel_int.setZero(); }
 
+	/**
+	 * @return the value of the velocity integrator
+	 */
+	matrix::Vector3f getIntegral() const { return _vel_int; }
+
 	/**
 	 * Get the controllers output local position setpoint
 	 * These setpoints are the ones which were executed on including PID output and feed-forward.

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);
+}

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -373,7 +373,7 @@ MulticopterPositionControl::parameters_update(bool force)
 				mavlink_log_critical(&_mavlink_log_pub, "Hover thrust has been constrained by min/max");
 			}
 
-			_control.setHoverThrust(_param_mpc_thr_hover.get());
+			_control.updateHoverThrust(_param_mpc_thr_hover.get());
 		}
 
 		_flight_tasks.handleParameterUpdate();
@@ -415,7 +415,7 @@ MulticopterPositionControl::poll_subscriptions()
 		hover_thrust_estimate_s hte;
 
 		if (_hover_thrust_estimate_sub.update(&hte)) {
-			_control.setHoverThrust(hte.hover_thrust);
+			_control.updateHoverThrust(hte.hover_thrust);
 		}
 	}
 }