PositionControl: deconflict hover thrust estimator, acceleration control

- Avoid constantly adjusting the velocity gains with the HTE
- Make sure the hover thrust integral update doesn't break
  even though its unit is acceleration and not unit thrust anymore

We need to convert the velocity gains to not contain/depend on the
hover thrust. In horizontal direction it doesn't make sense to scale
them with the hover thrust and in vertical direction the adjustments are
already done in the acceleration to collective thrust conversion.

src/modules/mc_pos_control/PositionControl/PositionControl.cpp

@@ -67,7 +67,7 @@ void PositionControl::setThrustLimits(const float min, const float max)
 
 void PositionControl::updateHoverThrust(const float hover_thrust_new)
 {
-	_vel_int(2) += hover_thrust_new - _hover_thrust;
+	_vel_int(2) += (hover_thrust_new - _hover_thrust) * (CONSTANTS_ONE_G / hover_thrust_new);
 	setHoverThrust(hover_thrust_new);
 }
 
@@ -147,8 +147,6 @@ void PositionControl::_velocityControl(const float dt)
 	Vector3f vel_error = _vel_sp - _vel;
 	Vector3f acc_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int - _vel_dot.emult(_gain_vel_d);
 
-	// For backwards compatibility of the gains to non-acceleration based control, needs to be overcome with configuration conversion
-	acc_sp_velocity *= CONSTANTS_ONE_G / _hover_thrust;
 	// No control input from setpoints or corresponding states which are NAN
 	ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity);
 
@@ -179,7 +177,7 @@ void PositionControl::_velocityControl(const float dt)
 	// Use tracking Anti-Windup for horizontal direction: during saturation, the integrator is used to unsaturate the output
 	// see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990
 	const Vector2f acc_sp_xy_limited = Vector2f(_thr_sp) * (CONSTANTS_ONE_G / _hover_thrust);
-	const float arw_gain = 2.f / (_gain_vel_p(0) * (CONSTANTS_ONE_G / _hover_thrust));
+	const float arw_gain = 2.f / _gain_vel_p(0);
 	vel_error.xy() = Vector2f(vel_error) - (arw_gain * (Vector2f(_acc_sp) - acc_sp_xy_limited));
 
 	// Make sure integral doesn't get NAN
@@ -188,7 +186,7 @@ void PositionControl::_velocityControl(const float dt)
 	_vel_int += vel_error.emult(_gain_vel_i) * dt;
 
 	// limit thrust integral
-	_vel_int(2) = math::min(fabsf(_vel_int(2)), _lim_thr_max) * sign(_vel_int(2));
+	_vel_int(2) = math::min(fabsf(_vel_int(2)), CONSTANTS_ONE_G) * sign(_vel_int(2));
 }
 
 void PositionControl::_accelerationControl()

src/modules/mc_pos_control/PositionControl/PositionControl.hpp

@@ -163,11 +163,6 @@ 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.
@@ -205,7 +200,7 @@ private:
 	float _lim_thr_max{}; ///< Maximum collective thrust allowed as output [-1,0] e.g. -0.1
 	float _lim_tilt{}; ///< Maximum tilt from level the output attitude is allowed to have
 
-	float _hover_thrust{}; ///< Thrust [0,1] with which the vehicle hovers not aacelerating down or up with level orientation
+	float _hover_thrust{}; ///< Thrust [0,1] with which the vehicle hovers not accelerating down or up with level orientation
 
 	// States
 	matrix::Vector3f _pos; /**< current position */

src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp

@@ -76,7 +76,7 @@ public:
 	PositionControlBasicTest()
 	{
 		_position_control.setPositionGains(Vector3f(1.f, 1.f, 1.f));
-		_position_control.setVelocityGains(Vector3f(1.f, 1.f, 1.f), Vector3f(1.f, 1.f, 1.f), Vector3f(1.f, 1.f, 1.f));
+		_position_control.setVelocityGains(Vector3f(20.f, 20.f, 20.f), Vector3f(20.f, 20.f, 20.f), Vector3f(20.f, 20.f, 20.f));
 		_position_control.setVelocityLimits(1.f, 1.f, 1.f);
 		_position_control.setThrustLimits(0.1f, 0.9f);
 		_position_control.setTiltLimit(1.f);
@@ -387,6 +387,4 @@ TEST_F(PositionControlBasicTest, UpdateHoverThrust)
 	// 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

@@ -362,9 +362,12 @@ MulticopterPositionControl::parameters_update(bool force)
 		}
 
 		_control.setPositionGains(Vector3f(_param_mpc_xy_p.get(), _param_mpc_xy_p.get(), _param_mpc_z_p.get()));
-		_control.setVelocityGains(Vector3f(_param_mpc_xy_vel_p.get(), _param_mpc_xy_vel_p.get(), _param_mpc_z_vel_p.get()),
-					  Vector3f(_param_mpc_xy_vel_i.get(), _param_mpc_xy_vel_i.get(), _param_mpc_z_vel_i.get()),
-					  Vector3f(_param_mpc_xy_vel_d.get(), _param_mpc_xy_vel_d.get(), _param_mpc_z_vel_d.get()));
+		// backwards compatibility scale for velocity gains to non-acceleration based control, needs to be overcome with configuration conversion
+		const float hover_scale = 20.f;
+		_control.setVelocityGains(Vector3f(_param_mpc_xy_vel_p.get(), _param_mpc_xy_vel_p.get(),
+						   _param_mpc_z_vel_p.get()) * hover_scale,
+					  Vector3f(_param_mpc_xy_vel_i.get(), _param_mpc_xy_vel_i.get(), _param_mpc_z_vel_i.get()) * hover_scale,
+					  Vector3f(_param_mpc_xy_vel_d.get(), _param_mpc_xy_vel_d.get(), _param_mpc_z_vel_d.get()) * hover_scale);
 		_control.setVelocityLimits(_param_mpc_xy_vel_max.get(), _param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get());
 		_control.setThrustLimits(_param_mpc_thr_min.get(), _param_mpc_thr_max.get());
 		_control.setTiltLimit(M_DEG_TO_RAD_F * _param_mpc_tiltmax_air.get()); // convert to radians!